////////////////////////////////////////////////////////////////////////////
//
//  This file is part of RTTeensyLink
//
//  Copyright (c) 2015, richards-tech, LLC
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy of 
//  this software and associated documentation files (the "Software"), to deal in 
//  the Software without restriction, including without limitation the rights to use, 
//  copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 
//  Software, and to permit persons to whom the Software is furnished to do so, 
//  subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all 
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
//  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
//  PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 
//  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 
//  OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 
//  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#include "RTTeensyLinkUtils.h"

//	RTTeensyLinkRXFrameInit initializes the structure for a new frame using frameBuffer for storage

void RTTeensyLinkRXFrameInit(RTTEENSYLINK_RXFRAME *RXFrame, RTTEENSYLINK_FRAME *frameBuffer)
{
    RXFrame->complete = false;
    RXFrame->length = 0;
    RXFrame->bytesLeft = 0;
    RXFrame->frameBuffer = frameBuffer;
}

//  RTTeensyLinkReassemble takes a sequence of received bytes and tries to complete a frame. It returns true if ok
//  false if error. The caller can determine if the frame is complete by checking the complete flag.

bool RTTeensyLinkReassemble(RTTEENSYLINK_RXFRAME *RXFrame, unsigned char data)
{
    bool flag = true;

 
    ((unsigned char *)(RXFrame->frameBuffer))[RXFrame->length] = data;  // save byte in correct place
    switch (RXFrame->length) {
        case 0:                                             // waiting for sync0
            if (RXFrame->frameBuffer->sync0 == RTTEENSYLINK_MESSAGE_SYNC0) {
                RXFrame->length = 1;
            }
            break;

        case 1:                                             // waiting for sync1
            if (RXFrame->frameBuffer->sync1 == RTTEENSYLINK_MESSAGE_SYNC1) {
                RXFrame->length = 2;
            } else {
                RXFrame->length = 0;                        // try again if not correct two byte sequence
            }
            break;

        case 2:                                             // should be message length
            if (RXFrame->frameBuffer->messageLength <= RTTEENSYLINK_MESSAGE_MAX_LEN) {
                RXFrame->length = 3;
                RXFrame->bytesLeft = RXFrame->frameBuffer->messageLength + 1;   // +1 to allow for the checksum
            } else {
                RXFrame->length = 0;                        // discard this and resync frame
                flag = false;
            }
            break;

        default:
            RXFrame->length++;
            RXFrame->bytesLeft--;
            if (RXFrame->bytesLeft == 0) {                  // a complete frame!
                if (!RTTeensyLinkCheckChecksum(RXFrame->frameBuffer)) {
                    RTTeensyLinkRXFrameInit(RXFrame, RXFrame->frameBuffer);
                    flag = false;                           // flag the error
                } else {
                    //  this is a valid frame (so far)
                    RXFrame->complete = true;
                }
            }
            break;
    }
    return flag;
}

//  RTTeensyLinkSetChecksum correctly sets the checksum field on an RCP frame prior to transmission
//

void RTTeensyLinkSetChecksum(RTTEENSYLINK_FRAME *frame)
{
    int cksm;
    int i;
    unsigned char *data;

    for (i = 0, cksm = 0, data = (unsigned char *)&(frame->message); i < frame->messageLength; i++)
        cksm += *data++;                                    // add up checksum
    frame->frameChecksum = (255 - cksm) + 1;                // 2s complement
}


//  RTTeensyLinkCheckChecksum checks a received frame's checksum.
//
//  It adds up all the bytes from the nFrameCksm byte to the end of the frame. The result should be 0.

bool RTTeensyLinkCheckChecksum(RTTEENSYLINK_FRAME *frame)
{
    int length;
    int i;
    unsigned char *data;
    unsigned char cksm;

    length = frame->messageLength + 1;
    cksm = 0;
    data = (unsigned char *)&(frame->frameChecksum);

    for (i = 0; i < length; i++)
        cksm += *data++;

    return cksm == 0;
}

//  UC2 and UC4 Conversion routines
//

long RTTeensyLinkConvertUC4ToLong(RTTEENSYLINK_UC4 UC4)
{
    long val;

    val = UC4[3];
    val += (long)UC4[2] << 8;
    val += (long)UC4[1] << 16;
    val += (long)UC4[0] << 24;
    return val;
}

void RTTeensyLinkConvertLongToUC4(long val, RTTEENSYLINK_UC4 UC4)
{
    UC4[3] = val & 0xff;
    UC4[2] = (val >> 8) & 0xff;
    UC4[1] = (val >> 16) & 0xff;
    UC4[0] = (val >> 24) & 0xff;
}

int RTTeensyLinkConvertUC2ToInt(RTTEENSYLINK_UC2 UC2)
{
    int val;

    val = UC2[1];
    val += (int)UC2[0] << 8;
    return val;
}

unsigned int RTTeensyLinkConvertUC2ToUInt(RTTEENSYLINK_UC2 UC2)
{
    unsigned int val;

    val = UC2[1];
    val += (unsigned int)UC2[0] << 8;
    return val;
}


void RTTeensyLinkConvertIntToUC2(int val, RTTEENSYLINK_UC2 UC2)
{
    UC2[1] = val & 0xff;
    UC2[0] = (val >> 8) & 0xff;
}


void RTTeensyLinkCopyUC2(RTTEENSYLINK_UC2 destUC2, RTTEENSYLINK_UC2 sourceUC2)
{
    destUC2[0] = sourceUC2[0];
    destUC2[1] = sourceUC2[1];
}