#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "xc.h"
#include "math.h"
#include "UART.h"
#include "config.h"
#include "extern.h"
#include "conv.h"
#include "CalibrationConstants.h"
#include "libq.h"
#include "Maxon.h"

unsigned int CalcFieldCRC(unsigned int* pDataArray, unsigned int numberOfints) {
    unsigned int shifter;
    unsigned int c;
    unsigned int carry;
    unsigned int CRC = 0;
    //Calculate pDataArray word by word
    while (numberOfints--) {
        shifter = 0x8000; //Initialize BitX to Bit15
        c = *pDataArray++; //Copy next Dataint to c
        do {
            carry = CRC & 0x8000; //Check if Bit15 of CRC is set
            CRC <<= 1; //CRC = CRC * 2
            if (c & shifter) CRC++; //CRC = CRC + 1, if BitX is set in c
            if (carry) CRC ^= 0x1021; //CRC = CRC XOR G(x), if carry is true
            shifter >>= 1; //Set BitX to next lower Bit, shifter = shifter/2

        } while (shifter);
    }
    return CRC;
}

void StuffAndSendMaxonData(char c) {
    putU1(c);
    if (c == (char) DLE) {
        putU1(c);
    }
    return;
}


void SendMaxonFrame(Maxon_Frame* frame_ptr) {  //This will return with success/failure indicated in Maxon_CommError.  (0 indicates success, RS232_TIMEOUT_ERROR if a packetisn't recieved, and RS232_CRC_ERROR if a packet is recieved but has invalid crc)
    char *frame_char_ptr = (char *) frame_ptr;
    unsigned int *frame_int_ptr = (unsigned int *) frame_ptr;
    int i;
    unsigned long start_time_ctr;
    char c;

   _U1RXIE = 0;  //Disable UART Interrupt
   while (U1STAbits.URXDA)  //Flush recieve buffer
     c = U1RXREG;
    Maxon_CommError = RS232_TIMEOUT_ERROR; //Mark response as invalid  Successful frame response will change this.
    i_ctr = 0;  //Sets UART recieve character counter to zero to be ready to recieve new packet, not sure why this is neccessary.
    _U1RXIF = 0; //Clear Interrupt
    _U1RXIE = 1;  //Enable Interrupt

    frame_ptr->Data[frame_ptr->Len] = 0x0000; // Required to make CRC compute correctly.
    frame_ptr->CRC = CalcFieldCRC(&(frame_int_ptr[1]), frame_ptr->Len + 2);

    putU1(frame_char_ptr[0]);
    putU1(frame_char_ptr[1]);

    for (i = 2; i < 2 * frame_ptr->Len + 4; i++)
        StuffAndSendMaxonData(frame_char_ptr[i]);

    for (i = sizeof (Maxon_Frame) - 2; i<sizeof (Maxon_Frame); i++)
        StuffAndSendMaxonData(frame_char_ptr[i]);

    start_time_ctr = CAN_PacketSend_ctr; //CAN_PacketSend_ctr counts up at the CAN packet rate (10Hz).
    while ((Maxon_CommError == RS232_TIMEOUT_ERROR) && (CAN_PacketSend_ctr < (start_time_ctr + RS232_TIMEOUT))); //Wait up to 500ms for response
    Status.bits.Maxon_CommError = Maxon_CommError;
    //This will return with success/failure indicated in Status.bits.Maxon_CommError.
    //0 indicates success,
    //RS232_TIMEOUT_ERROR (1) if a packet isn't recieved in 100*RS232_TIMEMOUT ms, and
    //RS232_CRC_ERROR (2) if a packet is recieved but has invalid crc, set in the UART_Recieve interrupt.

}


void ReadMaxonObject(unsigned char NodeID, unsigned int Index, unsigned char SubIndex) //Implements "ReadObject" described in section 2.1.1.1 of EPOS4-Communicatoion-Guide-En.pdf
{
    Maxon_Frame *frame_ptr = &Maxon_FrameA;
    unsigned char *frame_char_ptr;
    frame_char_ptr = (unsigned char *) frame_ptr;
    int CmdAttempt = 0;


    do {

        frame_ptr->dle = DLE;
        frame_ptr->stx = STX;
        frame_ptr->OpCode = 0x60;
        frame_ptr->Len = 2;
        frame_char_ptr[4] = NodeID;
        frame_char_ptr[5] = (Index & 0xff);
        frame_char_ptr[6] = (Index >> 8) & 0xff;
        frame_char_ptr[7] = SubIndex;
        SendMaxonFrame(&Maxon_FrameA); //This blocks until read is complete or timeout occurs
    } while ((++CmdAttempt < RS232_ATTEMPTS) && Status.bits.Maxon_CommError);


    return;
}

unsigned char ReadMaxonByte(unsigned char NodeID, unsigned int Index, unsigned char SubIndex) {
    unsigned char *frame_char_ptr;
    frame_char_ptr = (unsigned char *) &Maxon_FrameA;

    ReadMaxonObject(NodeID, Index, SubIndex);

    return frame_char_ptr[8];
}

unsigned int ReadMaxonWord(unsigned char NodeID, unsigned int Index, unsigned char SubIndex) {
    ReadMaxonObject(NodeID, Index, SubIndex);

    return Maxon_FrameA.Data[2];
}

unsigned long ReadMaxonDWord(unsigned char NodeID, unsigned int Index, unsigned char SubIndex) {
    unsigned long *frame_long_ptr;
    frame_long_ptr = (unsigned long *) &Maxon_FrameA;
    ReadMaxonObject(NodeID, Index, SubIndex);

    return frame_long_ptr[2];
}

void WriteMaxonObject(unsigned char NodeID, unsigned int Index, unsigned char SubIndex, unsigned long data) //Implements "WriteObject" described in section 2.1.1.1 of EPOS4-Communicatoion-Guide-En.pdf
{
    Maxon_Frame *frame_ptr = &Maxon_FrameA;
    unsigned char *frame_char_ptr;
    frame_char_ptr = (unsigned char *) frame_ptr;
    int CmdAttempt = 0;


    do {
        frame_ptr->dle = DLE;
        frame_ptr->stx = STX;

        frame_ptr->OpCode = 0x68;
        frame_ptr->Len = 4;
        frame_char_ptr[4] = NodeID;
        frame_char_ptr[5] = (Index & 0xff);
        frame_char_ptr[6] = (Index >> 8) & 0xff;
        frame_char_ptr[7] = SubIndex;
        frame_ptr->Data[2] = (unsigned int) (data & 0xffff);
        frame_ptr->Data[3] = (unsigned int) (data >> 16) & 0xffff;

        SendMaxonFrame(&Maxon_FrameA); //This blocks until response read is successful or timeout occurs
    } while ((++CmdAttempt < RS232_ATTEMPTS) && Status.bits.Maxon_CommError);

    return;
}