/*
 * File:   Display.c
 * Author: Christian
 *
 * Created on 27 de febrero de 2018, 15:56
 */


#include "main.h"

/* ************************************************************************ *
 *  \x2 == Caracter final. Tras este,termina la transmision de la ventana.
 *  \x3 == Levantar flag UART_MF.
 *  \0  == Transmitir Linea Home.
 *  \n  == Comenzar a transmitir siguiente linea.
 * ************************************************************************ */
const char* VentanaUART_FLASH[VENTANA_WIDTH]={
    #ifdef HOME_Line 
        "\x1b[H\x2                       ",
    #endif
    #ifdef CLRSCR_Line
        "\x1b[2J\x3                     ",
    #endif
    #ifdef RESETTERM_Line
        "\x1b[c\x3                         ",
    #endif
    #ifdef TITLE_Line
        "Robot balancin:          \n",
    #endif
    #ifdef CPU_Line
        "CPU:                     \n",
    #endif
    #ifdef CRONO_Line
        "Crono:  ---/--/----\n\n\n\n\n\n\n\n",
    #endif
    #ifdef INCL_Line
        "Inclinacion:  Y:----\n\n\n\n\n\n\n\n",
    #endif
    #ifdef POUT_Line
        "Pout=               \n\n\n\n\n\n\n\n",
    #endif
    #ifdef PID_Line
        "PID                 \n\n\n\n\n\n\n\n",
    #endif
    #ifdef PWM1_Line
        " PWM1:     0/1024\n        ",
    #endif
    #ifdef PWM2_Line
        " PWM2:     0/1024\n        ",
    #endif
    #ifdef PWM3_Line
        " PWM3:     0/1024\n       ",
    #endif
    #ifdef PWM4_Line
        " PWM4:     0/1024\n       ",
    #endif
    #ifdef Rueda_D_line
        "Rueda D:    0% EA:0\n      ",
    #endif
    #ifdef Rueda_I_line
        "Rueda I:    0% EA:0\n      ",
    #endif
    #ifdef AJUSTE_Line
        "Ajuste: 000 0000         \n",
    #endif
    #ifdef CMD_Line
        "Cmd. \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef CDATA_Line
        "Data \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef EMPTY_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef MENSAJES_Line
        "Mensajes:\n",
    #endif
    #ifdef MESSAGE1_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef MESSAGE2_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef MESSAGE3_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef MESSAGE4_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef MESSAGE5_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef MESSAGE6_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef MESSAGE7_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef MESSAGE8_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef MESSAGE9_Line
        "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n",
    #endif
    #ifdef THEEND_Line
        "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0"
    #endif
    };

unsigned char W_UART_Y;
unsigned char W_UART_X;

char VENTANA_UART[VENTANA_HEIGHT][VENTANA_WIDTH];


void Inic_Display()
{
    DISPLAY_Copiar_FLASH_RAM_IO();
//Se empezara a transmitir enviando clrscr
    CLEAR_SCREEN;
}
void DISPLAY_Copiar_FLASH_RAM_IO()
{
    int i,j;
    for(i=0;i<VENTANA_HEIGHT;i++)
        for(j=0;j<VENTANA_WIDTH;j++)
            VENTANA_UART[i][j] = VentanaUART_FLASH[i][j];
}

int DISPLAY_enviarString(char* buffer)
{
    if(DIRECT_MODE){
        return UART_enviarString(buffer);
    }
    else{
        return DISPLAY_cambiar_VENTANA_m((char*)buffer);
    }
}

void DISPLAY_cambiar_VENTANA_c(char c, int linea, int pos)
{
    if(linea<2||pos>VENTANA_WIDTH-2){return;}
    
    _U2TXIE = 0;
    
    VENTANA_UART[linea][pos] = c;
    
    uart_cf.WIN= 1; //Levantar flag del UART
    _U2TXIE = 1;
}
void DISPLAY_cambiar_VENTANA_s(char* string, int linea, int pos, char flush)
{
    
    if(linea<2){return;}
//    linea += 2;
    
    _U2TXIE = 0;
    int len = length(string);
    
    int i=0;
    while(i<len && i+pos<VENTANA_WIDTH-1){
        VENTANA_UART[linea][i+pos] = string[i];//Copiar el string en W_UART
        i++;    
    }
    
    if (flush && i+pos<VENTANA_WIDTH-1){
        DISPLAY_borrar_VENTANA(linea,pos+i,VENTANA_WIDTH);
    }else{
        uart_cf.WIN= 1; //Levantar flag del UART
        _U2TXIE = 1;
    }
}
void DISPLAY_borrar_VENTANA(int linea, int pos, int len)
{
    
    if(linea<2){return;}
    
    _U2TXIE = 0;
    
    int i=0;
    while(i<len && i+pos<VENTANA_WIDTH-1){
        VENTANA_UART[linea][i+pos] = '\n';
        i++;    
    }
    
    uart_cf.WIN= 1; //Levantar flag del UART
    _U2TXIE = 1;
    CLEAR_SCREEN
}
int DISPLAY_cambiar_VENTANA_m(char* messageString)
{
    _U2TXIE = 0;
    
    char exitCond=0;
    int i, j=0, line = MESSAGE1_Line;
    int len = length(messageString);
    
    for(line=MESSAGE1_Line; line<MENSAJES_Line+MESSAGE_LineAmount; line++)
    {
        i=0;
        while(i<VENTANA_WIDTH-1)
        {
            switch(messageString[j])
            {
                case '\n':
                    VENTANA_UART[line][i] = messageString[j];
                    i=VENTANA_WIDTH; //Terminar While
                    break;
                case '\0':
                    i=VENTANA_WIDTH; //Terminar While
                    exitCond=1;      //Terminar For
                    break;
                default:
                    VENTANA_UART[line][i] = messageString[j];//Copiar el string en W_UART
                    i++;
                    break;
            }
            j++;
        }
        if(exitCond)break;
    }
    
    if (i<(VENTANA_WIDTH)){
        DISPLAY_borrar_VENTANA(line,i,VENTANA_WIDTH-i);
    }else {
        uart_cf.WIN= 1; //Levantar flag del UART
        _U2TXIE = 1;
    }
    
    return len;
}
void DISPLAY_borrar_VENTANA_m()
{
    char c[MESSAGE_LineAmount];
    int i;
    for(i=0;i<MESSAGE_LineAmount;i++)c[i]='\n';
    DISPLAY_cambiar_VENTANA_m(c);
}



/** MODO DIRECTO **/

volatile bool RECORDING = false;
void record(bool force_DirectM)
{
    if(force_DirectM){
        enable_DirectMode();
    }
    enable_OutPut();
    
#ifdef BS_PID_DATA
    /* SP PID Data */
//    sprintf(c,"Set point:\t%f\n",B_SP);
//    UART_enviarString(c);
    sprintf(c,"Proportional gain:\t%f\n",BS_pG);
    DISPLAY_enviarString(c);
    sprintf(c,"Integral gain:\t%f\n",BS_iG);
    DISPLAY_enviarString(c);
    sprintf(c,"Derived gain:\t%f\n\n",BS_dG);
    DISPLAY_enviarString(c);
#endif
#ifdef BL_PID_DATA
    /* BL PID Data */
    sprintf(c,"Set point:\t%f\n",B_SP);
    DISPLAY_enviarString(c);
    sprintf(c,"Proportional gain:\t%f\n",B_pGain);
    DISPLAY_enviarString(c);
    sprintf(c,"Integral gain:\t%f\n",B_iGain);
    DISPLAY_enviarString(c);
    sprintf(c,"Derived gain:\t%f\n\n",B_dGain);
    DISPLAY_enviarString(c);
    
    enable_RUEDAS();
    enable_BalancePID();
    
//                            PIDT_resetOscilometer(1);
//                            PID_tuner(true);
//                            
//                            
//                            LCD_ClearScreen();
#endif
#ifdef SP_PID_DATA
    /* SP PID Data */
    sprintf(c,"Set point:\t%f\n",S_SP);
    DISPLAY_enviarString(c);
    sprintf(c,"Proportional gain:\t%f\n",S_pGain);
    DISPLAY_enviarString(c);
    sprintf(c,"Integral gain:\t%f\n",S_iGain);
    DISPLAY_enviarString(c);
    sprintf(c,"Derived gain:\t%f\n\n",S_dGain);
    DISPLAY_enviarString(c);
    
    enable_RUEDAS();
    enable_BalancePID();
    WL_registerMileStone(2 s,enable_SP_PID_Delayed,1);
#endif
#ifdef INERTIA_DATA
    /* INERTIA Data */
    enable_RUEDAS();
    enable_BalancePID();
#endif
#ifdef OSCIM_DATA
    /* INERTIA Data */
    
    DISPLAY_enviarString("Oscilometer data\n");
    
    PID_S_tuner(true);
    
    
    LCD_ClearScreen();
#endif
#ifdef VELOCIMETRO_DATA
    /* VELODIMETRO Data */
    
    DISPLAY_enviarString("Velocimetro data\n");
    
    enable_RUEDAS();
    enable_BalancePID();
    enable_SpeedPID();
    
    PIDT_resetOscilometer();
    PID_TUNING_IS_ON = true;
//    PID_tuner(true);
    
    keepRunningTuner = true;
    WL_registerMileStone(10 s,PID_TUNER_returnToPIDTunerTimingTest,0);
    WL_registerMileStone(10,PID_TUNER_returnToPIDTunerTimingTest,2);
//    WL_registerMileStone(/*(tim-=10)*/10,SP_TESTS,0);
    
    LCD_ClearScreen();
#endif
#ifdef SENSORFUSION_DATA
    /* SENSOR FUSION Data */
    
    DISPLAY_enviarString("Fusrion de sensores data\n");
    
    enable_RUEDAS();
    enable_BalancePID();

    
    LCD_ClearScreen();
#endif
         
    DISPLAY_enviarString("\n###\n");
    UART_start_transmision();
}
 void directModeOutput(void){
#ifdef OSCIM_DATA
                /* INERTIA Data  (Max 42 chars)*/
                sprintf(c,"%0.2f\t%0.0f\t%0.3f\t%0.3f\t%d\t%d\t%d\n",*FUNCTION,Osc_ATime_mean,*tuningGain,Osc_Peakdif_mean,direction,fliped,PIDT_StateMachine);
//            sprintf(c,"%f\t%f\t%0.2f\n",*FUNCTION,peak_mean,peakdif_mean);
                DISPLAY_enviarString(c);
#endif
#ifdef VELOCIMETRO_DATA
//                sprintf(c,"%0.2f\t%0.0f\t%0.3f\t%0.3f\t%d\t%d\t%d\n",*FUNCTION,oscillation_mean,*tuningGain,peakdif_mean,side,fliped,PIDT_StateMachine);
                sprintf(c,"%f\t%f\t%f\n",*FUNCTION,L,R);
                DISPLAY_enviarString(c);
#endif
#ifdef SENSORFUSION_DATA
//                sprintf(c,"%0.2f\t%0.0f\t%0.3f\t%0.3f\t%d\t%d\t%d\n",*FUNCTION,oscillation_mean,*tuningGain,peakdif_mean,side,fliped,PIDT_StateMachine);
                sprintf(c,"%0.3f\t%0.3f\t%0.3f\t%0.3f\t%d\n",GYR_angle,ACC_angle,B_PV,B_err,B_Pout);
                DISPLAY_enviarString(c);
#endif
#ifdef BL_PID_DATA
                /* BL PID Data*/
                sprintf(c,"%0.3f\t",B_PV);
                DISPLAY_enviarString(c);
                sprintf(c,"%d\t",B_Pout);
                DISPLAY_enviarString(c);
                sprintf(c,"%0.3f\t",B_err);
                DISPLAY_enviarString(c);
                sprintf(c,"%0.3f\t",B_errOT);
                DISPLAY_enviarString(c);
                sprintf(c,"%0.3f\n",B_dPV);
                DISPLAY_enviarString(c);
//                p_int_ec(c,B_Pout,1,'\t',c_max);
//                UART_enviarString(c);
#endif
#ifdef BS_PID_DATA
                /* SP PID Data */
                sprintf(c,"%f\t",B_SP);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",errMeanAux);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",errAvOT);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",dErr);
                DISPLAY_enviarString(c);
#endif
#ifdef BL_PID_DATA
                /* BL PID Data*/
                sprintf(c,"%f\t",B_PV);
                DISPLAY_enviarString(c);
                sprintf(c,"%d\t",B_Pout);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",B_err);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",B_errOT);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",B_dPV);
                DISPLAY_enviarString(c);
                p_int_ec(c,B_Pout,1,'\t',c_max);
                DISPLAY_enviarString(c);
#endif
#ifdef SP_PID_DATA
                /* SP PID Data */
                sprintf(c,"%f\t",S_PV);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",S_err);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",S_errOT);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",S_dPV);
                DISPLAY_enviarString(c);
#endif
#ifdef INERTIA_DATA
                /* INERTIA Data */
                sprintf(c,"%f\t",accData[1]);
                DISPLAY_enviarString(c);
                sprintf(c,"%f\t",accData[2]);
                DISPLAY_enviarString(c);
#endif
                UART_start_transmision();
            }
void stop(bool stop_PID)
{
    disable_OutPut();
    keepRunningTuner = false;
    
    if(stop_PID){
        disable_RUEDAS();
        disable_BalancePID();
        disable_SpeedPID();
        PID_TUNING_IS_ON=0;
    }
}