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

#include <LFramework/Debug.h>
#include <LFramework/IO/Terminal/TerminalAnsi.h>
#include <LFramework/Threading/Thread.h>
#include <cstring>
#include <usart.h>
#include <tim.h>

#include <Usb/Usb.h>

#include <LFramework/DeviceNetwork/Node.h>
#include <LFramework/DeviceNetwork/Device/UsbTransmitter.h>
#include <LFramework/DeviceNetwork/TaskManager.h>

using namespace LFramework;
using namespace LFramework::DeviceNetwork;


class TestTask : public LFramework::DeviceNetwork::Task {
public:
	bool packet(PacketHeader header, const void* data) override {
		lfDebug() << "Task packet receive: " << header.id << ":" << header.size;
		return true;
	}

	void run(ITaskContext* context) override {
		lfDebug() << "Task started";
		MaxPacket packet;
		packet.header.id = 7;
		packet.header.size = 3;
		packet.payload[0] = 0;
		packet.payload[1] = 0;
		packet.payload[2] = 0;
		while(!context->isExitRequested()){
			context->readPackets();
			bool writeResult = context->packet(packet.header, packet.payload.data());
			//lfDebug() << "Task packet write: " << (writeResult ? "OK" : "FAIL");
			Threading::ThisThread::sleepForMs(10);
		}
		lfDebug() << "Task stopped";
	}
};


class TestTaskManager : public LFramework::DeviceNetwork::TaskManager {
public:
	Task* createTask() {
		return new TestTask();
	}
	void deleteTask(Task* task) {
		delete task;
	}
};

#define MOTOR_DSHOT1200_MHZ 24
#define MOTOR_DSHOT600_MHZ 12
#define MOTOR_DSHOT300_MHZ 6
#define MOTOR_DSHOT150_MHZ 3
#define MOTOR_BIT_0     7
#define MOTOR_BIT_1     14
#define MOTOR_BITLENGTH 20

#define MOTOR_DMA_BUFFER_SIZE 17

uint32_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE];

static bool dmaRunning = false;

void motorSetSpeed(std::uint16_t value, bool requestTelemetry = false){
	//cap value
	if(value > 2047){
		value = 2047;
	}

	//add request telemetry bit
	value = (value << 1) | (requestTelemetry ? 1 : 0);


	bool useDshotTelemetry = false;  //WTF?

	// compute checksum
	uint16_t csum = 0;
	uint16_t csum_data = value;
	for (int i = 0; i < 3; i++) {
		csum ^=  csum_data;
		csum_data >>= 4;
	}

	if (useDshotTelemetry) {
		csum = ~csum;
	}

	//append checksum
	value = (value << 4) | (csum & 0x0f);

	//fill PWM DMA buffer
	for(int i = 0; i < 16; ++i){
		dmaBuffer[i] = (value & (0x8000 >> i)) ? MOTOR_BIT_1 : MOTOR_BIT_0;
	}

	//Set additional (tail) value to zero to make sure that gap between packets is at zero level
	dmaBuffer[16] = 0;

	//Start timer DMA
    dmaRunning = true;
    if(HAL_TIM_PWM_Start_DMA(&htim5, TIM_CHANNEL_2, dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK){
    	lfDebug() << "Failed to start timer DMA";
    }

    //Wait DMA complete
    while(dmaRunning){
		asm("nop");
	}
}



extern "C" void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) {
	//HAL_TIM_PWM_Stop_DMA(&htim5, TIM_CHANNEL_2);
	  __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC2);
	  (void)HAL_DMA_Abort_IT(htim->hdma[TIM_DMA_ID_CC2]);

	//lfDebug() << "S";
	dmaRunning = false;
}




std::uint16_t speedValues[] {
	48,
	100,
	300
};


int speedsCount = std::extent_v<decltype(speedValues)>;
int currentSpeedId =0;



extern"C" void StartDefaultTask(void const * argument){
	Terminal::out << Terminal::Ansi::Cursor::MoveHome() << Terminal::Ansi::Viewport::ClearScreen();
	Debug::Log() << "Hello!";


	bool buttonState = false;

	int id = 0;


	for(int i = 0; i < 2500; ++i){
		motorSetSpeed(0);
		Threading::ThisThread::sleepForMs(1);
	}
	//lfDebug() << "Armed";


	lfDebug() << "Cycle running...";
	for(;;){

		motorSetSpeed(speedValues[currentSpeedId]);
		Threading::ThisThread::sleepForMs(1);
		HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);

		if(id % 100 == 0){
			bool newButtonState = HAL_GPIO_ReadPin(Button_GPIO_Port, Button_Pin) == GPIO_PIN_SET;

			if(!buttonState && newButtonState){
				//lfDebug() << "Button pressed";

				currentSpeedId = (currentSpeedId + 1) % speedsCount;

				auto speed = speedValues[currentSpeedId];
				lfDebug() << "New speed: " << speed;

			}
			buttonState = newButtonState;

		}
	}
}

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