/*
 * optical_bus.h.c
 *
 *  Created on: 18 àâã. 2020 ã.
 *      Author: stud
 */

#include <adc_tools.h>
#include <edrk_main.h>
#include <optical_bus.h>
#include <params.h>
#include <sync_tools.h>
#include <vector.h>

#include "IQmathLib.h"
#include "xp_project.h"


OPTICAL_BUS_DATA optical_write_data = OPTICAL_BUS_DATA_DEFAULT;
OPTICAL_BUS_DATA optical_read_data = OPTICAL_BUS_DATA_DEFAULT;

void parse_task_from_optical_bus(void);


#pragma CODE_SECTION(optical_bus_update_data_write,".fast_run");
void optical_bus_update_data_write(void)
{

    optical_write_data.data.cmd.bit.alarm    = edrk.summ_errors;

    optical_write_data.data.cmd.bit.master = edrk.auto_master_slave.local.bits.master;
    optical_write_data.data.cmd.bit.slave = edrk.auto_master_slave.local.bits.slave;
    optical_write_data.data.cmd.bit.maybe_master = edrk.auto_master_slave.local.bits.try_master;


//    optical_write_data.cmd.bit.controlMode = ;

//    optical_write_data.cmd.bit.err_optbus =
//    optical_write_data.cmd.bit.master =
//    optical_write_data.cmd.bit.maybe_master =
//    optical_write_data.cmd.bit.pwm_status =
//

    if (edrk.Status_Rascepitel_Ok==0)
    {
      if (edrk.RunZahvatRascepitel)
        optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_REQUEST_AND_THIS_OFF;// 10 - çàïðîñ íà âêëþ÷åíèå, ñâîé âûêëþ÷åí
      else
          optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_OFF;//00 - ìîæíî âêëþ÷àòü ðàñöåïèòåëü, ñâîé âûêëþ÷åí
    }
    else
    {

      if (edrk.Status_Ready.bits.ready_final==0) // ñõåìà ðàçîáðàíà
      {
          if (edrk.RunUnZahvatRascepitel)
              optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_REQUEST_AND_THIS_OFF;// 10 - çàïðîñ íà âûêëþ÷åíèå, ñâîé âêëþ÷åí
          else
            optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON;// 01 - ìîæíî âêëþ÷àòü ðàñöåïèòåëü, ñâîé âêëþ÷åí
      }
      else
      {
          if (edrk.you_can_on_rascepitel==0)
              optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_DISABLE_THIS_ON; // 11 - çàïðåò ïîäêëþ÷åíèÿ ðàñöåïèòåëÿ, ñâîé âêëþ÷åí
          else
              optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON; // 01 - ìîæíî âêëþ÷àòü ðàñöåïèòåëü, ñâîé âêëþ÷åí
      }
    }

    if (edrk.Status_Ready.bits.ready7 || (edrk.Status_Ready.bits.ready5 && edrk.Status_Ready.bits.ImitationReady2) )
      optical_write_data.data.cmd.bit.ready_cmd = CODE_READY_CMD_READY2;
    else
        if (edrk.SumSbor)
            optical_write_data.data.cmd.bit.ready_cmd = CODE_READY_CMD_READY1TO2;
        else
            optical_write_data.data.cmd.bit.ready_cmd = CODE_READY_CMD_READY1;

//    optical_write_data.cmd.bit.ready_to_go =
//    optical_write_data.cmd.bit.slave =
//
//    optical_write_data.cmd.bit.start_pwm =
    optical_write_data.data.cmd.bit.statusQTV = edrk.from_shema_filter.bits.QTV_ON_OFF;
//    optical_write_data.cmd.bit.stop_pwm =

    optical_write_data.data.cmd.bit.sync_1_2 = sync_data.local_flag_sync_1_2;
    optical_write_data.data.cmd.bit.sync_line_detect = !sync_data.timeout_sync_signal;

//  optical_write_data.data1 = f.Mode == 1 ?  f.fzad * 60.0 + 0.5 :
//                              f.Mode == 2 ? f.p_zad / 1000.0 :
//                              0;
//  optical_write_data.data2 = rotor.direct_rotor == -1 ? -_IQtoIQ15(rotor.iqFout) :
//                                                          _IQtoIQ15(rotor.iqFout);
//  optical_write_data.data3 = _IQtoIQ15(analog.iqIq_zadan);
//  optical_write_data.data4.bit.controlMode = f.Mode == 2 ? 1 : 0;
//  optical_write_data.data4.bit.leading_ready = f.Ready2 && (f.Mode == 1 || f.Mode == 2) ? 1 : 0;
//  optical_write_data.data4.bit.leading_Go = edrk.Go;



}



#pragma CODE_SECTION(optical_bus_write,".fast_run");
void optical_bus_write(void)
{
#if(USE_TK_3)
    // check error write PBUS OPTICAL BUS
    project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]);

    // write data to OPTICAL BUS
    project.cds_tk[3].optical_data_out.buf[0] = optical_write_data.data.pzad_or_wzad;
    project.cds_tk[3].optical_data_out.buf[1] = optical_write_data.data.angle_pwm;
    project.cds_tk[3].optical_data_out.buf[2] = optical_write_data.data.iq_zad_i_zad;
    project.cds_tk[3].optical_data_out.buf[3] = optical_write_data.data.cmd.all;

    optical_write_data.status = 1;


    project.cds_tk[3].optical_bus_write_data(&project.cds_tk[3]);
#endif
}





#pragma CODE_SECTION(optical_bus_read_old,".fast_run");
void optical_bus_read_old(void)
{

//	// check error write PBUS OPTICAL BUS
//    project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]);
//// read data from OPTICAL BUS
//    project.cds_tk[3].read_pbus(&project.cds_tk[3]);
//    // check error read PBUS OPTICAL BUS
//    project.cds_tk[3].optical_bus_check_error_read(&project.cds_tk[3]);
//
//
//    if (project.cds_tk[3].optical_data_in.new_data_ready)
//    {
//        if (project.cds_tk[3].optical_data_in.overfull_new_data)
//            optical_read_data.overfull_data++;
//
//        optical_read_data.raw.pzad_or_wzad = (int)project.cds_tk[3].optical_data_in.buf[0];
//        optical_read_data.raw.angle_pwm = (int)project.cds_tk[3].optical_data_in.buf[1];
//        optical_read_data.raw.iq_zad = (int)project.cds_tk[3].optical_data_in.buf[2];
//        optical_read_data.raw.cmd.all = project.cds_tk[3].optical_data_in.buf[3];
//
//
//        optical_read_data.data.pzad_or_wzad = optical_read_data.raw.pzad_or_wzad;
//        optical_read_data.data.angle_pwm = optical_read_data.raw.angle_pwm ;
//        optical_read_data.data.iq_zad = optical_read_data.raw.iq_zad;
//        optical_read_data.data.cmd.all = optical_read_data.raw.cmd.all;
//
//        optical_read_data.code_status.bit.ready = 1;
//        optical_read_data.code_status.bit.recive_error = 0;
//        optical_read_data.code_status.bit.overfull = 0;
//        optical_read_data.code_status.bit.timeout = 0;
//        optical_read_data.code_status.bit.wait = 0;
//
//        project.cds_tk[3].optical_data_in.new_data_ready = 0;
//
//        return (optical_read_data.code_status);
//
//    }
//    else
//    {
//
//        if (project.cds_tk[3].optical_data_in.ready == 1)
//        {
//            optical_read_data.code_status.bit.ready = 0;
//
//            if (project.cds_tk[3].optical_data_in.raw_local_error)
//              optical_read_data.code_status.bit.recive_error = 1;
//            else
//                optical_read_data.code_status.bit.recive_error = 0;
//
//            optical_read_data.code_status.bit.overfull = 0;
//            optical_read_data.code_status.bit.timeout = 0;
//            optical_read_data.code_status.bit.wait = 1;
//
//            return (optical_read_data.code_status);
//        }
//
//    }
//
//
//    if (project.cds_tk[3].optical_data_out.ready == 0) {
//
//        project.cds_tk[3].optical_data_out.error_not_ready_count += 1;
//
////        optical_read_data.code_status.bit.ready = 0;
//        optical_read_data.code_status.bit.send_error = 1;
////        optical_read_data.code_status.bit.overfull = 0;
////        optical_read_data.code_status.bit.timeout = 0;
////        optical_read_data.code_status.bit.wait = 0;
//
////        optical_write_data.status = 2;
////        return (optical_read_data.code_status);
//    }
//    else
//        optical_read_data.code_status.bit.send_error = 0;
//
//
//    if (project.cds_tk[3].optical_data_in.ready == 0) {
//
////        f.read_task_from_optical_bus = 0;
//        project.cds_tk[3].optical_data_in.error_not_ready_count += 1;
//
//        optical_read_data.code_status.bit.ready = 0;
// //       optical_read_data.code_status.bit.send_error = 0;
//        //optical_read_data.code_status.bit.recive_error = 1;
//        optical_read_data.code_status.bit.overfull = 0;
//        optical_read_data.code_status.bit.timeout = 1;
//        optical_read_data.code_status.bit.wait = 0;
//
//        optical_read_data.data.pzad_or_wzad = 0;
//        optical_read_data.data.angle_pwm = 0;
//        optical_read_data.data.iq_zad = 0;
//        optical_read_data.data.cmd.all = 0;
//
//  //      optical_read_data.status = 2;
//
//  //      return (project.cds_tk[3].optical_data_in.raw_local_error);
//    }
//    else
//    {
//
//
//
//    }
//
//    return (optical_read_data.code_status);
//
//
////    optical_read_data.status = 1;
//
// //   return (project.cds_tk[3].optical_data_in.raw_local_error);
//
////    if (f.flag_leading == 0 && f.flag_distance && f.Ready1){
////		parse_task_from_optical_bus();
////    } else {
////    	f.read_task_from_optical_bus = 0;
////    }
////    rotor.iqFrotFromOptica = _IQ15toIQ(optical_read_data.data2); //Frot
////    analog.iqIq_zad_from_optica = _IQ15toIQ(optical_read_data.data3);

}


#pragma CODE_SECTION(optical_bus_read,".fast_run");
void optical_bus_read(void)
{

    // check error write PBUS OPTICAL BUS
//    project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]);
// read data from OPTICAL BUS
#if(USE_TK_3)
    project.cds_tk[3].read_pbus(&project.cds_tk[3]);
#endif

    return;
}

#pragma CODE_SECTION(optical_bus_read_clear_count_error,".fast_run");
void optical_bus_read_clear_count_error(void)
{
#if(USE_TK_3)
    project.cds_tk[3].optical_data_in.local_count_error = 0;
#endif

    return;
}



#pragma CODE_SECTION(optical_bus_get_status_and_read,".fast_run");
STATUS_DATA_READ_OPT_BUS optical_bus_get_status_and_read(void)
{
    STATUS_DATA_READ_OPT_BUS status_read;
    // check error write PBUS OPTICAL BUS
 //   project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]);
// read data from OPTICAL BUS
//    project.cds_tk[3].read_pbus(&project.cds_tk[3]);

#if(USE_TK_3)
    // check error read PBUS OPTICAL BUS
    project.cds_tk[3].optical_bus_check_error_read(&project.cds_tk[3]);

    status_read.all = project.cds_tk[3].optical_data_in.status_read.all;

    project.cds_tk[3].optical_data_in.prev_status_read.all = status_read.all;
    project.cds_tk[3].optical_data_in.status_read.all = 0;

    optical_read_data.status = project.cds_tk[3].optical_data_in.ready;


    if (status_read.bit.new_data_ready)
    {
        if (status_read.bit.overfull_new_data)
            optical_read_data.overfull_data++;

        optical_read_data.raw.pzad_or_wzad = (int)project.cds_tk[3].optical_data_in.buf[0];
        optical_read_data.raw.angle_pwm = (int)project.cds_tk[3].optical_data_in.buf[1];
        optical_read_data.raw.iq_zad_i_zad = (int)project.cds_tk[3].optical_data_in.buf[2];
        optical_read_data.raw.cmd.all = project.cds_tk[3].optical_data_in.buf[3];


        optical_read_data.data.pzad_or_wzad = optical_read_data.raw.pzad_or_wzad;
        optical_read_data.data.angle_pwm = optical_read_data.raw.angle_pwm ;
        optical_read_data.data.iq_zad_i_zad = optical_read_data.raw.iq_zad_i_zad;
        optical_read_data.data.cmd.all = optical_read_data.raw.cmd.all;


        return (status_read); // ìû ïîëó÷èëè äàííûå

    }
    else
    {
        // íåò äàííûõ?
       // îáåóëÿåì â ïðåðûâàíèè optical_bus, à íå òóò!

//        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;



    }
#else
    status_read.all = 0;
#endif
    return status_read; //íåò äàííûõ èëè îøèáêè

}




void parse_task_from_optical_bus(void) {

 //   if (optical_read_data.data4.bit.leading_ready == 1) {
//        f.read_task_from_optical_bus = 1;
//        f.Mode = optical_read_data.data4.bit.controlMode == 0 ? 1 : 2;
//        if (f.Mode == 1) {
//            f.fzad = (float)optical_read_data.data1 / 60.0;
//            f.iq_fzad = _IQ(f.fzad / NORMA_FROTOR);
//            limit_mzz_zad_turns();
//        }
//        if (f.Mode == 2) {
//            f.p_zad = (float)optical_read_data.data1 * 1000.0;
//            f.iq_p_zad = _IQ(f.p_zad/NORMA_ACP/NORMA_ACP);
//            //
//            if (f.iq_p_zad > f.iq_p_limit_zad) { f.iq_p_zad = f.iq_p_limit_zad;}
//            if (f.iq_p_zad < -f.iq_p_limit_zad) { f.iq_p_zad = -f.iq_p_limit_zad;}
//
//            limit_mzz_zad_power();
//        }

//        if (f.Ready1 && f.Ready2) {
//            if (((f.Mode == 1 && f.fzad != 0) || (f.Mode == 2 && f.p_zad != 0))
//            		&& (!f.Is_Blocked || (f.Is_Blocked && edrk.Go))
//					&& (f.rotor_stopped == 0)
////					&& (faults.faults5.bit.rotor_stopped == 0)
//					//&& (optical_read_data.data4.bit.leading_Go == 1)
//					){
//                edrk.Go = 1;
//            } else {
//                if (edrk.Go) {
//                    f.p_zad = 0;
//                    f.fzad = 0;
//                    f.iq_fzad = 0;
//                    f.iq_p_zad = 0;
//                    if(f.Mode == 1)
//                    {
//                        if(a.iqk < 1677722 || rotor.iqFout < 48210  //1677722 ~ 0.1 êîýôôèöèåíò 503316 = 3% 838860 = 5%
//                            || (analog.iqIm_1 < 1677721 && analog.iqIm_2 < 1677721))    //1677721 ~ 300A
//                        {
//                            edrk.Go = 0;
//                            f.iq_fzad = 0;
//                        }
//
//                    }
//                    if(edrk.Go && f.Mode == 2)
//                    {
//                        if(analog.iqW < 186413)
//                        {
//                            if(a.iqk < 1677722 ||		  //1677722 ~ 0.1 êîýôôèöèåíò
//                            	(analog.iqIm_1 < 1677721 && analog.iqIm_2 < 1677721))	//1677721 ~ 300A
//                            {
//                                edrk.Go = 0;
//                            }
//                        }
//                    }
//
//                } else {
//                    f.iq_mzz_zad = _IQ(500.0/NORMA_MZZ);
//                }
//            }
//        } else {
//            edrk.Go = 0;
//        }


    //        _IQ15toIQ(optical_read_data.data3); //Iq_zad
//    } else {
 //       f.read_task_from_optical_bus = 0;
//    }

}