#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "rdi.h"

int
rdi_verify_checksum (const char *buf, int len)
{
    /* Bytes 3,4 contain the number of bytes from the start
       of the current ensemble up to, but no including, the 2-byte checksum 
    */
    uint16_t nbytes = *((uint16_t *) (buf+2));
    if (nbytes != (len-2))
        return 0;

    /* The last two bytes contain a modulo 65536 checksum.  the ExplorerDVL
       computes the checksum by summing all the bytes in the output buffer
       excluding the checksum.
    */
    uint16_t checksum = *((uint16_t *) (buf+len-2));

    uint16_t mychecksum = 0;
    const char *mybuf=buf;
    for (int i=0; i < nbytes; i++) {
        mychecksum += *((uint8_t *) mybuf++);
    }
    
    if (mychecksum == checksum)
        return 1;
    else
        return 0;
}

int
rdi_parse_pd4 (const char *buf, int len, rdi_pd4_t *pd4)
{
    if (!rdi_verify_checksum (buf, len) || len!=RDI_PD4_LEN)
        return 0;

    memcpy (pd4, buf, sizeof (rdi_pd4_t));
    if (pd4->header_id!=RDI_PD45_HEADER || pd4->data_id!=RDI_PD4_DATA_ID)
        return 0;
    else
        return 1;
}

int
rdi_parse_pd5 (const char *buf, int len, rdi_pd5_t *pd5)
{
    if (!rdi_verify_checksum (buf, len) || len!=RDI_PD5_LEN)
        return 0;

    memcpy (pd5, buf, sizeof (rdi_pd5_t));
    if (pd5->header_id!=RDI_PD45_HEADER || pd5->data_id!=RDI_PD5_DATA_ID)
        return 0;
    else
        return 1;
}
