#include "Freenove_WS2812_Lib_for_ESP32.h"

/*********************************************************************************/
// Constants

// Pin 21 is GPIO only
#define PIN_BUTTON            21   // Calibration button

// Apparently there is a dedicated 2812 controller in the ESP32-S3 that lives on this pin
// and this pin only.
#define PIN_2812              48   // Data bus for the 2812 LED

// These pins are shared with the second ADC unit. We're not using that unit in this project
// but for future projects as my boards become more dense I need to remember that each pin
// choice is a tradeoff. At a certain point we run out of available pins for adding stuff on,
// and have to start getting creative about how we use the pins we have left.

#define PIN_LED_X             16   // Red X axis LED
#define PIN_LED_Y             18   // Green Y axis LED
#define PIN_LED_Z             15   // Yellow Z axis LED (button)

#define PWM_FREQUENCY         1000
#define PWM_BITWIDTH          12

// Pins 1 and 2 are on ADC channel 0 and 1.
#define PIN_JOY_X            1
#define PIN_JOY_Y            2
// Pin 47 is GPIO only
#define PIN_JOY_Z            47
// But Pin 12 is where the tutorial has it
//#define PIN_JOY_Z            12

#define BUTTON_STABLETIME    100
#define JOYSTICK_STABLETIME  3000

// Button states
#define BUTTON_STATE_BOUNCING     (1 << 0)
#define BUTTON_STATE_DOWN         (1 << 1)
#define BUTTON_STATE_UP           (1 << 2)
#define BUTTON_STATE_HELD         (1 << 3)

// Joystick states
#define JOYSTICK_STATE_CALIBRATING (1 << 0)
#define JOYSTICK_STATE_CALIBRATED  (1 << 1)
#define JOYSTICK_STATE_READING     (1 << 2)
#define JOYSTICK_STATE_READY       (1 << 3)

// These aren't pins, they're just defines that help us identify the lights on the 2812 unit
#define LED_2812_LEFT         0    // 2812 LEDs are ordered from connection point at left
#define LED_2812_TOPLEFT      1    // and go clockwise
#define LED_2812_TOP          2
#define LED_2812_TOPRIGHT     3
#define LED_2812_RIGHT        4
#define LED_2812_BOTTOMRIGHT  5
#define LED_2812_BOTTOM       6
#define LED_2812_BOTTOMLEFT   7

// Error flags

#define ERRNO_SUCCESS         0
#define ERRNO_NULLPOINTER     1
#define ERRNO_IO              2
#define ERRNO_TIMEOUT         3
#define ERRNO_CALIBRATION     4

#define ERROR(x) errno=x; return x;

// Uses the Freenove WS2812 Lib for ESP32@2.0.1

/**********************************************************************************/
// Interface structures

typedef struct Calibration {
	// When did calibration start
	uint32_t start_time;
	// Low calibration range
	uint32_t low;
	// High calibration range
	uint32_t high;
	// How long we should sample values on this joystick
	uint32_t stable_time;
} Calibration;

typedef struct Button {
	// What pin is this button on
	uint8_t pin;
	// What is the state of this button (bitmask of BUTTON_STATE_*)
	uint8_t state;
	// Is this button pressed when it is LOW or HIGH?
	uint32_t pressedvalue;
	// How long do we wait when debouncing this button
	uint32_t debouncetime;
	// When did we start debouncing
	uint32_t debounce_start_time;
} Button;

typedef struct JoystickAxis {
	// What ADC pin is this joystick axis connected to
	uint8_t pin;
	// Calibration data
	Calibration calibration;
	// Position as of the last read time.
	uint32_t position;
} JoystickAxis;

typedef struct Joystick {
	// X axis data
	JoystickAxis x;
	// Y axis data
	JoystickAxis y;
	// This is the Z axis switch on the joystick control
	Button button;
	// What is the state of this joystick (bitmask of JOYSTICK_STATE_*)
	uint8_t state;
} Joystick;

/*******************************************************************************/
// Global Variables

int errno;
Button calibration_button;
Joystick js;

alignas(Freenove_ESP32_WS2812) uint8_t strip_storage[sizeof(Freenove_ESP32_WS2812)];
Freenove_ESP32_WS2812 *strip;

/******************************************************************************/
// Initialization Functions
void initSerial()
{
	Serial.begin(115200);
}

int init2812(Freenove_ESP32_WS2812 **strip)
{
	Freenove_ESP32_WS2812 *obj = new(strip_storage) Freenove_ESP32_WS2812(8, PIN_2812, 0, TYPE_GRB);
	if ( obj == NULL ) {
		ERROR(ERRNO_NULLPOINTER);
	}
  obj->setBrightness(10);
	obj->begin();
	*strip = obj;
	return ERRNO_SUCCESS;
}

int initJoystick(Joystick *js)
{
	if ( js == NULL ) {
		ERROR(ERRNO_NULLPOINTER);
	}
	js->button.pin = PIN_JOY_Z;
	js->button.debouncetime = BUTTON_STABLETIME;
	js->button.pressedvalue = LOW;
	pinMode(js->button.pin, INPUT_PULLUP);
	js->x.pin = PIN_JOY_X;
	js->x.calibration.stable_time = JOYSTICK_STABLETIME;
	js->y.pin = PIN_JOY_Y;
	js->y.calibration.stable_time = JOYSTICK_STABLETIME;
	return ERRNO_SUCCESS;
}

int initButton(Button *button)
{
	pinMode(button->pin, INPUT);
	return ERRNO_SUCCESS;
}

int initLEDs()
{
	// Attach LEDC to the joystick status LED pins
	ledcAttach(PIN_LED_X, PWM_FREQUENCY, PWM_BITWIDTH);
	ledcAttach(PIN_LED_Y, PWM_FREQUENCY, PWM_BITWIDTH);
	pinMode(PIN_LED_Z, OUTPUT);
	return ERRNO_SUCCESS;
}

/****************************************************************************************/
// IO Routines

// Calibrate a given joystick
int calibrateJoystick(Joystick *js)
{
	uint32_t x = 0;
	uint32_t y = 0;
	uint32_t curmillis = 0;
	if ( js == NULL ) {	
		ERROR(ERRNO_NULLPOINTER);
	}
	curmillis = millis();
	if ( js->x.calibration.start_time == 0 || js->y.calibration.start_time == 0 ) {
		// Starting a new calibration cycle
		Serial.printf("Starting a new calibration cycle\n");
		js->x.calibration.low = 65535;
		js->x.calibration.high = 0;
		js->y.calibration.low = 65535;
		js->y.calibration.high = 0;
		js->x.calibration.start_time = curmillis;
		js->y.calibration.start_time = curmillis;
		return ERRNO_SUCCESS;
	}
	if ( (curmillis - js->x.calibration.start_time) >= js->x.calibration.stable_time ) {
		js->state = JOYSTICK_STATE_CALIBRATED;
		js->x.calibration.start_time = 0;
		js->y.calibration.start_time = 0;
		Serial.printf("Calibrated joystick to x: <%d, %d> y <%d, %d>\n",
									js->x.calibration.low,
									js->x.calibration.high,
									js->y.calibration.low,
									js->y.calibration.high);
		return ERRNO_SUCCESS;
	}
	x = analogRead(js->x.pin);
	if ( x < js->x.calibration.low ) {
		js->x.calibration.low = x;
	} else if ( x > js->x.calibration.high ) {
		js->x.calibration.high = x;
	}
	y = analogRead(js->y.pin);
	if ( y < js->y.calibration.low ) {
		js->y.calibration.low = y;
	} else if ( y > js->y.calibration.high ) {
		js->y.calibration.high = y;
	}
	return ERRNO_SUCCESS;
}

// Read the state of a given button
int readButton(Button *button)
{
	// We could improve this method further. It doesn't *really* debounce. It just introduces a delay between the
	// point of first contact and the time when the value is read. A true debouncing would wait until the state was
	// *stable* for a given period of time. I may come back and change it to work this way later.
	
	uint32_t pinvalue;
	uint32_t curmillis;
	uint32_t buttonvalue;
	if ( button == NULL ) {	
		ERROR(ERRNO_NULLPOINTER);
	}
	curmillis = millis();
	buttonvalue = digitalRead(button->pin);
	if ( (button->state & BUTTON_STATE_BOUNCING) == BUTTON_STATE_BOUNCING ) {
		if ( (curmillis - button->debounce_start_time) >= button->debouncetime ) {
			if ( buttonvalue == button->pressedvalue ) {
				button->state = BUTTON_STATE_DOWN;
			} else {
				button->state = BUTTON_STATE_UP;
			}
			Serial.printf("Button on pin %d has debounced state %d\n", button->pin, button->state);
			button->debounce_start_time = 0;
			return ERRNO_SUCCESS;
		}
		return ERRNO_SUCCESS;
	}
	if ( buttonvalue != button->pressedvalue && (
				 (button->state & BUTTON_STATE_DOWN) == BUTTON_STATE_DOWN ||
				 (button->state & BUTTON_STATE_HELD) == BUTTON_STATE_HELD)){
		// We are beginning to release. Debounce.
		Serial.printf("Debouncing button on pin %d for release\n", button->pin);
		button->state = button->state | BUTTON_STATE_BOUNCING;
		button->debounce_start_time = curmillis;
	}
	if ( buttonvalue == button->pressedvalue && (
				 (button->state & BUTTON_STATE_DOWN) != BUTTON_STATE_DOWN &&
				 (button->state & BUTTON_STATE_HELD) != BUTTON_STATE_HELD)){
		// We are beginning to press. Debounce.
		Serial.printf("Debouncing button on pin %d for press\n", button->pin);
		button->state = button->state | BUTTON_STATE_BOUNCING;
		button->debounce_start_time = curmillis;
	}
	
	return ERRNO_SUCCESS;
}

int readJoystick(Joystick *js)
{
	uint32_t adcvalue = 0;
	if ( js == NULL ) {	
		ERROR(ERRNO_NULLPOINTER);
	}
	js->state = js->state | JOYSTICK_STATE_READING;
	
	adcvalue = analogRead(js->x.pin);
	if ( adcvalue < js->x.calibration.low || adcvalue > js->x.calibration.high ) {
		js->x.position = adcvalue;
	} else {
		// Snap to center
		js->x.position = 2048;
	}
	adcvalue = analogRead(js->y.pin);
	if ( adcvalue < js->y.calibration.low || adcvalue > js->y.calibration.high ) {
		js->y.position = adcvalue;
	} else {
		// Snap to center
		js->y.position = 2048;
	}
	//Serial.printf("Joystick x %d y %d\n", js->x.position, js->y.position);
	
	readButton(&js->button);
	js->state = js->state ^ JOYSTICK_STATE_READING;
	if ( errno != ERRNO_SUCCESS ) {
		Serial.printf("Failed to read joystick button : %d\n", errno);
		return errno;
	}
	js->state = js->state | JOYSTICK_STATE_READY;
	return ERRNO_SUCCESS;
}

int displayLEDs(Joystick *js)
{
	if ( js == NULL ) {	
		ERROR(ERRNO_NULLPOINTER);
	}

	// Is the joystick fully ready? Display state on the LEDs
	if ( ( js->state & JOYSTICK_STATE_READY ) == JOYSTICK_STATE_READY ) {
		ledcWrite(PIN_LED_X, js->x.position);
		ledcWrite(PIN_LED_Y, js->y.position);
		if ( ( js->button.state & BUTTON_STATE_DOWN ) == BUTTON_STATE_DOWN ) {
			digitalWrite(PIN_LED_Z, HIGH);
		} else {
			digitalWrite(PIN_LED_Z, LOW);
		}
		return ERRNO_SUCCESS;
	}
	
	// Is the joystick calibrating? Turn all the LEDs off
	if ( js->state == JOYSTICK_STATE_CALIBRATING ) {
		ledcWrite(PIN_LED_X, 1);
		ledcWrite(PIN_LED_Y, 1);
		digitalWrite(PIN_LED_Z, LOW);
	}
	return ERRNO_SUCCESS;
}

int display2812(Joystick *js, Freenove_ESP32_WS2812 *strip)
{
	if ( js == NULL ) {	
		ERROR(ERRNO_NULLPOINTER);
	}
	strip->setLedColorData(LED_2812_LEFT, 0, 0, 0);
	strip->setLedColorData(LED_2812_TOPLEFT, 0, 0, 0);
	strip->setLedColorData(LED_2812_TOP, 0, 0, 0);
	strip->setLedColorData(LED_2812_TOPRIGHT, 0, 0, 0);
	strip->setLedColorData(LED_2812_RIGHT, 0, 0, 0);
	strip->setLedColorData(LED_2812_BOTTOMRIGHT, 0, 0, 0);
	strip->setLedColorData(LED_2812_BOTTOM, 0, 0, 0);
	strip->setLedColorData(LED_2812_BOTTOMLEFT, 0, 0, 0);
	
	// Is the joystick fully ready? Display direction on the 2812
	if ( ( js->state & JOYSTICK_STATE_READY ) == JOYSTICK_STATE_READY ) {
		// Light up the correct LED_2812_* depending on the value of the X/Y axes on the joystick
		if ( js->x.position < 1024 && (js->y.position < 3072 && js->y.position > 1024) ) {
			strip->setLedColorData(LED_2812_LEFT, 128, 128, 128);
		} else if ( js->x.position < 1024 && js->y.position < 1024 ) {
			strip->setLedColorData(LED_2812_TOPLEFT, 128, 128, 128);
		} else if ( ( js->x.position > 1024 && js->x.position < 3072 ) && js->y.position < 1024 ) {
			strip->setLedColorData(LED_2812_TOP, 128, 128, 128);
		} else if ( js->x.position > 3072 && js->y.position < 1024 ) {
			strip->setLedColorData(LED_2812_TOPRIGHT, 128, 128, 128);
		} else if ( js->x.position > 3072 && ( js->y.position < 3072 && js->y.position > 1024) ) {
			strip->setLedColorData(LED_2812_RIGHT, 128, 128, 128);
		} else if ( js->x.position > 3072 && js->y.position > 3072 ) {
			strip->setLedColorData(LED_2812_BOTTOMRIGHT, 128, 128, 128);
		} else if ( ( js->x.position > 1024 && js->x.position < 3072 ) && js->y.position > 3072 ) {
			strip->setLedColorData(LED_2812_BOTTOM, 128, 128, 128);
		} else if ( js->x.position < 1024 && js->y.position > 3072 ) {
			strip->setLedColorData(LED_2812_BOTTOMLEFT, 128, 128, 128);
		}	
	}
	strip->show();
	
	return ERRNO_SUCCESS;
}

/**********************************************************************************************/
// Setup and Loop

void setup() {
	errno = ERRNO_SUCCESS;
	memset(&js, 0x00, sizeof(Joystick));
	memset(&calibration_button, 0x00, sizeof(Button));

	calibration_button.pin = PIN_BUTTON;
	calibration_button.debouncetime = BUTTON_STABLETIME;
	calibration_button.pressedvalue = LOW;
	
	initSerial();
	if ( init2812(&strip) != ERRNO_SUCCESS ) Serial.printf("Failed to initialize 2812 LED array : %d\n", errno);
	if ( initLEDs() != ERRNO_SUCCESS ) Serial.printf("Failed to initialize LED GPIO pins : %d\n", errno);
	if ( initJoystick(&js) != ERRNO_SUCCESS ) Serial.printf("Failed to initialized Joystick datastructure : %d\n", errno);
	if ( initButton(&calibration_button) != ERRNO_SUCCESS ) Serial.printf("Failed to initialize Calibration button and GPIO pin : %d\n", errno);
}

void loop() {
	if ( readButton(&calibration_button) != ERRNO_SUCCESS ) Serial.printf("Failed to read calibration button : %d\n", errno);
	if ( calibration_button.state == BUTTON_STATE_DOWN && js.state != JOYSTICK_STATE_CALIBRATING ) {
		// A calibrating joystick cannot be in any other state, so don't mask
		Serial.printf("Calibrating joystick\n");
		calibration_button.state = BUTTON_STATE_HELD;
	  js.state = JOYSTICK_STATE_CALIBRATING;
	}
	if ( js.state == JOYSTICK_STATE_CALIBRATING ) {
		if ( calibrateJoystick(&js) != ERRNO_SUCCESS ) {
			Serial.printf("Failed to calibrate joystick : %d\n", errno);
		}
	}
	if ( ( js.state & JOYSTICK_STATE_CALIBRATED) == JOYSTICK_STATE_CALIBRATED ) {
		if ( readJoystick(&js) != ERRNO_SUCCESS ) {
			Serial.printf("Failed to read joystick state : %d\n", errno);
		}
	}
	if ( displayLEDs(&js) != ERRNO_SUCCESS ) {
		Serial.printf("Failed to display LEDs : %d\n", errno);
	}
	if ( display2812(&js, strip) != ERRNO_SUCCESS ) {
		Serial.printf("Failed to display 2812 LEDs : %d\n", errno);
	}
}