/*
 * File:   main.c
 * Author: Christian Mares
 *
 * Created on 13 de diciembre de 2017, 18:05
 */

#include <p24FJ128GA010.h>

#include "main.h"

//Locales
    /* Module Check Flags */
    volatile IO_CF_t io_cf;                          
    volatile UART_CF_t uart_cf;                          
    volatile PWM_CF_t pwm_cf;                         
    volatile RUEDAS_CF_t ruedas_cf;                       
    volatile SENSOR_CF_t sensor_cf;                            
    volatile PID_CF_t pid_cf;
    volatile LOCOMO_CF_t locomo_cf;
    
    volatile char* IO_cf      = (void*)&io_cf;             
    volatile char* UART_cf    = (void*)&uart_cf; 
    volatile char* PWM_cf     = (void*)&pwm_cf;  
    volatile char* RUEDAS_cf  = (void*)&ruedas_cf; 
    volatile char* SENSOR_cf  = (void*)&sensor_cf;  
    volatile char* PID_cf     = (void*)&pid_cf;
    volatile char* LOCOMO_cf  = (void*)&locomo_cf;

    /* Miscelaneas */
    char c[40]; //String para escribir ensajes con proposito general
    char* c_max=c+39, c_lp;
    
    int d1=DELAY_1,d2=DELAY_2,d3=DELAY_3,d4=DELAY_4;
    
    /* Uso de CPU */
    double cpu;
    unsigned int cpu_free_cycles;
    bool SHOW_CPU = TRUE;    
    
void Inic_System()
{        
//    OSCILADOR
        OSCILLATOR_Initialize();
//    ENTRADA/SALIDA
        Inic_IO();
#ifdef __DEBUG  
        while(!IO_S4);
        io_cf.S4 = 0;
#endif
//    Timer 1 (para tener funcion delay() )
        TMR1_Initialize();
//    LCD
        Inic_LCD();
        
        LCD_ClearScreen();       
        LCD_Send_String("Robot balancin\r");
        LCD_Send_String("Inicializando...\r");
        
    //Cambiar brebemente RD14 de input a output soluciona el problema del Power On Reset del MM7150
        TRISDbits.TRISD14=0;
            LATDbits.LATD14=1;
            LED_D7=1;
            delay(10*DECIMAS);
            LATDbits.LATD14=0;
            LED_D7=0;
        TRISDbits.TRISD14=1;
        
//    PUERTO SERIE
        Init_UART(DIRMOD);
        Inic_Display();
        DISPLAY_enviarString(Clrscr);
        DISPLAY_enviarString(Home);
        DISPLAY_enviarString("Inicializando\n");
        UART_start_transmision();

        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);
        DISPLAY_cambiar_VENTANA_s(PID_equation,PID_Line,6,0);
        
//    PWM
        Inic_PWM();
        Inic_Ruedas();
//    Input Capture
        IC_Initialize();
//    I2C y realcionados
        I2C1_Initialize();
        TMR2_Initialize();
//    Interrupciones externas
        EXT_INT_Initialize();
    
        Inic_SensorModule();
        
        Enable_ACCELEROMETER();
        Enable_GIROSCOPIO();
        
        Inic_Timer4(); 
        TMR5_Initialize();
        WL_init();
        DWL_init();
        
    cronometro(0,1,0);
    
        TMR5_Stop();
        TMR5_SoftwareCounterClear();
        TMR5_Start();
    
    // Inicializacion del menu LCD 
    Init_menu_LCD(true);
    
    // Inicializar PID
    Inic_PID();
    
//    get_speed_raw_M();
    get_speed_cms_M();
}

int main(void) 
{       
    /* ************************************** */
    /* = Inicializar  modulos  del  sistema = */
    /* ************************************** */
                  Inic_System();
    /* ************************************** */
    /* = ================================== = */
    /* ************************************** */

    
    _TFCTON = 1; // Arrancar cuenta del TMR4
    while(!go);  // Esperar a la primera interrupcion TMR4
    go = 0;

//    enable_RUEDAS();
//    RUEDAS_setSpeed_left_p1000(25);
//    double ey = IC1_Sum;
//    LCD_ClearScreen();
//    while (ey < 1920/4){
//        sprintf(c,"%0.0f\t%f",ey*4,ey*0.04254240051736178343751496248191);
//        setLCDC(0x44);LCD_Send_String(c);
//        ey = IC1_Sum;
//    }
//    RUEDAS_setSpeed_left_p1000(0);
//        ey = IC1_Sum;
//        sprintf(c,"%0.0f\t%f",ey*4,ey*0.04254240051736178343751496248191);
//        setLCDC(0x44);LCD_Send_String(c);
    
    
//    LCD_Home();
//    LCD_Send_String("Power: xxxx/1000\rL:      R:");
//    
//    enable_RUEDAS();
//    int power = 0;
//    int incremento = 1;
//    char motor=0;
//    int i=0;
//    while(1){
//        if(IO_S3){
////            if(motor == 0){
////                set_dutyCicle_1(power-=incremento);
////            }
////            else if(motor == 1){
////                set_dutyCicle_2(power-=incremento);
////            }
////            else if(motor == 2){
////                set_dutyCicle_3(power-=incremento);
////            }
////            else if(motor == 3){
////                set_dutyCicle_4(power-=incremento);
////            }
////            
//            RUEDAS_setSpeed_right_p1000(power-=incremento);
//            RUEDAS_setSpeed_left_p1000(power);
//            
//            sprintf(c,"%5d",power);
//            setLCDC(0x06);LCD_Send_String(c);
//            
//            IO_S3 = 0;
//        }
//        if(IO_S6){
////            if(motor == 0){
////                set_dutyCicle_1(power+=incremento);
////            }
////            else if(motor == 1){
////                set_dutyCicle_2(power+=incremento);
////            }
////            else if(motor == 2){
////                set_dutyCicle_3(power+=incremento);
////            }
////            else if(motor == 3){
////                set_dutyCicle_4(power+=incremento);
////            }
//            RUEDAS_setSpeed_right_p1000(power+=incremento);
//            RUEDAS_setSpeed_left_p1000(power);
//            
//            sprintf(c,"%5d",power);
//            setLCDC(0x06);LCD_Send_String(c);
//            
//            IO_S6= 0;
//        }
//        if(IO_S5){
////            if(motor == 0){
////                motor = 1;
////            }
////            else if(motor == 1){
////                motor = 2;
////            }
////            else if(motor == 2){
////                motor = 3;
////            }
////            else if(motor == 3){
////                motor = 0;
////            }
//            
//            if(incremento == 1){
//                incremento = 5;
//            }
//            else if(incremento == 5){
//                incremento = 10;
//            }
//            else if(incremento == 10){
//                incremento = 50;
//            }
//            else if(incremento == 50){
//                incremento = 100;
//            }
//            else if(incremento == 100){
//                incremento = 1;
//            }
//            sprintf(c,"%d",incremento);
//            setLCDC(0x00);LCD_Send_String(c);
//            IO_S5 = 0;
//        }
//        if(IO_S4){
////            set_dutyCicle_1(power=0);
////            set_dutyCicle_2(power);
////            set_dutyCicle_3(power);
////            set_dutyCicle_4(power);
//            
//            RUEDAS_setSpeed_right_p1000(power=0);
//            RUEDAS_setSpeed_left_p1000(power);
//            
//            sprintf(c,"%5d",power);
//            setLCDC(0x06);LCD_Send_String(c);
//            
//            IO_S4 = 0;
//        }
//        
//        
//        if(d2 <= 0){
//            if(!PULSADOR_S5 && !debounce_S5){
//                debounce_S5 = debounce_T;
//                IO_S5 = 1;
//            }
//
//            if(debounce_S3){ debounce_S3--;}
//            if(debounce_S6){ debounce_S6--;}
//            if(debounce_S5){ debounce_S5--;}
//            if(debounce_S4){ debounce_S4--;}
//                        
//            sprintf(c,"%6.2f"/*,get_speed_raw_L()/*/,get_speed_rpm_L()/**/);
//            setLCDC(0x42);LCD_Send_String(c);
//            sprintf(c,"%6.2f"/*,get_speed_raw_R()/*/,get_speed_rpm_R()/**/);
//            setLCDC(0x4a);LCD_Send_String(c);
//            
////            i=!i;
////            sprintf(c,"%d",i);
////            setLCDC(0x40);LCD_Send_String(c);
//            
//            if(OUTPUT_ENABLED){
//                if (!DIRECT_MODE){
//                    refrescar_Ventana_UART();
//                }
//            }
//            
//            d2=10;
//        }
//        d2--;
//        while(!go);
//        go=0;
//    }
    
//    bias = get_bias_Nontilting(5);
//    
//    sprintf(c,"%6.4f",bias);
//    setLCDC(0x40);LCD_Send_String(c);
//    UART_enviarString(c);
//    render_Text_Box();
    
//    disable_OutPut();
    
//    delay(5000);
    
    while(1)
    {
        if(PID_BALANCE){

            /* Leer sensores */
            ACC_readSensor(accData);
            Curr_GYRO = GYRO_getData();
            
            Sensor_fusion();
            Balance_PID();

            pid_cf.BALANCE = 0;
        }
        if(PID_SPEED){

            double aux;
            switch(sampling_wheel){
                case 1:
                    aux = get_speed_cms_R();
                    break;
                case -1:
                    aux = get_speed_cms_L();
                    break;
                case 0:
                default:
                    aux = get_speed_cms_M();
                    break;
            }
            Speed_PID(aux);
            
            pid_cf.SPEED = 0;
        }
        
        if(PID_TUNING_IS_ON){
            PIDT_Oscilometer2(); //El proceso de calibrado ha comenzado. El oscilometro empieza a funcionar
            if(PID_B_TUNING){
                PID_B_tuner(false);
                pid_cf.B_TUNING = 0;
            }
            if(PID_S_TUNING){
                PID_S_tuner(false);
                pid_cf.S_TUNING = 0;
            }
        }
        
        if((OUTPUT_ENABLED && DIRECT_MODE) && (d4 <= 0)){
            directModeOutput();
            
            d4=DELAY_4;
        }
        
        /* delays por iteraciones */
        if(d1 <= 0){   // 100 iteraciones cuando T=0.01s, 10 cuando T=0.1s
            /* UART */
            if(OUTPUT_ENABLED){
                if (!DIRECT_MODE){
                    refrescar_Ventana_UART();
                }
            }
            d1=DELAY_1;
        }
        else if(d2 <= 0){   // 10 iteraciones cuando T=0.01s, 1 cuando T=0.1s
            
            // Atencion al pulsador 5 en el contexto del menu LCD
            if(!PULSADOR_S5 && TRIS_PULSADOR_S5 && !debounce_S5){
                if(mLCD_cursor.string != menu_LCD_main){
                    if(mLCD_cursor.estado == DV_Velocim);
                    if(mLCD_cursor.estado == TG_Auto_B || mLCD_cursor.estado == TG_Auto_S || mLCD_cursor.estado == M_controles) PID_TUNING_IS_ON=0;
                    if(mLCD_cursor.estado == M_listaAcciones){
                        WL_deleteAllMilestones();
                        DWL_deleteAllMilestones();
                        disable_RUEDAS();
                        disable_BalancePID();
                        disable_SpeedPID();
                        set_Differential(0);
                        change_S_toStraight();
                    }
                    mLCD_cursor.estado = mainMenu;
                    mLCD_cursor.string = menu_LCD_main;
                }else{
                    mLCD_cursor.estado = mainMenu;
                    mLCD_cursor.string = menu_LCD_main+(M_clrOLled/10);
                }
                LCD_ClearScreen();setLCDC(0x41);LCD_Send_String(*mLCD_cursor.string);
                debounce_S5 = debounce_T;
            }
            if(debounce_S3){ debounce_S3--;} // Este codigo evita 
            if(debounce_S6){ debounce_S6--;} // que un pulsador se
            if(debounce_S5){ debounce_S5--;} // active mas de una 
            if(debounce_S4){ debounce_S4--;} // vez involuntariamente
                        
            if(*IO_cf){     // Si se detecta input (pulsadores) se ejecuta el
                menu_LCD(); // menu LCD
            }
             
            mLCD_realTimeDisplay();// Displays en tiempo real del menu LCD
            
            d2=DELAY_2;
        }
        else if(d3 <= 0){   // 2 iteraciones cuando T=0.01s, 1 cuando T=0.1s
            
            if(TRIS_PULSADOR_S5){
                updateTiltLeds();
            }
            
            if(!get_BetweenRecoveryMargin()){ // Si el robot se inclina fuera de
                disable_RUEDAS();             // un margen seguro, se desactivan
                disable_BalancePID();         // ruedas y PID
            }
            
            Locomotion_watch();
            
            d3=DELAY_3;
        }
        
        //Actualizar delays
        d1--;
        d2--;
        d3--; 
        d4--;
        
    //Control de duración del bucle principal y uso de CPU
        loopEndHold();
    }
    return 0;
}

void cronometro(uint8_t m, uint8_t secs, uint16_t f){
    
    LCD_ClearScreen();
    
    uint16_t fsec = 0;
    uint8_t sec = 0,
            min = 0,
            count=0;
    
    assign(c,"xx:xx:xxx",c_max);    // Preparar string para LCD
    p_uint_fl(c+6,fsec,2,1,c_max);
    
    TMR1_Stop();
    TMR1_SoftwareCounterClear();
    TMR1_Start();
    while(1){
        TMR1_SoftwareCounterClear();
        LCD_Home();
        LCD_Send_String(c);
        if(count++==4){
            fsec++;
            count=0;
        }
        if(fsec==100){
            sec++;
            fsec=0;
        }
        p_uint_fl(c+6,fsec,2,1,c_max);
        if(sec==60){
            min++;
            sec=0;
        }
        p_uint_fl(c+3,sec,2,0,c_max);
        if(min==99){
            min=0;
        }
        p_uint_fl(c,min,2,0,c_max);
        while(TMR1_SoftwareCounterGet()<10);
        
        if(min==m && sec==secs && fsec==f){
            return;
        }
    }
    while(TMR1_SoftwareCounterGet()<60000);
    
}

void updateTiltLeds(void){
                if(get_TiltDirection()){
                    LED_D9=1;
                    LED_D8=0;
                    LED_D7=0;
                }else if(get_BetweenBlindSpot()){
                    LED_D9=0;
                    LED_D8=1;
                    LED_D7=0;
                }else{
                    LED_D9=0;
                    LED_D8=0;
                    LED_D7=1;
                }
            }

int L3_count = 10;
unsigned int OverloadLedDelay;
unsigned long int CPU_counter = 0;
void loopEndHold(void){
    if(go == 0){ //Comprobar si el TMR4 ha no llegado a interrumpir antes de terminar esta iteracon
        //            cpu = (double)_TFCTMR/(double)_TFCPR; //Calcular uso de CPU
        
        OverloadLedDelay -= OverloadLedDelay?1:0; //Control del LED de aviso
        
        if(TRIS_PULSADOR_S5){
            LED_D5=OverloadLedDelay; // Apagar si termina el delay
            LED_D4 = !LED_D4;
            if(!L3_count--){L3_count=10; LED_D3 = !LED_D3;}
        }
        CPU_counter=0;
        while(!go){  //CPU_counter cuenta tiempo hasta la siguiente iteracion del main loop. Cada cuenta vale por 7 ciclos de CPU, por tanto: (CPU_counter*7)+1== Ciclos de CPU libres
            CPU_counter++;
        }      //Esperar a que TMR4 vuelva a interrumpir para hacer sigueinte iteracion
        go=0;
        cpu_free_cycles = (CPU_counter*10)+1; //Calcular ciclos de CPU libres
        
    }else{ //Si TMR4 ya habia interrumpido antes del final de la iteracion encender luz de aviso
        if(TRIS_PULSADOR_S5)LED_D5 = 0;                 //Encender LED de aviso
        go=0;
        OverloadLedDelay=OLED_DELAY;
    }
}