/*
 * oscil_can.c
 *
 *  Created on: 24 ìàÿ 2020 ã.
 *      Author: yura
 */

#include "oscil_can.h"

#include "CAN_Setup.h"
#include "global_time.h"



#pragma DATA_SECTION(oscil_can, ".slow_vars")
OSCIL_CAN oscil_can = OSCIL_CAN_DEFAULTS;



//int oscil_buffer[OSCIL_CAN_NUMBER_CHANNEL][OSCIL_CAN_NUMBER_POINTS];



void oscil_clear_buffer(OSCIL_CAN_handle oc)
{
   unsigned int i,k;

   for (i=0;i<OSCIL_CAN_NUMBER_CHANNEL;i++)
       for (k=0;k<(OSCIL_CAN_NUMBER_POINTS+OSCIL_CAN_NUMBER_POINTS_ADD);k++)
           oc->oscil_buffer[i][k] = 0;

   for (i=0;i<OSCIL_CAN_NUMBER_CHANNEL;i++)
       for (k=0;k<OSCIL_CAN_NUMBER_POINTS;k++)
           oc->temp_oscil_buffer[i][k] = 0;


   oc->current_position = 0;
 //  oc->enable_rewrite = 1;

}


void oscil_send_buffer(OSCIL_CAN_handle oc)
{
   static unsigned int old_time = 0;
   static int prev_send_to_can = 0;
   unsigned long old_t;
   unsigned int i;
   int real_mbox;
   static int flag_send_buf = 0;


//   if (flag_send_buf)
//      {
//
//
//
//        return;
//      }



   oc->global_enable = TerminalUnites[oc->number_can_box_terminal_oscil][0] & 0x1;
   oc->send_after_cmd =  (TerminalUnites[oc->number_can_box_terminal_oscil][0] & 0x2) >> 1;
   oc->cmd_send =  (TerminalUnites[oc->number_can_box_terminal_oscil][0] & 0x4) >> 2;
   oc->stop_update_on_error =  (TerminalUnites[oc->number_can_box_terminal_oscil][0] & 0x8) >> 3;
   oc->stop_update_on_stop_pwm =  (TerminalUnites[oc->number_can_box_terminal_oscil][0] & 0x10) >> 4;

   TerminalUnites[oc->number_can_box_terminal_oscil][0] &= ~0x4; // clear cmd_send

   oc->number_ch = TerminalUnites[oc->number_can_box_terminal_oscil][1];
   oc->number_points = TerminalUnites[oc->number_can_box_terminal_oscil][2];
   oc->step = TerminalUnites[oc->number_can_box_terminal_oscil][3];


   if (oc->global_enable==0)
       return;

   real_mbox = get_real_out_mbox(TERMINAL_TYPE_BOX, oc->number_can_box_terminal_oscil);

   // áûëà êîìàíäà íà îòïðàâêó ïîñûëêè è îíà åùå íå óøëà, òîãäà ñðáàñûâàåì ñ÷åò÷èê âðåìåíè ïàóçû ìåæäó ïîñûëêàìè,
   // ò.å. OSCIL_TIME_WAIT ìåæäó êîíöîì îòïðàâêè ïîñûëêè è íîâîé ïîñûëêè.
   if (prev_send_to_can && CAN_cycle_full_free(real_mbox,CAN_BOX_STAT_OFF)==0)
   {
       old_time = (unsigned int)global_time.miliseconds;
       return;
   }
   prev_send_to_can = 0;

   if (oc->send_after_cmd==0)
   {
    if (!detect_pause_milisec(OSCIL_TIME_WAIT,&old_time))
       return;
   }


   if ( (oc->send_after_cmd==0 || (oc->send_after_cmd==1 &&  oc->cmd_send==1 )   )    )
   {

       oc->cmd_send = 0; // clear cmd

       if (CAN_cycle_full_free(real_mbox,CAN_BOX_STAT_ON))
       {

 //          oc->enable_rewrite = 0;


           old_t = oc->current_position;//
//           old_t = global_time.microseconds;

           oc->prepare_data_can(oc);

//           oc->timer_send = (global_time.microseconds - old_t);
           oc->timer_send = (oc->current_position - old_t);

           flag_send_buf = 1;

           CAN_cycle_send(
                   TERMINAL_TYPE_BOX,
                   oc->number_can_box_terminal_oscil,
                               0,
                               &(oc->temp_oscil_buffer[0][0]),  (oc->number_ch * oc->number_points) , CAN_BOX_EXTENDED_ADR, CAN_BOX_PRIORITY_LOW );

           prev_send_to_can = 1;
 //          while (CAN_cycle_free(real_mbox)==0);

//           oc->timer_send = (global_time.microseconds - old_t)/100;


           oc->enable_rewrite = 1;


//           if (cur_position_buf_modbus16_can >= SIZE_MODBUS_TABLE)
//           {
//               cur_position_buf_modbus16_can = 0;
////               modbus_table_can_out[ADR_CAN_TEST_PLUS_ONE].all++;
//           }
//
//           if ((cur_position_buf_modbus16_can + SIZE_BUF_WRITE_TO_MODBUS16_CAN) >= SIZE_MODBUS_TABLE)
//               count_write_to_modbus_can = SIZE_MODBUS_TABLE - cur_position_buf_modbus16_can;
//           else
//               count_write_to_modbus_can = SIZE_BUF_WRITE_TO_MODBUS16_CAN;
//
//           if (CAN_cycle_free(real_mbox))
//           {
//               CAN_cycle_send(
//                   MPU_TYPE_BOX,
//                   edrk.flag_second_PCH,
//                   cur_position_buf_modbus16_can + 1,
//                   &modbus_table_can_out[cur_position_buf_modbus16_can].all,
//                   count_write_to_modbus_can, 0);
//
//               cur_position_buf_modbus16_can = cur_position_buf_modbus16_can + SIZE_BUF_WRITE_TO_MODBUS16_CAN;
//           }
//
//





       }
   }


}


//#pragma CODE_SECTION(oscil_next_position,".fast_run");
void oscil_next_position(OSCIL_CAN_handle oc)
{
   static int prev_status_pwm   = 0;
   static int prev_status_error = 0;
   static int cmd_stop_after_stop_pwm = 0;
   static int cmd_stop_after_stop_error = 0;

   static int count_write_after_stop = 0;

   static int prev_stop_update_on_stop_pwm = 0;
   static int prev_stop_update_on_stop_error = 0;

//////////
   if (oc->stop_update_on_error)
   {
       if (oc->status_error && prev_status_error==0)
       {
           // ïîñëå âûñòàâëåíèÿ îøèáêè ìû òîðìîçèì ëîã
           count_write_after_stop = OSCIL_CAN_NUMBER_POINTS_AFTER_STOP;
           cmd_stop_after_stop_error = 1;
       }

       if (oc->status_error==0 && prev_status_error)
       {
           // ïîñëå ñáðîñà îøèáîê ìû çàïóñêàåì ëîã.
           cmd_stop_after_stop_error = 0;
       }

   }
   else
       cmd_stop_after_stop_error = 0;

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

   if (oc->stop_update_on_stop_pwm==1 && prev_stop_update_on_stop_pwm==0)
   {
       // ïîñëå âêëþ÷åíèÿ íàñòðîéêè onPWM ìû òîðìîçèì ëîã åñëè îí çàïèñûâàëñÿ.
       cmd_stop_after_stop_pwm = 1;
   }

   if (oc->stop_update_on_stop_pwm)
   {
       if (oc->status_pwm==0 && prev_status_pwm)
       {
           // ïîñëå âûêëþ÷åíèÿ PWM ìû òîðìîçèì ëîã åñëè îí çàïèñûâàëñÿ.
           count_write_after_stop = OSCIL_CAN_NUMBER_POINTS_AFTER_STOP;
           cmd_stop_after_stop_pwm = 1;
       }

       if (oc->status_pwm && prev_status_pwm==0)
       {
           // ïîñëå âêëþ÷åíèÿ PWM ìû çàïóñêàåì ëîã.
           cmd_stop_after_stop_pwm = 0;
       }
   }
   else
       cmd_stop_after_stop_pwm = 0;

   prev_stop_update_on_stop_pwm = oc->stop_update_on_stop_pwm;
   prev_stop_update_on_stop_error = oc->stop_update_on_error;
   prev_status_error = oc->status_error;
   prev_status_pwm = oc->status_pwm;
////////////



   oc->current_step++;
   if (oc->current_step>=oc->step)
   {
       oc->current_step = 0;

       if (cmd_stop_after_stop_error || cmd_stop_after_stop_pwm)
       {
         if (count_write_after_stop)
         {
            count_write_after_stop--;
            oc->current_position++;
            oc->code_status_log = OSCIL_CODE_STATUS_LOG_RUN_TO_STOP;
         }
         else
           oc->code_status_log = OSCIL_CODE_STATUS_LOG_STOP;
       }
       else
       {
           oc->current_position++;
           oc->code_status_log = OSCIL_CODE_STATUS_LOG_RUN;
       }

       if (oc->current_position==(OSCIL_CAN_NUMBER_POINTS+OSCIL_CAN_NUMBER_POINTS_ADD))
           oc->current_position = 0;
   }


}


#pragma CODE_SECTION(oscil_prepare_data_can,".fast_run");
void oscil_prepare_data_can(OSCIL_CAN_handle oc)
{
    unsigned int old_position, t_position;
    int i, k;


    old_position = oc->current_position;

    for (i=0;i<OSCIL_CAN_NUMBER_CHANNEL;i++)
    {
        t_position = old_position;
        for (k=OSCIL_CAN_NUMBER_POINTS-1;k>=0;k--)
        {
            if (t_position==0)
            {
                t_position = (OSCIL_CAN_NUMBER_POINTS+OSCIL_CAN_NUMBER_POINTS_ADD)-1;
            }
            else
                t_position = t_position - 1;

            oc->temp_oscil_buffer[i][k] = oc->oscil_buffer[i][t_position];

        }

    }

}