/*
 *	wiiuse
 *
 *	Written By:
 *		Michal Wiedenbauer	< shagkur >
 *		Dave Murphy			< WinterMute >
 *		Hector Martin		< marcan >
 *		Cesar Mauri
 *
 *	Copyright 2009 - 11
 *
 *	This file is part of wiiuse, fWIIne and SITPLUS.
 *
 *	This program is free software; you can redistribute it and/or modify
 *	it under the terms of the GNU General Public License as published by
 *	the Free Software Foundation; either version 3 of the License, or
 *	(at your option) any later version.
 *
 *	This program is distributed in the hope that it will be useful,
 *	but WITHOUT ANY WARRANTY; without even the implied warranty of
 *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *	GNU General Public License for more details.
 *
 *	You should have received a copy of the GNU General Public License
 *	along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *	$Header$
 *
 */

#include <stdio.h>
#include <sys/types.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#ifdef WIN32
	#include <Winsock2.h>
#endif

#include "definitions.h"
#include "wiiuse_internal.h"
#include "dynamics.h"
#include "events.h"
#include "io.h"

/* motion plus scaling factors for slow/normal and fast motion */
#define SLOW_C 0.0726318359375f
#define FAST_C 0.33014470880681818181818181818182f

/* motion plus calibration */
#define CALIBRATION_ZERO 8063				/* theorical motion plus zero */
#define CALIBRATION_OFFSET 250				/* acceptance offset*/
#define CALIBRATION_MAX_DEVIATION	5.0f	/* max. deviation */

void wiiuse_motion_plus_check(struct wiimote_t *wm,byte *data,unsigned short len)
{
	int val;
	if(data == NULL)
	{
	    wiiuse_read_data_cb(wm, wiiuse_motion_plus_check, wm->motion_plus_id, WM_EXP_ID, 6);
	}
	else
	{
		WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
		WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
		WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
		val = (data[3] << 16) | (data[2] << 24) | (data[4] << 8) | data[5];
		if(val == EXP_ID_CODE_MOTION_PLUS)
		{
			/* handshake done */
			wm->event = WIIUSE_MOTION_PLUS_ACTIVATED;
			memset (&wm->exp.mp, 0, sizeof(struct motion_plus_t));
			wm->exp.type = EXP_MOTION_PLUS;
			wm->exp.mp.rx_zero= CALIBRATION_ZERO;
			wm->exp.mp.ry_zero= CALIBRATION_ZERO;
			wm->exp.mp.rz_zero= CALIBRATION_ZERO;
			/* adjust counter to indicate if auto-calibration should be used */
			if (!(wm->flags & WIIUSE_MP_AUTO_CALIBRATION)) 
				wm->exp.mp.calib_buff_count= MOTION_PLUS_CALIB_BUFF_SIZE+1;
			WIIUSE_DEBUG("Motion plus connected");			
			WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP);
			wiiuse_set_ir_mode(wm);
		}
	}
}

static void wiiuse_set_motion_plus_clear2(struct wiimote_t *wm,byte *data,unsigned short len)
{
	WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
	WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
	WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
	wiiuse_set_ir_mode(wm);
	wiiuse_status(wm);
}

static void wiiuse_set_motion_plus_clear1(struct wiimote_t *wm,byte *data,unsigned short len)
{
	ubyte val = 0x00;
	wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_set_motion_plus_clear2);
}


void wiiuse_set_motion_plus(struct wiimote_t *wm, int status)
{
	ubyte val;
	
	if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE))
		return;

	if (!status && wm->exp.type!= EXP_MOTION_PLUS) return;
	if (status && wm->exp.type== EXP_MOTION_PLUS) return;

	WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
	if(status)
	{
		val = 0x04;
		wiiuse_write_data_cb(wm, WM_EXP_MOTION_PLUS_ENABLE, &val, 1, wiiuse_motion_plus_check);
	}
	else
	{
		disable_expansion(wm);
		val = 0x55;
		wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_set_motion_plus_clear1);
	}
}

void motion_plus_disconnected(struct motion_plus_t* mp)
{
	WIIUSE_DEBUG("Motion plus disconnected");
	memset(mp, 0, sizeof(struct motion_plus_t));
}

/* compute average of a set of values */
static
float average (short * values, int nvalues)
{
	int i;
	int accum= 0;

	for (i = 0; i< nvalues; ++i) accum+= values[i];

	return (float) accum / (float) nvalues;
}

/* compute the average absolute deviation of a set of values */
static
float avg_abs_deviation (short * values, int nvalues, float average)
{
	int i;
	float accum= 0.0f;

	for (i = 0; i< nvalues; ++i) accum+= fabs(((float) values[i]) - average);		

	return accum / (float) nvalues;
}

void motion_plus_event(struct motion_plus_t* mp, byte* msg)
{
	short rx, ry, rz;
	unsigned char xslow, yslow, zslow;

	/*
	   Byte 7 6 5 4 3 2            1              0
	   0    Yaw Down Speed<7:0>
	   1    Roll Left Speed<7:0>
	   2    Pitch Left Speed<7:0>
	   3    Yaw Down Speed<13:8>   Yaw slow mode  Pitch slow mode
	   4    Roll Left Speed<13:8>  Roll slow mode Extension connected
	   5    Pitch Left Speed<13:8> 1              0

	   See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
	*/

	rx = ((msg[5] & 0xFC) << 6) | msg[2]; // Pitch
	ry = ((msg[4] & 0xFC) << 6) | msg[1]; // Roll
	rz = ((msg[3] & 0xFC) << 6) | msg[0]; // Yaw

	/* filter incorrect values */
	if (rx == 0x3fff || ry == 0x3fff || rz == 0x3fff) return;

	/* store raw values */
	mp->rx = rx;
	mp->ry = ry;
	mp->rz = rz;

	/* store flags */
	mp->ext = msg[4] & 0x1;
	mp->status = (msg[3] & 0x3) | ((msg[4] & 0x2) << 1); // roll, yaw, pitch

	/* convert to deg/s */
	xslow= msg[3] & 0x1;
	yslow= msg[4] & 0x2;
	zslow= msg[3] & 0x2;

	mp->sx= (float) (rx - mp->rx_zero) * (xslow? SLOW_C : FAST_C);
	mp->sy= (float) (ry - mp->ry_zero) * (yslow? SLOW_C : FAST_C);
	mp->sz= (float) (rz - mp->rz_zero) * (zslow? -SLOW_C : -FAST_C);

	/* auto-calibration */
	if (mp->calib_buff_count<= MOTION_PLUS_CALIB_BUFF_SIZE) {		
		/* ongoing calibration */

		/* readings are within a reasonable range? */
		if (!xslow || !yslow || !zslow ||
			rx< (CALIBRATION_ZERO - CALIBRATION_OFFSET) || rx> (CALIBRATION_ZERO + CALIBRATION_OFFSET) ||
			ry< (CALIBRATION_ZERO - CALIBRATION_OFFSET) || ry> (CALIBRATION_ZERO + CALIBRATION_OFFSET) ||
			rz< (CALIBRATION_ZERO - CALIBRATION_OFFSET) || rz> (CALIBRATION_ZERO + CALIBRATION_OFFSET)) {
			/* no, reset values */
			mp->calib_buff_count= 0;
			mp->calib_buff_index= 0;
			return;
		}

		/* store values in calibration buffer */
		mp->rx_hist[mp->calib_buff_index]= rx;
		mp->ry_hist[mp->calib_buff_index]= ry;
		mp->rz_hist[mp->calib_buff_index]= rz;
		++mp->calib_buff_index;
		if (mp->calib_buff_index== MOTION_PLUS_CALIB_BUFF_SIZE) mp->calib_buff_index= 0;
		if (mp->calib_buff_count< MOTION_PLUS_CALIB_BUFF_SIZE) ++mp->calib_buff_count;

		/* when buffer full compute averages and deviations */
		if (mp->calib_buff_count== MOTION_PLUS_CALIB_BUFF_SIZE) {
			float xavg, xdev, yavg, ydev, zavg, zdev;

			xavg= average (mp->rx_hist, MOTION_PLUS_CALIB_BUFF_SIZE);
			xdev= avg_abs_deviation (mp->rx_hist, MOTION_PLUS_CALIB_BUFF_SIZE, xavg);
			if (xdev> CALIBRATION_MAX_DEVIATION) return;

			yavg= average (mp->ry_hist, MOTION_PLUS_CALIB_BUFF_SIZE);
			ydev= avg_abs_deviation (mp->ry_hist, MOTION_PLUS_CALIB_BUFF_SIZE, yavg);
			if (ydev> CALIBRATION_MAX_DEVIATION) return;

			zavg= average (mp->rz_hist, MOTION_PLUS_CALIB_BUFF_SIZE);
			zdev= avg_abs_deviation (mp->rz_hist, MOTION_PLUS_CALIB_BUFF_SIZE, zavg);
			if (zdev> CALIBRATION_MAX_DEVIATION) return;

			/* calibration done, store values */
			mp->rx_zero= (short) (xavg + 0.5f);
			mp->ry_zero= (short) (yavg + 0.5f);
			mp->rz_zero= (short) (zavg + 0.5f);

			/* inc. counter to indicate finished calibration */
			++mp->calib_buff_count;

			WIIUSE_DEBUG("Motion plus auto-calibration done");
		}
	}		
}
