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

// RUDDER CONTROLLER DATA PACKET DEFINITIONS. SRC_ID = 3
/************************************************************************************
 * Data_ID = 0
 * Byte 0-1 <--
 * Byte 2-3 <--
 * Byte 4-5 <--
 * Byte 6-7 <--
 *
 * Data_ID = 1
 * Byte 0-1 <--
 * Byte 2-3 <--
 * Byte 4-5 <--
 * Byte 6-7 <--
 *
 * Data_ID = 2
 * Byte 0-1 <--
 * Byte 2-3 <--
 * Byte 4-5 <--
 * Byte 6-7 <--
 *
 ************************************************************************************/



void SendCANPackets(int Xvel,int Yvel,int Zvel,
                   char BeamAmp1,char BeamAmp2,char BeamAmp3,
                   char BeamCorr1,char BeamCorr2,char BeamCorr3,
                   fractional FilteredHeading,fractional RudderCmd,
                   IMU_Packet *IMU_Data)
{
    int DataID;
    int arg1, arg2, arg3, arg4;
    static int seq_num = 0;

    DataID = 0;
    if (!C1TR01CONbits.TXREQ0) //If previous request for buffer zero isn't still waiting
    {
        if (raw == 0)
            AssembleMessageData_FourInts(&message[DataID],
                Xvel, Yvel, Zvel,
                (int) (200 * ConvertFractToEngUnits(Pressure_Buf, Pressure_cnt, Pressure_eng))
                ); //Casting these floats into the int arguments here causes an address error if the compiler optimizations are >0.  Mystery.
        else
            AssembleMessageData_FourInts(&message[DataID],
                Xvel, Yvel, Zvel,
                Pressure_Buf
                ); //Casting these floats into the int arguments here causes an address error if the compiler optimizations are >0.
        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
    {
        AssembleMessageData_EightBytes(&message[DataID],
                BeamAmp1, BeamAmp2, BeamAmp3, BeamCorr1, BeamCorr2, BeamCorr3,
                (char) (HeadingTarget & 0x00ff), //Low Byte
                (char) ((HeadingTarget >> 8) & 0x00ff)); //High Byte
        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
    {
        AssembleMessageData_FourInts(&message[DataID],
                MeasuredHeading_Buf,
                FilteredHeading,
                RudderCmd,
               (signed int) 100 * ConvertFractToEngUnits(WingPos_Buf, WingPos_cnt, WingPos_eng)
                );
        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
    {
        AssembleMessageData_FourInts(&message[DataID],
                Q15(IMU_Data->qtn[0]),
                Q15(IMU_Data->qtn[1]),
                Q15(IMU_Data->qtn[2]),
                Q15(IMU_Data->qtn[3])
                );
        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
    {
        AssembleMessageData_FourInts(&message[DataID],
                Q15(IMU_Data->mag[0]), //Gauss (Q15)
                Q15(IMU_Data->mag[1]),
                Q15(IMU_Data->mag[2]),
                Status.status
                );
        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
    {
        AssembleMessageData_FourInts(&message[DataID],
                (signed int) (1000 * IMU_Data->ang_rate[0]), //milli-rad/sec^2
                (signed int) (1000 * IMU_Data->ang_rate[1]),
                (signed int) (1000 * IMU_Data->ang_rate[2]),
                IMU_Data->VpeStatus
                );
        message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num);
        AssembleCAN_Packet(DataID, &message[DataID]);
        C1TR45CONbits.TXREQ5 = 1; //Initiates a request
    }

    DataID = 6;
    if (!C1TR67CONbits.TXREQ6) //If previous request for this buffer isn't still waiting
    {
        AssembleMessageData_FourInts(&message[DataID],
                (signed int) (1000 * IMU_Data->accel[0]), //mm/sec^2
                (signed int) (1000 * IMU_Data->accel[1]),
                (signed int) (1000 * IMU_Data->accel[2]),
                f_RudderPosition
                );
        message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num);
        AssembleCAN_Packet(DataID, &message[DataID]);
        C1TR67CONbits.TXREQ6 = 1; //Initiates a request
    }

//    DataID = 7;
//    if (!C1TR67CONbits.TXREQ7) //If previous request for this buffer isn't still waiting
//    {
//        AssembleMessageData_FourInts(&message[DataID],
//                kCoeffs_Heading[0],
//                kCoeffs_Heading[1],
//                (unsigned int) (HeadingControlUpdatePeriod >> 8) + HeadingFilterTimeConstant,
//                kCoeffs_Heading[2]
//                );
//        message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num);
//        AssembleCAN_Packet(DataID, &message[DataID]);
//        C1TR67CONbits.TXREQ7 = 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


    }