#include "modulation.h"
#include "app_state.h"      /* task, TIM10_coflag, TO10, TIM10_period, TO10_counter, LD_blinker,
                               LD1_param, LD2_param, LD1_curr_setup, LD2_curr_setup, temp16,
                               htim4, htim8, htim10, htim11 */
#include "laser_hw.h"       /* MPhD_T */
#include "temp_control.h"   /* PID_Controller_Temp */
/* Set_LTEC() приходит из main.h */

/* --- Stop_TIM10 --- */
void Stop_TIM10(void)
{
	HAL_TIM_Base_Stop_IT(&htim10);
	TIM10_coflag = 0;
	TO10 = 0;
}

void Sweep_RunCurrent1(void)
{
	HAL_StatusTypeDef st;
	/* --- тело case TT_CHANGE_CURR_1 --- */
    //calculating timer periods for ADC clock and Mach-Zander modulator
    //ADC clock
    //TIM4 -> ARR = 60; // for 1.5 MHz
    //TIM4 -> ARR = 91; // for 1 MHz
    //TIM4 -> ARR = 45; // for 2 MHz

    //online calculation for debug purposes:
    //manually varying TIM4 -> ARR by debugger while running
    //TIM4 -> CCR3 = (TIM4 -> ARR +1)/2 - 1;


    //Mach-Zander clock (should be half of ADC clock freq)
    //TIM11 -> ARR = (TIM4 -> ARR +1)*2 - 1;
    //TIM11 -> CCR1 = (TIM11 -> ARR +1)/2 - 1;


    //for stm_ADC
    //TIM4 -- ADC clock. Rising edge -- for SYNC_DETector_ON
    //TIM4 -- ADC clock. Falling edge -- for SYNC_DETector_OFF

    //TIM4 freq is equal to TIM11
    //TIM4 phase should be adjustable to get correct sampling moment



    Set_LTEC(TT_CHANGE_CURR_2, task.curr);
    (void) MPhD_T(TT_CHANGE_TEMP_1);
    LD1_param.LD_CURR_TEMP = MPhD_T(TT_CHANGE_TEMP_1);
    (void) MPhD_T(TT_CHANGE_TEMP_2);
    LD2_param.LD_CURR_TEMP = MPhD_T(TT_CHANGE_TEMP_2);
    temp16=PID_Controller_Temp(&LD1_curr_setup, &LD1_param, 1);
    Set_LTEC(TT_CHANGE_TEMP_1, temp16);//Drive Laser TEC 1
    temp16=PID_Controller_Temp(&LD2_curr_setup, &LD2_param, 2);
    Set_LTEC(TT_CHANGE_TEMP_2, temp16);//Drive Laser TEC 2

    // Toggle pin for oscilloscope
    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_SET); //start of the whole frequency sweep procedure (start trigger pulse)
    //for (uint32_t i = 100000; i; --i){;}


    st = HAL_TIM_Base_Start_IT(&htim10);
    if (st != HAL_OK)
        while(1);

    uint16_t step_counter = 0;
    uint16_t trigger_counter = 3;
    //uint16_t trigger_step = (uint8_t )((task.max_param - task.current_param)/task.delta_param * 10);
    uint16_t trigger_step = 50;
    
    uint16_t task_sheduler = 0;



    HAL_TIM_PWM_Stop(&htim11, TIM_CHANNEL_1); //start modulating by Mach-Zander modulator
    HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_3); //start ADC clock
    TIM11 -> CR1 &= ~(1 << 3); //disables one-pulse mode
    TIM4 -> CR1 &= ~(1 << 3); //disables one-pulse mode



    TIM11 -> CNT = 0;
    TIM4 -> CNT = 0;

    HAL_TIM_PWM_Start(&htim11, TIM_CHANNEL_1); //start modulating by Mach-Zander modulator
    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3); //start ADC clock
    //TIM4 -> CNT = 0;

    TIM4 -> CNT = TIM4 -> ARR - 20; // not zero to make phase shift that will be robust to big delay in RF subsystem (up to ~400 ns)
    TIM11 -> CNT = 0;

    uint32_t curr_step_N = 0;
    uint32_t postAmpl_1_en_curr_step_N = ((task.max_param - task.current_param)/3.0 * 1.0) / task.delta_param;
    uint32_t postAmpl_2_en_curr_step_N = ((task.max_param - task.current_param)/3.0 * 2.0) / task.delta_param;


    while (task.current_param < task.max_param)
    {
        if (TIM10_coflag)
        {
            Set_LTEC(TT_CHANGE_CURR_1, task.current_param);
            HAL_GPIO_TogglePin(GPIOG, GPIO_PIN_9); //toggle PG9. Change indicates that current step started.

            //TIM11 -> CNT = 0; // to link modulator phase
            //TIM4 -> CNT = 0; // to link ADC clock phase
            task.current_param += task.delta_param;
            TO10 = 0;
            TIM10_coflag = 0;
            
            if (curr_step_N == postAmpl_1_en_curr_step_N){
                HAL_GPIO_WritePin(OUT_6_GPIO_Port, OUT_6_Pin, GPIO_PIN_SET);
            }
            if (curr_step_N == postAmpl_2_en_curr_step_N){
                HAL_GPIO_WritePin(OUT_7_GPIO_Port, OUT_7_Pin, GPIO_PIN_SET);;
            }



            //HAL_GPIO_WritePin(GPIOG, GPIO_PIN_9, GPIO_PIN_SET); // set the current step laser current trigger
            //HAL_GPIO_WritePin(GPIOG, GPIO_PIN_9, GPIO_PIN_RESET);
//*
            if (step_counter % trigger_step == 0){ //trigger at every 60 step
                //OUT_trigger(trigger_counter);
                ++trigger_counter;
            }
            ++step_counter;

            //HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_RESET); //stop trigger pulse
//*/
/*
            ++task_sheduler;
            if (task_sheduler >= 10){
                task_sheduler = 0;
            }
            //maintain stable temperature of laser 2
            if (task_sheduler == 0){
                (void) MPhD_T(TT_CHANGE_TEMP_2);
                LD2_param.LD_CURR_TEMP = MPhD_T(TT_CHANGE_TEMP_2);
                temp16=PID_Controller_Temp(&LD2_curr_setup, &LD2_param, 2);
                Set_LTEC(TT_CHANGE_TEMP_2, temp16);//Drive Laser TEC 2
            }
            //maintain stable temperature of laser 1
            //*
            if (task_sheduler == 5){
                (void) MPhD_T(TT_CHANGE_TEMP_1);
                LD1_param.LD_CURR_TEMP = MPhD_T(TT_CHANGE_TEMP_1);
                temp16=PID_Controller_Temp(&LD1_curr_setup, &LD1_param, 1);
                Set_LTEC(TT_CHANGE_TEMP_1, temp16);//Drive Laser TEC 1
            }
//*/
        }
    }
    TIM11 -> DIER |= 1; //enable update interrupt. In this IRQ handler we will set both tims to one-pulse mode.
    //TIM11 -> CR1 |= 1 << 3; //sets timer to one-pulse mode. So it will turn off at the next UpdateEvent
    //TIM4 -> CR1 |= 1 << 3; //sets timer to one-pulse mode. So it will turn off at the next UpdateEvent
    //but one-pulse mode should be disabled

    //HAL_TIM_PWM_Stop(&htim11, TIM_CHANNEL_1); //start modulating by Mach-Zander modulator
    //HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_3); //start ADC clock



    Stop_TIM10();
    HAL_GPIO_WritePin(OUT_6_GPIO_Port, OUT_6_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(OUT_7_GPIO_Port, OUT_7_Pin, GPIO_PIN_RESET);

    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_RESET); //stop trigger pulse


    task.current_param = task.min_param;
    Set_LTEC(TT_CHANGE_CURR_1, task.current_param);
    if (task.tau > 3)
    {
        TIM10_period = htim10.Init.Period;
        htim10.Init.Period = 9999;
        TO10_counter = (task.tau - 1) * 100;
    }
    HAL_TIM_Base_Start_IT(&htim10);
}

void Blink_RunCurrent2(void)
{
	HAL_StatusTypeDef st;
	/* --- тело case TT_CHANGE_CURR_2 (без break;) --- */
    //Blink laser 2
//*
    Set_LTEC(TT_CHANGE_CURR_1, task.curr);
    (void) MPhD_T(TT_CHANGE_TEMP_1);
    LD1_param.LD_CURR_TEMP = MPhD_T(TT_CHANGE_TEMP_1);
    (void) MPhD_T(TT_CHANGE_TEMP_2);
    LD2_param.LD_CURR_TEMP = MPhD_T(TT_CHANGE_TEMP_2);
    temp16=PID_Controller_Temp(&LD1_curr_setup, &LD1_param, 1);
    Set_LTEC(TT_CHANGE_TEMP_1, temp16);//Drive Laser TEC 1
    temp16=PID_Controller_Temp(&LD2_curr_setup, &LD2_param, 2);
    Set_LTEC(TT_CHANGE_TEMP_2, temp16);//Drive Laser TEC 2

    LD_blinker.task_type = 2;
    LD_blinker.state = 0; // 0 -- disabled (do nothing); 1 -- update LD current; 2 -- blinking, LD ON now; 3 -- blinking, LD OFF now
    //LD_blinker.param = task.current_param;
    LD_blinker.param = 0;
    LD_blinker.param = 1000; // LD2 current (in unspecified units)
    LD_blinker.signal_port = OUT_9_GPIO_Port;
    LD_blinker.signal_pin = OUT_9_Pin;

    TIM8->ARR = 10000; //zero to LD_blinker.param change frequency (also in unspecified units).
                        //When it is too low -- Desktop app crashes (there is not so much compute sources on MCU to anwser to desktop`s questions)
    st = HAL_TIM_Base_Start_IT(&htim8);
    if (st != HAL_OK)
        while(1);
//						*/

    // Toggle pin for oscilloscope
    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_SET);
    uint32_t i = 10000; while (--i){}
    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_RESET);
    LD_blinker.state = 2;

    st = HAL_TIM_Base_Start_IT(&htim10);
    if (st != HAL_OK)
        while(1);
    while (task.current_param < task.max_param)
    {
        if (TIM10_coflag)
        {
            //Set_LTEC(TT_CHANGE_CURR_2, task.current_param);
            //LD_blinker.param = task.current_param;
            //++LD_blinker.param;
            task.current_param += task.delta_param;
            TO10 = 0;
            TIM10_coflag = 0;


        }
    }
    HAL_TIM_Base_Stop(&htim10);
    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_SET);

    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_RESET);

    HAL_TIM_Base_Stop_IT(&htim8);
    TIM8->CNT = 0;

    Stop_TIM10();
    task.current_param = task.min_param;
    Set_LTEC(TT_CHANGE_CURR_2, task.current_param);
    if (task.tau > 3)
    {
        TIM10_period = htim10.Init.Period;
        htim10.Init.Period = 9999;
        TO10_counter = (task.tau - 1) * 100;
    }
    HAL_TIM_Base_Start_IT(&htim10);


//*/

    /*  // Backup
    Set_LTEC(TT_CHANGE_CURR_1, task.curr);
    (void) MPhD_T(TT_CHANGE_TEMP_1);
    LD1_param.LD_CURR_TEMP = MPhD_T(TT_CHANGE_TEMP_1);
    (void) MPhD_T(TT_CHANGE_TEMP_2);
    LD2_param.LD_CURR_TEMP = MPhD_T(TT_CHANGE_TEMP_2);
    temp16=PID_Controller_Temp(&LD1_curr_setup, &LD1_param, 1);
    Set_LTEC(TT_CHANGE_TEMP_1, temp16);//Drive Laser TEC 1
    temp16=PID_Controller_Temp(&LD2_curr_setup, &LD2_param, 2);
    Set_LTEC(TT_CHANGE_TEMP_2, temp16);//Drive Laser TEC 2

    // Toggle pin for oscilloscope
    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_SET);
    HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_RESET);

    st = HAL_TIM_Base_Start_IT(&htim10);
    if (st != HAL_OK)
        while(1);
    while (task.current_param < task.max_param)
    {
        if (TIM10_coflag)
        {
            Set_LTEC(TT_CHANGE_CURR_2, task.current_param);
            task.current_param += task.delta_param;
            TO10 = 0;
            TIM10_coflag = 0;


        }
    }
    Stop_TIM10();
    task.current_param = task.min_param;
    Set_LTEC(TT_CHANGE_CURR_2, task.current_param);
    if (task.tau > 3)
    {
        TIM10_period = htim10.Init.Period;
        htim10.Init.Period = 9999;
        TO10_counter = (task.tau - 1) * 100;
    }
    HAL_TIM_Base_Start_IT(&htim10);
    */

}