//=========================================================================
// Summary  :
// Filename : MMDvl.cc
// Author   : paley
// Project  :
// Revision : 1
// Created  : 2001/06/29
// Modified : 2001/06/29
//=========================================================================
// Description :  Dvl messages
//=========================================================================

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

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

MMDvl::MMDvl(float vx, float vy, float vz, float range, int updateTime)
    : MMFloatArray(4)
{
	setVx(vx);
	setVy(vy);
	setVz(vz);
	setRange(range);
	setUpdateTime(updateTime);
}

void MMDvl::toString(char* str)
{
	sprintf(str,"tMMDvl:  vx=%f, vy=%f, vz=%f, r=%f, t=%d",
		getVx(),getVy(),getVz(),getRange(),getUpdateTime());
}

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

MMDvlBeamVelocities::MMDvlBeamVelocities(float v1, float v2, float v3, float v4) :
	MMFloatArray(4)
{
	setVelocity1(v1);
	setVelocity2(v2);
	setVelocity3(v3);
	setVelocity4(v4);
}

void MMDvlBeamVelocities::toString(char* str)
{
	sprintf(str,"tMMDvlBeamVelocities:  v1=%f, v2=%f, v3=%f, v4=%f",
		getVelocity1(),getVelocity2(),getVelocity3(),getVelocity4());
}

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

MMDvlBeamRanges::MMDvlBeamRanges(float r1, float r2, float r3, float r4) :
	MMFloatArray(4)
{
	setRange1(r1);
	setRange2(r2);
	setRange3(r3);
	setRange4(r4);
}

void MMDvlBeamRanges::toString(char* str)
{
	sprintf(str,"tMMDvlBeamRanges:  r1=%f, r2=%f, r3=%f, r4=%f",
		getRange1(),getRange2(),getRange3(),getRange4());
}

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

MMDvlAttTemp::MMDvlAttTemp(float yaw, float pitch, float roll, float temp, int updateTime) :
    MMNavigationAttitude(roll, pitch, yaw, updateTime)
{
    setTemp(temp);
}

const char* MMDvlAttTemp::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_temp);
    return pcEnd;
}

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