/* 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 "wmp.h"

#define TWO_PI 6.28318531f
#define PI 3.14159265f

#define MIN_X 15830
#define MAX_X 28300
#define CENTRE_X 22100

#define MIN_Y 18530
#define MAX_Y 28200
#define CENTRE_Y 22100

#define MIN_Z 15800
#define MAX_Z 28304
#define CENTRE_Z 22100

#define MIN_THR 16500
#define MAX_THR 28275

#define MIN_REAL_THR 15830

#define CENTRE_ZONE 100

/* 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;

bool armed = FALSE;

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);
}

void stick_input(void) {
	float x, y, z, throttle;
	if (timer_allvalid()) {
		x = timer_input(0);
		y = timer_input(1);
		throttle = timer_input(2);
		z = timer_input(3);

		if (!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)) &&
			    (wmp_zero == FALSE)) {
				putstr("ARMED!!!\r\n");
				armed = TRUE;
			}

		}

		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;
	}

	motor_set_throttle(throttle);

	/* 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++;
}