/* 
 * File:   to_do_list.v
 * Author: Christian
 *
 * Created on 31 de mayo de 2019, 10:42
 */

#include "main.h"

uint16_t tim = 500;
int toheside = 20;
double x = 0;

void actionList(ACTIONLISTS al,unsigned char estado){
    
    switch(al){
        case lightshow:
            switch(estado){
                case 0:
                    TRIS_PULSADOR_S5=0;
                    LED_D3 = 1;
                    LED_D4 = 0;
                    LED_D5 = 0;
                    LED_D6 = 0;
                    LED_D7 = 0;
                    LED_D8 = 0;
                    LED_D9 = 0;
                    LED_D10 = 0;
                    WL_registerMileStone(100,lightshow,1);
                    break;
                case 1:
                    LED_D4 = 1;
                    WL_registerMileStone(100,lightshow,2);
                    break;
                case 2:
                    LED_D5 = 1;
                    WL_registerMileStone(100,lightshow,3);
                    break;
                case 3:
                    LED_D3 = 0;
                    LED_D6 = 1;
                    WL_registerMileStone(100,lightshow,4);
                    break;
                case 4:
                    LED_D4 = 0;
                    LED_D7 = 1;
                    WL_registerMileStone(100,lightshow,5);
                    break;
                case 5:
                    LED_D5 = 0;
                    LED_D8 = 1;
                    WL_registerMileStone(100,lightshow,6);
                    break;
                case 6:
                    LED_D6 = 0;
                    LED_D9 = 1;
                    WL_registerMileStone(100,lightshow,18);
                    break;
                case 18:
                    LED_D5 = 0;
                    LED_D10 = 1;
                    WL_registerMileStone(100,lightshow,19);
                    break;
                case 19:
                    LED_D5 = 1;
                    LED_D10 = 0;
                    WL_registerMileStone(100,lightshow,7);
                    break;
                case 7:
                    LED_D6 = 1;
                    LED_D9 = 0;
                    WL_registerMileStone(100,lightshow,8);
                    break;
                case 8:
                    LED_D5 = 1;
                    LED_D8 = 0;
                    WL_registerMileStone(100,lightshow,9);
                    break;
                case 9:
                    LED_D4 = 1;
                    LED_D7 = 0;
                    WL_registerMileStone(100,lightshow,10);
                    break;
                case 10:
                    LED_D3 = 1;
                    LED_D6 = 0;
                    WL_registerMileStone(100,lightshow,20);
                    break;
                case 20:
                    LED_D3 = 0;
                    LED_D6 = 1;
                    WL_registerMileStone(100,lightshow,11);
                    break;
                case 11:
                    LED_D4 = 0;
                    LED_D7 = 1;
                    WL_registerMileStone(100,lightshow,12);
                    break;
                case 12:
                    LED_D5 = 0;
                    LED_D8 = 1;
                    WL_registerMileStone(100,lightshow,13);
                    break;
                case 13:
                    LED_D6 = 0;
                    LED_D9 = 1;
                    WL_registerMileStone(100,lightshow,14);
                    break;
                case 14:
                    LED_D7 = 0;
                    LED_D10 = 1;
                    WL_registerMileStone(100,lightshow,15);
                    break;
                case 15:
                    LED_D8 = 0;
                    WL_registerMileStone(100,lightshow,16);
                    break;
                case 16:
                    LED_D9 = 0;
                    WL_registerMileStone(100,lightshow,17);
                    break;
                case 17:
                    LED_D10 = 0;
                    TRIS_PULSADOR_S5=1;
                    break;
            }
            break;
        case enable_SP_PID_Delayed:
            switch(estado){
                case 0:
                    enable_RUEDAS();
                    enable_BalancePID();
                    
                    WL_registerMileStone(2 s,enable_SP_PID_Delayed,1);
                    break;
                case 1:
                    enable_SpeedPID();
                    
                    break;
                default:
                    return;
                    break;
            } 
            break;
        case SP_TESTS:
            switch(estado){
                case 0:
                    enable_RUEDAS();
                    enable_BalancePID();
                    
                    WL_registerMileStone(2 s,SP_TESTS,1);
                    break;
                case 1:
                    B_SP -= 3;
                    
                    WL_registerMileStone(2 s,SP_TESTS,2);
                    break;
                case 2:
                    B_SP += 6;
                    
                    WL_registerMileStone(2 s,SP_TESTS,3);
                    break;
                case 3:
                    B_SP -= 3;
                    
                    break;
                default:
                    return;
                    break;
            } 
            break;
        case SPEED_TESTS:
            switch(estado){
                case 0:
                    enable_RUEDAS();
                    enable_BalancePID();
                    
                    WL_registerMileStone(2 s,SPEED_TESTS,1);
                    break;
                case 1:
                    enable_SpeedPID();
                    set_SpeedGoal(cms_to_raw(1.5));
                    
                    WL_registerMileStone(2 s,SPEED_TESTS,2);
                    break;
                case 2:
                    set_SpeedGoal(cms_to_raw(-1.5));
                    
                    WL_registerMileStone(2 s,SPEED_TESTS,3);
                    break;
                case 3:
                    set_SpeedGoal(0.0);
                    disable_SpeedPID();
                    
                    break;
                default:
                    return;
                    break;
            } 
            break;
        case PID_TUNER_returnToPIDTuner:
            switch(estado){
                case 0:
                    pid_cf.TUNING = 1;
                    break;
                case 1:
                    pid_cf.TUNING = 1;
                    
                    if (keepRunningTuner){
                        WL_registerMileStone(PIDTFreq,PID_TUNER_returnToPIDTuner,1);
                    }
                    break;
                default:
                    break;
            } 
            break;
        case PID_B_TUNER_returnToPIDTuner:
            switch(estado){
                case 0:
                    pid_cf.B_TUNING = 1;
                    break;
                case 1:
                    pid_cf.B_TUNING = 1;
                    
                    if (keepRunningTuner){
                        WL_registerMileStone(PIDTFreq,PID_B_TUNER_returnToPIDTuner,1);
                    }
                    break;
                default:
                    break;
            } 
            break;
        case PID_S_TUNER_returnToPIDTuner:
            switch(estado){
                case 0:
                    pid_cf.S_TUNING = 1;
                    break;
                case 1:
                    pid_cf.S_TUNING = 1;
                    
                    if (keepRunningTuner){
                        WL_registerMileStone(PIDTFreq,PID_S_TUNER_returnToPIDTuner,1);
                    }
                    break;
                default:
                    break;
            } 
            break;
        case PID_TUNER_returnToPIDTunerTimingTest:
            switch(estado){
                case 0:
                    keepRunningTuner = false;
                    WL_registerMileStone(3 s,PID_TUNER_returnToPIDTunerTimingTest,1);
                    break;
                case 1:
                   RUEDAS_setSpeed_right_p1000(0);
                   RUEDAS_setSpeed_left_p1000(0);
                    break;
                case 2:
                    Nop();
                    int power = (int)(sinl((x+=1)/_180PI)*100+25);
                    RUEDAS_setSpeed_right_p1000(power);
                    RUEDAS_setSpeed_left_p1000(power);
//                    PID.TUNING = 1;
                    if (keepRunningTuner){
                        WL_registerMileStone(10,PID_TUNER_returnToPIDTunerTimingTest,2);
                    }
                    break;
                default:
                    Nop();
                    int aux = B_Pout;
                    B_Pout = toheside;
                    toheside = aux;
                    
                    sprintf(c,"%3f",B_Pout);
                    setLCDC(0x40);LCD_Send_String(c);
                    pid_cf.TUNING = 1;
                    
                    if (keepRunningTuner){
                        WL_registerMileStone(/*(tim-=10)*/110,PID_TUNER_returnToPIDTunerTimingTest,100);
                    }
                    
                    break;
            } 
            break;
        case paseito:
            switch(estado){
                case 0:
                    enable_RUEDAS();
                    enable_BalancePID();
                    
                    WL_registerMileStone(2 s,paseito,1);
                    
                    break;
                case 1:
                    enable_SpeedPID();
                    
                    WL_registerMileStone(10, paseito,2);
                    
                    break;
                case 2:
                    
//                    WL_registerMileStone(1 s, paseito,3);
//                    set_SpeedGoal(0.0);
                    
//                    go_x_cms2(219);
//                    rotate_x_laps2(1,4,2); // 90º
//                    go_x_cms2(60);
//                    rotate_x_laps2(4,9,2); // 120º
//                    go_x_cms2(65);
//                    rotate_x_laps2(4,9,1); // 120º
//                    go_x_cms2(55);
//                    go_x_cms2(-102);
//                    rotate_x_laps2(1,4,-1); // 90º
//                    go_x_cms2(153);
                    
                    go_x_cms2(60);
                    rotate_x_laps2(1,4,1); // 90º
                    go_x_cms2(90);
                    go_x_cms2(-60);
                    rotate_x_laps2(1,1,1); // 90º
//                    go_x_cms2(40);
                    go_x_cms2(0);
                    
                    
                    break;
                case 3:
                    /* Preparacion avance rectilineo */
                    change_S_toStraight();
                    
                    set_Differential(0);
                    
                    IC1_Sum = 0;
                    IC2_Sum = 0;
                    set_sampling_wheel(0);
                    distanceWatch = &IC1_Sum;
                    
                    set_SpeedGoal(20.0);
                    break;
                case 8:
                    /* Preparacion avance rectilineo */
                    change_S_toStraight();
                    
                    set_Differential(0);
                    
                    IC1_Sum = 0;
                    IC2_Sum = 0;
                    set_sampling_wheel(0);
                    distanceWatch = &IC1_Sum;
                    
                    set_SpeedGoal(-20.0);
                    break;
                case 9:
                    /* Preparacion avance rectilineo */
                    change_S_toStraight();
                    
                    set_Differential(0);
                    
                    IC1_Sum = 0;
                    IC2_Sum = 0;
                    set_sampling_wheel(0);
                    distanceWatch = &IC1_Sum;
                    
                    set_SpeedGoal(0.0);
                    break;
                case 4:
                    /* Preparacion giro Izq Ad*/
                    change_S_toTurn();
                    
                        IC1_Sum = 0;
                        distanceWatch = &IC1_Sum;
                        set_sampling_wheel(-1);
                        set_Differential(-1000);
                        
                    
                    set_SpeedGoal(10);
                    break;
                case 5:
                    /* Preparacion giro Izq At*/
                    change_S_toTurn();
                    
                        IC1_Sum = 0;
                        distanceWatch = &IC1_Sum;
                        set_sampling_wheel(-1);
                        set_Differential(-1000);
                    
                    set_SpeedGoal(-10);
                    break;
                case 6:
                    /* Preparacion giro Der Ad*/
                    change_S_toTurn();
                    
                        IC2_Sum = 0;
                        distanceWatch = &IC2_Sum;
                        set_sampling_wheel(1);
                        set_Differential(1000);
                        
                    set_SpeedGoal(10);
                    break;
                case 7:
                    /* Preparacion giro Der At*/
                    change_S_toTurn();
                    
                        IC2_Sum = 0;
                        distanceWatch = &IC2_Sum;
                        set_sampling_wheel(1);
                        set_Differential(1000);
                        
                    set_SpeedGoal(-10);
                    
                    break;
            }
            break;
        default:
            break;
    }
    return;
}