#include "motormixer.h"

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

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;
	}
	
	//pitch = ((ppmsum.data[1] - 500) * thrust) >> 10;
	//roll  = ((ppmsum.data[0] - 500) * thrust) >> 10;
	//yaw   = ((ppmsum.data[3] - 500) * thrust) >> 10;
	
	//pitch = (-i2c_thread.x) >> 5;
	//roll = i2c_thread.y >> 5;
	//yaw = i2c_thread.z >> 5;
	
	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;
}