#include <stdio.h>
#include "xc.h"
#include "UART.h"
#include "config.h"
#include "extern.h"

#ifdef DEBUG

void InitU1(void)
{
    U1BRG = BRATE;
    U1MODE = U_ENABLE_H | U_NOFLOWCONTROL;  //Enable in high speed mode without flow control
    U1STA = U_TX;
    TRTS1 = 0;  // make RTS output
    RTS1 = 1;  //set TRS default status
    _U1RXIF = 0; //Clear Interrupt
    _U1RXIE = 1;  //Enable Interrupt
    RTS1 = 0;  //Accept characters.
}  //InitU1


void InitU2(void)
{
    U2BRG = BRATE;
    U2MODE = U_ENABLE_H | U_NOFLOWCONTROL;  //Enable in high speed mode without flow control
    U2STA = U_TX;
    TRTS2 = 0;  // make RTS output
    RTS2 = 1;  //set TRS default status
    _U2RXIF = 0; //Clear Interrupt
    _U2RXIE = 1;  //Enable Interrupt
    RTS2 = 0;  //Accept characters.
}  //InitU2

int putU1( int c)
{
    while(U1STAbits.UTXBF);  //This needs a timeout.
    U1TXREG = c;
    return c;
}  //putU1

void putsU1(char *s)
{
    while(*s)
        putU1(*s++);  // No protection for non null-terminated arguments.
}
int getsU1(char *input_string)
{
    U1_StringReceived = 0; 
    U1_BufCtr = 0;  //Clear input buffer
    while(U1_StringReceived == 0);  // Blocking Read, could benefit from timeout
    for(int i = 0; i < U1_StringReceived; i++)
       input_string[i] =  U1_inputBuf[i];
    input_string[U1_StringReceived] = 0; //Null Terminate

    return U1_StringReceived;
}

int GetFloatU1(float *val) {
    char InputString[U1_InputBufSize];
    getsU1(InputString);
    float tmp_float;
    int n = sscanf(InputString, "%f", &tmp_float);
    if (n == 1) {
        *val = tmp_float;
        return 0;
    } else
        return -1;
}

int GetIntU1(int *val) {
    char InputString[U1_InputBufSize];
    getsU1(InputString);
    int tmp_int;
    int n = sscanf(InputString, "%d", &tmp_int);
    if (n == 1) {
        *val = tmp_int;
        return 0;
    } else
        return -1;
}


char getU1(void) {
    RTS1 = 0;
    while (!U1STAbits.URXDA);
    RTS1 = 1;
    return U1RXREG;
} //getU1


int putU2( int c)
{
    //while (CTS);
    while(U2STAbits.UTXBF);  //This needs a timeout.
    U2TXREG = c;
    return c;
}  //putU2

void putsU2(char *s)
{
    while(*s)
        putU2(*s++);  // No protection for non null-terminated arguments.
}

char getU2(void) {
    RTS2 = 0;
    while (!U2STAbits.URXDA);
    RTS2 = 1;
    return U2RXREG;
} //getU2

#endif