/*
 * AP_CommLink.cpp
 *
 *  Created on: Apr 30, 2011
 *      Author: jgoppert
 */

#include "../FastSerial/FastSerial.h"
#include "AP_CommLink.h"
#include "AP_Navigator.h"
#include "AP_Guide.h"
#include "AP_Controller.h"
#include "AP_HardwareAbstractionLayer.h"
#include "AP_RcChannel.h"
#include "../AP_GPS/AP_GPS.h"
#include "../AP_Math/AP_Math.h"
#include "../AP_IMU/AP_IMU.h"
#include "../AP_Compass/AP_Compass.h"
#include "AP_BatteryMonitor.h"

namespace apo {

uint8_t MavlinkComm::_nChannels = 0;
uint8_t MavlinkComm::_paramNameLengthMax = 13;

AP_CommLink::AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide,
		AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
	_link(link), _navigator(navigator), _guide(guide),
			_controller(controller), _hal(hal) {
}

MavlinkComm::MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide,
		AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
			AP_CommLink(link, nav, guide, controller, hal),

			// options
			_useRelativeAlt(true),

			// commands
			_sendingCmds(false), _receivingCmds(false),
			_cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()),
			_cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0),
			_cmdMax(30), _cmdNumberRequested(0),

			// parameters
			_parameterCount(0), _queuedParameter(NULL),
			_queuedParameterIndex(0) {

	switch (_nChannels) {
	case 0:
		mavlink_comm_0_port = link;
		_channel = MAVLINK_COMM_0;
		_nChannels++;
		break;
	case 1:
		mavlink_comm_1_port = link;
		_channel = MAVLINK_COMM_1;
		_nChannels++;
		break;
	default:
		// signal that number of channels exceeded
		_channel = MAVLINK_COMM_3;
		break;
	}
}

void MavlinkComm::send() {
	// if number of channels exceeded return
	if (_channel == MAVLINK_COMM_3)
		return;
}

void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
	//_hal->debug->printf_P(PSTR("send message\n"));

	// if number of channels exceeded return
	if (_channel == MAVLINK_COMM_3)
		return;

	uint64_t timeStamp = micros();

	switch (id) {

	case MAVLINK_MSG_ID_HEARTBEAT: {
		mavlink_msg_heartbeat_send(_channel, mavlink_system.type,
				MAV_AUTOPILOT_ARDUPILOTMEGA);
		break;
	}

	case MAVLINK_MSG_ID_ATTITUDE: {
		mavlink_msg_attitude_send(_channel, timeStamp,
				_navigator->getRoll(), _navigator->getPitch(),
				_navigator->getYaw(), _navigator->getRollRate(),
				_navigator->getPitchRate(), _navigator->getYawRate());
		break;
	}

	case MAVLINK_MSG_ID_GLOBAL_POSITION: {
		mavlink_msg_global_position_send(_channel, timeStamp,
				_navigator->getLat() * rad2Deg,
				_navigator->getLon() * rad2Deg, _navigator->getAlt(),
				_navigator->getVN(), _navigator->getVE(),
				_navigator->getVD());
		break;
	}

	case MAVLINK_MSG_ID_GPS_RAW: {
		mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
				_navigator->getLat() * rad2Deg,
				_navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0,
				_navigator->getGroundSpeed(),
				_navigator->getYaw() * rad2Deg);
		break;
	}

		/*
		 case MAVLINK_MSG_ID_GPS_RAW_INT: {
		 mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(),
		 _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0,
		 _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg);
		 break;
		 }
		 */

	case MAVLINK_MSG_ID_SCALED_IMU: {
		/*
		 * accel/gyro debug
		 */
		/*
		 Vector3f accel = _hal->imu->get_accel();
		 Vector3f gyro = _hal->imu->get_gyro();
		 Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"),
		 accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z);
		 */
		Vector3f accel = _hal->imu->get_accel();
		Vector3f gyro = _hal->imu->get_gyro();
		mavlink_msg_raw_imu_send(_channel, timeStamp, 1000 * accel.x,
				1000 * accel.y, 1000 * accel.z, 1000 * gyro.x,
				1000 * gyro.y, 1000 * gyro.z, _hal->compass->mag_x,
				_hal->compass->mag_y, _hal->compass->mag_z); // XXX THIS IS NOT SCALED FOR MAG
	}

	case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
		int16_t ch[8];
		for (int i = 0; i < 8; i++)
			ch[i] = 0;
		for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) {
			ch[i] = 10000 * _hal->rc[i]->getPosition();
			//_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]);
		}
		mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2],
				ch[3], ch[4], ch[5], ch[6], ch[7], 255);
		break;
	}

	case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
		int16_t ch[8];
		for (int i = 0; i < 8; i++)
			ch[i] = 0;
		for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++)
			ch[i] = _hal->rc[i]->getPwm();
		mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2],
				ch[3], ch[4], ch[5], ch[6], ch[7], 255);
		break;
	}

	case MAVLINK_MSG_ID_SYS_STATUS: {

		uint16_t batteryVoltage = 0; // (milli volts)
		uint16_t batteryPercentage = 1000; // times 10
		if (_hal->batteryMonitor) {
			batteryPercentage = _hal->batteryMonitor->getPercentage()*10;
			batteryVoltage = _hal->batteryMonitor->getVoltage()*1000;
		}
		mavlink_msg_sys_status_send(_channel, _controller->getMode(),
				_guide->getMode(), _hal->getState(), _hal->load * 10,
				batteryVoltage, batteryPercentage, _packetDrops);
		break;
	}

	case MAVLINK_MSG_ID_WAYPOINT_ACK: {
		sendText(SEVERITY_LOW, PSTR("waypoint ack"));
		//mavlink_waypoint_ack_t packet;
		uint8_t type = 0; // ok (0), error(1)
		mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId,
				_cmdDestCompId, type);

		// turn off waypoint send
		_receivingCmds = false;
		break;
	}

	case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
		mavlink_msg_waypoint_current_send(_channel,
				_guide->getCurrentIndex());
		break;
	}

	default: {
		char msg[50];
		sprintf(msg, "autopilot sending unknown command with id: %d", id);
		sendText(SEVERITY_HIGH, msg);
	}

	} // switch
} // send message

void MavlinkComm::receive() {
	//_hal->debug->printf_P(PSTR("receive\n"));
	// if number of channels exceeded return
	//
	if (_channel == MAVLINK_COMM_3)
		return;

	// receive new packets
	mavlink_message_t msg;
	mavlink_status_t status;
	status.packet_rx_drop_count = 0;

	// process received bytes
	while (comm_get_available(_channel)) {
		uint8_t c = comm_receive_ch(_channel);

		// Try to get a new message
		if (mavlink_parse_char(_channel, c, &msg, &status))
			_handleMessage(&msg);
	}

	// Update packet drops counter
	_packetDrops += status.packet_rx_drop_count;
}

void MavlinkComm::sendText(uint8_t severity, const char *str) {
	mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str);
}

void MavlinkComm::sendText(uint8_t severity, const prog_char_t *str) {
	mavlink_statustext_t m;
	uint8_t i;
	for (i = 0; i < sizeof(m.text); i++) {
		m.text[i] = pgm_read_byte((const prog_char *) (str++));
	}
	if (i < sizeof(m.text))
		m.text[i] = 0;
	sendText(severity, (const char *) m.text);
}

void MavlinkComm::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {
}

/**
 * sends parameters one at a time
 */
void MavlinkComm::sendParameters() {
	//_hal->debug->printf_P(PSTR("send parameters\n"));
	// Check to see if we are sending parameters
	while (NULL != _queuedParameter) {
		AP_Var *vp;
		float value;

		// copy the current parameter and prepare to move to the next
		vp = _queuedParameter;
		_queuedParameter = _queuedParameter->next();

		// if the parameter can be cast to float, report it here and break out of the loop
		value = vp->cast_to_float();
		if (!isnan(value)) {

			char paramName[_paramNameLengthMax];
			vp->copy_name(paramName, sizeof(paramName));

			mavlink_msg_param_value_send(_channel, (int8_t*) paramName,
					value, _countParameters(), _queuedParameterIndex);

			_queuedParameterIndex++;
			break;
		}
	}

}

/**
 * request commands one at a time
 */
void MavlinkComm::requestCmds() {
	//_hal->debug->printf_P(PSTR("requesting commands\n"));
	// request cmds one by one
	if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) {
		mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId,
				_cmdDestCompId, _cmdRequestIndex);
	}
}

void MavlinkComm::_handleMessage(mavlink_message_t * msg) {

	uint32_t timeStamp = micros();

	switch (msg->msgid) {
	_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid);

	case MAVLINK_MSG_ID_HEARTBEAT: {
		mavlink_heartbeat_t packet;
		mavlink_msg_heartbeat_decode(msg, &packet);
		_hal->lastHeartBeat = micros();
		break;
	}

	case MAVLINK_MSG_ID_GPS_RAW: {
		// decode
		mavlink_gps_raw_t packet;
		mavlink_msg_gps_raw_decode(msg, &packet);

		_navigator->setTimeStamp(timeStamp);
		_navigator->setLat(packet.lat * deg2Rad);
		_navigator->setLon(packet.lon * deg2Rad);
		_navigator->setAlt(packet.alt);
		_navigator->setYaw(packet.hdg * deg2Rad);
		_navigator->setGroundSpeed(packet.v);
		_navigator->setAirSpeed(packet.v);
		//_hal->debug->printf_P(PSTR("received hil gps raw packet\n"));
		/*
		 _hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"),
		 packet.lat,
		 packet.lon,
		 packet.alt);
		 */
		break;
	}

	case MAVLINK_MSG_ID_ATTITUDE: {
		// decode
		mavlink_attitude_t packet;
		mavlink_msg_attitude_decode(msg, &packet);

		// set dcm hil sensor
		_navigator->setTimeStamp(timeStamp);
		_navigator->setRoll(packet.roll);
		_navigator->setPitch(packet.pitch);
		_navigator->setYaw(packet.yaw);
		_navigator->setRollRate(packet.rollspeed);
		_navigator->setPitchRate(packet.pitchspeed);
		_navigator->setYawRate(packet.yawspeed);
		//_hal->debug->printf_P(PSTR("received hil attitude packet\n"));
		break;
	}

	case MAVLINK_MSG_ID_ACTION: {
		// decode
		mavlink_action_t packet;
		mavlink_msg_action_decode(msg, &packet);
		if (_checkTarget(packet.target, packet.target_component))
			break;

		// do action
		sendText(SEVERITY_LOW, PSTR("action received"));
		switch (packet.action) {

		case MAV_ACTION_STORAGE_READ:
			AP_Var::load_all();
			break;

		case MAV_ACTION_STORAGE_WRITE:
			AP_Var::save_all();
			break;

		case MAV_ACTION_CALIBRATE_RC:
		case MAV_ACTION_CALIBRATE_GYRO:
		case MAV_ACTION_CALIBRATE_MAG:
		case MAV_ACTION_CALIBRATE_ACC:
		case MAV_ACTION_CALIBRATE_PRESSURE:
		case MAV_ACTION_REBOOT:
		case MAV_ACTION_REC_START:
		case MAV_ACTION_REC_PAUSE:
		case MAV_ACTION_REC_STOP:
		case MAV_ACTION_TAKEOFF:
		case MAV_ACTION_LAND:
		case MAV_ACTION_NAVIGATE:
		case MAV_ACTION_LOITER:
		case MAV_ACTION_MOTORS_START:
		case MAV_ACTION_CONFIRM_KILL:
		case MAV_ACTION_EMCY_KILL:
		case MAV_ACTION_MOTORS_STOP:
		case MAV_ACTION_SHUTDOWN:
		case MAV_ACTION_CONTINUE:
		case MAV_ACTION_SET_MANUAL:
		case MAV_ACTION_SET_AUTO:
		case MAV_ACTION_LAUNCH:
		case MAV_ACTION_RETURN:
		case MAV_ACTION_EMCY_LAND:
		case MAV_ACTION_HALT:
			sendText(SEVERITY_LOW, PSTR("action not implemented"));
			break;
		default:
			sendText(SEVERITY_LOW, PSTR("unknown action"));
			break;
		}
		break;
	}

	case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: {
		sendText(SEVERITY_LOW, PSTR("waypoint request list"));

		// decode
		mavlink_waypoint_request_list_t packet;
		mavlink_msg_waypoint_request_list_decode(msg, &packet);
		if (_checkTarget(packet.target_system, packet.target_component))
			break;

		// Start sending waypoints
		mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid,
				_guide->getNumberOfCommands());

		_cmdTimeLastSent = millis();
		_cmdTimeLastReceived = millis();
		_sendingCmds = true;
		_receivingCmds = false;
		_cmdDestSysId = msg->sysid;
		_cmdDestCompId = msg->compid;
		break;
	}

	case MAVLINK_MSG_ID_WAYPOINT_REQUEST: {
		sendText(SEVERITY_LOW, PSTR("waypoint request"));

		// Check if sending waypiont
		if (!_sendingCmds)
			break;

		// decode
		mavlink_waypoint_request_t packet;
		mavlink_msg_waypoint_request_decode(msg, &packet);
		if (_checkTarget(packet.target_system, packet.target_component))
			break;

		_hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq);
		AP_MavlinkCommand cmd(packet.seq);

		mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex());
		mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,
				wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue,
				wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y,
				wp.z);

		// update last waypoint comm stamp
		_cmdTimeLastSent = millis();
		break;
	}

	case MAVLINK_MSG_ID_WAYPOINT_ACK: {
		sendText(SEVERITY_LOW, PSTR("waypoint ack"));

		// decode
		mavlink_waypoint_ack_t packet;
		mavlink_msg_waypoint_ack_decode(msg, &packet);
		if (_checkTarget(packet.target_system, packet.target_component))
			break;

		// check for error
		//uint8_t type = packet.type; // ok (0), error(1)

		// turn off waypoint send
		_sendingCmds = false;
		break;
	}

	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
		sendText(SEVERITY_LOW, PSTR("param request list"));

		// decode
		mavlink_param_request_list_t packet;
		mavlink_msg_param_request_list_decode(msg, &packet);
		if (_checkTarget(packet.target_system, packet.target_component))
			break;

		// Start sending parameters - next call to ::update will kick the first one out

		_queuedParameter = AP_Var::first();
		_queuedParameterIndex = 0;
		break;
	}

	case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: {
		sendText(SEVERITY_LOW, PSTR("waypoint clear all"));

		// decode
		mavlink_waypoint_clear_all_t packet;
		mavlink_msg_waypoint_clear_all_decode(msg, &packet);
		if (_checkTarget(packet.target_system, packet.target_component))
			break;

		// clear all waypoints
		uint8_t type = 0; // ok (0), error(1)
		_guide->setNumberOfCommands(1);
		_guide->setCurrentIndex(0);

		// send acknowledgement 3 times to makes sure it is received
		for (int i = 0; i < 3; i++)
			mavlink_msg_waypoint_ack_send(_channel, msg->sysid,
					msg->compid, type);

		break;
	}

	case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: {
		sendText(SEVERITY_LOW, PSTR("waypoint set current"));

		// decode
		mavlink_waypoint_set_current_t packet;
		mavlink_msg_waypoint_set_current_decode(msg, &packet);
		Serial.print("Packet Sequence:");
		Serial.println(packet.seq);
		if (_checkTarget(packet.target_system, packet.target_component))
			break;

		// set current waypoint
		Serial.print("Current Index:");
		Serial.println(_guide->getCurrentIndex());
		Serial.flush();
		_guide->setCurrentIndex(packet.seq);
		mavlink_msg_waypoint_current_send(_channel,
				_guide->getCurrentIndex());
		break;
	}

	case MAVLINK_MSG_ID_WAYPOINT_COUNT: {
		sendText(SEVERITY_LOW, PSTR("waypoint count"));

		// decode
		mavlink_waypoint_count_t packet;
		mavlink_msg_waypoint_count_decode(msg, &packet);
		if (_checkTarget(packet.target_system, packet.target_component))
			break;

		// start waypoint receiving
		if (packet.count > _cmdMax) {
			packet.count = _cmdMax;
		}
		_cmdNumberRequested = packet.count;
		_cmdTimeLastReceived = millis();
		_receivingCmds = true;
		_sendingCmds = false;
		_cmdRequestIndex = 0;
		break;
	}

	case MAVLINK_MSG_ID_WAYPOINT: {
		sendText(SEVERITY_LOW, PSTR("waypoint"));

		// Check if receiving waypiont
		if (!_receivingCmds) {
			//sendText(SEVERITY_HIGH, PSTR("not receiving commands"));
			break;
		}

		// decode
		mavlink_waypoint_t packet;
		mavlink_msg_waypoint_decode(msg, &packet);
		if (_checkTarget(packet.target_system, packet.target_component))
			break;

		// check if this is the requested waypoint
		if (packet.seq != _cmdRequestIndex) {
			char warningMsg[50];
			sprintf(warningMsg,
			"waypoint request out of sequence: (packet) %d / %d (ap)",
			packet.seq, _cmdRequestIndex);
			sendText(SEVERITY_HIGH, warningMsg);
			break;
		}

		_hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"),
				packet.x,
				packet.y,
				packet.z);

		// store waypoint
		AP_MavlinkCommand command(packet);
		//sendText(SEVERITY_HIGH, PSTR("waypoint stored"));
		_cmdRequestIndex++;
		if (_cmdRequestIndex == _cmdNumberRequested) {
			sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
			_receivingCmds = false;
			_guide->setNumberOfCommands(_cmdNumberRequested);
			//sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
		} else if (_cmdRequestIndex > _cmdNumberRequested) {
			_receivingCmds = false;
		}
		_cmdTimeLastReceived = millis();
		break;
	}

	case MAVLINK_MSG_ID_PARAM_SET: {
		sendText(SEVERITY_LOW, PSTR("param set"));
		AP_Var *vp;
		AP_Meta_class::Type_id var_type;

		// decode
		mavlink_param_set_t packet;
		mavlink_msg_param_set_decode(msg, &packet);
		if (_checkTarget(packet.target_system, packet.target_component))
			break;

		// set parameter

		char key[_paramNameLengthMax + 1];
		strncpy(key, (char *) packet.param_id, _paramNameLengthMax);
		key[_paramNameLengthMax] = 0;

		// find the requested parameter
		vp = AP_Var::find(key);
		if ((NULL != vp) && // exists
				!isnan(packet.param_value) && // not nan
				!isinf(packet.param_value)) { // not inf

			// add a small amount before casting parameter values
			// from float to integer to avoid truncating to the
			// next lower integer value.
			const float rounding_addition = 0.01;

			// fetch the variable type ID
			var_type = vp->meta_type_id();

			// handle variables with standard type IDs
			if (var_type == AP_Var::k_typeid_float) {
				((AP_Float *) vp)->set_and_save(packet.param_value);

			} else if (var_type == AP_Var::k_typeid_float16) {
				((AP_Float16 *) vp)->set_and_save(packet.param_value);

			} else if (var_type == AP_Var::k_typeid_int32) {
				((AP_Int32 *) vp)->set_and_save(
						packet.param_value + rounding_addition);

			} else if (var_type == AP_Var::k_typeid_int16) {
				((AP_Int16 *) vp)->set_and_save(
						packet.param_value + rounding_addition);

			} else if (var_type == AP_Var::k_typeid_int8) {
				((AP_Int8 *) vp)->set_and_save(
						packet.param_value + rounding_addition);
			} else {
				// we don't support mavlink set on this parameter
				break;
			}

			// Report back the new value if we accepted the change
			// we send the value we actually set, which could be
			// different from the value sent, in case someone sent
			// a fractional value to an integer type
			mavlink_msg_param_value_send(_channel, (int8_t *) key,
					vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is...
		}

		break;
	} // end case


	}
}

uint16_t MavlinkComm::_countParameters() {
	// if we haven't cached the parameter count yet...
	if (0 == _parameterCount) {
		AP_Var *vp;

		vp = AP_Var::first();
		do {
			// if a parameter responds to cast_to_float then we are going to be able to report it
			if (!isnan(vp->cast_to_float())) {
				_parameterCount++;
			}
		} while (NULL != (vp = vp->next()));
	}
	return _parameterCount;
}

AP_Var * _findParameter(uint16_t index) {
	AP_Var *vp;

	vp = AP_Var::first();
	while (NULL != vp) {

		// if the parameter is reportable
		if (!(isnan(vp->cast_to_float()))) {
			// if we have counted down to the index we want
			if (0 == index) {
				// return the parameter
				return vp;
			}
			// count off this parameter, as it is reportable but not
			// the one we want
			index--;
		}
		// and move to the next parameter
		vp = vp->next();
	}
	return NULL;
}

// check the target
uint8_t MavlinkComm::_checkTarget(uint8_t sysid, uint8_t compid) {
	/*
	 char msg[50];
	 sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid,
	 mavlink_system.sysid, compid, mavlink_system.compid);
	 sendText(SEVERITY_LOW, msg);
	 */
	if (sysid != mavlink_system.sysid) {
		//sendText(SEVERITY_LOW, PSTR("system id mismatch"));
		return 1;

	} else if (compid != mavlink_system.compid) {
		//sendText(SEVERITY_LOW, PSTR("component id mismatch"));
		return 0; // XXX currently not receiving correct compid from gcs

	} else {
		return 0; // no error
	}
}

} // apo