#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>

using namespace LFramework;
using namespace LFramework::USB;

__attribute__((used)) UsbDBulkInterface usbInterface;
__attribute__((used)) 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);
	}
};


__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++;
	}
}

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

	};*/


	void byteDelay() {
		DWT_Delay_us(28);
	}

	bool polling = false;
	uint32_t pollBytesDone = 0;
	uint32_t pollTotalSizeExpected = 0;

	void beginPoll() {
		if(polling){
			endPoll();
		}

		polling = true;
		pollBytesDone = 0;

		selectDevice();
		byteDelay();
		//lfDebug() << "[!] Begin poll";
		//auto result = poll(data);

	}

	uint8_t poll(uint8_t data) {
		uint8_t result = 0xFF;
		if(!polling){
			lfDebug() << "Error: Not polling";
			return result;
		}

		HAL_SPI_TransmitReceive(&hspi2, &data, &result, 1, HAL_MAX_DELAY);

		byteDelay();

		lfDebug() << "Poll: in=" << (int)data << " out: " << (int)result;

	//	pollBytesDone++;

	/*	if(pollBytesDone == 2){
			pollTotalSizeExpected = HeaderSize + (result & 0x0f) * 2;
		}*/

		//lfDebug() << "Poll: " << (int)result;
		/*if(pollTotalSizeExpected == pollBytesDone){
			endPoll();
		}*/



		return result;
	}

	void endPoll() {
		lfDebug() << "End poll";
		polling = false;
		deselectDevice();
		byteDelay();
	}

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


	/*uint8_t shift(uint8_t data){
		uint8_t result = 0xff;
		HAL_SPI_TransmitReceive(&hspi2, &data, &result, 1, HAL_MAX_DELAY);
		byteDelay();
		return result;
	}

	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);
		DWT_Delay_us(100);
	}
	void deselectDevice() {
		HAL_GPIO_WritePin(GAMEPAD_CS_GPIO_Port, GAMEPAD_CS_Pin, GPIO_PIN_SET);
		DWT_Delay_us(100);
	}
private:
	uint8_t rxBuffer[HeaderSize + MaxDataSize];
};


enum class UsbRequestType : uint8_t {
	PollBegin,
	Poll,
	EndPoll
};

struct UsbRequest {
	UsbRequestType type;
	uint8_t data;
};

struct UsbResponse {
	uint8_t data;
};


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;


	usbContext.configurationStringDescriptor = "RealPad";
	usbContext.interfaceStringDescriptor = "RealPad";
	usbContext.productStringDescriptor = "RealPad DualShock Device";
	usbContext.manufacturerStringDescriptor = "L";
	usbContext.serialStringDescriptor = "123456";
	UsbDevice::start(&usbContext);


	auto usbInterface = usbContext.getInterface(0);
	while(!usbInterface->isOpen()){
		vTaskDelay(1);
	}

	auto txEp = static_cast<UsbDEndpoint*>(usbInterface->getEndpoint(true, 0));
	auto rxEp = static_cast<UsbDEndpoint*>(usbInterface->getEndpoint(false, 0));

	Ps1Gamepad gamepad;


	/*while(true){
		auto& val = gamepad.read();
		lfDebug() << "0x" << hex(val.dummy)<< " 0x" << hex(val.id) << " 0x" << hex(val.padID);

		vTaskDelay(1000);
	}*/




	uint8_t rxBuffer[64];


	/*uint8_t txBuffer[5] = {0x01, 0x42, 0x00, 0x00, 0x00};

	while(true){
		gamepad.selectDevice();

		lfDebug() << "Begin";
		auto log = Debug::Info();

		for(int i = 0; i < sizeof(txBuffer); ++i){
			auto b = gamepad.shift(txBuffer[i]);
			log << "0x" << hex(b) << ", ";
		}

		gamepad.deselectDevice();

		vTaskDelay(100);
	}*/

	while(true){
		//receive packet
		PollingTransfer rxTransfer{};
		rxTransfer.buffer = rxBuffer;
		rxTransfer.size = sizeof(rxBuffer);
		rxEp->transferAsync(&rxTransfer);
		while(rxTransfer.getState() == PollingTransfer::State::Pending){
			portYIELD();
		}

		if(rxTransfer.actualSize == 0){ //Zero length packet == new session! Reset device to default digital state.
			//gamepad.setControllerMode(false, false); //DO NOT DO SUCH THINGS! Multitap detect failed because of this crap!

			//Send zero response
			PollingTransfer txTransfer;
			txTransfer.buffer = nullptr;
			txTransfer.size = 0;
			txEp->transferAsync(&txTransfer);
			while(txTransfer.getState() == PollingTransfer::State::Pending){
				portYIELD();
			}

		}else{
			if(rxTransfer.actualSize == sizeof(UsbRequest)){
				const UsbRequest& request = *(const UsbRequest*)&rxBuffer[0];

				UsbResponse response{0xff};

				if(request.type == UsbRequestType::PollBegin){
					DWT_Delay_us(100);
					 gamepad.beginPoll();
				}else if(request.type == UsbRequestType::Poll){
					response.data = gamepad.poll(request.data);
				}else if(request.type == UsbRequestType::EndPoll){
					gamepad.endPoll();
				}


				//Send poll response
				PollingTransfer txTransfer;
				txTransfer.buffer = &response;
				txTransfer.size = sizeof(response);
				txEp->transferAsync(&txTransfer);
				while(txTransfer.getState() == PollingTransfer::State::Pending){
					portYIELD();
				}
			}
		}
	}
}

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(;;);
}