#include "xc.h"
#include "config.h"
#include "extern.h"
#include "CalibrationConstants.h"
#include "conv.h"

// POWER CONVERTER DATA PACKET DEFINITIONS. SRC_ID = 2
/*******************************************************************************
 * Data_ID = 0
 * Byte 0-1 <-- "PC RPM"               (signed int)     1 cnt = 0.5RPM          xxxx.x
 * Byte 2-3 <-- "PC Bus Voltage (V)"   (unsigned int)   1 cnt = 0.01 Volts      xxx.x
 * Byte 4-5 <-- "PC Winding Current"   (signed int)     1 cnt = 0.002 Amps      xx.xx
 * Byte 6-7 <-- "PC Battery Current"   (signed int)     1 cnt = 0.002 Amps      xx.xx
 *
 * Data_ID = 1
 * Byte 0-1 <-- "PC Status"            (unsigned int)  BitField                 xxxxx
 * Byte 2-3 <-- "PC Load Dump Current" (signed int)    1 cnt = 0.002 Amps       xx.xx
 * Byte 4-5 <-- "Target Voltage"       (unsigned int)  1 cnt = 0.01 Volts       xxx.x
 * Byte 6-7 <-- "Target Current"       (signed int)    1 cnt = 0.002 Amps       xx.xx
 *  
 * Data_ID = 2
 * Byte 0-1 <-- "Diff Press Sensor"    (signed int)    1 cnt = 0.001psi         x.xxx
 * Byte 2-3 <-- "RPM Std Dev"          (unsigned int)  1 cnt = 0.5RPM           xxx.x
 * Byte 4-5 <-- "PC Scale"             (unsigned int)  1 cnt = 0.01 Scale Units x.xx
 * Byte 6-7 <-- "PC Retract"           (unsigned int)  1 cnt = 0.01 Retract Units x.xx
 *
 * Data_ID = 3
 * Byte 0-1 <-- "Aux Torque Sensor"    (signed int)    1 cnt = 1mV              xxxxx
 * Byte 2-3 <-- "Bias Current"         (signed int)    1cnt = 0.002 Amps        xx.xx
 * Byte 4-5 <-- "Charge Current Limit" (signed int)    1cnt = 0.002 Amps        xx.xx
 * Byte 6-7 <-- "Draw Current Limit"   (signed int)    1cnt = 0.002 Amps        xx.xx
 *
 */



void SendCANPackets()
{
    int DataID;
    int arg1, arg2, arg3, arg4;
    static int seq_num = 0;

    //Send data over CAN
    DataID = 0;
    if (!C1TR01CONbits.TXREQ0) //If previous request for buffer zero isn't still waiting
    {
        arg1 = (signed int) CRSF_RPM * ConvertFractToEngUnits(R_RPM, RPMs_cnt, RPMs_eng); //Speed
        arg2 = (unsigned int) CRSF_VOLTAGE * ConvertFractToEngUnits(R_VBus, Cal.VBus_cnt, Cal.VBus_eng); //Voltage
        arg3 = (signed int) CRSF_CURRENT * ConvertFractToEngUnits(R_QuadratureCurrent, Cal.IWind_cnt, Cal.IWind_eng);
        arg4 = (signed int) CRSF_CURRENT * ConvertFractToEngUnits(R_IBatt, Cal.IBatt_cnt, Cal.IBatt_eng);

        AssembleMessageData_FourInts(&message[DataID], arg1, arg2, arg3, arg4);
        message[0].id = Assemble_packetID(CAN_SRCID, DataID, seq_num);
        AssembleCAN_Packet(DataID, &message[DataID]);
        C1TR01CONbits.TXREQ0 = 1; //Initiates a request
    }
    
    DataID = 1;
    if (!C1TR01CONbits.TXREQ1) //If previous request for this buffer isn't still waiting
    {
        arg1 = (unsigned int) Status;
        arg2 = (signed int) CRSF_CURRENT*ConvertFractToEngUnits(R_LoadDumpCurrent, LD_Current_cnt, LD_Current_eng); //Load Dump Current
        arg3 = (unsigned int) CRSF_VOLTAGE*ConvertFractToEngUnits(ShuntReg_PID.controlReference, Cal.VBus_cnt, Cal.VBus_eng); //TargetVoltage
        arg4 = (signed int) CRSF_CURRENT*ConvertFractToEngUnits(Iq_PID.controlReference, Cal.IWind_cnt, Cal.IWind_eng);

        AssembleMessageData_FourInts(&message[DataID], arg1, arg2, arg3, arg4);
        message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num);
        AssembleCAN_Packet(DataID, &message[DataID]);
        C1TR01CONbits.TXREQ1 = 1; //Initiates a request
    }

    DataID = 2;
    if (!C1TR23CONbits.TXREQ2) //If previous request for this buffer isn't still waiting
    {
        arg1 = (unsigned int) CRSF_PRESSURE*ConvertFractToEngUnits(R_DiffPress, Cal.DiffPress_cnt, Cal.DiffPress_eng); //Differential Pressure Sensor  
        arg2 = (signed int) CRSF_RPM*ConvertFractToEngUnits(RPM_StdDev, RPMs_cnt, RPMs_eng); 
        arg3 = (unsigned int) scale_factor.Value;  //Internal scaling and reporting scaling are the same.
        arg4 = (unsigned int) retract_factor.Value;  //Internal scaling and reporting scaling are the same.

        AssembleMessageData_FourInts(&message[DataID], arg1, arg2, arg3, arg4);
        message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num);
        AssembleCAN_Packet(DataID, &message[DataID]);
        C1TR23CONbits.TXREQ2 = 1; //Initiates a request
    }

    DataID = 3;
    if (!C1TR23CONbits.TXREQ3) //If previous request for this buffer isn't still waiting
        {
            arg1 = (signed int) CRSF_VOLTAGE*ConvertFractToEngUnits(R_AuxTorque, Cal.AuxTorque_cnt, Cal.AuxTorque_eng);
            arg2 = (signed int) CRSF_CURRENT*ConvertFractToEngUnits(BiasCurrent.Value, Cal.IWind_cnt, Cal.IWind_eng); //Bias Current Setting
            arg3 = (signed int) CRSF_CURRENT * ConvertFractToEngUnits(MaxBattChargeCurrent.Value, Cal.IBatt_cnt, Cal.IBatt_eng);
            arg4 = (signed int) CRSF_CURRENT * ConvertFractToEngUnits(MaxBattDrawCurrent.Value, Cal.IBatt_cnt, Cal.IBatt_eng);
    
            AssembleMessageData_FourInts(&message[DataID],arg1,arg2,arg3,arg4);
            message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num);
            AssembleCAN_Packet(DataID, &message[DataID]);
            C1TR23CONbits.TXREQ3 = 1; //Initiates a request
       }


    // seq_num++; //Increment sequence number for next time. ##### Disabled for lab Testing######
    if (seq_num >= (1 << SEQ_WIDTH))
        seq_num = 0; //Rollover sequence number when it exceeds specified width

    }