//=========================================================================
// Summary  :
// Filename : MMNavigation.cc
// Author   : paley
// Project  :
// Revision : 1
// Created  : 2001/05/01
// Modified : 2001/05/14
//=========================================================================
// Description :  Navigation message
//=========================================================================

#include <stdio.h>
#include "MMNavigation.h"

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

MMNavigationPosition::MMNavigationPosition(float latitude, float longitude,
	float northingRate,	float eastingRate, float speed, float depth,
	float altitude, int updateTime)
{
	setLatitude(latitude);
	setLongitude(longitude);
	setNorthingRate(northingRate);
	setEastingRate(eastingRate);
	setSpeed(speed);
	setDepth(depth);
	setAltitude(altitude);
	setUpdateTime(updateTime);
}

const char* MMNavigationPosition::pcRead(const char* sourceBuffer)
{
	const char* pcEnd = ::pcRead(sourceBuffer, &m_latitude);
	pcEnd = ::pcRead(pcEnd, &m_longitude);
	pcEnd = ::pcRead(pcEnd, &m_northingRate);
	pcEnd = ::pcRead(pcEnd, &m_eastingRate);
	pcEnd = ::pcRead(pcEnd, &m_speed);
	pcEnd = ::pcRead(pcEnd, &m_depth);
	pcEnd = ::pcRead(pcEnd, &m_altitude);
	pcEnd = ::pcRead(pcEnd, &m_updateTime);
	return pcEnd;
}

char* MMNavigationPosition::pcWrite(char* destBuffer) const
{
	char* pcEnd = ::pcWrite(destBuffer,m_latitude);
	pcEnd = ::pcWrite(pcEnd,m_longitude);
	pcEnd = ::pcWrite(pcEnd,m_northingRate);
	pcEnd = ::pcWrite(pcEnd,m_eastingRate);
	pcEnd = ::pcWrite(pcEnd,m_speed);
	pcEnd = ::pcWrite(pcEnd,m_depth);
	pcEnd = ::pcWrite(pcEnd,m_altitude);
	pcEnd = ::pcWrite(pcEnd,m_updateTime);
	return pcEnd;
}

void MMNavigationPosition::toString(char* str)
{
	sprintf(str,"tMMNavigationPosition: lat=%f, lon=%f, nrate=%f, erate=%f, speed=%f, "
		"depth=%f, alt=%f, time=%d",getLatitude(),getLongitude(),getNorthingRate(),
		getEastingRate(),getSpeed(),getDepth(),getAltitude(),getUpdateTime());
}


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

MMNavigationAttitude::MMNavigationAttitude(float roll, float pitch, float yaw,
		int updateTime)
{
	setRoll(roll);
	setPitch(pitch);
	setYaw(yaw);
	setUpdateTime(updateTime);
}

const char* MMNavigationAttitude::pcRead(const char* sourceBuffer)
{
	const char* pcEnd = ::pcRead(sourceBuffer, &m_roll);
	pcEnd = ::pcRead(pcEnd, &m_pitch);
	pcEnd = ::pcRead(pcEnd, &m_yaw);
	pcEnd = ::pcRead(pcEnd, &m_updateTime);
	return pcEnd;
}

char* MMNavigationAttitude::pcWrite(char* destBuffer) const
{
	char* pcEnd = ::pcWrite(destBuffer,m_roll);
	pcEnd = ::pcWrite(pcEnd,m_pitch);
	pcEnd = ::pcWrite(pcEnd,m_yaw);
	pcEnd = ::pcWrite(pcEnd,m_updateTime);
	return pcEnd;
}

void MMNavigationAttitude::toString(char* str)
{
	sprintf(str,"tMMNavigationAttitude: roll=%f, pitch=%f, yaw=%f, time=%d",
		getRoll(),getPitch(),getYaw(),getUpdateTime());
}