/*
 * optical_bus_tools.c
 *
 *  Created on: 19 авг. 2024 г.
 *      Author: user
 */
#include <adc_tools.h>
#include <alg_simple_scalar.h>
#include <alg_uf_const.h>
#include <break_regul.h>
#include <edrk_main.h>
#include <optical_bus.h>
#include <params.h>
#include <params_norma.h>
#include <params_pwm24.h>
#include <project.h>
#include <v_pwm24_v2.h>
#include <v_rotor.h>
#include <vector.h>
#include "global_time.h"
#include "IQmathLib.h"
#include "oscil_can.h"
#include "uf_alg_ing.h"
#include "MemoryFunctions.h"
#include "RS_Functions.h"
#include "v_rotor_22220.h"
#include "log_to_memory.h"
#include "log_params.h"





///////////////////////////////////////////////////////////////////


#pragma CODE_SECTION(optical_bus_read_write_interrupt,".fast_run2");
void optical_bus_read_write_interrupt(void)
{


    static unsigned int prev_error_read = 0, count_read_optical_bus_error = 0, count_read_optical_bus_old_data = 0;
    static unsigned int max_count_read_old_data_optical_bus = 15;
    static unsigned int flag_disable_resend = 0;
    static unsigned int count_resend = 0, cc=0;
//    static STATUS_DATA_READ_OPT_BUS buf_status[10];
    static STATUS_DATA_READ_OPT_BUS optbus_status;
    static unsigned int  tt=0;
    static unsigned int  cmd_wdog_sbus = 0, count_wait_wdog_sbus = 0, wdog_sbus = 0;
    static int prepare_time = 0;


    static unsigned int  t_finish_optbus = 14, t_read_optbus = 6, t_write_optbus = 10, max_count_read_error_optical_bus = 15, max_count_read_wdog_optical_bus = 48;
    static int cmd_optbus = 0;

    static int flag_enable_read=0, flag_finish_read = 0, flag_enable_write = 0, flag_finish_write = 0, count_wait_write = 0;

    if (prepare_time==0)
    {
        if (edrk.flag_second_PCH==0)
        {
            t_read_optbus = 6;
            t_write_optbus = 8;
            t_finish_optbus = 20;
        }

        if (edrk.flag_second_PCH==1)
        {
            t_read_optbus = 12;
            t_write_optbus = 14;
            t_finish_optbus = 10;
        }
        prepare_time = 1;
    }

    if (flag_special_mode_rs==1)
      return;

    if (edrk.KvitirProcess)
      return;

    if (edrk.disable_interrupt_timer2)
        return;

#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_17_ON;
#endif

//i_led2_on_off(1);


//#if (ENABLE_LOG_INTERRUPTS)
//    add_log_interrupts(2);
//#endif


//    pause_1000(100);
    if (optical_read_data.flag_clear)
    {
  //    stage_1 = 0;
      optical_read_data.timer = 0;
      optical_read_data.flag_clear = 0;
      optical_read_data.error_wdog = 0;
//      count_wait_wdog_sbus = 0;

//      if (optical_read_data.data_was_update_between_pwm_int==0)
//          sum_count_err_read_opt_bus++;
//
//      optical_read_data.data_was_update_between_pwm_int = 0;

  //    optical_read_data.data_was_update_between_pwm_int = 0;
      cc = 0;
      prev_error_read = 0;
    }


#if(_ENABLE_PWM_LINES_FOR_TESTS)
    if (optical_read_data.timer==0)
    {
      PWM_LINES_TK_16_ON;
    }
    else
    {
        PWM_LINES_TK_16_OFF;
    }
#endif



//    else
    optical_read_data.timer++;

//    if (edrk.into_pwm_interrupt==1)
//    {
//        if (optical_read_data.timer>=2)
//        {
//          optical_read_data.timer--;
//          optical_read_data.timer--;
//        }
//        flag_disable_resend = 0;
//        count_resend = 0;
//
//    }

//
//
//
//      if (stage_1==0)
//          tt = t1;
//
//      if (stage_1==1)
//          tt = t2;

    if (edrk.ms.another_bs_maybe_on==1 && edrk.flag_second_PCH==0 /*edrk.auto_master_slave.local.bits.master*/ )
    {

        if (optical_read_data.data.cmd.bit.wdog_tick)
        {
  //          i_led1_on();
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_21_ON;
#endif
        }
        else
        {
    //        i_led1_off();
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_21_OFF;
#endif

        }

        optical_write_data.data.cmd.bit.wdog_tick =  optical_read_data.data.cmd.bit.wdog_tick;

        if (optical_write_data.data.cmd.bit.wdog_tick)
        {
    //        i_led2_on();
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_22_ON;
#endif

        }
        else
        {
     //       i_led2_off();
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_22_OFF;
#endif
        }



    }


    if (edrk.ms.another_bs_maybe_on==1 && edrk.flag_second_PCH==1 /*edrk.auto_master_slave.local.bits.slave*/ )
    {

        if (optical_write_data.data.cmd.bit.wdog_tick)
        {
    //        i_led2_on();
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_22_ON;
#endif

        }
        else
        {
     //       i_led2_off();
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_22_OFF;
#endif
        }



        if (optical_read_data.data.cmd.bit.wdog_tick)
        {
   //         i_led1_on();
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_21_ON;
#endif
        }
        else
        {
     //       i_led1_off();
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_21_OFF;
#endif
        }




        // пишем
        optical_write_data.data.cmd.bit.wdog_tick = wdog_sbus;
        if (cmd_wdog_sbus==0)
        {
//            optical_write_data.data.cmd.bit.wdog_tick = wdog_sbus;
            count_wait_wdog_sbus = 0;
            cmd_wdog_sbus++;
        }
        else
        // ждем подтверждения
        if (cmd_wdog_sbus==1)
        {
          if (optical_read_data.data.cmd.bit.wdog_tick == wdog_sbus) //&& prev_error_read==0
          {
 //               result_code_wdog_sbus = 1;
              optical_read_data.count_error_wdog = count_wait_wdog_sbus;
                count_wait_wdog_sbus = 0;
                wdog_sbus = !wdog_sbus;
                cmd_wdog_sbus = 0;
          }
          else
          {
              if (count_wait_wdog_sbus<max_count_read_wdog_optical_bus )  //6
              {
                count_wait_wdog_sbus++;
              }
              else
              {
                  if (optical_read_data.error_wdog==0)
                  {
                      // не было ошибок и вдруг есть!

//                      i_led2_toggle();
//                      pause_1000(10);
//                      i_led2_toggle();
//                      pause_1000(10);
                  }
     //             if (optical_read_data.data.cmd.bit.alarm==0) // пока так!
                  optical_read_data.error_wdog = 1;  // даем ошибку

              }


          }
        }


    }

#if(_ENABLE_PWM_LINES_FOR_TESTS)

//    if (count_wait_wdog_sbus==0)
//    {
//        PWM_LINES_TK_16_ON;
//    }
//    else
//    {
//        PWM_LINES_TK_16_OFF;
//    }


#endif


#if(_ENABLE_PWM_LINES_FOR_TESTS)
    if (optical_read_data.error_wdog)
    {
        PWM_LINES_TK_23_ON;
        PWM_LINES_TK_23_ON;
    }
    else
    {
        PWM_LINES_TK_23_OFF;
    }
#endif



#define TIME_WAIT_CMD_WRITE     5 // сколько тиков едет запись до приемника, после этого можно попробовать прочитать данные
if (edrk.flag_second_PCH==1)
{
    switch (cmd_optbus)
    {
        case 0 : if (optical_read_data.timer==t_read_optbus)
                    cmd_optbus = 1;
                break;
        case 1:         flag_enable_read = 1;
                        flag_finish_read = 0;
                        flag_enable_write = 0;
                        flag_finish_write = 0;
                        count_wait_write = 0;
                        cmd_optbus = 2;
                break;

        case 2: if (flag_finish_read)
                {
                    flag_enable_write = 1;
                    count_wait_write = TIME_WAIT_CMD_WRITE;
                    cmd_optbus = 3;
                }
                break;

        case 3: if (flag_enable_write==0)
                {
                    if (count_wait_write)
                    {
                       count_wait_write--;
                    }
                    else
                        cmd_optbus = 4;
                }
                break;

        case 4: if  (optical_read_data.timer>=t_finish_optbus)
                    cmd_optbus = 1;
                break;
        case 5:
                break;
        case 6:
                break;
        case 7:
                break;
        case 8:
                break;
        case 9:
                break;
        default:
            break;
     }
}



if (edrk.flag_second_PCH==0)
{
    switch (cmd_optbus)
    {
        case 0 : if (optical_read_data.timer==t_write_optbus)
                    cmd_optbus = 1;
                break;

        case 1:         flag_enable_read = 0;
                        flag_finish_read = 0;
                        flag_enable_write = 1;
                        flag_finish_write = 0;
                        count_wait_write = TIME_WAIT_CMD_WRITE;
                        cmd_optbus = 2;
                break;

        case 2: if (flag_enable_write==0)
                {
                    if (count_wait_write)
                    {
                       count_wait_write--;
                    }
                    else
                        cmd_optbus = 3;
                }
                break;
        case 3:         flag_enable_read = 1;
                        flag_finish_read = 0;
                        flag_enable_write = 0;
                        flag_finish_write = 0;
                        count_wait_write = 0;
                        cmd_optbus = 4;
                break;

        case 4: if (flag_finish_read)
                {
                    //flag_enable_write = 1;
                    //count_wait_write = 2;
                    //cmd_optbus = 2;
                    flag_enable_read = 0;
                    flag_finish_read = 0;
                    flag_enable_write = 1;
                    flag_finish_write = 0;
                    count_wait_write = TIME_WAIT_CMD_WRITE;
                    cmd_optbus = 2; // минуем case 5: и case 1:
                }
                break;

        case 5: if (optical_read_data.timer>=t_finish_optbus)
                    cmd_optbus = 1;
                break;
        case 6:
                break;
        case 7:
                break;
        case 8:
                break;
        case 9:
                break;
        case 10:
                break;
        default:
            break;
     }
}
//    if (optical_read_data.timer==t_read_optbus)
//        flag_run_cycle = 1;
//
//    if (flag_run_cycle==t_read_optbus)
//    {
//        flag_enable_read = 1;
//        flag_finish_read = 0;
//        flag_enable_write = 0;
//        flag_finish_write = 0;
//        flag_run_cycle = 0;
//    }
//
//
#if(_ENABLE_PWM_LINES_FOR_TESTS)
        PWM_LINES_TK_18_OFF;
#endif

    if (flag_enable_read)
    {

#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_19_ON;
#endif

#if(USE_TK_3)
        project.cds_tk[3].read_pbus(&project.cds_tk[3]);
#endif
        optbus_status = optical_bus_get_status_and_read();

#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_19_OFF;
#endif
        cc++;

        if (optbus_status.bit.new_data_ready)
        {

            prev_error_read = 0;
            optical_read_data.count_read_optical_bus_old_data = 0;

            if (optical_read_data.data_was_update_between_pwm_int<10000)
              optical_read_data.data_was_update_between_pwm_int += 1;

            count_read_optical_bus_error = 0;
            flag_finish_read = 1;
            flag_enable_read = 0;
        }

        if (optbus_status.bit.receiver_error || optbus_status.bit.bad_status12 )
        {

            prev_error_read = 1;

            if (count_read_optical_bus_error<=max_count_read_error_optical_bus)
              count_read_optical_bus_error++;
            else
            {
                        optical_read_data.data.pzad_or_wzad = 0;
                        optical_read_data.data.angle_pwm = 0;
                        optical_read_data.data.iq_zad_i_zad = 0;
                        optical_read_data.data.cmd.all = 0;
                        flag_finish_read = 1;
            }
        }


        if (optbus_status.bit.old_data)
        {

//            prev_error_read = 1;

            flag_finish_read = 1;

            if (optical_read_data.count_read_optical_bus_old_data<=max_count_read_old_data_optical_bus)
                optical_read_data.count_read_optical_bus_old_data++;
            else
            {
                        optical_read_data.data.pzad_or_wzad = 0;
                        optical_read_data.data.angle_pwm = 0;
                        optical_read_data.data.iq_zad_i_zad = 0;
                        optical_read_data.data.cmd.all = 0;
            }
        }


#if(_ENABLE_PWM_LINES_FOR_TESTS)
        if (optbus_status.bit.new_data_ready)
        {
            PWM_LINES_TK_18_ON;
        }
#endif

#if(_ENABLE_PWM_LINES_FOR_TESTS)
        if (optbus_status.bit.receiver_busy || optbus_status.bit.receiver_error || optbus_status.bit.bad_status12)
        {
      //      PWM_LINES_TK_16_ON;
        }
#endif

    }

//
//
//    if (flag_finish_read)
//    {
//        flag_enable_write = 1;
//        count_wait_write = 10;
//    }


    if (flag_enable_write)
    {
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_20_ON;
#endif
#if(_ENABLE_PWM_LINES_FOR_TESTS)
    static unsigned int ccc = 0;
    ccc++;
    optical_write_data.data.angle_pwm = ccc;
#endif
            optical_bus_write();


#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_20_OFF;
#endif
            flag_enable_write = 0;
    }
//
//    if (count_wait_write)
//    {
//        count_wait_write--;
//    }
//    else
//        flag_finish_write = 1;
//
//
//
//// read
//
//    if  (optical_read_data.timer==(t_read_optbus-1))
//        prev_error_read = 1;
//    else
//    if  (optical_read_data.timer==t_read_optbus || prev_error_read==1)
//    {
//
//#if(_ENABLE_PWM_LINES_FOR_TESTS)
//        PWM_LINES_TK_18_OFF;
//#endif
//
//#if(_ENABLE_PWM_LINES_FOR_TESTS)
//    PWM_LINES_TK_19_ON;
//#endif
//
//        project.cds_tk[3].read_pbus(&project.cds_tk[3]);
//
//        optbus_status = optical_bus_get_status_and_read();
//
//#if(_ENABLE_PWM_LINES_FOR_TESTS)
//    PWM_LINES_TK_19_OFF;
//#endif
//        cc++;
//
//        if (optbus_status.bit.new_data_ready)
//        {
//
//            prev_error_read = 0;
//
//            if (optical_read_data.data_was_update_between_pwm_int<10000)
//              optical_read_data.data_was_update_between_pwm_int += 1;
//
//            count_read_optical_bus_error = 0;
//        }
//
//        if (optbus_status.bit.receiver_error || optbus_status.bit.bad_status12 )
//        {
//
//            prev_error_read = 1;
//
//            if (count_read_optical_bus_error<=max_count_read_error_optical_bus)
//              count_read_optical_bus_error++;
//            else
//            {
//                        optical_read_data.data.pzad_or_wzad = 0;
//                        optical_read_data.data.angle_pwm = 0;
//                        optical_read_data.data.iq_zad_i_zad = 0;
////                            optical_read_data.data.cmd.all = 0x40; // временно! alarm = 1
//                        optical_read_data.data.cmd.all = 0;
//            }
//        }
//
//#if(_ENABLE_PWM_LINES_FOR_TESTS)
//        if (optbus_status.bit.new_data_ready)
//        {
//            PWM_LINES_TK_18_ON;
//        }
//#endif
//
//#if(_ENABLE_PWM_LINES_FOR_TESTS)
//        if (optbus_status.bit.receiver_busy || optbus_status.bit.receiver_error || optbus_status.bit.bad_status12)
//        {
//      //      PWM_LINES_TK_16_ON;
//        }
//#endif
//
//    }
//
//
//
//
//// write
//
//        if  (optical_read_data.timer==t_write_optbus)
//        {
//#if(_ENABLE_PWM_LINES_FOR_TESTS)
//    PWM_LINES_TK_20_ON;
//#endif
//
//            optical_bus_write();
//
//
//#if(_ENABLE_PWM_LINES_FOR_TESTS)
//    PWM_LINES_TK_20_OFF;
//#endif
//        }



// finish

//        if  (optical_read_data.timer>=t_finish_optbus)
//        {
//            optical_read_data.timer = 0;
//        }

//         if (prev_error_read==0)
//             i_led2_off();


//        if  (optical_read_data.timer==t2)
//        {
//
// //           if (edrk.flag_second_PCH==0)
//               stage_2 = 1;
//  //          else
//   //            stage_1 = 1;
//
//            optical_read_data.timer = 0;
//        }

//        if  (optical_read_data.timer>=t3)
//        {
//            optical_read_data.timer = 0;
//        }





//
//      if  (stage_1==2 && prev_stage1!=stage_1)
//      {
// //         i_led2_on();
////          if (flag_disable_resend==0)
////          {
//  //            if (edrk.ms.another_bs_maybe_on==1 && (edrk.auto_master_slave.local.bits.master )  )
//
// //          if (edrk.flag_second_PCH==0)
//           {
//               i_led2_on();
//               i_led2_off();
//               i_led2_on();
//
//               optical_bus_write();
//           }
////           else
////           {
////               i_led2_on();
////
////           optical_bus_read();
////           optbus_status = optical_bus_get_status_and_read();
////           buf_status[cc] = optbus_status;
////           cc++;
////
////           if (optbus_status.bit.new_data_ready)
////               optical_read_data.data_was_update_between_pwm_int = 1;
////
////                     if (optbus_status.bit.receiver_busy || optbus_status.bit.receiver_error || optbus_status.bit.bad_status12
////                                                      )
////                     {
////                         i_led1_on();
////                                        i_led1_off();
////
////                     }
////
////           }
//           stage_1 = 0;
//      }

//      if  (stage_1==1 && prev_stage1!=stage_1)
//      {
//
////          if (edrk.flag_second_PCH==1)
////          {
////              i_led2_on();
////              i_led2_off();
////              i_led2_on();
////
////              optical_bus_write();
////          }
////          else
// //         {
//              i_led2_on();
//
//          optical_bus_read();
//          optbus_status = optical_bus_get_status_and_read();
//          buf_status[cc] = optbus_status;
//          cc++;
//
//          if (optbus_status.bit.new_data_ready)
//              optical_read_data.data_was_update_between_pwm_int = 1;
//
//          if (optbus_status.bit.receiver_busy || optbus_status.bit.receiver_error || optbus_status.bit.bad_status12
//                                           )
//          {
//              i_led1_on();
//                             i_led1_off();
//
//          }
//
//      //    }
//
//
//  //        stage_1 = 0;
//      }

  //    prev_stage1 = stage_1;
 //   }
//        if (edrk.flag_second_PCH==1)
//        {
//          i_led2_off();
//        }

//i_led2_on_off(0);

//#if (ENABLE_LOG_INTERRUPTS)
//    add_log_interrupts(102);
//#endif

#if(_ENABLE_PWM_LINES_FOR_TESTS)
    PWM_LINES_TK_17_OFF;
#endif


}
///////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////


