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

// BATTERY CONTROLLER DATA PACKET DEFINITIONS. SRC_ID = 0
/************************************************************************************
 * Data_ID = 0
 * Byte 0-1 <-- "BC Voltage"        (unsigned int)      1 cnt = 0.01 Volts      xxx.x
 * Byte 2-3 <-- "BC Ips"            (unsigned int)      1 cnt = 0.001 Amps      x.xx
 * Byte 4-5 <-- "BC V Balance"      (unsigned int)      1 cnt = 0.001 Volts     x.xx
 * Byte 6-7 <-- "BC V Stopcharge"   (unsigned int)      1 cnt = 0.001 Volts     x.xx
 *
 * Data_ID = 1
 * Byte 0-1 <-- "BC Ground Fault"   (unsigned int)      1 cnt = 0.001 mAmps     xx.xx
 * Byte 2-3 <-- "BC Hydrogen"       (unsigned int)      1 cnt = 0.001 Volts     x.xx
 * Byte 4-5 <-- "BC Status"         (unsigned int)      BitField                xxxxx
 * Byte 6-7 <--
 *
 ************************************************************************************/

void SendCANPackets(int VBatt, int Ips, int V_Balance, int V_StopCharge, int Rgroundfault, int H2Sensor)
{
    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 = (unsigned int) 100*ConvertFractToEngUnits(VBatt, VBatt_cnt, VBatt_eng);
        arg2 = (unsigned int) ConvertFractToEngUnits(Ips, Ips_cnt, Ips_eng);
        arg3 = (unsigned int) 1000*ConvertFractToEngUnits(V_Balance, V_Balance_cnt, V_Balance_eng);
        arg4 = (unsigned int) 1000*ConvertFractToEngUnits(V_StopCharge, V_StopCharge_cnt, V_StopCharge_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.TXREQ0 = 1; //Initiates a request
    }

    DataID = 1;
    if (!C1TR01CONbits.TXREQ1) //If previous request for this buffer isn't still waiting
    {
        arg1 = (signed int) 1000*ConvertFractToEngUnits(Rgroundfault, Rgroundfault_cnt, Rgroundfault_eng);
        arg2 = (signed int) 1000*ConvertFractToEngUnits(H2Sensor, H2Sensor_cnt, H2Sensor_eng);
        arg3 = Status.status;
        arg4 = (unsigned int) 0;

        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
    }

   

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

    }