/* stick.c */

#ifdef WE_HAVE_SQRT
#include <math.h>
#else
#include "fisqrt.h"
#endif
#include "matrix.h"
#include "stick.h"
#include "dcm.h"
#include "uart.h"
#include "timer.h"
#include "trig.h"
#include "motor.h"
#include "status.h"
#include "watchdog.h"
#include "log.h"

#define TWO_PI 6.28318531f
#define PI 3.14159265f

#define MIN_X 8720
#define MAX_X 23800
#define CENTRE_X 16260

#define MIN_Y 8720
#define MAX_Y 23800
#define CENTRE_Y 16260

#define MIN_Z 8720
#define MAX_Z 23800
#define CENTRE_Z 16300

#define MIN_THR 9720
#define MAX_THR 23750

#define MIN_REAL_THR 8720

#define CENTRE_ZONE 100

/* With new TX firmware, CPPM:
 *             x      y    thr      z
 * centre: 16260, 16258, 16000, 16300
 * min:     8720,  8718, 8720,  8722 
 * max:    23790, 23817, 23750, 23803
 */

/* Full scale is a roll/pitch angle of 30 degrees from the vertical */
#define SCALE_X (TWO_PI*30.0/360.0 / (MAX_X-CENTRE_X))
#define SCALE_Y (TWO_PI*30.0/360.0 / (MAX_Y-CENTRE_Y))

/* Full scale is a complete rotation in one second */
#define SCALE_Z (TWO_PI / (MAX_Z-CENTRE_Z))

/* 0 is min throttle, 1 is max throttle */
#define SCALE_THROTTLE (1.0f/(MAX_THR - MIN_THR))

float stick_yaw = 0.0f;

unsigned int stick_counter;

void stick_update(float x, float y, float z)
{
	float tz = delta_t * z;

	stick_yaw += tz;

	if (stick_yaw <= -PI)
	    stick_yaw += TWO_PI;

	if (stick_yaw > PI)
	    stick_yaw -= TWO_PI;

#if 0
	z = stick_yaw;
#endif

	x = sine(x);
	y = sine(y);
#if 0
	z = 1.0/fisqrt(1-x*x-y*y);
#endif

	dcm_attitude_error(x, y, z);
}

#ifdef STICK_DEBUG_CALIBRATE
void stick_debug_calibrate()
{
	unsigned int t1 = timer_input(0);
	unsigned int t2 = timer_input(1);
	unsigned int t3 = timer_input(2);
	unsigned int t4 = timer_input(3);
        putstr("S:(");
        putint(t1);
        putstr(",");
        putint(t2);
        putstr(",");
        putint(t3);
        putstr(",");
        putint(t4);
        putstr(")\r\n");
}
#endif

void stick_input(void) {
	float x, y, z, throttle;
	unsigned int xi, yi, zi, throttlei;

	if (timer_allvalid()) {
		xi = timer_input(0);
		yi = timer_input(1);
		throttlei = timer_input(2);
		zi = timer_input(3);

		log_put_uint16(xi);
		log_put_uint16(yi);
		log_put_uint16(throttlei);
		log_put_uint16(zi);

		x = xi;
		y = yi;
		throttle = throttlei;
		z = zi;

#ifdef STICK_DEBUG_CALIBRATE
		if ((stick_counter % 100) == 0)
			stick_debug_calibrate();
#endif

		if (!status_armed()) {
			if ((throttle < MIN_THR) &&
			    (x > (CENTRE_X - CENTRE_ZONE)) &&
			    (x < (CENTRE_X + CENTRE_ZONE)) &&
			    (y > (CENTRE_Y - CENTRE_ZONE)) &&
			    (y < (CENTRE_Y + CENTRE_ZONE)) &&
			    (z > (CENTRE_Z - CENTRE_ZONE)) &&
			    (z < (CENTRE_Z + CENTRE_ZONE)))
				    status_set_ready(STATUS_MODULE_STICK, TRUE);
			else
				    status_set_ready(STATUS_MODULE_STICK,FALSE);

		}

		x -= CENTRE_X;
		y -= CENTRE_Y;
		z -= CENTRE_Z;
		x = x * SCALE_X;
		y = y * SCALE_Y;
		z = z * SCALE_Z;
		throttle = (throttle - MIN_THR) * SCALE_THROTTLE;
		if (throttle < 0.0)
			throttle = 0.0;
	} else {
		x = 0.0f;
		y = 0.0f;
		z = 0.0f;
		throttle = 0.0f;
		status_set_ready(STATUS_MODULE_STICK,FALSE);
	}

	motor_set_throttle(throttle);

	watchdog_kick(WATCHDOG_STICK);

	/* So the controls are back to front. Let's fix that. */
	x = -x;
	y = -y;
	z = -z;

#if 0
	if ((stick_counter % 100) == 0) {
		putstr("[");
		putint_s((int)(z * 10000));
		putstr("] ");
	}
#endif

#if 1
	stick_update(x, y, z);
#else
	if ((stick_counter % 100) == 0)
		stick_update(x, y, z);
#endif

/*
	if ((stick_counter & 3) == 0)
		stick_send_packet();
*/
	stick_counter++;
}