#include "xc.h"
#include "can.h"
#include "extern.h"

//Assemble 29 bit id from components, 4 bit SRC, 5 bit dataID, and 10bit seq_num
unsigned long Assemble_packetID(unsigned int src, unsigned int dataID, unsigned int seq_num)
{
    unsigned long result = 0;
    result = ((unsigned long) src) << (SEQ_WIDTH+ID_WIDTH);
    result |= ((unsigned long) dataID) << SEQ_WIDTH;
    result |= (unsigned long) seq_num;

    return result;  //(0x1fffffff & result);
}



void AssembleMessageData_TwoFloats(CANmsg *message, float float1, float float2)
{
    char *char_ptr;

    char_ptr = (char *) &float1;
    message->data[0] = char_ptr[0];
    message->data[1] = char_ptr[1];
    message->data[2] = char_ptr[2];
    message->data[3] = char_ptr[3];
    char_ptr = (char *) &float2;
    message->data[4] = char_ptr[0];
    message->data[5] = char_ptr[1];
    message->data[6] = char_ptr[2];
    message->data[7] = char_ptr[3];

    message->data_length = 8;
}

void AssembleMessageData_FourInts(CANmsg *message, int int1, int int2, int int3, int int4)
{
    char *char_ptr;

    char_ptr = (char *) &int1;
    message->data[0] = char_ptr[0];
    message->data[1] = char_ptr[1];
    char_ptr = (char *) &int2;
    message->data[2] = char_ptr[0];
    message->data[3] = char_ptr[1];
    char_ptr = (char *) &int3;
    message->data[4] = char_ptr[0];
    message->data[5] = char_ptr[1];
    char_ptr = (char *) &int4;
    message->data[6] = char_ptr[0];
    message->data[7] = char_ptr[1];

    message->data_length = 8;
}

void AssembleMessageData_EightBytes(CANmsg *message, char byte1, char byte2, char byte3, char byte4, char byte5, char byte6, char byte7,char byte8)
{
    message->data[0] = byte1;
    message->data[1] = byte2;
    message->data[2] = byte3;
    message->data[3] = byte4;
    message->data[4] = byte5;
    message->data[5] = byte6;
    message->data[6] = byte7;
    message->data[7] = byte8;

    message->data_length = 8;
}


int AssembleCAN_Packet(int bufnum, CANmsg *message)
{
    unsigned int word0 = 0x0000;
    unsigned int word1 = 0x0000;
    unsigned int word2 = 0x0000;
    
/* Message Format: */
/* Word0 : 0bUUUx xxxx xxxx xxxx */
/* |____________||| */
/* SID10:0 SRR IDE (bit 0) */
/* Word1 : 0bUUUU xxxx xxxx xxxx */
/* |_____________| */
/* EID17:6 */
/* Word2 : 0bxxxx xxx0 UUU0 xxxx */
/* |_____|| |__| */
/* EID5:0 RTR DLC */
/* Remote Transmission Request Bit for standard frames */
/* SRR-> ?0? Normal Message */
/* ?1? Message will request remote transmission */
/* Substitute Remote Request Bit for extended frames */
/* SRR-> should always be set to ?1? as per CAN specification */
/* Extended Identifier Bit */
/* IDE-> ?0? Message will transmit standard identifier */
/* ?1? Message will transmit extended identifier */
/* Remote Transmission Request Bit for extended frames */
/* RTR-> ?0? Message transmitted is a normal message */
/*       ?1? Message transmitted is a remote message */
/* Do not care for standard frames */
/* check to see if the message has an extended ID */
if(message->frame_type==CAN_FRAME_EXT)
{
/* get the extended message id EID28..18*/
word0=(message->id & 0x1FFC0000) >> 16;
/* set the SRR and IDE bit */
word0=word0+0x0003;
/* the the value of EID17..6 */
word1=(message->id & 0x0003FFC0) >> 6;
/* get the value of EID5..0 for word 2 */
word2=(message->id & 0x0000003F) << 10;
}
else
{
/* get the SID */
word0=((message->id & 0x000007FF) << 2);
}
/* check to see if the message is an RTR message */
if(message->message_type==CAN_MSG_RTR)
{
if(message->frame_type==CAN_FRAME_EXT)
word2=word2 | 0x0200;
else
word0=word0 | 0x0002;
ECAN1MSGBUF[bufnum][0]=word0;
ECAN1MSGBUF[bufnum][1]=word1;
ECAN1MSGBUF[bufnum][2]=word2;
}
else
{
word2=word2+(message->data_length & 0x0F);
ECAN1MSGBUF[bufnum][0]=word0;
ECAN1MSGBUF[bufnum][1]=word1;
ECAN1MSGBUF[bufnum][2]=word2;
/* fill the data */
ECAN1MSGBUF[bufnum][3]=((message->data[1] << 8) + message->data[0]);
ECAN1MSGBUF[bufnum][4]=((message->data[3] << 8) + message->data[2]);
ECAN1MSGBUF[bufnum][5]=((message->data[5] << 8) + message->data[4]);
ECAN1MSGBUF[bufnum][6]=((message->data[7] << 8) + message->data[6]);
}
    return 0;
}



//Adapted from AN1249 EXAMPLE 17: COPYING MESSAGE FROM DMA INTO SRAM DS01249A-page 20© 2009 Microchip Technology Inc.
void DisAssembleCAN_Packet(int bufnum, CANmsg *message)
{
    unsigned int ide = 0;
    unsigned int rtr = 0;
    unsigned long id = 0;
    /* Standard Message Format: */
    /* Word0 : 0bUUUx xxxx xxxx xxxx */
    /* |____________||| */
    /* SID10:0 SRR IDE (bit 0) */
    /* Word1 : 0bUUUU xxxx xxxx xxxx */
    /* |_____________| */
    /* EID17:6 */
    /* Word2 : 0bxxxx xxx0 UUU0 xxxx */
    /* |_____|| |__| */
    /* EID5:0 RTR DLC */
    /* Word3-Word6: Data bytes */
    /* Word7: Filter hit code bits */
    /* Remote Transmission Request Bit for standard frames */
    /* SRR-> "0 " Normal Message */
    /* "1" Message will request remote transmission */
    /* Substitute Remote Request Bit for extended frames */
    /* SRR-> Should always be set to ?1? as per CAN specification */
    /* Extended Identifier Bit */
    /* IDE-> "0" Message will transmit standard identifier */
    /* "1" Message will transmit extended identifier */
    /* Remote Transmission Request Bit for extended frames */
    /* RTR-> "0" Message transmitted is a normal message */
    /*"1" Message transmitted is a remote message */
    /* Don't care for standard frames */
    /* read word 0 to see the message type */
    ide = ECAN1MSGBUF[bufnum][0] & 0x0001;
    /* check to see what type of message it is */
    /* message is standard identifier */
    if (ide == 0) {
        message->id = (ECAN1MSGBUF[bufnum][0] & 0x1FFC) >> 2;
        message->frame_type = CAN_FRAME_STD;
        rtr = ECAN1MSGBUF[bufnum][0] & 0x0002;
    }        /* mesage is extended identifier */
    else {
        id = ECAN1MSGBUF[bufnum][0] & 0x1FFC;
        message->id = id << 16;
        id = ECAN1MSGBUF[bufnum][1] & 0x0FFF;
        message->id = message->id + (id << 6);
        id = (ECAN1MSGBUF[bufnum][2] & 0xFC00) >> 10;
        message->id = message->id + id;
        message->frame_type = CAN_FRAME_EXT;
        rtr = ECAN1MSGBUF[bufnum][2] & 0x0200;
    }
    /* check to see what type of message it is */
    /* RTR message */
    if (rtr == 1) {
        message->message_type = CAN_MSG_RTR;
    }        /* normal message */
    else {
        message->message_type = CAN_MSG_DATA;
        message->data[0] = (unsigned char) ECAN1MSGBUF[bufnum][3];
        message->data[1] = (unsigned char) ((ECAN1MSGBUF[bufnum][3] & 0xFF00) >> 8);
        message->data[2] = (unsigned char) ECAN1MSGBUF[bufnum][4];
        message->data[3] = (unsigned char) ((ECAN1MSGBUF[bufnum][4] & 0xFF00) >> 8);
        message->data[4] = (unsigned char) ECAN1MSGBUF[bufnum][5];
        message->data[5] = (unsigned char) ((ECAN1MSGBUF[bufnum][5] & 0xFF00) >> 8);
        message->data[6] = (unsigned char) ECAN1MSGBUF[bufnum][6];
        message->data[7] = (unsigned char) ((ECAN1MSGBUF[bufnum][6] & 0xFF00) >> 8);
        message->data_length = (unsigned char) (ECAN1MSGBUF[bufnum][2] & 0x000F);
    }
}