#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
 
 ************************************************************************************/

void SendCANPackets(unsigned int *CellVoltage, unsigned int CurrMeas, unsigned long CellLoad)
{
    int DataID;
    unsigned 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) CurrMeas;
        arg2 = (unsigned int) CellVoltage[0];
        arg3 = (unsigned int) CellVoltage[1];
        arg4 = (unsigned int) CellVoltage[2];

        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 = (unsigned int) CellVoltage[3];
        arg2 = (unsigned int) CellVoltage[4];
        arg3 = (unsigned int) CellVoltage[5];
        arg4 = (unsigned int) CellVoltage[6];

        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) CellVoltage[7];
        arg2 = (unsigned int) CellVoltage[8];
        arg3 = (unsigned int) CellVoltage[9];
        arg4 = (unsigned int) CellVoltage[10];

        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 = (unsigned int) CellVoltage[11];
        arg2 = (unsigned int) CellVoltage[12];
        arg3 = (unsigned int) CellVoltage[13];
        arg4 = (unsigned int) CellVoltage[14];

        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
    }

    DataID = 4;
    if (!C1TR45CONbits.TXREQ4) //If previous request for this buffer isn't still waiting
    {
        arg1 = (unsigned int) CellVoltage[15];
        arg2 = (unsigned int) CellVoltage[16];
        arg3 = (unsigned int) CellVoltage[17];
        arg4 = 0;

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

        DataID = 5;
    if (!C1TR45CONbits.TXREQ5) //If previous request for this buffer isn't still waiting
    {
        arg1 = (unsigned int) ((CellLoad >> 16) && 0xffff);
        arg2 = (unsigned int) (CellLoad && 0xffff);
        arg3 = 0;
        arg4 = 0;

        AssembleMessageData_FourInts(&message[DataID], arg1, arg2, arg3, arg4);
        message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num);
        AssembleCAN_Packet(DataID, &message[DataID]);
        C1TR45CONbits.TXREQ5 = 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

    }