#include "goose_HW.h"

int goose_HW_decode_length(uint8_t* buff, int index, uint32_t* length)
{
    switch(buff[index])
    {
        case 0x0 ... 0x7F: length[0] = buff[index++];break;
        case 0x81: index++; length[0] = buff[index++];break;
        case 0x82: index++; length[0] = (buff[index++]<<8)|buff[index++];break;
        case 0x83: index++; length[0] = (buff[index++]<<16)|(buff[index++]<<8)|buff[index++];break;
        case 0x84: index++; length[0] = (buff[index++]<<24)|(buff[index++]<<16)|(buff[index++]<<8)|buff[index++];break;
        default : length[0] = 0x0;break;
    }
    return index;
}

int goose_HW_decode_PowerRequestResult(goose_HW_t* dev, uint8_t* buff, int length, PowerReply_t* ack)
{
    int index = 0;
    uint32_t m;

    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->connect_id = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->serialnumb = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->type = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->result = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->mode = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x87)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->requestV = (buff[index]<<24)|(buff[index]<<16)|(buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x87)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->requestI = (buff[index]<<24)|(buff[index]<<16)|(buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->resv0 = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->resv1 = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    dev->rx_bytes = 0;
    return 0;
}

int goose_HW_decode_ShutdownRequestResult(goose_HW_t* dev, uint8_t* buff, int length, ShutdownReply_t* ack)
{
    int index = 0;
    uint32_t m;

    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->connect_id = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->serialnumb = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->reason = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->result = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    if (buff[0] == 0x86)
    {
        index = goose_HW_decode_length(buff, index+1, &m);
        ack->reject = (buff[index]<<8)|buff[index+1];
        index += m; 
    }
    dev->rx_bytes = 0;
    return 0;
}

int goose_HW_decode_APDU(goose_HW_t* dev, uint8_t* buff, int length)
{
    if (dev)
    {
        int index = 0;
        uint32_t m = 0;
        uint32_t SetEntries = 0;
        if (buff[0] == 0x61)
        {
            index = goose_HW_decode_length(buff, index+1, &m);
            if (m > 0 && (m == (length - index)))
            {
                if (buff[index]==0x80)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    memcpy(dev->goCBRef_cache, &buff[index], m);
                    dev->goCBRef_cache[m]='\0';
                    index += m;
                }
                if (buff[index]==0x81)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    uint32_t timeAllowedToLive = buff[index];
                    index += m;
                }
                if (buff[index]==0x82)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    memcpy(dev->dataSetRef_cache, &buff[index], m);
                    dev->dataSetRef_cache[m]='\0';
                    index += m;
                }
                if (buff[index]==0x83)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    memcpy(dev->goID_cache, &buff[index], m);
                    dev->goID_cache[m]='\0';
                    index += m;
                }
                if (buff[index]==0x84)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    uint32_t utc_s = buff[index];
                    uint32_t utc_ms = buff[index+4];
                    index += m;
                }
                if (buff[index]==0x85)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    uint32_t timeAllowedToLive = buff[index];
                    index += m;
                }
                if (buff[index]==0x86)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    uint32_t stNum = buff[index];
                    index += m;
                }
                if (buff[index]==0x87)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    uint32_t simulation = buff[index];
                    index += m;
                }
                if (buff[index]==0x88)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    uint32_t confRev = buff[index];
                    index += m;
                }
                if (buff[index]==0x89)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    uint32_t needsCommission = buff[index];
                    index += m;
                }
                if (buff[index]==0x8a)
                {
                    index = goose_HW_decode_length(buff, index+1, &m); 
                    SetEntries = buff[index];
                    index += m;
                }
                if (buff[index]==0xab)
                {
                    index = goose_HW_decode_length(buff, index+1, &m);
                    if (strcmp(dev->goID_cache, "CC/0$GO$PowerRequestReply")==0 && SetEntries == 9)
                    {
                        return goose_HW_decode_PowerRequestResult(dev, &buff[index], length - index, &dev->power_reply);
                    }
                    if (strcmp(dev->goID_cache, "CC/0$GO$ShutdownRequestReply")==0 && SetEntries == 5)
                    {
                        return goose_HW_decode_ShutdownRequestResult(dev, &buff[index], length - index,  &dev->shutdown_reply);
                    }
                    dev->tx_bytes = 0;
                    return -9;
                }
                dev->tx_bytes = 0;
                return -8;
            }
            dev->tx_bytes = 0;
            return -7;
        }
        dev->tx_bytes = 0;
        return -6;
    }
    return -1;
}
