#include "packet.h"

#include <stdint.h>

#include "cpustatusui.h"
#include "decoder.h"
#include "protocol.h"


void encoder_build_packet(
        Encoder *enc,
        uint8_t *data,
        uint8_t length)
{
    uint32_t p=0;


    memcpy(
        &enc->packet[p],
        PACKET_MAGIC,
        MAGIC_SIZE);

    p+=MAGIC_SIZE;


    enc->packet[p++]=length;


    memcpy(
        &enc->packet[p],
        data,
        length);

    p+=length;


    uint16_t crc =
        crc16(
            &enc->packet[0],
            p);


    enc->packet[p++]=crc&0xff;
    enc->packet[p++]=crc>>8;


    enc->packetSize=p;
}

bool decoder_parse_packet(Decoder *dec)
{
    if(dec->bufferSize < MAGIC_SIZE + 1 + 2)
        return false;

    if(memcmp(dec->buffer,
              PACKET_MAGIC,
              MAGIC_SIZE))
    {
        printf("Bad magic\n");
        return false;
    }

    uint8_t length =
        dec->buffer[MAGIC_SIZE];

    uint32_t packetSize =
        MAGIC_SIZE +
        1 +
        length +
        2;

    if(dec->bufferSize < packetSize)
        return false;

    uint16_t received =
        dec->buffer[packetSize-2] |
        (dec->buffer[packetSize-1] << 8);

    uint16_t calculated =
        crc16(dec->buffer,
              packetSize-2);

    if(received != calculated)
    {
        printf("CRC FAIL\n");
        return true;
    }

    printf("PACKET OK len=%d\n", length);

    printf("Payload:\n");

    for(int i=0;i<length;i++)
        printf("%02X ",
            dec->buffer[MAGIC_SIZE+1+i]);

    printf("\n");

    return true;
}

uint16_t crc16(
        const uint8_t *data,
        uint32_t len)
{
    uint16_t crc = 0xFFFF;


    for(uint32_t i=0;i<len;i++)
    {
        crc ^= data[i];


        for(int b=0;b<8;b++)
        {
            if(crc & 1)
                crc =
                (crc >> 1) ^ 0xA001;
            else
                crc >>= 1;
        }
    }


    return crc;
}