/****************************************************************************/
/* Copyright (c) 2006 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : smac.cc                                                       */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 03/17/2006                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

#include "stdafx.h"

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/timeb.h>

#include "smac.h"

const unsigned int smac::_headerSize = sizeof(SMAC_RECORD_HEADER);
const unsigned int smac::_sanSize = sizeof(SMAC_SAN_RECORD);
const unsigned int smac::_statSize = sizeof(SMAC_STAT_RECORD);
const unsigned int smac::_cmdSize = sizeof(SMAC_CMD_RECORD);

// Index into this array controlled by BhavrType enum in smac.h
const char* smac::_behaviorNames[] = {"Abort", "Diving", "Survey", "Ascending"};

smac::smac()
{
}

smac::~smac()
{
}

// CRC32 checksum algorithm
unsigned int smac::calcChecksum(const char *packet, unsigned long packet_size)
{
 	const char * pcNext = packet;
 	const char * pcEnd  = packet + packet_size;

 	const int c1    = 52845;
 	const int c2    = 22719;
 	int r           = 55665;
 	unsigned int iChecksum   = 0;

 	for ( ; pcNext < pcEnd; ++pcNext )
 	{
 		unsigned char cipher = (unsigned char)( ( (*pcNext) ^ (r >> 8)) );
 		r = (cipher + r) * c1 + c2;
 		iChecksum += cipher;
 	}

 	return iChecksum;
}

unsigned int smac::calcChecksum(SMAC_RECORD_HEADER *hdr)
{
	return calcChecksum((const char*)hdr,
		sizeof(SMAC_RECORD_HEADER) - sizeof(unsigned long));
}

// Set basic header stuff that is the same for all records
void smac::setHeader(SMAC_RECORD_HEADER *header)
{
  
  if (header)
  {
    header->version = 1;
	header->parameter = 0;
	header->checksum = 0;

	struct timeb tb;
	ftime(&tb);
	header->ts_sec = tb.time;
	header->ts_msec = tb.millitm;
  }

}

// Take a completed SAN record and a blank header. complete the header and
// flatten the both of them into an array (which better be large enough).
// Append a checksum and return the resulting packet_size.
int smac::encodePacket(SMAC_RECORD_HEADER *header, SMAC_SAN_RECORD *san,
                 const char *packet, unsigned long *packet_size)
{
  if (NULL == header || NULL == san || NULL == packet || NULL == packet_size)
    return 1;

  setHeader(header);

  // This is a SAN record
  header->rec_size = _sanSize;
  header->record_type = smac::SAN;
  header->checksum = calcChecksum(header);

  // Place data in packet array
  memcpy((void*)packet, (void const*)header, _headerSize);
  memcpy((void*)(packet+_headerSize), (void const*)san, _sanSize);

  // Append the checksum
  unsigned int chksum = calcChecksum(packet, _headerSize+_sanSize);

  memcpy((void*)(packet+_headerSize+_sanSize), (void const*)&chksum, sizeof(chksum));

  // wrap it up
  *packet_size = _headerSize + _sanSize + sizeof(unsigned int);
  printPacket(packet, *packet_size);

  return 0;
}
  
// Take a completed STAT record and a blank header. complete the header and
// flatten the both of them into an array (which better be large enough).
// Append a checksum and return the resulting packet_size.
int smac::encodePacket(SMAC_RECORD_HEADER *header, SMAC_STAT_RECORD *st,
                            const char *packet, unsigned long *packet_size)
{
  if (NULL == header || NULL == st || NULL == packet || NULL == packet_size)
    return 1;

  setHeader(header);

  // This is a STAT record
  header->rec_size = _statSize;
  header->record_type = smac::STAT;
  header->checksum = calcChecksum(header);

  
  // Place data in packet array
  memcpy((void*)packet, (void const*)header, _headerSize);
  memcpy((void*)(packet+_headerSize), (void const*)st, _statSize);

  // Append the checksum
  unsigned int chksum = calcChecksum(packet, _headerSize+_statSize);
  memcpy((void*)(packet+_headerSize+_statSize), (void const*)&chksum, sizeof(chksum));
  
  // wrap it up
  *packet_size = _headerSize + _statSize + sizeof(unsigned int);

  return 0;
}
  
// Take a completed CMD record and a blank header. complete the header and
// flatten the both of them into an array (which better be large enough).
// Append a checksum and return the resulting packet_size.
int smac::encodePacket(SMAC_RECORD_HEADER *header, SMAC_CMD_RECORD *cmd,
                            const char *packet, unsigned long *packet_size)
{
  if (NULL == header || NULL == cmd || NULL == packet || NULL == packet_size)
    return 1;

  setHeader(header);

  // This is a CMD record
  header->rec_size = _cmdSize;
  header->record_type = smac::CMD;
  header->checksum = calcChecksum(header);
  
  // Place data in packet array
  memcpy((void*)packet, (void const*)header, _headerSize);
  memcpy((void*)(packet+_headerSize), (void const*)cmd, _cmdSize);

  // Append the checksum
  unsigned int chksum = calcChecksum(packet, _headerSize+_cmdSize);
  memcpy((void*)(packet+_headerSize+_cmdSize), (void const*)&chksum, sizeof(chksum));
  
  // wrap it up
  *packet_size = _headerSize + _cmdSize + sizeof(unsigned int);

  return 0;
}

// Take a packet received from the surface application, return the header
// information. Additionally, validate the checksum. Return 0 if checksum
// is valid, -1 if invalid, 1 if there's any other problem.
// Return 2 if header checksums failed to match
int smac::decodeHeader(const char *packet, unsigned long packet_size,
                       SMAC_RECORD_HEADER *header)
{
  if (NULL == packet || NULL == header || packet_size < _headerSize)
    return 1;

  SMAC_RECORD_HEADER *hdr = (SMAC_RECORD_HEADER*)packet;
  if (hdr->version !=1 || hdr->record_type>=smac::INVALID || 0 == hdr->rec_size)
    return 1;

  unsigned int hchk = calcChecksum(hdr);
  if (hchk != hdr->checksum) return 2;

  // Copy info from packet to header parameter
  header->version = hdr->version;
  header->rec_size = hdr->rec_size;
  header->ts_sec = hdr->ts_sec;
  header->ts_msec = hdr->ts_msec;
  header->record_type = hdr->record_type;
  header->parameter = hdr->parameter;
  
  // Calculate checksum and compare to value in packet
  // Return -1 if not equal
  unsigned int *packetSum = (unsigned int*)(packet+_headerSize + hdr->rec_size);
  unsigned int calcSum = calcChecksum(packet, _headerSize + hdr->rec_size);
  
  printf("Checksum check: %d (%d)\n", calcSum, *packetSum);
  
  if (calcSum != *packetSum)
    return -1;

  return 0;
}

// Take a packet received from the surface app, return the SAN record within.
// Return 0 if everything goes well, -1 if checksum is invalid, 1 if there's
// some other problem (like this isn't a SAN record).
// Return 2 if header checksums failed to match
int smac::decodeSanRecord(const char *packet, unsigned long packet_size,
                          SMAC_SAN_RECORD *san)
{
  if (NULL == packet || NULL == san || packet_size < _headerSize+_sanSize)
    return 1;

  SMAC_RECORD_HEADER *hdr = (SMAC_RECORD_HEADER*)packet;
  if (hdr->version != 1 ||
      hdr->record_type != smac::SAN || _sanSize != hdr->rec_size)
    return 1;

  unsigned int hchk = calcChecksum(hdr);
  if (hchk != hdr->checksum) return 2;

  // Copy info from packet to san parameter
  memcpy(san, (SMAC_SAN_RECORD*)(packet+_headerSize), sizeof(SMAC_SAN_RECORD));

  // Calculate checksum and compare to value in packet
  // Return -1 if not equal
  unsigned int *packetSum = (unsigned int*)(packet+_headerSize+hdr->rec_size);
  unsigned int calcSum = calcChecksum(packet, _headerSize+hdr->rec_size);
  
  if (calcSum != *packetSum)
    return -1;

  return 0;
}

// Take a packet received from the surface app, return the STAT record within.
// Return 0 if everything goes well, -1 if checksum is invalid, 1 if there's
// some other problem (like this isn't a STAT record).
// Return 2 if header checksums failed to match
int smac::decodeStatRecord(const char *packet, unsigned long packet_size,
                           SMAC_STAT_RECORD *st)
{
  if (NULL == packet || NULL == st || packet_size < _headerSize+_statSize)
    return 1;

  SMAC_RECORD_HEADER *hdr = (SMAC_RECORD_HEADER*)packet;
  if (hdr->version != 1 ||
      hdr->record_type != smac::STAT || _statSize != hdr->rec_size)
    return 1;

  unsigned int hchk = calcChecksum(hdr);
  if (hchk != hdr->checksum) return 2;

  // Copy info from packet to san parameter
  memcpy(st, (SMAC_STAT_RECORD*)(packet+_headerSize), sizeof(SMAC_STAT_RECORD));

  // Calculate checksum and compare to value in packet
  // Return -1 if not equal
  unsigned int *packetSum = (unsigned int*)(packet+_headerSize+hdr->rec_size);
  unsigned int calcSum = calcChecksum(packet, _headerSize+hdr->rec_size);
  
  if (calcSum != *packetSum)
    return -1;

  return 0;
}

// Take a packet received from the surface app, return the CMD record within.
// Return 0 if everything goes well, -1 if checksum is invalid, 1 if there's
// some other problem (like this isn't a CMD record).
// Return 2 if header checksums failed to match
int smac::decodeCmdRecord(const char *packet, unsigned long packet_size,
                          SMAC_CMD_RECORD *cmd)
{
  if (NULL == packet || NULL == cmd || packet_size < _headerSize+_cmdSize)
    return 1;

  SMAC_RECORD_HEADER *hdr = (SMAC_RECORD_HEADER*)packet;
  if (hdr->version != 1 ||
      hdr->record_type != smac::CMD || _cmdSize != hdr->rec_size)
    return 1;

  unsigned int hchk = calcChecksum(hdr);
  if (hchk != hdr->checksum) return 2;

  // Copy info from packet to san parameter
  memcpy(cmd, packet+_headerSize, sizeof(SMAC_CMD_RECORD));;

  // Calculate checksum and compare to value in packet
  // Return -1 if not equal
  unsigned int *packetSum = (unsigned int*)(packet+_headerSize+hdr->rec_size);
  unsigned int calcSum = calcChecksum(packet, _headerSize+hdr->rec_size);

  printf("Checksum check: %d (%d)\n", calcSum, *packetSum);
  
  if (calcSum != *packetSum)
    return -1;

  return 0;
}

void smac::printPacket(const char *packet, unsigned long packet_size)
{
  SMAC_RECORD_HEADER hdr;
  int stat = 0;
  
  printf("packet_size = %ld\n", packet_size);  
  if ((stat = decodeHeader(packet, packet_size, &hdr)) != 0)
  {
    printf("Problem decoding header: %d\n", stat);
    //return;
  }

  printf("=============== SMAC record dump ================\n");
  for (unsigned int i = 0; i < packet_size;)
  {
    printf("%02X:",packet[i++]);
    if ((i % 8) == 0) printf(" ");
    if ((i % 16) == 0) printf("\n");
  }
  printf("\n");
  
  printf("hdr.version   = %d  hdr.rec_size = %-4d\n",hdr.version, hdr.rec_size);
  printf("hdr.timestamp = %ld.%ld\n", hdr.ts_sec, hdr.ts_msec);
  printf("hdr.rec_type  = %d  hdr.param = %d\n", hdr.record_type, hdr.parameter);
  printf("hdr.checksum  = %d\n", hdr.checksum);
  printf("=================================================\n");

}