/*
 * File:   UART.c
 * Author: Christian Mares
 *
 * Created on 15 de diciembre de 2017, 17:10
 */

#include "main.h"

int TX_Interrupt_callTimes=0;

char errorMessageString[26*MESSAGE_LineAmount];
char* eMS_max = errorMessageString+ (26*MESSAGE_LineAmount);
char* eMS_lp;

// ================ UART2 Serial Port ======================
//extern volatile char DIRECT_MODE;
//extern volatile char BL_PID_ENABLED;

char CMD= OUTPUT?0:1,   RX;
volatile bool OUTPUT_ENABLED = OUTPUT;
volatile bool DIRECT_MODE = DIRMOD;

char PID_equation[50];
char* PID_e_max=PID_equation+50,PID_e_lp;


/** RX buffer (FIFO)**/
#define RX_BUF_SIZE 20
#define RX_BUF_Last (RX_BUF_pos-1)

unsigned char RX_BUF_pos = 0;
char RX_Buffer[RX_BUF_SIZE];

char RXBUF_Vacio(void)
{
    return RX_BUF_pos==0;
}
char RXBUF_Lleno(void)
{
    return (char)(RX_BUF_pos == RX_BUF_SIZE);
}
void RXBUF_Aniadir1(char rx)
{
    if(RXBUF_Lleno()){return;}
    
    RX_Buffer[RX_BUF_pos] = rx;
    RX_BUF_pos++; 
}
char RXBUF_LeerPrim(void)
{
    if(RXBUF_Vacio()){return '\x1b';}
    
    int i;
    char ret = RX_Buffer[0];
    
    RX_BUF_pos--;
    for(i=0; i<RX_BUF_pos; i++){
        RX_Buffer[i] = RX_Buffer[i+1];
    }
    
    return ret;
}
void RXBUF_Vaciar(void)
{
    int i;
    for(i=0; i<RX_BUF_SIZE; i++){
        RX_Buffer[i] = '\0';
    }
    RX_BUF_pos = 0;
}

/** CMD buffer (LIFO)**/
#define CMD_BUF_SIZE 20
#define CDATA_BUF_SIZE 8
#define CMD_BUF_Last (CMD_BUF_pos-1)
#define CDATA_BUF_Last (CDATA_BUF_pos-1)

unsigned char CMD_BUF_pos = 0;
char CMD_Buffer[CMD_BUF_SIZE];
unsigned char CDATA_BUF_pos = 0;
char CDATA_Buffer[CDATA_BUF_SIZE];

char CMDBUF_Lleno(void)
{
    return (char)(CMD_BUF_pos == CMD_BUF_SIZE);
}
void CMDBUF_Aniadir1(char rx)
{
    if(CMDBUF_Lleno()){return;}
    
    CMD_Buffer[CMD_BUF_pos] = rx;
    CMD_BUF_pos++; 
}
void CMDBUF_BorrUlt(void)
{
    CMD_Buffer[CMD_BUF_pos]='\0';
    CMD_BUF_pos--; 
}
void CMDBUF_Vaciar(void)
{
    int i;
    for(i=0; i<CMD_BUF_SIZE; i++){
        CMD_Buffer[i] = '\0';
    }
    CMD_BUF_pos = 0;
}

char CDATABUF_Lleno(void)
{
    return (char)(CDATA_BUF_pos == CDATA_BUF_SIZE);
}
void CDATABUF_Aniadir1(char rx)
{
    CDATA_Buffer[CDATA_BUF_pos] = rx;
    CDATA_BUF_pos++; 
}
void CDATABUF_BorrUlt(void)
{
    CDATA_Buffer[CDATA_BUF_pos]='\0';
    CDATA_BUF_pos--; 
}
void CDATABUF_Vaciar(void)
{
    int i;
    for(i=0; i<CDATA_BUF_SIZE; i++){
        CDATA_Buffer[i] = '\0';
    }
    CDATA_BUF_pos = 0;
}
int CDATABUF_Parse()
{
    if(CDATA_Buffer[0]!='\0'){
            int CDATA_int=0,i=1;
            CDATA_int = CDATA_Buffer[0]-48;
            while(CDATA_Buffer[i]!='\0' && i<8){
                CDATA_int*= 10;
                CDATA_int+= CDATA_Buffer[i]-48;
                i++;
            }
            return CDATA_int;
    }
    return 0;
}

/** TX Buffer **/
char TX;
    
/** Direct mode buffer (FIFO)**/
#define DMB_size 400
char DM_Buffer[DMB_size], *DMB_max=DM_Buffer+DMB_size;
char *DMB_first, *DMB_cap;

int DMB_Anadir1(char c){
    if( (DMB_cap+1)!=DMB_first && ((DMB_cap+1)!=DMB_max || DMB_first!=&DM_Buffer[0])){
        *(DMB_cap++)=c;
        if(DMB_cap==DMB_max){
            DMB_cap = &DM_Buffer[0];
        }
        return 0;
    }
    return 1;
}

/**    Err    **/
unsigned int  Errores;

/**/
#define number_LEN 10
char number[number_LEN];

void Init_UART()
{
    U2MODE = 0; 
    _BRGH_U2 = 0;
    U2BRG = BAUD_RATEREG_2_BRGH0;
    U2STA = 0;
    _OERR_U2=0;      // Según versión corrige bug
    
    _URXISEL1_U2 = 0; //Seleccionar modo de interrupcion:
    _URXISEL0_U2 = 1; //Interrumpir al transmitir ultimo bit del TSR
    
//    U2TXREG='@'; // Enviar byte de prueba
    
    _UARTEN_U2 = 1; // Habilita la UART_2
    _UTXEN_U2 = 1; //Habilita transmisión,
    
    //Interrupciones
    //Transmision
//    _U2TXIP = 1;   // Prioridad baja
    _U2TXIF = 0;  // Borrar flag
    _U2TXIE = 1; // Habilita interrupcion
    //Recepcion
    _U2RXIP = 3;  //Idem
    _U2RXIF = 0; 
    _U2RXIE = 1;
    //Error
    _U2ERIP = 1;  //Idem
    _U2ERIF = 0;  
    _U2ERIE = 1; 
    
    
    //* ##Sustituir: --> delay()/
    delay(400);
    //* ························/
    
//    while(!_TRMT_U2){}
    
    DMB_first = &DM_Buffer[0];
    DMB_cap=DMB_first;
    
    Nop();
    Nop();
    Nop();
    Nop();
    
}

void UART_put(char c)
{
    while ( _UTXBF_U2 == 1); // Espera mientras buffer Tx está lleno
    U2TXREG = c; // Carga dato a transmitir
}
char UART_get(void)
{
    while ( !_URXDA_U2); // Espera a que llegue un dato
    return U2RXREG; // recoge el dato
}

int UART_enviarString(char* buffer)
{
    int i;
    unsigned int len= length(buffer);
    for (i = len; i; --i)
        DMB_Anadir1( *(buffer++));
    return(len);
}
int UART_enviarString_L(char* buffer, int len)
{
    int i;
    for (i = len; i; --i)
        DMB_Anadir1( *(buffer++));
    return(len);
}

void UART_start_transmision()
{
    U2TXREG=0;
}

void CMD_exeChar()
{
    char c = RXBUF_LeerPrim();
    switch(c){
        case '8':
            if(LED_D3)LED_D3=0;else LED_D3=1;
            break;
        case '7':
            if(LED_D4)LED_D4=0;else LED_D4=1;
            break;
        case '6':
            if(LED_D5)LED_D5=0;else LED_D5=1;
            break;
        case '5':
            if(LED_D6)LED_D6=0;else LED_D6=1;
            break;
        case '4':
            if(LED_D7)LED_D7=0;else LED_D7=1;
            break;
        case '3':
            if(LED_D8)LED_D8=0;else LED_D8=1;
            break;
        case '2':
            if(LED_D9)LED_D9=0;else LED_D9=1;
            break;
        case '1':
            if(LED_D10)LED_D10=0;else LED_D10=1;
            break;
        case 'q':
            CLEAR_SCREEN
            break;
        case 't':
            toggle_DirectMode();
            if(DIRECT_MODE) DISPLAY_enviarString("Modo directo: 1");
            else DISPLAY_enviarString("Modo directo: 0");
            break;
        case 'r':
            //Something por aqui
            break;
        case 'A': //Flecha arriba
            RUEDAS_Forward();
            break;
        case 'B': //Flecha abajo
            RUEDAS_Backward();
            break;
        case 'C': //Flecha derecha
            Nop();
            break;
        case 'D': //Flecha izquierda
            Nop();
            break;
        case '\r':
            CMD=1;
            RX='\0';
            DISPLAY_cambiar_VENTANA_c(':',CMD_Line,3); 
            DISPLAY_borrar_VENTANA(CMD_Line,5,20);
            break;
        case '\x1b':
            concat(errorMessageString,"ERROR: RX_Buffer vacio\n",__FILE__,eMS_max);
            eMS_lp = append(errorMessageString,"\n(linea:",eMS_max);
            p_int(eMS_lp,__LINE__,1,eMS_max);
            append(errorMessageString,")",eMS_max);
            DISPLAY_enviarString(errorMessageString);
            break;
        default:
            break;
    }
}
void CMD_exeString()
{
    if(!strcmp(CMD_Buffer,"clrscr")){
        CLEAR_SCREEN
    }
    else if(!strcmp(CMD_Buffer,"clear message")){
        DISPLAY_cambiar_VENTANA_m("\n\n\n\n\n\n");
    }
    else if(!(strcmp(CMD_Buffer,"enable") && (strcmp(CMD_Buffer,"en")) && (strcmp(CMD_Buffer,"en whe")))){
        enable_BalancePID();
        enable_RUEDAS();
        DISPLAY_cambiar_VENTANA_c(_LATB10+48,Rueda_D_line,18);
        DISPLAY_cambiar_VENTANA_c(_LATB11+48,Rueda_I_line,18);
    }
    else if(!(strcmp(CMD_Buffer,"disable")&& (strcmp(CMD_Buffer,"dis")) && (strcmp(CMD_Buffer,"dis whe")))){
        disable_BalancePID();
        disable_RUEDAS();
        DISPLAY_cambiar_VENTANA_c(_LATB10+48,Rueda_D_line,18);
        DISPLAY_cambiar_VENTANA_c(_LATB11+48,Rueda_I_line,18);
    }
    else if(!(strcmp(CMD_Buffer,"pid"))){
        toggle_BalancePID();
    }
    else if(!(strcmp(CMD_Buffer,"ruedad"))){
        if(CDATA_Buffer[0]!='\0'){
            RUEDAS_setSpeed_right_p1000(CDATABUF_Parse());
        }
    }
    else if(!(strcmp(CMD_Buffer,"ruedai"))){
        if(CDATA_Buffer[0]!='\0'){
            RUEDAS_setSpeed_left_p1000(CDATABUF_Parse());
        }
    }
    else if(!(strcmp(CMD_Buffer,"pgain"))){
            change_pGain((double)CDATABUF_Parse()/1000);
    }
    else if(!(strcmp(CMD_Buffer,"igain"))){
            change_iGain((double)CDATABUF_Parse()/1000);
    }
    else if(!(strcmp(CMD_Buffer,"dgain"))){
            change_dGain((double)CDATABUF_Parse()/1000);
    }
    else if(! (strcmp(CMD_Buffer,"key_mode") && (strcmp(CMD_Buffer,"key")) && (strcmp(CMD_Buffer,"key mode")))){
        CMD=0;
        CMDBUF_Vaciar();
        DISPLAY_cambiar_VENTANA_c('.',CMD_Line,3); 
    }
    else if(! (strcmp(CMD_Buffer,"sp") )){
//        change_SP((double)CDATABUF_Parse()*0.0001);
    }
    else if(! (strcmp(CMD_Buffer,"lista") )){
//        actionListTest(0);
    }

    CMDBUF_Vaciar();    // Vaciar buffer de comando
    CDATABUF_Vaciar();  // Vaciar buffer de dato
}

void refrescar_Ventana_UART()
{
    sprintf(c,"%4f",B_Pout);
    DISPLAY_cambiar_VENTANA_s(c,POUT_Line,5,0);
    
    sprintf(c,PID_equation,B_err,B_errOT,B_dPV);
    DISPLAY_cambiar_VENTANA_s(c,PID_Line,1,0);
    
//    sprintf(c,/*"%1.3f*/"%d",B_Pout);
//    cambiar_VENTANA_s(c,INCL_Line,16,0);
//    
//    sprintf(c,"%3.2f",cpu);
//    cambiar_VENTANA_s(c,CPU_Line,5,0);
    
    if (*PWM_cf){
        if(PWM_DCC){
            char c[6];
            //                    sprintf(c,"%u",OC1RS);
            p_uint_fl(c,OC1RS,5,1,c+5);
            DISPLAY_cambiar_VENTANA_s(c,PWM1_Line,7,0);
            p_uint_fl(c,OC2RS,5,1,c+5);
            DISPLAY_cambiar_VENTANA_s(c,PWM2_Line,7,0);
            p_uint_fl(c,OC3RS,5,1,c+5);
            DISPLAY_cambiar_VENTANA_s(c,PWM3_Line,7,0);
            p_uint_fl(c,OC4RS,5,1,c+5);
            DISPLAY_cambiar_VENTANA_s(c,PWM4_Line,7,0);
            
            PWM_DCC = 0;
        }
    }
    
    if(RUEDAS_cf){
        if(RUEDAS_P100){
            char c[5];
            p_int_fl(c,Right_p1000,5,1,c+4);
            DISPLAY_cambiar_VENTANA_s(c,Rueda_D_line,8,0);
            p_int_fl(c,Left_p1000,5,1,c+4);
            DISPLAY_cambiar_VENTANA_s(c,Rueda_I_line,8,0);
            
            ruedas_cf.P100 = 0;
        }
    }
    
    if(IO_GainChange){
        sprintf(PID_equation,"%s*%1.0f  %s*%1.0f  %s*%1.0f\n","%1.1f",B_pGain,"%1.1f",B_iGain,"%1.0f",B_dGain);
        
        io_cf.GainChange = 0;
    }
    
    if (UART_cf){
        if(UART_WIN ){
            UART_start_transmision();
            uart_cf.WIN= 0;
        }
        //                if(/*UART_CMD &&*/ !RXBUF_Vacio()){
        //                    CMD_exeChar();
        //                    LOWER_UARTflag(UART_CMD_b);
        //                }
    }
    
    return;
}

void toggle_DirectMode()
{
    while(!_TRMT_U2); // Esperar a que terminen transmisiones
    DIRECT_MODE = !DIRECT_MODE; // Pasar a modo directo
//    _U2TXIE = DIRECT_MODE;       // Desactivar interrupciones
//    _U2TXIF = 0;
    
    if(DIRECT_MODE)     DISPLAY_enviarString("\x1b[2J\x1b[H");
    if(!DIRECT_MODE)    uart_cf.WIN= 1;
}
void disable_DirectMode(){
    while(!_TRMT_U2); // Esperar a que terminen transmisiones
    DIRECT_MODE = false;
    uart_cf.WIN= 1;
}
void enable_DirectMode(){
    while(!_TRMT_U2); // Esperar a que terminen transmisiones
    DIRECT_MODE = true;
    DISPLAY_enviarString("\x1b[2J\x1b[H");
}

void toggle_OutPut()
{
    OUTPUT_ENABLED=!OUTPUT_ENABLED;
}
void disable_OutPut(){
    OUTPUT_ENABLED=FALSE;
}
void enable_OutPut(){
    OUTPUT_ENABLED=TRUE;
}

//============================== RUTINAS DE ATENCION ===========================
void __attribute__((interrupt, auto_psv)) _U2TXInterrupt(void){
    if(!DIRECT_MODE){
        TX = VENTANA_UART[W_UART_X][W_UART_Y];
        /* ************************************************************************ *
         *  \x2 == Caracter final. Tras este,termina la transmision de la ventana.
         *  \x3 == Transmitir linea Home levantando flag UART_MF.
         *  \0  == Transmitir linea Home.
         *  \n  == Comenzar a transmitir siguiente linea.
         * ************************************************************************ */
        switch(TX){
            case '\x2'://HOME -> TEXT_START
                TEXT_START
//                U2TXREG = 0; 
                break;
            case '\x3'://LAST_CHARACTER -> HOME
                uart_cf.WIN= 1; // Fuerza otra iteracion
                CURSOR_HOME
                U2TXREG = 0;
                break;
            case '\0'://LAST_CHARACTER -> HOME
                CURSOR_HOME
                U2TXREG = 0;
                break;
            case '\n':
                U2TXREG = NEWLINE_CHAR;
                NEXT_LINE
                break;
            default:
                U2TXREG = TX;
                NEXT_CHARACTER
                break;
        }
    }else{/* DIRECT MODE */
        
        if(DMB_first != DMB_cap){
            U2TXREG = *(DMB_first++);
            if(DMB_first==DMB_max){
                DMB_first=DM_Buffer;
            }
        }
    }
    
    _U2TXIF = 0;
} 
void __attribute__((interrupt, auto_psv)) _U2RXInterrupt(void){
    
    while(_URXDA_U2){
        RX = U2RXREG;
        if(CMD){
            if((RX < 48 || RX > 59) && RX != '\x7f'){ //LETRAS
                if(!CMDBUF_Lleno()){
                    switch(RX){
                        case '\r':
                            DISPLAY_borrar_VENTANA(CMD_Line,5,20);
                            if(!CMDBUF_Lleno())CMD_Buffer[CMD_BUF_pos]='\0';
                            DISPLAY_borrar_VENTANA(CDATA_Line,5,20);
                            if(!CDATABUF_Lleno())CDATA_Buffer[CDATA_BUF_pos]='\0';
                            CMD_exeString();
                            break;
                        case '\x8':
                            DISPLAY_borrar_VENTANA(CMD_Line,5+CMD_BUF_Last,1);
                            CMDBUF_BorrUlt(); 
                            break;
                        default:
                            CMDBUF_Aniadir1(RX);
                            DISPLAY_cambiar_VENTANA_c(RX,CMD_Line,5+CMD_BUF_Last);
                            break;
                    }
                }else{
                    if(RX=='\x8'){
                        DISPLAY_borrar_VENTANA(CMD_Line,5+CMD_BUF_Last,1);
                        CMDBUF_BorrUlt();
                        DISPLAY_borrar_VENTANA_m();
                    }else{
                        concat(errorMessageString,"CMD_Buffer lleno.\nBACKSPACE para hacer espacio.\n",__FILE__,eMS_max);
                        eMS_lp = append(errorMessageString,"\n(linea:",eMS_max);
                        p_int(eMS_lp,__LINE__,1,eMS_max);
                        append(errorMessageString,")",eMS_max);
                        DISPLAY_enviarString(errorMessageString);
                    }
                }
            }else{ //NUMEROS
                if(!CDATABUF_Lleno()){
                    switch(RX){
                        case '\x7f':
                            DISPLAY_borrar_VENTANA(CDATA_Line,5+CDATA_BUF_Last,1);
                            CDATABUF_BorrUlt(); 
                            break;
                        default:
                            CDATABUF_Aniadir1(RX);
                            DISPLAY_cambiar_VENTANA_c(RX,CDATA_Line,5+CDATA_BUF_Last);
                            break;
                    }
                }else{
                    if(RX=='\x7f'){
                        DISPLAY_borrar_VENTANA(CDATA_Line,5+CDATA_BUF_Last,1);
                        CDATABUF_BorrUlt();
                        DISPLAY_borrar_VENTANA_m();
                    }else{
                        concat(errorMessageString,"CDATA_Buffer lleno.\nSUPRIMIR para hacer espacio.\n",__FILE__,eMS_max);
                        eMS_lp = append(errorMessageString,"\n(linea:",eMS_max);
                        p_int(eMS_lp,__LINE__,1,eMS_max);
                        append(errorMessageString,")",eMS_max);
                        DISPLAY_enviarString(errorMessageString);
                    }
                }
            }
        }else{
            if(RX!='\x1b'){
                if(!CMDBUF_Lleno()){
                    RXBUF_Aniadir1(RX);
                    if(RX!='\r')DISPLAY_cambiar_VENTANA_c(RX,CMD_Line,5); 
                    uart_cf.CMD= 1;
                }else{
                    concat(errorMessageString," ERROR: RX_Buffer lleno\n",__FILE__,eMS_max);
                    eMS_lp = append(errorMessageString,"\n(linea:",eMS_max);
                    p_int(eMS_lp,__LINE__,1,eMS_max);
                    append(errorMessageString,")",eMS_max);
                    DISPLAY_enviarString(errorMessageString);
                }
            }else{
                switch (RX = UART_get()){
                    case '[':
                        if(!CMDBUF_Lleno()){
                            RXBUF_Aniadir1(RX = UART_get());
                            if(RX!='\r')DISPLAY_cambiar_VENTANA_c(RX,CMD_Line,5); 
                            uart_cf.CMD= 1;
                        }else{
                            concat(errorMessageString," ERROR: RX_Buffer lleno\n",__FILE__,eMS_max);
                            eMS_lp = append(errorMessageString,"\n(linea:",eMS_max);
                            p_int(eMS_lp,__LINE__,1,eMS_max);
                            append(errorMessageString,")",eMS_max);
                            DISPLAY_enviarString(errorMessageString);
                        }
                        break;
                    default:
                        concat(errorMessageString,"ERROR: Recibido 'ESC,[' y nada mas\n",__FILE__,eMS_max);
                        eMS_lp = append(errorMessageString,"\n(linea:",eMS_max);
                        p_int(eMS_lp,__LINE__,1,eMS_max);
                        append(errorMessageString,")",eMS_max);
                        DISPLAY_enviarString(errorMessageString);
                        break;
                }
            }
        }
    }
//    if(++TX_Interrupt_callTimes>=6)
//    {
//        Nop();
//        Nop();
//        Nop();
//        Nop();
//        TX_Interrupt_callTimes = 0;
//    }
    
    _U2RXIF = 0;
}
void __attribute__((interrupt, no_auto_psv)) _U2ErrInterrupt (void){
    Nop();
    Nop();
    Errores++;
    _U2ERIF = 0;
}