#include <Arduino.h>
#include <stddef.h>
#include <stdint.h>
#include "controls.h"
#include "error.h"

// For initializing B103-348 style joysticks
int initJoystick(Joystick *js, uint8_t pin_x, uint8_t pin_y, uint8_t pin_z)
{
	if ( js == NULL ) {
		ERROR(ERRNO_NULLPOINTER);
	}
	js->button.pin = pin_z;
	js->button.debouncetime = BUTTON_STABLETIME;
	js->button.pressedvalue = LOW;
	pinMode(js->button.pin, INPUT_PULLUP);
	js->x.pin = pin_x;
	js->x.calibration.stable_time = JOYSTICK_STABLETIME;
	js->y.pin = pin_y;
	js->y.calibration.stable_time = JOYSTICK_STABLETIME;
	return ERRNO_SUCCESS;
}

// For initializing simple buttons
int initButton(Button *button)
{
	pinMode(button->pin, INPUT);
	return ERRNO_SUCCESS;
}

// 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)
{
	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;
}