//=========================================================================
// Summary  :
// Filename : MMUtils.cc
// Author   : paley
// Project  :
// Revision : 1
// Created  : 2001/05/17
// Modified : 2001/05/17
//=========================================================================
// Description :  Utility message classes
//=========================================================================

#include <stdio.h>
#include "MMUtils.h"
#include "MMAlarm.h"
#ifdef WIN32
#include "C:\Program Files\Microsoft Visual Studio .NET\Vc7\include\time.h"
#else
#include <time.h>
#endif

// Assuming unsigned types
#define MIN_CHAR_VAL 	0
#define MAX_CHAR_VAL 	255
#define CHAR_RANGE 		255
#define MIN_SHORT_VAL 	0
#define MAX_SHORT_VAL 	65535
#define SHORT_RANGE 	65535


////////////////////////////////////////////////////
uchar MMUtil::int2char(int value)
{
	Assert(value >= MIN_CHAR_VAL && value <= MAX_CHAR_VAL);
	return (uchar) value;
}

int MMUtil::char2int(uchar value)
{
	return (int) value;
}

uchar MMUtil::int2char(int value, int min, int max)
{
	return double2char(value,min,max);
}

int MMUtil::char2int(uchar value, int min, int max)
{
	return (int) Rnd(char2double(value,min,max));
}

uchar MMUtil::double2char(double value, double min, double max)
{
	value = value > max ? max : value;
	value = value < min ? min : value;
	double d = max - min;
	Assert(d > 0);
	long out = Rnd((value - min)/d*(double)CHAR_RANGE + MIN_CHAR_VAL);
	Assert(out >= MIN_CHAR_VAL && out <= MAX_CHAR_VAL);
	uchar uchar_out = (uchar) out;
	Assert(fabs(char2double(uchar_out,min,max)-value) < d/(double)CHAR_RANGE);
	return uchar_out;
}

double MMUtil::char2double(uchar value, double min, double max)
{
	double d = max - min;
	Assert(d > 0);
	double out = (double)(value - MIN_CHAR_VAL)/(double)CHAR_RANGE*d + (double)min;
	Assert(out >= min && out <= max);
	if (fabs(out) < d/(double)CHAR_RANGE) out = 0.;
	return out;
}

ushort MMUtil::double2short(double value, double min, double max)
{
	value = value > max ? max : value;
	value = value < min ? min : value;
	double d = max - min;
	Assert(d > 0);
	long out = Rnd((value - min)/d*(double)SHORT_RANGE + MIN_SHORT_VAL);
	Assert(out >= MIN_SHORT_VAL && out <= MAX_SHORT_VAL);
	ushort ushort_out = (ushort) out;
	Assert(fabs(short2double(ushort_out,min,max)-value) < d/(double)SHORT_RANGE);
	return ushort_out;
}

double MMUtil::short2double(ushort value, double min, double max)
{
	double d = max - min;
	Assert(d > 0);
	double out = (double)(value - MIN_SHORT_VAL)/(double)SHORT_RANGE*d + (double)min;
	Assert(out >= min && out <= max);
	if (fabs(out) < d/(double)SHORT_RANGE) out = 0.;
	return out;
}

long int MMUtil::Rnd(double num_in)
{
	double temp;
	if ( num_in < 0 ) {
		temp = num_in - ceil(num_in);
		if(temp > -0.500000)
			return (long int)floor(num_in);
		else
			return (long int)ceil(num_in);
	} else {
		temp = num_in - floor(num_in);
		if(temp > 0.500000)
			return (long int)ceil(num_in);
		else
			return (long int)floor(num_in);
	}
}

double MMUtil::modPi( double angle )
{
    double ipart, result;

    result = 2*PI*modf( angle/(2*PI), &ipart );

    if ( result >= PI )
        result -= 2*PI;

    if ( result < -PI )
        result += 2*PI;

    return result;
}

// Mapping of device name to MMServerAlarm enumeration
short MMUtil::getServer(const char *device)
{
	char str[32];
	strcpy(str,device);
	for (int i=0; i<strlen(device); i++) str[i] = tolower(device[i]);
	if (strstr(str,"crossbow")) {
		return MMServerAlarm::Ahrs;
	} else if (strstr(str,"tailcone")) {
		return MMServerAlarm::TailCone;
	} else if (strstr(str,"parosci")) {
		return MMServerAlarm::DepthSensor;
	} else if (strstr(str,"power")) {
		return MMServerAlarm::PowerSystem;
	} else if (strstr(str,"battery")) {
		return MMServerAlarm::Battery;
	} else if (strstr(str,"adcp") || strstr(str,"dvl")) {
		return MMServerAlarm::Dvl;
	} else if (strstr(str,"homing")) {
		return MMServerAlarm::Homing;
	} else if (strstr(str,"gps")) {
		return MMServerAlarm::Gps;
	} else if (strstr(str,"rtcm")) {
		return MMServerAlarm::RTCM;
	} else if (strstr(str,"lusbl")) {
		return MMServerAlarm::LUSBLFilter;
	}
	return -1;
}

////////////////////////////////////////////////////
MMNumberedBoolean::MMNumberedBoolean(const ModemMessage *pMM)
{
	Assert(pMM);
	Assert(pMM->getType()==tGetMessageType());
	const char* pcEnd = pcRead(pMM->getBuffer());
	Assert(pcEnd==pMM->getBuffer()+pMM->getBufferSize());
}

MMNumberedBoolean::MMNumberedBoolean(int number, Boolean value)
{
	setNumber(number);
	setValue(value);
}

const char* MMNumberedBoolean::pcRead(const char* sourceBuffer)
{
	const char* pcEnd = ::pcRead(sourceBuffer, &m_number);
	pcEnd = ::pcRead(pcEnd, &m_value);
	return pcEnd;
}

char* MMNumberedBoolean::pcWrite(char* destBuffer) const
{
	char* pcEnd = ::pcWrite(destBuffer,m_number);
	pcEnd = ::pcWrite(pcEnd,m_value);
	return pcEnd;
}

////////////////////////////////////////////////////
MMBooleanArray::MMBooleanArray(const ModemMessage *pMM)
{
	Assert(pMM);
	Assert(pMM->getType()==tGetMessageType());
	const char* pcEnd = pcRead(pMM->getBuffer());
	Assert(pcEnd==pMM->getBuffer()+pMM->getBufferSize());
}

MMBooleanArray::MMBooleanArray(int number)
{
	m_number = MMUtil::int2char(number);
	for (int i=0; i<number; i++)
		m_value[i] = False;
}

const char* MMBooleanArray::pcRead(const char* sourceBuffer)
{
	const char* pcEnd = ::pcRead(sourceBuffer, &m_number);
	for (int i=0; i<m_number; i++)
		pcEnd = ::pcRead(pcEnd, &m_value[i]);
	return pcEnd;
}

char* MMBooleanArray::pcWrite(char* destBuffer) const
{
	char* pcEnd = ::pcWrite(destBuffer,m_number);
	for (int i=0; i<m_number; i++)
		pcEnd = ::pcWrite(pcEnd,m_value[i]);
	return pcEnd;
}

Boolean MMBooleanArray::getValue(int number)
{
	Assert(number >= 0 && number < getNumber());
	return m_value[number];
}

void MMBooleanArray::setValue(int number, Boolean value)
{
	Assert(number >= 0 && number < getNumber());
	m_value[number] = value;
}

////////////////////////////////////////////////////
MMNumberedFloat::MMNumberedFloat(const ModemMessage *pMM)
{
	Assert(pMM);
	Assert(pMM->getType()==tGetMessageType());
	const char* pcEnd = pcRead(pMM->getBuffer());
	Assert(pcEnd==pMM->getBuffer()+pMM->getBufferSize());
}

MMNumberedFloat::MMNumberedFloat(int number, float value, float min,
	float max)
{
	setNumber(number);
	setValue(value,min,max);
}

const char* MMNumberedFloat::pcRead(const char* sourceBuffer)
{
	const char* pcEnd = ::pcRead(sourceBuffer, &m_number);
	pcEnd = ::pcRead(pcEnd, &m_value);
	return pcEnd;
}

char* MMNumberedFloat::pcWrite(char* destBuffer) const
{
	char* pcEnd = ::pcWrite(destBuffer,m_number);
	pcEnd = ::pcWrite(pcEnd,m_value);
	return pcEnd;
}


////////////////////////////////////////////////////
MMFloatArray::MMFloatArray(const ModemMessage *pMM)
{
	Assert(pMM);
	Assert(pMM->getType()==tGetMessageType());
	const char* pcEnd = pcRead(pMM->getBuffer());
	Assert(pcEnd==pMM->getBuffer()+pMM->getBufferSize());
}

MMFloatArray::MMFloatArray(int number, int type)
{
    m_number = MMUtil::int2char(number);
    //fprintf(stdout, "Making FloatArray num: %d type: %s\n", number, ((type == 0) ? "Chars" : "Shorts"));
    setType(type);
    for (int i=0; i<number; i++) {
	switch(m_dataType) {
	case Chars:
	    m_value[i] = 0;
	    break;
	case Shorts:
	    m_shortValue[i] = 0;
	    break;
	}
    }
}

const char* MMFloatArray::pcRead(const char* sourceBuffer)
{
    const char* pcEnd = ::pcRead(sourceBuffer, &m_number);
    pcEnd = ::pcRead(pcEnd, &m_type);
    setType(getType());
    for (int i=0; i<m_number; i++) {
	switch(m_dataType) {
	case Chars:
	    pcEnd = ::pcRead(pcEnd, &m_value[i]);
	    break;
	case Shorts:
	    pcEnd = ::pcRead(pcEnd, &m_shortValue[i]);
	    break;
	}
    }
    return pcEnd;
}

char* MMFloatArray::pcWrite(char* destBuffer) const
{
    char* pcEnd = ::pcWrite(destBuffer,m_number);
    pcEnd = ::pcWrite(pcEnd, m_type);
    for (int i=0; i<m_number; i++) {
	switch(m_dataType) {
	case Chars:
	    pcEnd = ::pcWrite(pcEnd, m_value[i]);
	    break;
	case Shorts:
	    pcEnd = ::pcWrite(pcEnd, m_shortValue[i]);
	    break;
	}
    }
    return pcEnd;
}

float MMFloatArray::getValue(int number, float min, float max)
{
    Assert(number >= 0 && number < getNumber());
    switch(m_dataType) {
    case Chars:
	return MMUtil::char2double(m_value[number],min,max);
    case Shorts:
	return MMUtil::short2double(m_shortValue[number],min,max);
    }
    return 0.;
}

void MMFloatArray::setValue(int number, float value, float min, float max)
{
    Assert(number >= 0 && number < getNumber());
    switch(m_dataType) {
    case Chars:
	m_value[number] = MMUtil::double2char(value,min,max);
    case Shorts:
	m_shortValue[number] =  MMUtil::double2short(value,min,max);
    }
}

////////////////////////////////// 996105729

void MMUtil::getHHMMss(char * str)
{
  time_t t;
  time_t temp = time(&t);
  strftime(str, 40, "%H%M%S",gmtime(&t));
}

void MMUtil::stringToSNS(char * str, int* seconds, int *nanoseconds)
{
  short hours, mins;
  short secs, subSec;
  time_t t;
  time_t temp = time(&t);

  if( sscanf( str, "%2d%2d%2d.%2d", &hours, &mins, &secs, &subSec) == 4) {
    *nanoseconds = subSec * 1e7;
    *seconds = (hours * 3600) + (mins * 60) + secs + t - (t%86400); // number of seconds in a day--gets us the nearest day...
    // printf("GPSTime %d, time %d, secs since midnight %d\n",(hours * 3600) + (mins * 60) + secs,t,(t%86400));
  }

}

//=========================================================================
double MMUtil::latitude(const char *token)
{
  char buf[16];
  double value;

  // First two characters are degrees
  strncpy(buf, token, 2);
  value = atof(buf);

  token += 2;
  value += (atof(token) / 60.);

  return (value);
}

char *MMUtil::latitude(double value)
{
  static char string[16];
  double minutes = 60. * (value - int(value));

  sprintf(string, "%02d", (int )value);
  if (minutes < 10.)
    strcat(string, "0");

  sprintf(string + strlen(string), "%2.4f", minutes);

  return string;
}


//=========================================================================
double MMUtil::longitude(const char *token)
{
  char buf[16];
  double value;

  // First three characters are degrees
  strncpy(buf, token, 3);
  value = atof(buf);

  token += 3;
  value += (atof(token) / 60.);

  return (value);
}

char *MMUtil::longitude(double value)
{
  static char string[16];
  double minutes = 60. * (value - int(value));

  sprintf(string, "%03d", (int )value);
  if (minutes < 10.)
    strcat(string, "0");

  sprintf(string + strlen(string), "%2.4f", minutes);

  return string;
}