//
//  AdvancedMathCCode.m
//  WiiToMidi
//
//  Created by Mike Verdone on 28/03/07.
//  Copyright 2007 __MyCompanyName__. All rights reserved.
//
//  Updated by Mehmet Akten - April 2007
//  www.memo.tv 

#import "AdvancedMathCCode.h"

#include "math.h"


/******************* A few defines *******************/
// probably best to put these in a seperate file
#define msaMIN(n, min)					((n) < (min) ? (min) : (n))						// find min value
#define msaMAX(n, max)					((n) > (max) ? (max) : (n))						// find max value
#define msaSGN(n)						((n) > 0) ? 1 : (((n) < 0) ? - 1 : 0);			// return the sign of a number

//#define msaMAP_BYTE(n)					(128 + ((n) * 127))								// map -1..1 number to 0..255
#define msaCLAMP(n, min, max)			msaMIN(msaMAX(n, max), min)						// clamp n between min and max
#define msaCLAMP_BYTE(n)				msaCLAMP(n, 0, 255)								// clamp n between 0..255
//#define msaMAPNCLAMP_BYTE(n)			msaCLAMP_BYTE(msaMAP_BYTE(n))

#define msaLERP(a, b, t)				((a) + ((b)-(a)) * (t))							// linear interpolation between a and b based on t

#define msaPI							3.1415926536f									// good ole pi
#define msaRAD2DEG						(180 / msaPI)									// used for converting radian to degrees
#define msaROOT2						(1.41421356f)									// square root of 2
#define msaHALF_ROOT2					(msaROOT2/2)									// half of square root of 2
#define msaECC_SCALER					(1/(1-msaHALF_ROOT2))							// needed for normalizing Orientation Eccentricity

//#define DP(p)							*(data->p)										// used to access a pointer in a struct pointer called 'data'


/******************* Config Options *******************/
// Gravity should be read from calibration and passed to the orientation function.
// When resting flat my wiimote accZ is 156 and nunchuk accZ is 176!
float fGravity				= 156 - 128;


/******************* Some Helper Functions *******************/

// return the dot product of 2 2D vectors
inline float msaDot2(float x1, float y1,  float x2,  float y2) {
	return x1 * x2 + y1 * y2;
}

// return the dot product of 2 3D vectors
inline float msaDot3(float x1, float y1, float z1, float x2,  float y2, float z2) {
	return x1 * x2 + y1 * y2 + z1 * z2;
}

// takes a value -1 .. +1 and maps it cubically so its smooth at zero and the extents using the function 3x^2 - 2x^3
inline float msaCubicMap(float n) {	
	int iSign = n > 0 ? 1 : -1;				// whether n is +ve or -ve;
	n = fabs(n);
	return iSign * ( 3 * n * n - 2 * n * n * n);
}


// remove fDZone amount around zero from a 0..1 or -1..1 value
float msaDeadZone(float n, float fDZone) {
	float fs = msaSGN(n);
	n = fabs(n) - fDZone;
	if(n<0) n = 0;
	n = fs * n / (1 - fDZone);			// scale back to 0..1

	return n;
}



/******************* Main Update Functions *******************/

// helper function which calculates Vel and Pos for one axis
// inputs:
//	accX: acceleration read form controller, centered on ZERO, and NOT scaled
void _updateVelAndPos_Axis(float accX, float fTime, float fDeadZone, unsigned char *velX, unsigned char *posX) {

	float fAx, fVx, fPx;

	fAx = accX / 128.0f;			// adjust to be -1..1
	fAx = msaDeadZone(fAx, fDeadZone);
//	fAx = msaCubicMap(fAx);	

	
	fVx = *velX - 128;	
	fPx = *posX - 128;
					
	if(fAx == 0) {									// if accel is small
		fVx *= 0.6f;								// apply drag to the velocity, would be awesome to control this via midi!!!
//		fVx = msaDeadZone(fVx, 0.2f);				// and apply dead zone to it
	}
	
	fVx = fVx + fAx * fTime * 0.5f;		
//	fPx = *posX + fVx * fTime * 0.001f;		
	fPx = fPx + fAx * fTime * 0.5f;		

	fPx += 128;
	fVx += 128;										// velocity centered on 128

	*velX = msaCLAMP_BYTE(fVx);
	*posX = msaCLAMP_BYTE(fPx);
//	if(fAx!=0) NSLog(@"%.5f  %.5f  %.5f", fAx, fVx,  fPx);

}



void updateVelAndPos(unsigned char accX, unsigned char accY, unsigned char accZ,
	unsigned int timeSinceLastCass,
	unsigned char *velX, unsigned char *velY, unsigned char *velZ, 
	unsigned char *posX, unsigned char *posY, unsigned char *posZ)
{
	float fTime = timeSinceLastCass;			// time multiplier
	
	_updateVelAndPos_Axis(accX - 128.0f, fTime, fGravity / 256, velX, posX);
	_updateVelAndPos_Axis(128.0f - accY, fTime, fGravity / 256, velY, posY);	
	_updateVelAndPos_Axis(128 - accZ + fGravity, fTime, fGravity / 256, velZ, posZ);	
	
//	NSLog(@"%i  %i %i | %i  %i  %i", DP(posX), DP(posY),  DP(posY), DP(velX), DP(velY), DP(velZ);	
//	NSLog(@"%i  %i  %i", data->accZ, DP(velZ),  DP(posZ));
}



void updateOrientation(unsigned char accX, unsigned char accY, unsigned char accZ,
	unsigned int timeSinceLastCass,
	unsigned char *oriX, unsigned char *oriY)
{
	float fAx = accX - 128;
	float fAy = accY - 128;		
	float fAz = accZ - 128;
	
	float fGravityInc = 1 / fGravity;
	
	float fRoll = msaDot3(fAx, fAy, fAz, fGravityInc, 0, 0);		// -1 .. +1
	float fPitch = msaDot3(fAx, fAy, fAz, 0, -fGravityInc, 0);		// -1 .. +1
	
//	fRoll = msaCubicMap(msaCLAMP(fRoll, -1, 1));	// enable this once calibration is introduced
//	fPitch = msaCubicMap(msaCLAMP(fPitch, -1, 1));
	
/*
	// calculate anglular speed - can be transmitted via midi too
	static float fOldRoll = 0;			// obviously shouldnt be static, but should be stored
	static float fOldPitch = 0;
	
	float fRollVel = fRoll - fOldRoll;
	float fPitchVel = fPitch - fOldPitch;
	
	fRollVel = msaDeadZone(fRollVel, 0.001f);
	fPitchVel = msaDeadZone(fPitchVel, 0.001f);
*/


	float fEccentricity = msaDot2(fabs(fRoll), fabs(fPitch), msaHALF_ROOT2, msaHALF_ROOT2);
	fEccentricity = msaMIN(fEccentricity - msaHALF_ROOT2, 0)  * msaECC_SCALER; // 0..1 on how much the rotation is both axis
	
	float fEccStretchFactor = msaLERP(1, msaROOT2, fEccentricity);	// stretch orientation based on Ecc so they can reach the corners
				
	fRoll = 128 + fRoll * 128 * fEccStretchFactor;
	fPitch = 128 + fPitch * 128 * fEccStretchFactor;
				
	*oriX = msaCLAMP_BYTE(fRoll);
	*oriY = msaCLAMP_BYTE(fPitch);

//	NSLog(@"%3.2f  %3.2f %3.2f | %i  %i | %i  %i  %i",fRoll, fPitch, fEccentricity, *oriX, *oriY,  accX, accY, accZ);

}