/**********************************************************************
 * NAME: motormix.c                                                    *
 * AUTHOR: Philip Johansson                                            *
 * PURPOSE: Combine the control system outputs to motor values         *
 * INFORMATION:                                                        *
 * Each control loop has its output which is a combination of error    *
 * in the input unit times some tuned constants. These outputs are     *
 * read by the mixer, combined and scaled into a valid output for      *
 * each motor.                                                         *
 *                                                                     *
 *                                                                     *
 * GLOBAL VARIABLES:                                                   *
 * Variable		Type		Description                                *
 * --------		----		-----------                                *
 * 																	   *
 **********************************************************************/

#include "drivers/motormix.h"
#include "drivers/motors.h"
#include "utilities.h"
#include "drivers/sbus.h"
#include "drivers/failsafe_toggles.h"

/* An illustration of the motor configuration

  	________________|_______________
 	1CW  2CCW 3CCW 	| 4CW  5CCW  6CW
					|
					|
					|
		 ___________|___________
 		 7CCW  8CW	| 9CW  10CCW


 */

/* Set by EEPROM - This variable decides whether the control
 * system should be active or not when throttle is below min_throttle */
//bool pid_at_min_throttle = true;

/* An array containing the calculated motor outputs */
uint16_t motor_output[MOTOR_COUNT];

/* Default values for the mixerConfig */
// TODO: Implement in EEPROM
mixerConfig_s mixerConfig = {
	.minThrottle = 1000,
	.maxThrottle = 2000,
	.minCommand = 1050,
	.maxCommand = 1950,
	.minCheck = 1010,
	.pid_at_min_throttle = true,
	.motorstop = false,
	.yaw_reverse_direction = false
};

/* Used in "mixerUAV" to create the dynamic model of the UAV */
typedef struct {
	float throttle;
	float roll;
	float pitch;
	float yaw;
} motorMixer_s;


/* Table of each motors authority in every degree of freedom
 * Roll is defined positive clockwise seen from the back
 * Pitch is defined positive clockwise seen from left (getting altitude / lifting the nose)
 * Yaw is defined clockwise seen from above
 */
static const motorMixer_s mixerUAV[] = {
	/* Throttle, Roll,   Pitch,  Yaw */

	{	1.0f, 	 1.0f,	 1.0f, 	-1.0f}, //M1
	{	1.0f, 	 1.0f,	 1.0f,	 1.0f}, //M2
	{ 	1.0f, 	 0.0f, 	 1.0f,	 0.0f}, //M3
	{	1.0f, 	 0.0f,	 1.0f,	 0.0f}, //M4
	{ 	1.0f, 	-1.0f,	 1.0f,	 1.0f}, //M5
	{ 	1.0f, 	-1.0f,	 1.0f,	-1.0f}, //M6
	{ 	1.0f, 	 1.0f,	-1.0f,	 1.0f}, //M7
	{ 	1.0f, 	 1.0f,	-1.0f,	-1.0f}, //M8
	{ 	1.0f, 	-1.0f,	-1.0f,	-1.0f}, //M9
	{ 	1.0f, 	-1.0f,	-1.0f,	 1.0f}, //M10
};


/***********************************************************************
* BRIEF: The motormixer                                                *
* INFORMATION: Sums the output from all control loops and adapts the   *
* result to a suitable motor signal                                    *
***********************************************************************/
void mix()
{


	/* Calculate what "hover" could be. Not used now but might be useful later */
	//uint16_t throttleIdle = mixerConfig.minThrottle + (throttleRange / 2);

	int16_t RPY_Mix[MOTOR_COUNT];		// Roll Pitch and Yaw variables array
	uint16_t RPY_Mix_Min = 0; 			// Stores the minimum desired command for any motor
	uint16_t RPY_Mix_Max = 0;			// Maximum desired command for any motor
	uint16_t throttle = sbusChannelData.chan3;	 // Import throttle value from remote

	// Might be used for some debug if necessary
	static bool motorLimitReached;

	if (flags_IsSet_ID(systemFlags_airmode_id)) // TODO: replace with check for Airmode
	{
		for (int i = 0; i < MOTOR_COUNT; i++)
		{
			/* Calculate desired output on each motor from the motor mix table
			 * Calculation is: Output from control system * weight from model for each motor
			 */
			RPY_Mix[i] = \
					PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
					PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
					PID_Out[YAW]	*	mixerUAV[i].yaw		*	((mixerConfig.yaw_reverse_direction) ? -1 : 1);

			// Update min and max values
			if (RPY_Mix[i] > RPY_Mix_Max) RPY_Mix_Max = RPY_Mix[i];
			if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
		}


		uint16_t throttleRange = mixerConfig.maxThrottle - mixerConfig.minThrottle; // The throttle range we have with current defines
		uint16_t RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min; // Range of the desired mixer outputs
		uint16_t throttleMin = mixerConfig.minThrottle;	// Import system variable
		uint16_t throttleMax = mixerConfig.maxThrottle; // Import


		/* Check if we have enough interval for the adjustments */
		// Check if we maxed out
		if (RPY_MixRange > throttleRange)
		{
			motorLimitReached = true; // Yes, we maxed out

			// Create a scale to the current mix for it to fit
			float mixReduction = (float) throttleRange / RPY_MixRange;
			// Apply the scaling to all outputs
			for (int i = 0; i < MOTOR_COUNT; i++)
				RPY_Mix[i] = RPY_Mix[i] * mixReduction;
		}
		// If we have the needed range no scaling is needed
		else
		{
			motorLimitReached = false; // Not used but could be helpfull for debug

			/* Update min and max throttle so we can add the
			 * calculated adjustments and still just max out */
			throttleMin += (RPY_MixRange / 2);
			throttleMax -= (RPY_MixRange / 2);
		}


		// Now we add desired throttle
		for (int i = 0; i < MOTOR_COUNT; i++)
		{
			// Constrain in within the regulation of the mix
			motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax);
			motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
		}
	}
	else // Airmode not active
	{
		int16_t maxMotor = 0;

		for (int i = 0; i < MOTOR_COUNT; i++) // Without airmode this includes throttle
		{
			RPY_Mix[i] = \
					throttle 		*	mixerUAV[i].throttle	+	\
					PID_Out[PITCH] 	* 	mixerUAV[i].pitch		+ 	\
					PID_Out[ROLL] 	*	mixerUAV[i].roll 		+ 	\
					PID_Out[YAW]	*	mixerUAV[i].yaw			*	((mixerConfig.yaw_reverse_direction) ? -1 : 1);

			// Find the maximum motor output
			if (RPY_Mix[i] > maxMotor) maxMotor = RPY_Mix[i];
		}

		int16_t maxThrottleDifference = 0;
		if (maxMotor > mixerConfig.maxThrottle)
			maxThrottleDifference = maxMotor - mixerConfig.maxThrottle;

		// Approach at mixing
		for (int i = 0; i < MOTOR_COUNT; i++)
		{
			RPY_Mix[i] -= maxThrottleDifference; // We reduce the highest overflow on all motors

			RPY_Mix[i] = constrain(RPY_Mix[i], mixerConfig.minThrottle, mixerConfig.maxThrottle);
			if (throttle < mixerConfig.minCheck)
			{
				if (mixerConfig.motorstop)
					RPY_Mix[i] = mixerConfig.minCommand;

				/* Motors set to idle with PIDs not active */
				else if (!mixerConfig.pid_at_min_throttle)
					RPY_Mix[i] = mixerConfig.minThrottle;

				/* Else */
				// Is per default to have the PIDS active.
				// Though authority is very low with low throttle
			}

			// Constrain in within the valid motor outputs
			motor_output[i] = constrain(RPY_Mix[i], mixerConfig.minCommand, mixerConfig.maxCommand);
		}
	}

	// Updating the command to the actuators
	for (int i = 0; i < MOTOR_COUNT; i++)
	{
		/* If engines are armed then give the output to the motors */
		if (flags_IsSet_ID(systemFlags_armed_id)) // TODO: replace with check for armed (IF ARMED)
			motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
		/* If not then stop motors */
		else
			motor_output[i] = mixerConfig.minThrottle;

		/* Update actuators */
		pwmAdjustSpeedOfMotor( i + 1 /* Motors start from Motor 1 */,motor_output[i]);
	}
}