//=========================================================================
// Summary  :
// Filename : MMDynamicControl.h
// Author   : paley
// Project  :
// Revision : 1
// Created  : 2001/05/17
// Modified : 2001/05/17
//=========================================================================
// Description :  Dynamic control messages
//=========================================================================

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

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

MMDynamicControlCommand::MMDynamicControlCommand(float speed, float vertical,
	float horizontal, int updateTime)
{
	setPropellerRpm(speed);
	setElevatorAngle(vertical);
	setRudderAngle(horizontal);
	setUpdateTime(updateTime);
}

const char* MMDynamicControlCommand::pcRead(const char* sourceBuffer)
{
	const char* pcEnd = ::pcRead(sourceBuffer, &m_speed);
	pcEnd = ::pcRead(pcEnd, &m_vertical);
	pcEnd = ::pcRead(pcEnd, &m_horizontal);
	pcEnd = ::pcRead(pcEnd, &m_updateTime);
	return pcEnd;
}

char* MMDynamicControlCommand::pcWrite(char* destBuffer) const
{
	char* pcEnd = ::pcWrite(destBuffer,m_speed);
	pcEnd = ::pcWrite(pcEnd,m_vertical);
	pcEnd = ::pcWrite(pcEnd,m_horizontal);
	pcEnd = ::pcWrite(pcEnd,m_updateTime);
	return pcEnd;
}

void MMDynamicControlCommand::toString(char* str)
{
	sprintf(str,"tMMDynamicControlCommand: prop=%f, elev=%f, rudder=%f, time=%d",
		getPropellerRpm(),getElevatorAngle(),getRudderAngle(),getUpdateTime());
}

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

MMDynamicControlCommandFull::MMDynamicControlCommandFull(float speed, float vertical,
		float horizontal, int speedMode, int verticalMode, int horizontalMode,
		int updateTime)
{
	// the modes must be set first!
	setSpeedMode(speedMode);
	setVerticalMode(verticalMode);
	setHorizontalMode(horizontalMode);

	// next set the values
	setSpeed(speed);
	setVertical(vertical);
	setHorizontal(horizontal);
	setUpdateTime(updateTime);
}

const char* MMDynamicControlCommandFull::pcRead(const char* sourceBuffer)
{
	const char* pcEnd = ::pcRead(sourceBuffer, &m_speed);
	pcEnd = ::pcRead(pcEnd, &m_vertical);
	pcEnd = ::pcRead(pcEnd, &m_horizontal);
	pcEnd = ::pcRead(pcEnd, &m_speedMode);
	pcEnd = ::pcRead(pcEnd, &m_verticalMode);
	pcEnd = ::pcRead(pcEnd, &m_horizontalMode);
	pcEnd = ::pcRead(pcEnd, &m_updateTime);
	return pcEnd;
}

char* MMDynamicControlCommandFull::pcWrite(char* destBuffer) const
{
	char* pcEnd = ::pcWrite(destBuffer,m_speed);
	pcEnd = ::pcWrite(pcEnd,m_vertical);
	pcEnd = ::pcWrite(pcEnd,m_horizontal);
	pcEnd = ::pcWrite(pcEnd,m_speedMode);
	pcEnd = ::pcWrite(pcEnd,m_verticalMode);
	pcEnd = ::pcWrite(pcEnd,m_horizontalMode);
	pcEnd = ::pcWrite(pcEnd,m_updateTime);
	return pcEnd;
}

void MMDynamicControlCommandFull::setSpeed(float speed)
{
	switch (getSpeedMode()) {
	case Rpm:
		setPropellerRpm(speed);
		break;
	case SpeedOpen:
	case Speed:
		m_speed = MMUtil::double2char(speed,MIN_SPEED,MAX_SPEED);
		break;
	}
}


void MMDynamicControlCommandFull::setVertical(float vertical)
{
	switch (getVerticalMode()) {
	case Elevator:
		setElevatorAngle(vertical);
		break;
	case Pitch:
		m_vertical = MMUtil::double2short(vertical,MIN_TIP_ANGLE,MAX_TIP_ANGLE);
		break;
	case Depth:
		m_vertical = MMUtil::double2short(vertical,MIN_DEPTH,MAX_DEPTH);
		break;
	}
}

void MMDynamicControlCommandFull::setHorizontal(float horizontal)
{
	switch (getHorizontalMode()) {
	case Rudder:
		setRudderAngle(horizontal);
		break;
	case Heading:
		m_horizontal = MMUtil::double2short(horizontal,MIN_HDG_ANGLE,MAX_HDG_ANGLE);
		break;
	}
}

float MMDynamicControlCommandFull::getSpeed()
{
	float value = 0.;
	switch (getSpeedMode()) {
	case Rpm:
		value = getPropellerRpm();
		break;
	case SpeedOpen:
	case Speed:
		value = MMUtil::char2double(m_speed,MIN_SPEED,MAX_SPEED);
		break;
	}
	return value;
}

float MMDynamicControlCommandFull::getVertical()
{
	float value = 0.;
	switch (getVerticalMode()) {
	case Elevator:
		value = getElevatorAngle();
		break;
	case Pitch:
		value = MMUtil::short2double(m_vertical,MIN_TIP_ANGLE,MAX_TIP_ANGLE);
		break;
	case Depth:
		value = MMUtil::short2double(m_vertical,MIN_DEPTH,MAX_DEPTH);
		break;
	}
	return value;
}

float MMDynamicControlCommandFull::getHorizontal()
{
	float value = 0.;
	switch (getHorizontalMode()) {
	case Rudder:
		value = getRudderAngle();
		break;
	case Heading:
		value = MMUtil::short2double(m_horizontal,MIN_HDG_ANGLE,MAX_HDG_ANGLE);
		break;
	}
	return value;
}

void MMDynamicControlCommandFull::toString(char* str)
{
	sprintf(str,"tMMDynamicControlCommandFull: speed=%f, vert=%f, horiz=%f, "
		"sMode=%d, vMode=%d, hMode = %d, time=%d",getSpeed(),getVertical(),
		getHorizontal(),getSpeedMode(),getVerticalMode(),getHorizontalMode(),
		getUpdateTime());
}