#include "motormixer.h"

#include <ch.h>
#include <hal.h>

template<class T>
inline T abs(T val) {
	if(val > 0) {
		return val;
	} else {
		return -val;
	}
}

namespace {
	uint16_t motors[4];
	
	void foo(PWMDriver*) {
		pwmEnableChannel(&PWMD2, 0, 1000 + motors[0]);
		pwmEnableChannel(&PWMD2, 1, 1000 + motors[1]);
		pwmEnableChannel(&PWMD2, 2, 1000 + motors[2]);
		pwmEnableChannel(&PWMD2, 3, 1000 + motors[3]);
	}

	static PWMConfig pwmcfg = {
		1000000,
		1000000 / 50,
		foo,
		{
			{PWM_OUTPUT_ACTIVE_HIGH, NULL},
			{PWM_OUTPUT_ACTIVE_HIGH, NULL},
			{PWM_OUTPUT_ACTIVE_HIGH, NULL},
			{PWM_OUTPUT_ACTIVE_HIGH, NULL}
		},
		0
	};
};

void MotorMixer::start() {
	pwmStart(&PWMD2, &pwmcfg);
	palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	palSetPadMode(GPIOA, 2, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	palSetPadMode(GPIOA, 3, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	
	pwmEnableChannel(&PWMD2, 0, 1000);
	pwmEnableChannel(&PWMD2, 1, 1000);
	pwmEnableChannel(&PWMD2, 2, 1000);
	pwmEnableChannel(&PWMD2, 3, 1000);
}

void MotorMixer::update(int16_t thrust, int16_t pitch, int16_t roll, int16_t yaw) {
	//thrust = ppmsum.data[2];
	if(thrust < 0) {
		thrust = 0;
	}
	if(thrust > 1000) {
		thrust = 1000;
	}
	
	// Check that total thrust will not be exceeded.
	if(abs(pitch) + abs(roll) > thrust) {
		// If saturated by pitch/roll, drop yaw component.
		yaw = 0;
		
		// Scale pitch and roll.
		int32_t df = ((abs(pitch) + abs(roll)) << 16) / thrust;
		
		pitch = (pitch << 16) / df;
		roll = (roll << 16) / df;
	
	} else if(abs(pitch) + abs(roll) + abs(yaw) > thrust) {
		// Scale yaw value.
		yaw = (yaw > 0 ? 1 : -1) * (thrust - abs(pitch) - abs(roll));
	}
	
	motors[0] = thrust + pitch + roll - yaw;
	motors[1] = thrust + pitch - roll + yaw;
	motors[2] = thrust - pitch + roll + yaw;
	motors[3] = thrust - pitch - roll - yaw;
	
	//sensordata[20] = motor_1;
	//sensordata[21] = motor_2;
	//sensordata[22] = motor_3;
	//sensordata[23] = motor_4;
}