/**
 *  \brief    Wiimote handling implementation
 *  \author   Aurelien Morelle <aurelien.morelle@parrot.com>
 *  \version  1.0
 *  \date     04/06/2007
 */

#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <stddef.h>
#include <stdio.h>
#include <unistd.h>

#include <generated_custom.h>

#if defined(WIIMOTE_SUPPORT)
	//#warning Wiimote support enabled
	#include <bluetooth/bluetooth.h>
	#include <cwiid.h>
#else
	//#warning Wiimote support disabled
#endif

#include "wiimote.h"

input_device_t wiimote_device = {
  "Wiimote",
  open_wiimote,
  update_wiimote,
  close_wiimote
};


#if !defined(WIIMOTE_SUPPORT)
//#warning Wiimote compilation disabled
C_RESULT open_wiimote(void)
{
  return C_FAIL;
}

C_RESULT update_wiimote(void)
{
  return C_FAIL;
}

C_RESULT close_wiimote(void)
{
  return C_OK;
}

#else // ! WIIMOTE_SUPPORT
#warning Wiimote compilation enabled

#include <VP_Os/vp_os_print.h>

static bdaddr_t bdaddr = {0};
static cwiid_wiimote_t *wiimote = NULL;

static struct acc_cal wm_cal;

C_RESULT open_wiimote(void)
{
  C_RESULT res = C_OK;

  printf("\nSearching for Wiimote - press both '1' and '2' on your Wiimote !!\n");

  if(cwiid_find_wiimote(&bdaddr, WIIMOTE_FIND_TIMEOUT) == 0)
  {
    if( ! (wiimote = cwiid_open(&bdaddr, 0)) )
      PRINT("Unable to connect to wiimote\n");
    else
      res = C_OK;
  }

  if(SUCCEED(res))
    if(cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN|CWIID_RPT_ACC))
      res = C_FAIL;

  if(SUCCEED(res))
    if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &wm_cal))
      res = C_FAIL;

  cwiid_set_led(wiimote,1);

  printf(" ... Wiimote found.\n");

  return res;
}

void print_state(struct cwiid_state *state)
{
  printf("Battery: %d%%\n", (int)(100.0 * state->battery / CWIID_BATTERY_MAX));
  printf("Buttons: %X\n", state->buttons);
  printf("Acc: x=%d y=%d z=%d\n", state->acc[CWIID_X], state->acc[CWIID_Y], state->acc[CWIID_Z]);
}

//extern void api_set_iphone_acceleros(bool_t enable, float32_t fax, float32_t fay, float32_t faz);

#define NEW_AMOUNT 0.1
#define OLD_AMOUNT (1.0-NEW_AMOUNT)

typedef struct{
	float x,y,z;
}vec3;

typedef struct{
	float r,p,t;
}vec3sph;


static void rumble(int flag)
		{
		static int localflag=0;
		if (flag!=localflag){
			localflag=flag;
			cwiid_set_rumble(wiimote,flag);
		}
}

static void leds(int flags)
{
		static int localflags=0;
		if (flags!=localflags){
			localflags=flags;
			cwiid_set_led(wiimote,flags);
		}
}

C_RESULT update_wiimote(void)
{
  C_RESULT res = C_OK;
  static struct cwiid_state state,previous_state;

  static int control_mode = 1;
  static int valid_domain = 0;

  int8_t x, y;
  static int start=0;
  static int select=0;

  static vec3 a={0,0,0},s={0,0,0};
  static vec3sph r={0,0,0},pr={0,0,0},rref={0,0,0};

  static float pitch=0.0f,roll=0.0f,gaz=0.0f,yaw=0.0f;

  float tmp;
  static int fly=0;



  if (cwiid_get_state(wiimote, &state))
    {
      fprintf(stderr, "Error getting state\n");
      res = C_FAIL;
    }


#define SWITCHING(X) ((state.buttons&X) && !(previous_state.buttons&X))
#define RELEASING(X) ((!state.buttons&X) && (previous_state.buttons&X))
#define PRESSED(X) ((state.buttons&X))
  static int flag_rumble=0;
#define RUMBLE_ON { if (!flag_rumble) {flag_rumble=1;  } }
  /* Sets how to use the wiimote */
#define MAXMODE 4
  if (SWITCHING(CWIID_BTN_MINUS)) {
	  control_mode--;
	  if (control_mode<1) control_mode=MAXMODE;
	  leds(1<<(control_mode-1));
  }
  if (SWITCHING(CWIID_BTN_PLUS))  {
	  control_mode++;
	  if (control_mode>MAXMODE) control_mode=1;
	  leds(1<<(control_mode-1));
  }

 /* Gets gravitation G projection on x,y,z axis */
	  a.x = - (float32_t) ((((double)state.acc[CWIID_X] - wm_cal.zero[CWIID_X]) /
				  (wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X])));
	  a.y = - (float32_t) ((((double)state.acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) /
				  (wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y])));
	  a.z = + (float32_t) ((((double)state.acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) /
			  (wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z])));

	  s.x = (a.x<0.0f)?(1.0f):(-1.0f);
	  s.y = (a.y<0.0f)?(1.0f):(-1.0f);
	  s.z = (a.z<0.0f)?(1.0f):(-1.0f);

	  float ax2 = a.x*a.x;
	  float ay2 = a.y*a.y;
	  float az2 = a.z*a.z;

	  r.r = sqrtf((ax2+ay2)+az2);

	  switch(control_mode)
	  {
	  case 1:
	  case 2:

	  	  if (r.r==0.0f) { r.p=r.t=0.0f; } else {
	  		  // Angle gauche/droite
	  		  r.p = asin(a.y);  if (isnan(r.p)) r.p=0.0f;

			  // Sur plan vertical radial
				  r.t = acos(a.z/(sqrtf(az2+ax2)));  if (isnan(r.t)) r.t=0.0f;
				  r.t*=s.x;
	  	  }
	  	  break;

	  case 3:
	  case 4:

	  	  	  if (r.r==0.0f) { r.p=r.t=0.0f; } else {
	  	  		  // Angle entre le projete de G sur le plan vertical longitudinal (yz) et l'axe z
	  				  r.p = acos(a.z/(sqrtf(az2+ay2)));  if (isnan(r.p)) r.p=0.0f;
	  				  /* If wiimote faces the ground */
	  				    //if (a.z<0.0f) r.p= M_PI-r.p;
	  				  r.p*=s.y;
	  			  // Idem sur le plan vertical radial
	  				  r.t = acos(a.z/(sqrtf(az2+ax2)));  if (isnan(r.t)) r.t=0.0f;
	  				  r.t*=s.x;
	  	  	  }
	  	  	  break;
	    }

	  r.r = (r.r+pr.r)/2.0f;
	  r.t = (r.t+pr.t)/2.0f;
	  r.p = (r.p+pr.p)/2.0f;



  switch(control_mode)
  {

		  case 1:
		  case 2:
			  /* Wiimote is handled horizontally.
			   * '2' button under left thumb
			   * directionnal cross under right thumb
			   */


			  /* 0 -> buttons facing sky */

			  if ((SWITCHING(CWIID_BTN_1)||SWITCHING(CWIID_BTN_2)||SWITCHING(CWIID_BTN_B))){  rref=r;  }

			  if (PRESSED(CWIID_BTN_1)||PRESSED(CWIID_BTN_2)||PRESSED(CWIID_BTN_B))
			  {
				  /* If wiimote facing ground */
				  if (a.z<0 && a.x>0)
				  {
					  rumble(1);
				  }
				  else
				  {
					  rumble(0);
					  leds(1<<(control_mode-1));
					  pitch = (r.t-rref.t)*1.0f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
					  roll  = -(r.p-rref.p)*0.75f; if (roll<-1.0f)  roll=-1.0f; if (roll>1.0f) roll=1.0f;
					  fly=1;
				  }
			  }
			  else
			  {
				  pitch=roll=0;
				  rumble(0);
				  leds(1<<(control_mode-1));
				  fly=0;
			  }


			  gaz  = (PRESSED(CWIID_BTN_LEFT))? 1.0f: (PRESSED(CWIID_BTN_RIGHT)) ? -1.0f : 0.0f;
			  yaw  = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP))   ? 1.0f : (control_mode==2) ? (-pitch*roll) : 0.0f;

			  break;


		  case 3:
		  case 4:

			  if (PRESSED(CWIID_BTN_B))
 			  {
				  if (a.z<-0.5f)
				  {
					  rumble(1);
				  }
				  else
				  {
					  rumble(0);
					  leds(1<<(control_mode-1));
					  pitch = -(r.p-rref.p)*1.5f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
					  roll  = (r.t-rref.t)*1.5f; if (roll<-1.0f)  roll=-1.0f; if (roll>1.0f) roll=1.0f;
					  fly=1;
				  }
			  }
			  else
				  {
				  	 rumble(0);
				     leds(1<<(control_mode-1));
				  	 fly=0;
				  	 pitch=roll=0;

				  }

			  if (SWITCHING(CWIID_BTN_B))  { rref = r; }

			  gaz  = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP)) ? +1.0f : 0.0f;
			  yaw  = (PRESSED(CWIID_BTN_LEFT))? -1.0f: (PRESSED(CWIID_BTN_RIGHT))   ? 1.0f : (control_mode==4) ? (-pitch*roll) : 0.0f;

  }
  /* Buttons common to all modes */
  	  if (SWITCHING(CWIID_BTN_A)) { start^=1;  ardrone_tool_set_ui_pad_start(start); }
  	  if (SWITCHING(CWIID_BTN_HOME)) {
  				  				  select^=1;
  				  				  ardrone_tool_set_ui_pad_select(select);
  				  				  ardrone_tool_set_ui_pad_start(0);
  				  			  }


	  //
	//printf("Wiimote mode 2 [ax %f][ay %f][az %f]\n\033[1A", a.x,a.y,a.z);
  	printf("Wiimode %i  [rr %3.2f][rp %3.2f][rt %3.2f]\n", control_mode,r.r,r.p,r.t);
  	printf("[Fly %i][Ptc %3.2f][Rl %3.2f][Yw %3.2f][Gz %3.2f][Strt. %i][Sel. %i]\n\033[2A", fly,pitch,roll,yaw,gaz,start,select);
  	ardrone_tool_set_progressive_cmd( fly,/*roll*/roll,/*pitch*/pitch,/*gaz*/gaz,/*yaw*/yaw,0.0,0.0);

  /*api_set_iphone_acceleros(
			   (state.buttons&CWIID_BTN_2 ? 1 : 0)|(state.buttons&CWIID_BTN_A ? 2 : 0),
			   a_x, a_y, a_z);*/

  previous_state = state;
  pr=r;

  return C_OK;
}

C_RESULT close_wiimote(void)
{
  cwiid_close(wiimote);

  return C_OK;
}

#endif // ! WIIMOTE_SUPPORT

