/* 
 * File:   menu_LCD.c
 * Author: Christian
 *
 * Created on 23 de mayo de 2019, 11:49
 */

#include "main.h"

/* | ------------------------------------- | */
/* | Textos de los menus, organizados por  | */
/* | menu en una array de strings cada uno | */
/* V                                       V */

/* Menu principal */
char *menu_LCD_main[15]={"Controles:       ",
                        "Calibrar PID:     ",
                        "Display:          ",
                        "Overload LED      ",
                        "Lista de acciones ",
                        "Calibrar PWM      ",
                        "Ajustar SetPoint: "};
#define menu_LCD_main_L 7

/* Menu controles*/
char* menu_LCD_controls[15]={"Control Eq     ",
//                           "SP PID         ",
                             "Control Vel    ",
                             "Recuperar pos  ",
                             "UART MD        ",
                             "UART Activar   ",
                             " "};
#define menu_LCD_controls_L 6
char* menu_LCD_Cont_recordStop[7]={"UART Grabar    ",
                                   "UART Stop      "};
bool volatile *(menu_LCD_controls_BOOLs[5]);    /* Cada opcion del menu de controles controla la activacion de una funcion del robot cuyo estado zse define por una variable booleana.
                                                 * Se crea un vector de punteros que apuntan a cada una de estas variables para poder acceder a ellas a traves de un indice.*/

/* Menu mostrar variables en tiempo real*/
char* menu_LCD_displayVar[15]={" PID comps        ",
                               " CPU %           ",
                               " Main SP         ",
                               " Velocimetro     ",
                               " Distancia       "};
#define menu_LCD_displayVar_L 5

/* Menu sintonizacion del PID*/
char* menu_LCD_gainTune[15]={"Eq. Manual ",
                             "Vel. Manual",
                             "Eq. Auto   ",
                             "Vel. Auto  "};
#define menu_LCD_gainTune_L 4
char* menu_LCD_ManualTune[15]={"pGain: ",
                               "iGain: ",
                               "dGain: "};
#define menu_LCD_ManualTnune_L 3

/* Menu ajuste del Set PWM*/
char* menu_LCD_pwmTune[15]={"Min            ",
                            "Max            ",};
#define menu_LCD_pwmTune_L 2
char* menu_LCD_PT_Mm[15]={"Motor  =A      ",
                          "Motor A=       ",
                          "Motor  =V      ",
                          "Motor V=       "};
#define menu_LCD_PT_Mm_L 4

/* Menu ajuste del Set Point*/
char* menu_LCD_SPTune[15]={"Equilibrio     ",
                           "Velocidad      "};
#define menu_LCD_SPTune_L 2
#define AjusteSP 0.05;
#define AjusteSpeedGoal 0.5;
double ST_Speedgoal = 0.0;
double ST_PVgoal = 0.0;
    
/* Cursor  del menu LCD*/
menu_LCD_cursor_obj mLCD_cursor;

/* Inicializa */
void init_LCD_controls_BOOLs(){
    menu_LCD_controls_BOOLs[0] = &BL_PID_ENABLED;
    menu_LCD_controls_BOOLs[1] = &SP_PID_ENABLED;
    menu_LCD_controls_BOOLs[2] = &SPID_AW_is_ON;
    menu_LCD_controls_BOOLs[3] = &DIRECT_MODE;
    menu_LCD_controls_BOOLs[4] = &OUTPUT_ENABLED;
    menu_LCD_controls_BOOLs[5] = &RECORDING;
}
    
void Init_menu_LCD(bool first_print){
    
    /* inicializar menus */
    init_LCD_controls_BOOLs();
    menu_LCD_controls[menu_LCD_controls_L-1] = menu_LCD_Cont_recordStop[0];
    
    /* inicializar cursor */
    mLCD_cursor.string = &menu_LCD_main[0]; 
    mLCD_cursor.estado = mainMenu;
    
    /* imprimir primera pantalla*/
    if(first_print){
        LCD_ClearScreen(); 
        setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);  
    }
}
    
   
/**/  
void menu_LCD_siguiente(char** menu,int L){
    mLCD_cursor.string = (mLCD_cursor.string< (&menu[0] + (L-1)))? mLCD_cursor.string+1: &menu[0];
}
void menu_LCD_anterior(char** menu,int L){
    mLCD_cursor.string = (mLCD_cursor.string> &menu[0])? mLCD_cursor.string-1: &menu[L-1];
}
void menu_LCD()
{
    if (IO_cf){
        switch(mLCD_cursor.estado/10){
            case mainMenu/10:
                if(IO_S3){
                    menu_LCD_anterior(menu_LCD_main,menu_LCD_main_L);
                    /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                    io_cf.S3 = 0;
                }
                if(IO_S6){
                    menu_LCD_siguiente(menu_LCD_main,menu_LCD_main_L);
                    /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                    io_cf.S6= 0;
                }
                if(IO_S4){
                    switch(mLCD_cursor.string-menu_LCD_main){
                        case 0:
                            mLCD_cursor.estado = M_controles;
                            mLCD_cursor.string = menu_LCD_controls;
                            /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                            setLCDC(0x4D);p_int(c,*menu_LCD_controls_BOOLs[0],1,c_max);LCD_Send_String(c);
                            break;
                        case 1:
                            mLCD_cursor.estado = M_Calib_PID;
                            mLCD_cursor.string = menu_LCD_gainTune;
                            /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                            break;
                        case 2:
                            mLCD_cursor.estado = M_displayVar;
                            mLCD_cursor.string = menu_LCD_displayVar;
                            /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                            break;
                        case 3:
//                            cursor.estado = clrOLled;
                            LED_D5 = 0;
                            break;
                        case 4:
                            mLCD_cursor.estado = M_listaAcciones;
//                            mLCD_cursor.string = menu_LCD_displayVar;
                            sprintf(c,"S4 para empezar");
                            setLCDC(0x41);LCD_Send_String(c);
                            
                            WL_registerMileStone(0.5 s,paseito,0);
//                            SPID_pout_enabled = false;
//                            S_Pout = 0.0;
                            
                            break;
                        case 5:
                            mLCD_cursor.estado = M_Calib_PWM;
                            mLCD_cursor.string = menu_LCD_pwmTune;
                            setLCDC(0x01);LCD_Send_String(*mLCD_cursor.string);
                            break;
                        case 6:
                            mLCD_cursor.estado = M_Ajustar_SP;
                            mLCD_cursor.string = menu_LCD_SPTune;
                            /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                            break;
                    }
                    io_cf.S4 = 0;
                }
                break;
            case M_controles/10:
                if(IO_S3){
                    menu_LCD_anterior(menu_LCD_controls,menu_LCD_controls_L);
                    /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                    
                    setLCDC(0x4D);p_int(c,*menu_LCD_controls_BOOLs[mLCD_cursor.string-menu_LCD_controls],1,c_max);LCD_Send_String(c);
                    io_cf.S3 = 0;
                }
                if(IO_S6){
                    menu_LCD_siguiente(menu_LCD_controls,menu_LCD_controls_L);
                    /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                    
                    setLCDC(0x4D);p_int(c,*menu_LCD_controls_BOOLs[mLCD_cursor.string-menu_LCD_controls],1,c_max);LCD_Send_String(c);
                    io_cf.S6= 0;
                }
                if(IO_S4){
                    switch(mLCD_cursor.string-menu_LCD_controls){
                        case 0:
                            if(BL_PID_ENABLED){
                                disable_BalancePID();
                                disable_RUEDAS();
                            }else{
                                enable_BalancePID();
                                enable_RUEDAS();
                            }
                            break;
                        case 1:
                            toggle_SpeedPID();
                            break;
                        case 2:
                            toggle_SPID_AntiWindup();
                            break;
                        case 3:
                            toggle_DirectMode();
                            break;
                        case 4:
                            toggle_OutPut();
                            break;
                        case 5:
                            if(menu_LCD_controls[menu_LCD_controls_L-1]==menu_LCD_Cont_recordStop[0]){
                                record(true);
                                menu_LCD_controls[menu_LCD_controls_L-1]=menu_LCD_Cont_recordStop[1];
                                /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                            }else{
                                stop(true);
                                menu_LCD_controls[menu_LCD_controls_L-1]=menu_LCD_Cont_recordStop[0];
                                /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                            }
                            break;
                    }
                    
                    setLCDC(0x4D);p_int(c,*menu_LCD_controls_BOOLs[mLCD_cursor.string-menu_LCD_controls],1,c_max);LCD_Send_String(c);
                    io_cf.S4 = 0;
                }
                break;
            case M_Calib_PID/10:
                switch (mLCD_cursor.estado){
                    case M_Calib_PID:
                            if(IO_S3){
                                menu_LCD_anterior(menu_LCD_gainTune,menu_LCD_gainTune_L);
                                /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                                
                                io_cf.S3 = 0;
                            }
                            if(IO_S6){
                                menu_LCD_siguiente(menu_LCD_gainTune,menu_LCD_gainTune_L);
                                /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                                
                                io_cf.S6= 0;
                            }
                            if(IO_S4){
                                switch(mLCD_cursor.string-menu_LCD_gainTune){
                                    case 0:
                                        mLCD_cursor.estado = TG_Balance;
                                        mLCD_cursor.string = menu_LCD_ManualTune;
                                        /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                                        sprintf(c,"%f",B_pGain);
                                        LCD_Send_String(c);
                                        break;
                                    case 1:
                                        mLCD_cursor.estado = TG_Speed;
                                        mLCD_cursor.string = menu_LCD_ManualTune;
                                        /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                                        sprintf(c,"%f",S_pGain);
                                        LCD_Send_String(c);
                                        break;
                                    case 2:
                                    case 3:
                                        mLCD_cursor.estado = TG_Balance+(mLCD_cursor.string-menu_LCD_gainTune);
                                        setLCDC(0x00);LCD_Send_String("Auto PID tuner\r S4 to Start..");
                                        break;
                                }
                                
                                io_cf.S4 = 0;
                            }
                            break;
                    case TG_Balance:
                        if(IO_S3){
                            switch(mLCD_cursor.string-menu_LCD_ManualTune){
                                case 0:
                                    B_pGain-=B_pG_Increment;
                                    sprintf(c,"%f",B_pGain);
                                    break;
                                case 1:
                                    B_iGain-=B_iG_Increment;
                                    sprintf(c,"%f",B_iGain);
                                    break;
                                case 2:
                                    B_dGain-=B_dG_Increment;
                                    sprintf(c,"%f",B_dGain);
                                    break;
                            }setLCDC(0x48);LCD_Send_String(c);
                            io_cf.S3 = 0;
                            io_cf.GainChange = 1;
                        }
                        if(IO_S6){
                            switch(mLCD_cursor.string-menu_LCD_ManualTune){
                                case 0:
                                    B_pGain+=B_pG_Increment;
                                    sprintf(c,"%f",B_pGain);
                                    break;
                                case 1:
                                    B_iGain+=B_iG_Increment;
                                    sprintf(c,"%f",B_iGain);
                                    break;
                                case 2:
                                    B_dGain+=B_dG_Increment;
                                    sprintf(c,"%f",B_dGain);
                                    break;
                            }setLCDC(0x48);LCD_Send_String(c);
                            io_cf.S6= 0;
                            io_cf.GainChange = 1;
                        }
                        if(IO_S4){
                            menu_LCD_siguiente(menu_LCD_ManualTune,menu_LCD_ManualTnune_L);
                            /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                            switch(mLCD_cursor.string-menu_LCD_ManualTune){
                                case 0:
                                    sprintf(c,"%f",B_pGain);
                                    break;
                                case 1:
                                    sprintf(c,"%f",B_iGain);
                                    break;
                                case 2:
                                    sprintf(c,"%f",B_dGain);
                                    break;
                            }LCD_Send_String(c);
                            io_cf.S4 = 0;
                        }
                        break;
                    case TG_Speed:
                        if(IO_S3){
                            switch(mLCD_cursor.string-menu_LCD_ManualTune){
                                case 0:
                                    S_pGain-=S_pG_Increment;
                                    sprintf(c,"%f",S_pGain);
                                    break;
                                case 1:
                                    S_iGain-=S_iG_Increment;
                                    twoCmWithGain = 2.0*S_iGain;
                                    sprintf(c,"%f",S_iGain);
                                    break;
                                case 2:
                                    S_dGain-=S_dG_Increment;
                                    sprintf(c,"%f",S_dGain);
                                    break;
                            }setLCDC(0x48);LCD_Send_String(c);
                            io_cf.S3 = 0;
                            io_cf.GainChange = 1;
                        }
                        if(IO_S6){
                            switch(mLCD_cursor.string-menu_LCD_ManualTune){
                                case 0:
                                    S_pGain+=S_pG_Increment;
                                    sprintf(c,"%f",S_pGain);
                                    break;
                                case 1:
                                    S_iGain+=S_iG_Increment;
                                    twoCmWithGain = 2.0*S_iGain;
                                    sprintf(c,"%f",S_iGain);
                                    break;
                                case 2:
                                    S_dGain+=S_dG_Increment;
                                    sprintf(c,"%f",S_dGain);
                                    break;
                            }setLCDC(0x48);LCD_Send_String(c);
                            io_cf.S6= 0;
                            io_cf.GainChange = 1;
                        }
                        if(IO_S4){
                            menu_LCD_siguiente(menu_LCD_ManualTune,menu_LCD_ManualTnune_L);
                            /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                            switch(mLCD_cursor.string-menu_LCD_ManualTune){
                                case 0:
                                    sprintf(c,"%f",S_pGain);
                                    break;
                                case 1:
                                    sprintf(c,"%f",S_iGain);
                                    break;
                                case 2:
                                    sprintf(c,"%f",S_dGain);
                                    break;
                            }LCD_Send_String(c);
                            io_cf.S4 = 0;
                        }
                        break;
                    case TG_Auto_B:
                        if(IO_S4){
                            PID_B_tuner(true);
                            
                            
                            LCD_ClearScreen();
                            
                            io_cf.S4 = 0;
                        }
                        break;
                    case TG_Auto_S:
                        if(IO_S4){
                            PID_S_tuner(true);
                            
                            
                            LCD_ClearScreen();
                            
                            io_cf.S4 = 0;
                        }
                        break;
                    default:
                        break;
                }
                break;
            case M_displayVar/10:
                        if(IO_S3){
                            mLCD_cursor.estado = M_displayVar;
                            menu_LCD_anterior(menu_LCD_displayVar,menu_LCD_displayVar_L);
                            /*LCD_ClearScreen();*/setLCDC(0x40);LCD_Send_String(*mLCD_cursor.string);
                            
                            io_cf.S3 = 0;
                        }
                        if(IO_S6){
                            mLCD_cursor.estado = M_displayVar;
                            menu_LCD_siguiente(menu_LCD_displayVar,menu_LCD_displayVar_L);
                            /*LCD_ClearScreen();*/setLCDC(0x40);LCD_Send_String(*mLCD_cursor.string);
                            
                            io_cf.S6= 0;
                        }
                        if(IO_S4){
                            mLCD_cursor.estado = (DV_PIDcomps + (mLCD_cursor.string-menu_LCD_displayVar));
                            LCD_ClearScreen();
                            switch(mLCD_cursor.estado){
                                case DV_PIDcomps:
                                    sprintf(c,"ER");
                                    setLCDC(0x00);LCD_Send_String(c);
                                    sprintf(c,"EOT");
                                    setLCDC(0x08);LCD_Send_String(c);
                                    sprintf(c,"dPV");
                                    setLCDC(0x40);LCD_Send_String(c);
                                    break;
                                case DV_CPUper:
                                    sprintf(c,"CPU free cicles");
                                    setLCDC(0x00);LCD_Send_String(c);
                                    break;
                                case DV_MainSP:
                                    sprintf(c,"        n");
                                    setLCDC(0x00);LCD_Send_String(c);
                                    break;
                                case DV_Velocim:
                                    sprintf(c,"L:           rps\rR:           rps");
                                    setLCDC(0x00);LCD_Send_String(c);
                                    break;
                                case DV_Distancia:
                                    
                                    sprintf(c,"Distancia\r             cms");
                                    setLCDC(0x00);LCD_Send_String(c);
                                    break;
                                default:
                                    break;
                            }
                            io_cf.S4 = 0;
                        }
                        break;
            case M_clrOLled/10:
                if(IO_S3){
                    io_cf.S3 = 0;
                }
                if(IO_S6){
                    io_cf.S6= 0;
                }
                if(IO_S4){
                    io_cf.S4 = 0;
                }
                break;
            case M_listaAcciones/10:
                if(IO_S3){
                    io_cf.S3 = 0;
                }
                if(IO_S6){
                    io_cf.S6= 0;
                }
                if(IO_S4){
                    io_cf.S4 = 0;
                }
                break;
            case M_Calib_PWM/10:
                switch(mLCD_cursor.estado){
                    case M_Calib_PWM:
                        if(IO_S3){
                            menu_LCD_anterior(menu_LCD_pwmTune,menu_LCD_pwmTune_L);
                            setLCDC(0x01);LCD_Send_String(*mLCD_cursor.string);
                            
                            IO_S3 = 0;
                        }
                        if(IO_S6){
                            menu_LCD_siguiente(menu_LCD_pwmTune,menu_LCD_pwmTune_L);
                            setLCDC(0x01);LCD_Send_String(*mLCD_cursor.string);
                            
                            IO_S6= 0;
                        }
                        if(IO_S4){
                            sprintf(c,"%5d  ",(mLCD_cursor.string-menu_LCD_pwmTune)?MaxPowerDC1:MinPowerDC1);
                            
                            mLCD_cursor.estado = PTm_BackLeft+4*(mLCD_cursor.string-menu_LCD_pwmTune);
                            mLCD_cursor.string = menu_LCD_PT_Mm;
                            
                            LCD_ClearScreen();
                            setLCDC(0x01);LCD_Send_String(*mLCD_cursor.string);
                            setLCDC(0x46);LCD_Send_String(c);
                            
                            IO_S4 = 0;
                        }
                        break;
                    case PTm_BackLeft:
                    case PTm_BackRight:
                    case PTm_ForwLeft:
                    case PTm_ForwRight:
                        if(IO_S3){
                            switch (mLCD_cursor.estado){
                                case PTm_BackLeft:
                                    MinPowerDC1 -= 1;
                                    sprintf(c,"%5d  ",MinPowerDC1);
                                    break;
                                case PTm_BackRight:
                                    MinPowerDC2 -= 1;
                                    sprintf(c,"%5d  ",MinPowerDC2);
                                    break;
                                case PTm_ForwLeft:
                                    MinPowerDC3 -= 1;
                                    sprintf(c,"%5d  ",MinPowerDC3);
                                    break;
                                case PTm_ForwRight:
                                    MinPowerDC4 -= 1;
                                    sprintf(c,"%5d  ",MinPowerDC4);
                                    break;
                                default:
                                    break;
                            }
                            RUEDAS_update_correction_coeficients();
                            setLCDC(0x46);LCD_Send_String(c);
                            
                            IO_S3 = 0;
                        }
                        if(IO_S6){
                            switch (mLCD_cursor.estado){
                                case PTm_BackLeft:
                                    MinPowerDC1 += 1;
                                    sprintf(c,"%5d  ",MinPowerDC1);
                                    break;
                                case PTm_BackRight:
                                    MinPowerDC2 += 1;
                                    sprintf(c,"%5d  ",MinPowerDC2);
                                    break;
                                case PTm_ForwLeft:
                                    MinPowerDC3 += 1;
                                    sprintf(c,"%5d  ",MinPowerDC3);
                                    break;
                                case PTm_ForwRight:
                                    MinPowerDC4 += 1;
                                    sprintf(c,"%5d  ",MinPowerDC4);
                                    break;
                                default:
                                    break;
                            }
                            RUEDAS_update_correction_coeficients();
                            setLCDC(0x46);LCD_Send_String(c);
                            
                            IO_S6= 0;
                        }
                        if(IO_S4){
                            switch (mLCD_cursor.estado){
                                case PTm_BackLeft:
                                    mLCD_cursor.estado = PTm_BackRight;
                                    sprintf(c,"%5d  ",MinPowerDC2);
                                    break;
                                case PTm_BackRight:
                                    mLCD_cursor.estado = PTm_ForwLeft;
                                    sprintf(c,"%5d  ",MinPowerDC3);
                                    break;
                                case PTm_ForwLeft:
                                    mLCD_cursor.estado = PTm_ForwRight;
                                    sprintf(c,"%5d  ",MinPowerDC4);
                                    break;
                                case PTm_ForwRight:
                                    mLCD_cursor.estado = PTm_BackLeft;
                                    sprintf(c,"%5d  ",MinPowerDC1);
                                    break;
                                default:
                                    break;
                            }
                            
                            setLCDC(0x46);LCD_Send_String(c);
                            
                            
                            menu_LCD_siguiente(menu_LCD_PT_Mm,menu_LCD_PT_Mm_L);
                            /*LCD_ClearScreen();*/setLCDC(0x01);LCD_Send_String(*mLCD_cursor.string);
                                                  
                            IO_S4 = 0;
                        }
                        break;
                    case PTM_BackLeft:
                    case PTM_BackRight:
                    case PTM_ForwLeft:
                    case PTM_ForwRight:
                        if(IO_S3){
                            switch (mLCD_cursor.estado){
                                case PTM_BackLeft:
                                    MaxPowerDC1 -= 1;
                                    sprintf(c,"%5d  ",MaxPowerDC1);
                                    break;
                                case PTM_BackRight:
                                    MaxPowerDC2 -= 1;
                                    sprintf(c,"%5d  ",MaxPowerDC2);
                                    break;
                                case PTM_ForwLeft:
                                    MaxPowerDC3 -= 1;
                                    sprintf(c,"%5d  ",MaxPowerDC3);
                                    break;
                                case PTM_ForwRight:
                                    MaxPowerDC4 -= 1;
                                    sprintf(c,"%5d  ",MaxPowerDC4);
                                    break;
                                default:
                                    break;
                            }
                            RUEDAS_update_correction_coeficients();
                            setLCDC(0x46);LCD_Send_String(c);
                            
                            IO_S3 = 0;
                        }
                        if(IO_S6){
                            switch (mLCD_cursor.estado){
                                case PTM_BackLeft:
                                    MaxPowerDC1 += 1;
                                    sprintf(c,"%5d  ",MaxPowerDC1);
                                    break;
                                case PTM_BackRight:
                                    MaxPowerDC2 += 1;
                                    sprintf(c,"%5d  ",MaxPowerDC2);
                                    break;
                                case PTM_ForwLeft:
                                    MaxPowerDC3 += 1;
                                    sprintf(c,"%5d  ",MaxPowerDC3);
                                    break;
                                case PTM_ForwRight:
                                    MaxPowerDC4 += 1;
                                    sprintf(c,"%5d  ",MaxPowerDC4);
                                    break;
                                default:
                                    break;
                            }
                            RUEDAS_update_correction_coeficients();
                            setLCDC(0x46);LCD_Send_String(c);
                            
                            IO_S6= 0;
                        }
                        if(IO_S4){
                            switch (mLCD_cursor.estado){
                                case PTM_BackLeft:
                                    mLCD_cursor.estado = PTM_BackRight;
                                    sprintf(c,"%5d  ",MaxPowerDC2);
                                    break;
                                case PTM_BackRight:
                                    mLCD_cursor.estado = PTM_ForwLeft;
                                    sprintf(c,"%5d  ",MaxPowerDC3);
                                    break;
                                case PTM_ForwLeft:
                                    mLCD_cursor.estado = PTM_ForwRight;
                                    sprintf(c,"%5d  ",MaxPowerDC4);
                                    break;
                                case PTM_ForwRight:
                                    mLCD_cursor.estado = PTM_BackLeft;
                                    sprintf(c,"%5d  ",MaxPowerDC1);
                                    break;
                                default:
                                    break;
                            }
                            
                            setLCDC(0x46);LCD_Send_String(c);
                            
                            
                            menu_LCD_siguiente(menu_LCD_PT_Mm,menu_LCD_PT_Mm_L);
                            /*LCD_ClearScreen();*/setLCDC(0x01);LCD_Send_String(*mLCD_cursor.string);
                                                  
                                                  IO_S4 = 0;
                        }
                        break;
                    default:
                        break;
                }
                break;
            case M_Ajustar_SP/10:
                switch (mLCD_cursor.estado){
                    case M_Ajustar_SP:
                            if(IO_S3){
                                menu_LCD_anterior(menu_LCD_SPTune,menu_LCD_SPTune_L);
                                /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                                
                                io_cf.S3 = 0;
                            }
                            if(IO_S6){
                                menu_LCD_siguiente(menu_LCD_SPTune,menu_LCD_SPTune_L);
                                /*LCD_ClearScreen();*/setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                                
                                io_cf.S6= 0;
                            }
                            if(IO_S4){
                                switch(mLCD_cursor.string-menu_LCD_SPTune){
                                    case 0:
                                        mLCD_cursor.estado = ST_BalanceSP;
                                        /*LCD_ClearScreen();*/
                                        setLCDC(0x01);LCD_Send_String("PV: ");
                                        setLCDC(0x41);LCD_Send_String("SP: ");
                                        sprintf(c,"%f",B_SP);LCD_Send_String(c);
                                        break;
                                    case 1:
                                        mLCD_cursor.estado = ST_SpeedSP;
                                        /*LCD_ClearScreen();*/
                                        setLCDC(0x00);LCD_Send_String("Set Speed Goal:");
                                        setLCDC(0x41);LCD_Send_String("SP:         cms");
                                        sprintf(c,"%f",S_SP);LCD_Send_String(c);
                                        break;
                                }
                                
                                io_cf.S4 = 0;
                            }
                            break;
                    case ST_BalanceSP:
                        if(IO_S3){
                            B_SP-= AjusteSP;
                            setLCDC(0x45);
                            sprintf(c,"%f",B_SP);LCD_Send_String(c);
                            io_cf.S3 = 0;
                        }
                        if(IO_S6){
                            B_SP+= AjusteSP;
                            setLCDC(0x45);
                            sprintf(c,"%f",B_SP);LCD_Send_String(c);
                            io_cf.S6= 0;
                        }
                        if(IO_S4){
//                            B_SP = ST_PVgoal; /** SetPoint pasa a ser igual al valor actual de ProcessVariable **/
                            setLCDC(0x45);
                            sprintf(c,"%f",B_SP);LCD_Send_String(c);
                            io_cf.S4 = 0;
                        }
                        break;
                    case ST_SpeedSP:
                        if(IO_S3){
                            ST_Speedgoal -=AjusteSpeedGoal;
                            setLCDC(0x45);
                            sprintf(c,"%5.1f",ST_Speedgoal);LCD_Send_String(c);
                            io_cf.S3 = 0;
                        }
                        if(IO_S6){
                            ST_Speedgoal +=AjusteSpeedGoal;
                            setLCDC(0x45);
                            sprintf(c,"%5.1f",ST_Speedgoal);LCD_Send_String(c);
                            io_cf.S6= 0;
                        }
                        if(IO_S4){
                            set_SpeedGoal(ST_Speedgoal);
                            
                            sprintf(c,"Assigned goal:\r   %5.1f    cms",ST_Speedgoal);
                            setLCDC(0x00);LCD_Send_String(c);
                            
                            io_cf.S4 = 0;
                        }
                        break;
                    default:
                        break;
                }
                break;
            default:
                break;
        }
    }
//    if(!PULSADOR_S5){
//        cursor.estado = mainMenu;
//        cursor.string = menu_LCD_main;
//        LCD_ClearScreen();setLCDC(0x41);LCD_Send_String(*cursor.string);
//    }
}

void mLCD_realTimeDisplay(void){
                switch(mLCD_cursor.estado){
                    case ST_BalanceSP:
                        sprintf(c,"%f",B_PV);
                        setLCDC(0x04);LCD_Send_String(c);
                        break;
                    case DV_PIDcomps:
                        sprintf(c,"%6.2f",B_err);
                        setLCDC(0x02);LCD_Send_String(c);
                        sprintf(c,"%5.1f",B_errOT);
                        setLCDC(0x0b);LCD_Send_String(c);
                        sprintf(c,"%6.2f",B_dPV);
                        setLCDC(0x43);LCD_Send_String(c);
                        break;
                    case DV_CPUper:
                        sprintf(c,"%7u",cpu_free_cycles);
                        setLCDC(0x43);LCD_Send_String(c);
                        break;
                    case DV_MainSP:
                        sprintf(c,"%4.1f",B_Pout);
                        setLCDC(0x02);LCD_Send_String(c);
                        sprintf(c,"%7.5f",S_Pout);
                        setLCDC(0x08);LCD_Send_String(c);
                        break;
                    case DV_Velocim:
                        sprintf(c,"%7.2f",S_PV);
                        setLCDC(0x04);LCD_Send_String(c);
                        sprintf(c,"%7.2f",S_PV);
                        setLCDC(0x44);LCD_Send_String(c);
                        break;
                    case DV_Distancia:
                        sprintf(c,"%ld",cms_to_counts(counts_to_cms(IC1_Sum)));
                        setLCDC(0x44);LCD_Send_String(c);
                        break;
                    default:
                        break;
                }
            }