#include <p24fxxxx.h>
#include <stdio.h>
#include "packet.h"
#include "sys_defs.h"
#include "serial.h"
#include "slot.h"

#define SER_CONSOLE 2
#define CMD_OFFSET 30

/**



*/

uint32_t pktCreate(uint8_t command, uint8_t *args, uint8_t nargs)
{
    uint32_t ret = 0;
    uint8_t i, j = 0;
    
    switch (command)
    {
        case INFO_CMD: case READ_CMD: case WRITE_CMD:
            // Push the two least significant bits of the command to the
            // three most significant in the packet
            ret |= command << CMD_OFFSET;
            break;
        default:
            return 0;
    }

    // Add additional arguments to the packet
    for (i = 0; i < nargs; i++) {
        // Pack by byte
        ret |= args[i] << (PKT_ADDR - (i * 8));
    }
    
    return ret;
}

void pktDebPrint(uint64_t pkt)
{
    char buff[64];
    
    unsigned int i;
    for (i = 0; i < 64; i++)
    {   
        unsigned int display = (pkt >> i) & 1;
        sprintf(buff, "%d", display);
    }
    sprintf(buff, "\r\n");
    serPutString(SER_CONSOLE, buff);
}