// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

#include <AP_Mount.h>

extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions];	// the aux. servo ch. assigned to each function

AP_Mount::AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs):
_gps(gps)
{
	_ahrs = ahrs;
	_current_loc = current_loc;

	//set_mode(MAV_MOUNT_MODE_RETRACT);
	set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink
	//set_mode(MAV_MOUNT_MODE_GPS_POINT); // FIXME: this is to test ONLY targeting

	_retract_angles.x=0;
	_retract_angles.y=0;
	_retract_angles.z=0;
}

//sets the servo angles for retraction, note angles are * 100
void AP_Mount::set_retract_angles(int roll, int pitch, int yaw)
{
	_retract_angles.x=roll;
	_retract_angles.y=pitch;
	_retract_angles.z=yaw;
}

//sets the servo angles for neutral, note angles are * 100
void AP_Mount::set_neutral_angles(int roll, int pitch, int yaw)
{
	_neutral_angles.x=roll;
	_neutral_angles.y=pitch;
	_neutral_angles.z=yaw;
}

//sets the servo angles for MAVLink, note angles are * 100
void AP_Mount::set_mavlink_angles(int roll, int pitch, int yaw)
{
	_mavlink_angles.x = roll;
	_mavlink_angles.y = pitch;
	_mavlink_angles.z = yaw;
}

// used to tell the mount to track GPS location
void AP_Mount::set_GPS_target_location(Location targetGPSLocation)
{
	_target_GPS_location=targetGPSLocation;
}

// This one should be called periodically
void AP_Mount::update_mount_position()
{
	switch(_mount_mode)
	{
		// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
		case MAV_MOUNT_MODE_RETRACT:
			_roll_angle =100*_retract_angles.x;
			_pitch_angle=100*_retract_angles.y;
			_yaw_angle  =100*_retract_angles.z;
			break;

		// move mount to a neutral position, typically pointing forward
		case MAV_MOUNT_MODE_NEUTRAL:
			_roll_angle =100*_neutral_angles.x;
			_pitch_angle=100*_neutral_angles.y;
			_yaw_angle  =100*_neutral_angles.z;
			break;

		// point to the angles given by a mavlink message
		case MAV_MOUNT_MODE_MAVLINK_TARGETING:
		{
			_roll_control_angle  = _mavlink_angles.x;
			_pitch_control_angle = _mavlink_angles.y;
			_yaw_control_angle   = _mavlink_angles.z;
			calculate();
			break;
		}

		// RC radio manual angle control, but with stabilization from the AHRS
		case MAV_MOUNT_MODE_RC_TARGETING:
		{
			G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle);
			G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle);
			G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle);
			if (_ahrs){
				calculate();
			} else {
				if (g_rc_function[RC_Channel_aux::k_mount_roll])
					_roll_angle  = rc_map(g_rc_function[RC_Channel_aux::k_mount_roll]);
				if (g_rc_function[RC_Channel_aux::k_mount_pitch])
					_pitch_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_pitch]);
				if (g_rc_function[RC_Channel_aux::k_mount_yaw])
					_yaw_angle   = rc_map(g_rc_function[RC_Channel_aux::k_mount_yaw]);
			}
			break;
		}

		// point mount to a GPS point given by the mission planner
		case MAV_MOUNT_MODE_GPS_POINT:
		{
			if(_gps->fix){
				calc_GPS_target_angle(&_target_GPS_location);
				calculate();
			}
			break;
		}
		default:
			//do nothing
			break;
	}

	// write the results to the servos
/*
	G_RC_AUX(k_mount_roll)->angle_out(_roll_angle);
	G_RC_AUX(k_mount_pitch)->angle_out(_pitch_angle);
	G_RC_AUX(k_mount_yaw)->angle_out(_yaw_angle);
*/
	// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
	G_RC_AUX(k_mount_roll)->closest_limit(_roll_angle/10);
	G_RC_AUX(k_mount_pitch)->closest_limit(_pitch_angle/10);
	G_RC_AUX(k_mount_yaw)->closest_limit(_yaw_angle/10);
}

void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
{
	_mount_mode=mode;
}

// Change the configuration of the mount
// triggered by a MavLink packet.
void AP_Mount::configure_msg(mavlink_message_t* msg)
{
	__mavlink_mount_configure_t packet;
	mavlink_msg_mount_configure_decode(msg, &packet);
	if (mavlink_check_target(packet.target_system, packet.target_component)) {
		// not for us
		return;
	}
	set_mode((enum MAV_MOUNT_MODE)packet.mount_mode);
	_stab_pitch = packet.stab_pitch;
	_stab_roll  = packet.stab_roll;
	_stab_yaw   = packet.stab_yaw;
}

// Control the mount (depends on the previously set mount configuration)
// triggered by a MavLink packet.
void AP_Mount::control_msg(mavlink_message_t *msg)
{
	__mavlink_mount_control_t packet;
	mavlink_msg_mount_control_decode(msg, &packet);
	if (mavlink_check_target(packet.target_system, packet.target_component)) {
		// not for us
		return;
	}

	switch (_mount_mode)
	{
	case MAV_MOUNT_MODE_RETRACT:  // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
		set_retract_angles(packet.input_b, packet.input_a, packet.input_c);
		if (packet.save_position)
		{
			// TODO: Save current trimmed position on EEPROM
		}
		break;

	case MAV_MOUNT_MODE_NEUTRAL:  //  Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM
		set_neutral_angles(packet.input_b, packet.input_a, packet.input_c);
		if (packet.save_position)
		{
			// TODO: Save current trimmed position on EEPROM
		}
		break;

	case MAV_MOUNT_MODE_MAVLINK_TARGETING:  // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
		set_mavlink_angles(packet.input_b, packet.input_a, packet.input_c);
		break;

	case MAV_MOUNT_MODE_RC_TARGETING:  // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
		break;

	case MAV_MOUNT_MODE_GPS_POINT:  // Load neutral position and start to point to Lat,Lon,Alt
		Location targetGPSLocation;
		targetGPSLocation.lat = packet.input_a;
		targetGPSLocation.lng = packet.input_b;
		targetGPSLocation.alt = packet.input_c;
		set_GPS_target_location(targetGPSLocation);
		break;
	}
}

// Return mount status information (depends on the previously set mount configuration)
// triggered by a MavLink packet.
void AP_Mount::status_msg(mavlink_message_t *msg)
{
	__mavlink_mount_status_t packet;
	mavlink_msg_mount_status_decode(msg, &packet);
	if (mavlink_check_target(packet.target_system, packet.target_component)) {
		// not for us
		return;
	}

	switch (_mount_mode)
	{
	case MAV_MOUNT_MODE_RETRACT:			// safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
	case MAV_MOUNT_MODE_NEUTRAL:			// neutral position (Roll,Pitch,Yaw) from EEPROM
	case MAV_MOUNT_MODE_MAVLINK_TARGETING:	// neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
	case MAV_MOUNT_MODE_RC_TARGETING:		// neutral position and start RC Roll,Pitch,Yaw control with stabilization
		packet.pointing_b = _roll_angle;	///< degrees*100
		packet.pointing_a = _pitch_angle;	///< degrees*100
		packet.pointing_c = _yaw_angle;		///< degrees*100
		break;
	case MAV_MOUNT_MODE_GPS_POINT:         // neutral position and start to point to Lat,Lon,Alt
		packet.pointing_a = _target_GPS_location.lat;	///< latitude
		packet.pointing_b = _target_GPS_location.lng;	///< longitude
		packet.pointing_c = _target_GPS_location.alt;	///< altitude
		break;
	}

	// status reply
	// TODO: is COMM_3 correct ?
	mavlink_msg_mount_status_send(MAVLINK_COMM_3, packet.target_system, packet.target_component,
			packet.pointing_a, packet.pointing_b, packet.pointing_c);
}

// Set mount point/region of interest, triggered by mission script commands
void AP_Mount::set_roi_cmd()
{
	// TODO get the information out of the mission command and use it
}

// Set mount configuration, triggered by mission script commands
void AP_Mount::configure_cmd()
{
	// TODO get the information out of the mission command and use it
}

// Control the mount (depends on the previously set mount configuration), triggered by mission script commands
void AP_Mount::control_cmd()
{
	// TODO get the information out of the mission command and use it
}

void
AP_Mount::calc_GPS_target_angle(struct Location *target)
{
	float GPS_vector_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)/(t7*2.0)))*.01113195;
	float GPS_vector_y = (target->lat-_current_loc->lat)*.01113195;
	float GPS_vector_z = (target->alt-_current_loc->alt);             // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
	float target_distance = 100.0*sqrt(GPS_vector_x*GPS_vector_x + GPS_vector_y*GPS_vector_y);  // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
	_roll_control_angle   = 0;
	_pitch_control_angle  = atan2(GPS_vector_z, target_distance);
	_yaw_control_angle    = atan2(GPS_vector_x, GPS_vector_y);
	// Converts +/- 180 into 0-360.
	if(_yaw_control_angle<0){
		_yaw_control_angle += 2*M_PI;
	}
}

// Auto-detect the mount gimbal type depending on the functions assigned to the servos
void
AP_Mount::update_mount_type()
{
	if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
	{
		_mount_type = k_pan_tilt;
	}
	if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL))
	{
		_mount_type = k_tilt_roll;
	}
	if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
	{
		_mount_type = k_pan_tilt_roll;
	}
}

// Inputs desired _roll_control_angle, _pitch_control_angle and _yaw_control_angle stabilizes them relative to the airframe
// and calculates output _roll_angle, _pitch_angle and _yaw_angle
void
AP_Mount::calculate()
{
	Matrix3f m;             ///< holds 3 x 3 matrix, var is used as temp in calcs
	Matrix3f cam;           ///< Rotation matrix earth to camera. Desired camera from input.
	Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
	float roll;
	float pitch;
	float yaw;
	if (_ahrs){
		m = _ahrs->get_dcm_matrix();
		m.transpose();
		cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle);
		gimbal_target = m * cam;
		gimbal_target.to_euler(&roll, &pitch, &yaw);
		_roll_angle  = degrees(roll)*100;
		_pitch_angle = degrees(pitch)*100;
		_yaw_angle   = degrees(yaw)*100;
	}
}

// This function is needed to let the HIL code compile
long
AP_Mount::rc_map(RC_Channel_aux* rc_ch)
{
	return (rc_ch->radio_in - rc_ch->radio_min) * (rc_ch->angle_max - rc_ch->angle_min) / (rc_ch->radio_max - rc_ch->radio_min) + rc_ch->angle_min;
}

// For testing and development. Called in the medium loop.
void
AP_Mount::debug_output()
{ 	Serial3.print("current   -     ");
	Serial3.print("lat ");
	Serial3.print(_current_loc->lat);
	Serial3.print(",lon ");
	Serial3.print(_current_loc->lng);
	Serial3.print(",alt ");
	Serial3.println(_current_loc->alt);

	Serial3.print("gps       -     ");
	Serial3.print("lat ");
	Serial3.print(_gps->latitude);
	Serial3.print(",lon ");
	Serial3.print(_gps->longitude);
	Serial3.print(",alt ");
	Serial3.print(_gps->altitude);
	Serial3.println();

	Serial3.print("target   -      ");
	Serial3.print("lat ");
	Serial3.print(_target_GPS_location.lat);
	Serial3.print(",lon ");
	Serial3.print(_target_GPS_location.lng);
	Serial3.print(",alt ");
	Serial3.print(_target_GPS_location.alt);
	Serial3.print(" hdg to targ ");
	Serial3.print(degrees(_yaw_control_angle));
	Serial3.println();
}