/* motor.c */

#include "stick.h"
#include "timer.h"
#include "dcm.h"
#include "uart.h"

float integral[3] = {0.0f, 0.0f, 0.0f};
float last[3];

float throttle = 0.0f;

#define Kp 0.2
#define Ki 0.04
#define Kd 0.08
#define Ka 0.0

#define Kp_y 0.2
#define Ki_y 0.00
#define Kd_y 0.00
#define Ka_y 0.0


/*
 * Perform a PID loop iteration.
 * roll and pitch are absolute values
 * yaw is, currently, a rate.
 */
void motor_pid_update(float troll, float mroll,
		      float tpitch, float mpitch,
		      float tyaw, float myaw)
{
	float derivative[3];
	float out[3];
	float motor[3];
	float roll, pitch, yaw;
	float error, max_error;
	float min_motor;
	int i;

	roll  = troll  - mroll;
	pitch = tpitch - mpitch;
	yaw   = tyaw   - myaw;

#if 0
        if ((stick_counter % 100) == 0) {
		putstr("{");
		putint_s((int)(tyaw * 10000));
		putstr(", ");
		putint_s((int)(myaw * 10000));
		putstr("}\r\n");
	}
#endif

	integral[0] += roll * delta_t;
	integral[1] += pitch * delta_t;
	integral[2] += yaw * delta_t;

	/* The measurements are the opposite sign to the error */
	derivative[0] = (-mroll  - last[0]) / delta_t;
	derivative[1] = (-mpitch - last[1]) / delta_t;
	derivative[2] = (-myaw   - last[2]) / delta_t;

	last[0] = -mroll;
	last[1] = -mpitch;
	last[2] = -myaw;

	out[0] = roll  * Kp   + integral[0] * Ki   + derivative[0] * Kd;
	out[1] = pitch * Kp   + integral[1] * Ki   + derivative[1] * Kd;
	out[2] = yaw   * Kp_y + integral[2] * Ki_y + derivative[2] * Kd_y;

	if (armed) {
		/* Front right */
		motor[0] = throttle + out[0] + out[1] + out[2];
		/* Front left */
		motor[1] = throttle - out[0] + out[1] - out[2];
		/* Rear left */
		motor[2] = throttle - out[0] - out[1] + out[2];
		/* Rear right */
		motor[3] = throttle + out[0] - out[1] - out[2];
	} else {
		motor[0] = 0.0;
		motor[1] = 0.0;
		motor[2] = 0.0;
		motor[3] = 0.0;
	}

	max_error = 0.0;
	min_motor = 1.0;
	for (i = 0; i < 3; i++) {
		if (motor[i] < 0.0)
			motor[i] = 0.0;
		if (motor[i] > 1.0f) {
			error = motor[i] - 1.0f;
			if (error > max_error)
				max_error = error;
		}
		if (motor[i] < min_motor)
			min_motor = motor[i];
	}

	if (max_error > 0.0) {
		for (i = 0; i < 3; i++) {
			motor[i] -= max_error;
			if (motor[i] < 0.0)
				motor[i] = 0.0;
		}
	}

	if (throttle <= 0.0) {
		motor[0] = 0.0;
		motor[1] = 0.0;
		motor[2] = 0.0;
		motor[3] = 0.0;
		integral[0] = 0.0;
		integral[1] = 0.0;
		integral[2] = 0.0;
	}
	if (max_error < min_motor) {
		float new_throttle2, new_out[3];
		new_throttle2 = (motor[0] + motor[1] + motor[2] + motor[3])/2.0;
		new_out[0] = (motor[0] + motor[3] - new_throttle2)/2.0;
		new_out[1] = (motor[0] + motor[1] - new_throttle2)/2.0;
		new_out[2] = (motor[0] + motor[2] - new_throttle2)/2.0;

		/* Anti-windup */
		for (i = 0; i < 3; i++) {
			if (new_out[i] > 1.0)
				integral[i] -= (new_out[i]-1.0) * Ka;
			if (new_out[i] < 0.0)
				integral[i] -= (new_out[i]) * Ka;
		}
	}

	timer_set_pwm_value(0, (int)(motor[0] * PWM_MAX));
	timer_set_pwm_value(1, (int)(motor[1] * PWM_MAX));
	timer_set_pwm_value(2, (int)(motor[2] * PWM_MAX));
	timer_set_pwm_value(3, (int)(motor[3] * PWM_MAX));
}

void motor_kill(void) {
	throttle = 0.0;
	timer_set_pwm_value(0, 0);
	timer_set_pwm_value(1, 0);
	timer_set_pwm_value(2, 0);
	timer_set_pwm_value(3, 0);
}

void motor_set_throttle(float t) {
	if (armed)
		throttle = t;
}