#include <Device/UsbDBulkInterface.h>
#include <FreeRTOS.h>
#include <task.h>
#include <stm32f7xx_hal.h>
#include <main.h>

#include <LFramework/USB/Device/USBDevice.h>
#include <LFramework/Debug.h>
#include <LFramework/IO/Terminal/TerminalAnsi.h>
#include <LFramework/Thread/Thread.h>
#include <cstring>
#include <usart.h>

#include <usbd_customhid.h>

#include <usb_device.h>

extern USBD_HandleTypeDef hUsbDeviceFS;

using namespace LFramework;
//using namespace LFramework::USB;

//UsbDBulkInterface usbInterface;
//UsbDDeviceContext usbContext(&usbInterface);

/*class PollingTransfer : public LFramework::USB::UsbDTransfer {
public:
	enum class State {
		Pending,
		Success,
		Fail
	};

	PollingTransfer(){
		callbackIsr = &PollingTransfer::transferComleteCallbackStatic;
	}
	void reset(){
		_state = State::Pending;
		size = 0;
		actualSize = 0;
	}
	State getState() const {
		return _state;
	}
private:
	State _state = State::Pending;
	void transferComleteCallback(bool success){
		_state = success ? State::Success : State::Fail;
	}

	static void transferComleteCallbackStatic(LFramework::USB::UsbDTransfer* _this, bool success){
		static_cast<PollingTransfer*>(_this)->transferComleteCallback(success);
	}
};*/

/*void rxThreadHandler(UsbDEndpoint* ep) {
	Debug::Log() << "Rx thread enter";
	uint8_t buffer[64];

	for(;;){
		PollingTransfer rxTransfer{};
		rxTransfer.buffer = buffer;
		rxTransfer.size = sizeof(buffer);
		ep->transferAsync(&rxTransfer);
		Debug::Log() << "USB Rx begin";
		while(rxTransfer.getState() == PollingTransfer::State::Pending){
			vTaskDelay(1);
		}
		Debug::Log() << "USB Rx end";
		Debug::Log() << "USB Rx size:" << rxTransfer.actualSize;
	}
}

void txThreadHandler(UsbDEndpoint* ep) {
	Debug::Log() << "Tx thread enter";
	uint8_t buffer[31];

	for(int i = 0; i < sizeof(buffer); ++i){
		buffer[i] = i;
	}

	while(true){
		PollingTransfer txTransfer;
		txTransfer.buffer = nullptr;
		txTransfer.size = 0;
		Debug::Log() << "USB Tx begin";
		ep->transferAsync(&txTransfer);
		while(txTransfer.getState() == PollingTransfer::State::Pending){
			vTaskDelay(1);
		}

		Debug::Log() << "USB Tx end";
	}
}*/


struct GamepadState {
	bool up;
	bool down;
	bool left;
	bool right;

	bool a;
	bool b;
	bool c;

	bool x;
	bool y;
	bool z;

	bool start;
	bool mode;
};

static void GamepadReadDelay() {
	for(volatile int i = 0; i < 100; ++i){

	}
}

__STATIC_INLINE void DWT_Delay_us(volatile uint32_t microseconds)
{

  /* Go to number of cycles for system */
  microseconds *= (HAL_RCC_GetHCLKFreq() / 1000000);

  DWT->CYCCNT = 0;
  /* Delay till end */
  while (DWT->CYCCNT < microseconds);
}



int eventId = 0;

void printState(bool oldState, bool newState, const char* name){
	if(oldState != newState){

		lfDebug() << eventId << ":" << name << (newState ? " pressed" : " released");
		eventId++;
	}
}

#pragma pack(push, 1)
struct GamepadReport {
	uint8_t reportId;
	uint16_t buttons;
};
#pragma pack(pop)

extern SPI_HandleTypeDef hspi2;

class Ps1Gamepad {
public:
	static constexpr size_t HeaderSize = 3;
	static constexpr size_t MaxDataSize = 9 * 2;

	struct Type {
		static constexpr uint8_t Digital = 4;
		static constexpr uint8_t Negcon = 2;
		static constexpr uint8_t AnalogueRed = 7;
		static constexpr uint8_t AnalogueGreen = 5;
	};
	struct Response {
		uint8_t dummy;
		uint8_t id;
		uint8_t padID;
		uint8_t buttons0;
		uint8_t buttons1;
		uint8_t analogRX;
		uint8_t analogRY;
		uint8_t analogLX;
		uint8_t analogLY;

		struct {
			uint8_t right;
			uint8_t left;
			uint8_t up;
			uint8_t down;
			uint8_t triangle;
			uint8_t circle;
			uint8_t cross;
			uint8_t square;
			uint8_t l1;
			uint8_t r1;
			uint8_t l2;
			uint8_t r2;

		}pressure;


		bool hasAnalogJoyData() {
			return (id & 0xf) >=3;
		}

		bool select() {
			return (buttons0 & (1 << 0)) == 0;
		}

		bool l3() {
			return (buttons0 & (1 << 1)) == 0;
		}
		bool r3() {
			return (buttons0 & (1 << 2)) == 0;
		}
		bool start() {
			return (buttons0 & (1 << 3)) == 0;
		}
		bool up() {
			return (buttons0 & (1 << 4)) == 0;
		}
		bool right() {
			return (buttons0 & (1 << 5)) == 0;
		}
		bool down() {
			return (buttons0 & (1 << 6)) == 0;
		}
		bool left() {
			return (buttons0 & (1 << 7)) == 0;
		}


		bool l2() {
			return (buttons1 & (1 << 0)) == 0;
		}
		bool r2() {
			return (buttons1 & (1 << 1)) == 0;
		}
		bool l1() {
			return (buttons1 & (1 << 2)) == 0;
		}
		bool r1() {
			return (buttons1 & (1 << 3)) == 0;
		}
		bool triangle() {
			return (buttons1 & (1 << 4)) == 0;
		}
		bool circle() {
			return (buttons1 & (1 << 5)) == 0;
		}
		bool cross() {
			return (buttons1 & (1 << 6)) == 0;
		}
		bool square() {
			return (buttons1 & (1 << 7)) == 0;
		}
	};

	const Response& read() {
		uint8_t txBuffer[5] = {0x01, 0x42, 0x00, 0x00, 0x00};
		executeCommand(txBuffer, sizeof(txBuffer));
		return *(Response*)&rxBuffer[0];
	}

	void enterConfigMode() {
		uint8_t cmd[]={ 0x01,0x43,0x00,0x01,0x00};
		executeCommand(cmd, sizeof(cmd));
	}

	void setControllerMode(bool analog, bool lockMode) {
		uint8_t cmd[]={ 0x01, 0x44, 0x00, (analog ? 0x01 : 0x00), (lockMode ? 0x03 : 0x5A), 0x00, 0x00, 0x00, 0x00 };
		executeCommand(cmd, sizeof(cmd));
	}


	struct DataMask {
		struct {
			uint8_t b0;
			uint8_t b1;
			uint8_t b2;
		} value;

		constexpr DataMask operator | (const DataMask& other) const{
			return {{value.b0 | other.value.b0, value.b1 | other.value.b1, value.b2 | other.value.b2,}};
		}

		static constexpr DataMask digitalB0() { return {{0x01, 0x00, 0x00}}; }
		static constexpr DataMask digitalB1() { return {{0x02, 0x00, 0x00}}; }
		static constexpr DataMask allDigital() { return digitalB0() | digitalB1();}

		static constexpr DataMask analogJoyRx() { return {{0x04, 0x00, 0x00}}; }
		static constexpr DataMask analogJoyRy() { return {{0x08, 0x00, 0x00}}; }
		static constexpr DataMask analogJoyLx() { return {{0x10, 0x00, 0x00}}; }
		static constexpr DataMask analogJoyLy() { return {{0x20, 0x00, 0x00}}; }

		static constexpr DataMask leftAnalogJoy() { return analogJoyLx() | analogJoyLy(); }
		static constexpr DataMask rightAnalogJoy() { return analogJoyRx() | analogJoyRy(); }
		static constexpr DataMask allAnalogJoy() { return leftAnalogJoy() | rightAnalogJoy(); }

		static constexpr DataMask pressureR() { return {{0x40, 0x00, 0x00}}; }
		static constexpr DataMask pressureL() { return {{0x80, 0x00, 0x00}}; }
		static constexpr DataMask pressureU() { return {{0x00, 0x01, 0x00}}; }
		static constexpr DataMask pressureD() { return {{0x00, 0x02, 0x00}}; }

		static constexpr DataMask pressureTriangle() { return {{0x00, 0x04, 0x00}}; }
		static constexpr DataMask pressureCircle() { return {{0x00, 0x08, 0x00}}; }
		static constexpr DataMask pressureCross() { return {{0x00, 0x10, 0x00}}; }
		static constexpr DataMask pressureSquare() { return {{0x00, 0x20, 0x00}}; }

		static constexpr DataMask pressureL1() { return {{0x00, 0x40, 0x00}}; }
		static constexpr DataMask pressureR1() { return {{0x00, 0x80, 0x00}}; }

		static constexpr DataMask pressureL2() { return {{0x00, 0x00, 0x01}}; }
		static constexpr DataMask pressureR2() { return {{0x00, 0x00, 0x02}}; }

		static constexpr DataMask allPressure() {
			return
					pressureR() | pressureL() | pressureU() | pressureD() |
					pressureTriangle() | pressureCircle() | pressureCross() | pressureSquare() |
					pressureL1() | pressureR1() | pressureL2() | pressureR2();
		}

		static constexpr DataMask allDigitalAndAnalogJoy() {
			return allDigital() | allAnalogJoy();
		}

		static constexpr DataMask all() {
			return allDigitalAndAnalogJoy() | allPressure();
		}

		static constexpr DataMask none() {
			return allDigitalAndAnalogJoy() | allPressure();
		}

		constexpr DataMask operator ~() {
			return {{static_cast<uint8_t>(~value.b0 & 0xff), static_cast<uint8_t>(~value.b1 & 0xff), static_cast<uint8_t>(~value.b2 & 0x03)}};
		}

	};

	//All buttons
	void setDataMask(DataMask mask) {
		uint8_t cmd[]={ 0x01, 0x4F, 0x00, mask.value.b0, mask.value.b1, mask.value.b2, 0x00, 0x00, 0x00 };
		executeCommand(cmd, sizeof(cmd));

	}

	void exitConfigMode() {
		uint8_t cmd[]={ 0x01, 0x43, 0x00, 0x00, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A };
		executeCommand(cmd, sizeof(cmd));
	}

	bool executeCommand(uint8_t* txBuffer, uint8_t size){
		selectDevice();
		int byteDelay = 1;
		DWT_Delay_us(byteDelay);

		//transfer header
		for(uint8_t i = 0; i < HeaderSize; ++i){
			HAL_SPI_TransmitReceive(&hspi2, &txBuffer[i], &rxBuffer[i], 1, HAL_MAX_DELAY);
			DWT_Delay_us(byteDelay);
		}

		auto expectedDataLength = (rxBuffer[1] & 0x0f) * 2;

		//transfer data
		for(uint8_t i = HeaderSize; i < size; ++i){
			HAL_SPI_TransmitReceive(&hspi2, &txBuffer[i], &rxBuffer[i], 1, HAL_MAX_DELAY);
			DWT_Delay_us(byteDelay);
		}

		//push zeroes if user issued incorrect length command
		for(uint8_t i = size; i < (HeaderSize + expectedDataLength); ++i){
			uint8_t zero = 0;
			HAL_SPI_TransmitReceive(&hspi2, &zero, &rxBuffer[i], 1, HAL_MAX_DELAY);
			DWT_Delay_us(byteDelay);
		}

		deselectDevice();
		return true;
	}
	void selectDevice() {
		HAL_GPIO_WritePin(GAMEPAD_CS_GPIO_Port, GAMEPAD_CS_Pin, GPIO_PIN_RESET);
	}
	void deselectDevice() {
		HAL_GPIO_WritePin(GAMEPAD_CS_GPIO_Port, GAMEPAD_CS_Pin, GPIO_PIN_SET);
	}
private:
	uint8_t rxBuffer[HeaderSize + MaxDataSize];
};


extern"C" void StartDefaultTask(void const * argument){

	Terminal::out << Terminal::Ansi::Cursor::MoveHome() << Terminal::Ansi::Viewport::ClearScreen();
	Debug::Log() << "Hello !";

	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
	DWT->LAR = 0xC5ACCE55;
	DWT->CYCCNT = 0;
	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;


	MX_USB_DEVICE_Init();


	Ps1Gamepad gamepad;

	Ps1Gamepad::Response oldState;
	oldState.buttons0 = 0xff;
	oldState.buttons1 = 0xff;

	gamepad.enterConfigMode();
	DWT_Delay_us(5);
	gamepad.setControllerMode(true, false);
	DWT_Delay_us(4); //not less than 3!!
	gamepad.exitConfigMode();
	DWT_Delay_us(4);

	gamepad.enterConfigMode();
	DWT_Delay_us(4);
	gamepad.setDataMask(Ps1Gamepad::DataMask::all());
	DWT_Delay_us(4); //not less than 3!!
	gamepad.exitConfigMode();
	DWT_Delay_us(4);

	while(true){
		//vTaskDelay(1000);




		//USBD_CUSTOM_HID_SendReport(&hUsbDeviceFS, (uint8_t*)&report, sizeof(report));

		DWT_Delay_us(16000);
		//gamepad.exitConfigMode();
		auto state = gamepad.read();


		printState(oldState.up(), state.up(), "UP");
		printState(oldState.down(), state.down(), "DOWN");
		printState(oldState.left(), state.left(), "LEFT");
		printState(oldState.right(), state.right(), "RIGHT");

		printState(oldState.l1(), state.l1(), "L1");
		printState(oldState.l2(), state.l2(), "L2");
		printState(oldState.r1(), state.r1(), "R1");
		printState(oldState.r2(), state.r2(), "R2");

		printState(oldState.l3(), state.l3(), "L3");
		printState(oldState.r3(), state.r3(), "R3");

		printState(oldState.circle(), state.circle(), "CIRCLE");
		printState(oldState.square(), state.square(), "SQUARE");
		printState(oldState.triangle(), state.triangle(), "TRIANGLE");
		printState(oldState.cross(), state.cross(), "CROSS");

		printState(oldState.select(), state.select(), "SELECT");
		printState(oldState.start(), state.start(), "START");


		/*if(oldState.up() != state.up()){
			if(state.up()){
				gamepad.enterConfigMode();
				gamepad.setControllerMode(true, true);
				gamepad.exitConfigMode();
			}
		}

		if(oldState.down() != state.down()){
			if(state.down()){
				gamepad.enterConfigMode();
				gamepad.setControllerMode(false, false);
				gamepad.exitConfigMode();
			}
		}
*/

		lfDebug() << "mode: " << hex(state.id);

		if(oldState.hasAnalogJoyData() != state.hasAnalogJoyData()){
			if(state.hasAnalogJoyData()){
				lfDebug() << "Analog data ENABLED";
			}else {
				lfDebug() << "Analog data DISABLED";
			}
		}

		if(state.hasAnalogJoyData() && oldState.hasAnalogJoyData()){
			if(state.analogRX != oldState.analogRX){
				lfDebug() << "Rx: " << state.analogRX;
			}
		}

		if(state.hasAnalogJoyData() && oldState.hasAnalogJoyData()){
			if(state.analogLX != oldState.analogLX){
				lfDebug() << "Lx: " << state.analogLX;
			}
		}


		uint8_t* old = (uint8_t*)&oldState;
		uint8_t* n = (uint8_t*)&state;
		bool headerPrinted = false;
		for(int i = 0; i < sizeof(state); ++i){
			if(n[i] != old[i]){
				if(!headerPrinted){
					lfDebug() << "[!] Change:";
					headerPrinted = true;
				}
				lfDebug() << "b" << i << ": " << n[i];
			}
		}


		oldState = state;



		/*lfDebug() << "Select device";
		gamepad.selectDevice();*/

		//HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
		//		DWT_Delay_us(500000);

		/*lfDebug() << "Deselect device";
		gamepad.deselectDevice();

		HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
				DWT_Delay_us(1000000);*/
		//


	}
}

extern "C" void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName){
	Debug::Log() << "Stack overflow in task " << (const char*)pcTaskName;
	for(;;);
}
extern "C" void vApplicationMallocFailedHook(void){
	Debug::Log() << "Malloc failed";
	for(;;);
}