#include  <stdint.h>
#include  <furi.h>         // Core API

#include  "wii_anal.h"
#include  "wii_i2c.h"
#include  "bc_logging.h"

#include  "gfx/images.h"     // Images
#include  "wii_anal_lcd.h"   // Drawing functions
#include  "wii_anal_keys.h"  // key mappings

// ** If you want to see what this source code looks like with all the MACROs expanded
// **   grep -v '#include  ' wii_ec_nunchuck.c | gcc -E -o /dev/stdout -xc -
# include  "wii_ec_macros.h"

//+============================================================================ ========================================
// Standard Nunchuck : 2 buttons, 1 analogue joystick, 1 3-axis accelerometer
// 
void  nunchuck_decode (wiiEC_t* const pec)
{
	ecDecNunchuck_t*  p   = &pec->dec[(pec->decN = !pec->decN)].nunchuck;
	uint8_t*          joy = pec->joy;

	p->btnC = !(joy[5] & 0x02);  // !{1}
	p->btnZ = !(joy[5] & 0x01);  // !{1}

	p->joyX = joy[0];  // {8}
	p->joyY = joy[1];  // {8}

	p->accX = ((uint16_t)joy[2] << 2) | ((joy[5] >>2) & 0x03);  // {10}
	p->accY = ((uint16_t)joy[3] << 2) | ((joy[5] >>4) & 0x03);  // {10}
	p->accZ = ((uint16_t)joy[4] << 2) | ((joy[5] >>6) & 0x03);  // {10}

	DEBUG(">%d> C:%c, Z:%c, Joy{x:%02X, y:%02X}, Acc{x:%03X, y:%03X, z:%03X}", pec->decN,
	      (p->btnC ? '#' : '.'), (p->btnZ ? '#' : '.'),
	      p->joyX, p->joyY,  p->accX, p->accY, p->accZ
	);
}

//+============================================================================ ========================================
// Give each button a unique character identifier
//
void  nunchuck_msg (wiiEC_t* const pec,  FuriMessageQueue* const queue)
{
	ecDecNunchuck_t*  new = &pec->dec[pec->decN].nunchuck;
	ecDecNunchuck_t*  old = &pec->dec[!pec->decN].nunchuck;

	eventMsg_t   msg = {
		.id    = EVID_WIIEC,
		.wiiEc = {
			.type = WIIEC_NONE,
			.in   = ' ',
			.val  = 0,
		}
	};

	BUTTON(btnC, 'c');
	BUTTON(btnZ, 'z');

	ANALOG(joyX, 'x');
	ANALOG(joyY, 'y');

	ACCEL(accX, 'x');
	ACCEL(accY, 'y');
	ACCEL(accZ, 'z');
}

//+============================================================================ ========================================
// https://www.hackster.io/infusion/using-a-wii-nunchuk-with-arduino-597254#toc-5--read-actual-calibration-data-from-the-device-14
//
void  nunchuck_calib (wiiEC_t* const pec,  ecCalib_t c)
{
	ecDecNunchuck_t*  src = &pec->dec[pec->decN].nunchuck;  // from input
	ecCalNunchuck_t*  dst = pec->calS.nunchuck;             // to calibration data

	if (c & CAL_RESET) {  // initialise ready for software calibration
		// LO is set to the MAXIMUM value (so it can be reduced)
		// HI is set to ZERO              (so it can be increased)
		RESET_LO_HI(accX, 10);  // 10bit value
		RESET_LO_HI(accY, 10);  // 10bit value
		RESET_LO_HI(accZ, 10);  // 10bit value

		RESET_LO_HI(joyX, 8);   // 8bit value
		RESET_LO_HI(joyY, 8);   // 8bit value
	}
	if (c & CAL_FACTORY) {  // (re)set to factory defaults
		//! "[4] LSB of Zero value of X,Y,Z axes" ...helpful!
		//! ...Well, my test nunchuck has bits set in the bottom 6 bits, so let's guess ;)

		// No value available - annecdotal tests suggest 8 is reasonable
		FACTORY_LO( accX, 8);
		FACTORY_LO( accY, 8);
		FACTORY_LO( accZ, 8);

		// @ 0G
		FACTORY_MID( accX, ((pec->calF[0] <<2) | ((pec->calF[3] >>4) &0x3)) ) ;
		FACTORY_MID( accY, ((pec->calF[1] <<2) | ((pec->calF[3] >>2) &0x3)) ) ;
		FACTORY_MID( accZ, ((pec->calF[2] <<2) | ((pec->calF[3]    ) &0x3)) ) ;

		// @ 1G
		FACTORY_HI( accX, ((pec->calF[4] <<2) | ((pec->calF[7] >>4) &0x3)) ) ;
		FACTORY_HI( accY, ((pec->calF[5] <<2) | ((pec->calF[7] >>2) &0x3)) ) ;
		FACTORY_HI( accZ, ((pec->calF[6] <<2) | ((pec->calF[7]    ) &0x3)) ) ;

		// Joysticks
		FACTORY_LO( joyX, pec->calF[ 9] ) ;
		FACTORY_MID(joyX, pec->calF[10] ) ;
		FACTORY_HI( joyX, pec->calF[ 8] ) ;

		FACTORY_LO( joyY, pec->calF[12] ) ;
		FACTORY_MID(joyY, pec->calF[13] ) ;
		FACTORY_HI( joyY, pec->calF[11] ) ;
	}
	if (c & CAL_TRACK) {  // track maximum and minimum values seen
		TRACK_LO_HI(accX);
		TRACK_LO_HI(accY);
		TRACK_LO_HI(accZ);

		TRACK_LO_HI(joyX);
		TRACK_LO_HI(joyY);
	}
	if (c & CAL_RANGE) {  // perform software calibration step
		RANGE_LO_HI(accX);
		RANGE_LO_HI(accY);
		RANGE_LO_HI(accZ);

		if (!(c & CAL_NOTJOY)) {  // double negative!
			RANGE_LO_HI(joyX);
			RANGE_LO_HI(joyY);
		}
	}
	if (c & CAL_CENTRE) {  // reset centre point of joystick
		CENTRE(accX);
		CENTRE(accY);
		CENTRE(accZ);

		CENTRE(joyX);
		CENTRE(joyY);
	}
}

//============================================================================= ========================================
// Accelerometer screen ...might this be useful for other controllers?
//
// https://bootlin.com/labs/doc/nunchuk.pdf
// X : Move Left/Right : -left / +right
// Y : Move Fwd/Bkwd   : -fwd  / +bkwd
// Z : Move Down/Up    : -down / +up
//
// Movement         in the direction of an axis changes   that axis   reading
// Twisting/tilting        around       an axis changes the other two readings
//
// EG. Move left will effect X ; turn left will effect Y & Z
//
#define  aw  110  // axis width
#define  ah  15   //      height   {0......7......14}
#define  am  7    //      midpoint {       7       }
#define  ar  7    //      range    {1234567 1234567}

enum {
	ACC_X   = 0,
	ACC_Y   = 1,
	ACC_Z   = 2,
	ACC_CNT = 3,
	ACC_1   = ACC_X,  // first
	ACC_N   = ACC_Z,  // last
};

//+============================================================================
static
void  nunchuck_showAcc (Canvas* const canvas,  state_t* const state)
{
	ecDecNunchuck_t*  d                = &state->ec.dec[state->ec.decN].nunchuck;
	ecCalNunchuck_t*  lo               = &state->ec.calS.nunchuck[1];
	ecCalNunchuck_t*  mid              = &state->ec.calS.nunchuck[2];
	ecCalNunchuck_t*  hi               = &state->ec.calS.nunchuck[3];

	int                y[ACC_CNT]      = {0, 0+(ah+4), 0+((ah+4)*2)};
	int                x               = 10;

	static uint16_t    v[ACC_CNT][aw]  = {0};
//	static uint16_t    tv[ACC_CNT][aw] = {0};

	static uint16_t    idx             = 0;
	static uint16_t    cnt             = aw -1;

	// Only record when scanner NOT-paused
	if (!state->pause) {
		uint16_t   dead = (1<<5);

		// Find axes y-offsets
		for (int a = ACC_1;  a <= ACC_N;  a++) {
			uint16_t*  dp = NULL;  // data value  (current reading)
			uint16_t*  lp = NULL;  //   lo value
			uint16_t*  mp = NULL;  //  mid value
			uint16_t*  hp = NULL;  //   hi value
			uint16_t*  vp = NULL;  // value       (result)

			switch (a) {
				case ACC_X:
					dp = &  d->accX;        // data (input)
					lp = & lo->accX;        // low  \.
					mp = &mid->accX;        // mid   > calibration
					hp = & hi->accX;        // high /
					vp = &v[  ACC_X][idx];  // value (where to store the result)
                    break;
				case ACC_Y:
					dp = &  d->accY;
					lp = & lo->accY;
					mp = &mid->accY;
					hp = & hi->accY;
					vp = &v[  ACC_Y][idx];
                    break;
				case ACC_Z:
					dp = &  d->accZ;
					lp = & lo->accZ;
					mp = &mid->accZ;
					hp = & hi->accZ;
					vp = &v[  ACC_Z][idx];
                    break;
				default:  break ;
			}

			// Again - qv. the joysick calibration:
			//   This is not the "right way" to do this, it is just "one way" to do it
			// ...mid point and extreme zones have a deadzone
			// ...the rest is evenly divided by the amount of space on the graph
			if ((*dp >= (*mp -dead)) && (*dp <= (*mp +dead)))  *vp = ar ;
			else if (*dp >= (*hp -dead))                       *vp = ah-1 ;
			else if (*dp <= (*lp +dead))                       *vp = 0 ;
			else if (*dp < *mp) {
				uint16_t  min   = ((*lp +dead) +1);
				uint16_t  max   = ((*mp -dead) -1);
				float     range = (max -min) +1;
				float     m     = range /(ar-1);  // 6 evenly(/fairly) divided zones
				          *vp   = ((int)((*dp -min) /m)) +1;

			} else {//if (*dp > *mp) 
				uint16_t  min   = ((*mp +dead) +1);
				uint16_t  max   = ((*hp -dead) -1);
				float     range = (max -min) +1;
				float     m     = range /(ar-1);  // 6 evenly(/fairly) divided zones
				          *vp   = ((int)((*dp -min) /m)) +1 +ar;
			}
		}

//! If we decide to offer "export to CSV"
//! I suggest we keep a second array of true-values, rather than do all the maths every time
//! Also - the data will need to me moved to the 'state' table - so a.n.other function can save it off
//		tv[ACC_X][idx] = d->accX;
//		tv[ACC_Y][idx] = d->accY;
//		tv[ACC_Z][idx] = d->accZ;

		// Prepare for the next datapoint
		if (++idx >= aw)  idx = 0 ;
		if (cnt)          cnt-- ;
	}

	// Auto-pause
	if (state->apause && !idx)  state->pause = true ;

	// *** Draw axes ***
	show(canvas, 0,y[ACC_X] +((ah -img_6x8_X.h) /2), &img_6x8_X, SHOW_SET_BLK);
	show(canvas, 0,y[ACC_Y] +((ah -img_6x8_Y.h) /2), &img_6x8_Y, SHOW_SET_BLK);
	show(canvas, 0,y[ACC_Z] +((ah -img_6x8_Z.h) /2), &img_6x8_Z, SHOW_SET_BLK);

	canvas_set_color(canvas, ColorBlack);
	for (int a = ACC_1;  a <= ACC_N;  a++) {
		canvas_draw_line(canvas, x-1,y[a]   , x   -1,y[a]+ah);
		canvas_draw_line(canvas, x  ,y[a]+ah, x+aw-1,y[a]+ah);

		// Mid & Peak lines
		for (int i = 1;  i < aw;  i += 3) {
			canvas_draw_dot(canvas, x+i,y[a]);
			canvas_draw_dot(canvas, x+i,y[a] +(ah /2));
		}
	}

	// Data (wiper display - see notes.txt for scrolling algorithm)
	int  end = idx ? idx : aw;
	for (int a = ACC_1;  a <= ACC_N;  a++) {
		canvas_draw_dot(canvas, x,y[a]+v[a][idx]);
		for (int i = 1;  i < end;  i++)  
			canvas_draw_line(canvas, x+i,y[a]+v[a][i-1] , x+i,y[a]+v[a][i]);
		if (!state->apause)
			for (int i = end+10;  i < aw -cnt;  i++)
				canvas_draw_line(canvas, x+i,y[a]+v[a][i-1] , x+i,y[a]+v[a][i]);
	}
	// Wipe bar
	if (end   < aw)  canvas_draw_line(canvas, x+end,y[0], x+end,y[2]+ah-1);
	if (++end < aw)  canvas_draw_line(canvas, x+end,y[0], x+end,y[2]+ah-1);
	if (++end < aw)  canvas_draw_line(canvas, x+end,y[0], x+end,y[2]+ah-1);

	// *** Mode buttons ***
	show(canvas, 0,55, &img_key_L, SHOW_SET_BLK);    // mode key

	if ((state->calib & CAL_RANGE) || state->pause)  state->flash++ ;

	// -pause- ...yeah, this got a little out of hand! LOL!
	if (state->pause || state->apause) {
		if (state->pause && state->apause && !idx) {
			if (state->flash &8) {
				show(canvas, 108,56, &img_key_U, SHOW_SET_BLK);
			} else {
				show(canvas, 108,56, &img_key_Ui, SHOW_SET_BLK);
				canvas_draw_line(canvas, x+aw,y[0], x+aw,y[2]+ah-1);
			}
		} else {
			show(canvas, 108,56, &img_key_Ui, SHOW_SET_BLK);
		}
	} else {
		show(canvas, 108,56, &img_key_U, SHOW_SET_BLK);  // pause
	}

	// -calibration-
	if (state->calib & CAL_RANGE) {
		show(canvas, 119,55, (state->flash &8) ? &img_key_OKi : &img_key_OK, SHOW_SET_BLK);
	} else {
		show(canvas, 119,55, &img_key_OK, SHOW_SET_BLK);
	}
}

#	undef  aw
#	undef  ah
#	undef  am
#	undef  ar

//+============================================================================ ========================================
// Default nunchuck screen
//
void  nunchuck_show (Canvas* const canvas,  state_t* const state)
{
	// Nunchucks have TWO scenes
	if (state->scene == SCENE_NUNCHUCK_ACC)  return nunchuck_showAcc(canvas, state) ;

	// Default scene
	ecDecNunchuck_t*  d  = &state->ec.dec[state->ec.decN].nunchuck;
	ecCalNunchuck_t*  c  = (state->hold) ? &state->ec.calS.nunchuck[(state->hold < 0) ? 0 : 4]
	                                     : (ecCalNunchuck_t*)d ;  //! danger will robinson!
	ecCalNunchuck_t*  js = state->ec.calS.nunchuck;

	// X, Y, Z
	show(canvas,  42,0, &img_6x8_X, SHOW_SET_BLK);
	show(canvas,  73,0, &img_6x8_Y, SHOW_SET_BLK);
	show(canvas, 104,0, &img_6x8_Z, SHOW_SET_BLK);

	canvas_draw_str_aligned(canvas, 0,14, AlignLeft, AlignTop, "Accel");
	canvas_draw_str_aligned(canvas, 0,28, AlignLeft, AlignTop, "Joy");

	// accel values
	showHex(canvas, 34,12, c->accX, 3,2);
	showHex(canvas, 65,12, c->accY, 3,2);
	showHex(canvas, 96,12, c->accZ, 3,2);
	// Joy values
	showHex(canvas, 38,27, c->joyX, 2,2);
	showHex(canvas, 69,27, c->joyY, 2,2);

	showJoy(canvas, 103,32, js[1].joyX, js[2].joyX, js[3].joyX,
	                        js[1].joyY, js[2].joyY, js[3].joyY, d->joyX,d->joyY, 8);

	// buttons
	canvas_set_color(canvas, ColorBlack);
	canvas_draw_str_aligned(canvas, 0,44, AlignLeft, AlignTop, "Button");

	if (!d->btnC) {
		canvas_draw_rframe(canvas, 36,42, 18,12, 6);
		show(canvas, 42,44, &img_6x8_C, SHOW_SET_BLK);
	} else {
		canvas_draw_rbox(canvas, 36,42, 18,12, 6);
		show(canvas, 42,44, &img_6x8_C, SHOW_SET_WHT);
		canvas_set_color(canvas, ColorBlack);
	}

	if (!d->btnZ) {
		canvas_draw_rframe(canvas, 64,40, 24,16, 2);
		show(canvas, 73,44, &img_6x8_Z, SHOW_SET_BLK);
	} else {
		canvas_draw_rbox(canvas, 64,40, 24,16, 2);
		show(canvas, 73,44, &img_6x8_Z, SHOW_SET_WHT);
	}

	// Navigation
	showPeakHold(state, canvas, state->hold);      // peak keys
	show(canvas, 0,55, &img_key_L, SHOW_SET_BLK);  // mode keys
	show(canvas, 9,55, &img_key_R, SHOW_SET_BLK);
}

//+============================================================================ ========================================
static
bool  nunchuck_keyAcc (const eventMsg_t* const msg,  state_t* const state)
{
	int  used = false;  // assume key is NOT-handled

	switch (msg->input.type) {
		case InputTypeShort:                         //# <!  After InputTypeRelease within INPUT_LONG_PRESS interval
			switch (msg->input.key) {
				case InputKeyDown:                   //# <D [ SHORT-DOWN ]
					used = true;                                                // Block trough-hold
					break;

				case InputKeyUp:                     //# <U [ SHORT-UP ]
					if (state->pause)  state->pause  = false ;                  // Paused?  Restart
					else               state->apause = !state->apause ;         // No?      toggle auto-pause
					used = true;
					break;

				case InputKeyLeft:                   //# <L [ SHORT-LEFT ]
					sceneSet(state, SCENE_NUNCHUCK);
					state->calib &= ~CAL_NOTJOY;                                // DO calibrate joystick in NUNCHUCK mode
					used = true;
					break;

				default:  break ;                    //# <?
			}
			break;

		default:  break ;
	}

	// Calibration keys
	if (!used)  used = key_calib(msg, state) ;

	return used;
}

//+============================================================================ ========================================
bool  nunchuck_key (const eventMsg_t* const msg,  state_t* const state)
{
	// Nunchucks have TWO scenes
	if (state->scene == SCENE_NUNCHUCK_ACC)  return nunchuck_keyAcc(msg, state) ;

	// Default scene
	int  used = false;  // assume key is NOT-handled

	switch (msg->input.type) {
		case InputTypeShort:                         //# <!  After InputTypeRelease within INPUT_LONG_PRESS interval
			switch (msg->input.key) {
				case InputKeyLeft:                   //# <L [ SHORT-LEFT ]
					sceneSet(state, SCENE_DUMP);
					used = true;
					break;

				case InputKeyRight:                  //# <R [ SHORT-RIGHT ]
					sceneSet(state, SCENE_NUNCHUCK_ACC);
					state->calib |= CAL_NOTJOY;                                 // do NOT calibrate joystick in _ACC mode
					used = true;
					break;
				default:  break ;                    //# <?
			}
			break;

		default:  break ;
	}

	// Calibration keys
	if (!used)  used = key_calib(msg, state) ;

	return used;
}