#include <FreeRTOS.h>
#include <LFramework/Debug.h>
#include <LibPad/PsIoPhy.h>
#include <LibPad/PsPad.h>
#include <SoftGL/BlockRasterizer.h>
#include <SoftGL/Engine/camera.h>
#include <SoftGL/Engine/cube_generator.h>
#include <SoftGL/Engine/gameobject.h>
#include <SoftGL/Engine/mesh.h>
#include <SoftGL/Graphics2D.h>
#include <SoftGL/PSDefault.h>
#include <SoftGL/PSDefaultColor.h>
#include <SoftGL/StaticBuffer.h>
#include <SoftGL/VSDefault.h>
#include <SoftGL/Vertex.h>
#include <SoftGL/texture_utils.h>
#include <cstdio>
#include <cstring>
#include <fatfs.h>
#include <main.h>
#include <stm32h7xx_hal_uart.h>
#include <string>
#include <task.h>
#include "SwapChain.h"
#include "UartReader.h"
#include "EscController.h"
#include <atomic>

#include "sdcard.h"
//#include <SoftGL/Engine/camera_controller.h>

#include <stm32h7xx_ll_usart.h>

EscController escController;
std::atomic<std::uint16_t> throttle = 0;


extern "C" void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) {
    if(htim == &htim2){
        escController.onTimPwmPulseFinished(htim);
    }

}


void readString() {

    char buffer[4];

    for (int i = 0; i < sizeof(buffer); ++i) {
        while (!LL_USART_IsActiveFlag_RXNE(LF_TERMINAL_STREAM_USART)) {
        }

        std::uint8_t data = LL_USART_ReceiveData8(LF_TERMINAL_STREAM_USART);
        buffer[i] = (char)data;
    }

    lfDebug() << "Received chars: ";

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

    //++data;
    // LF_TERMINAL_STREAM_USART->
}

extern "C" void mainThread() {
    lfDebug() << "Hello!";

    escController.sendThrottle(0);

    UartReader reader;
    reader.start([](const char* data, std::size_t length) {
        char* end = nullptr;
        auto number = strtol(data, &end, 10);
        if (data == end) {
            lfDebug() << "Received not a number";
        } else {
            lfDebug() << "Received number: " << number;

            if(number < 0){
                number = 0;
            }

            if(number > 1999){
                number = 1999;
            }

            throttle = (std::uint16_t)number;
        }
    });

    while (true) {
        //readString();
        escController.sendThrottle(throttle.load());
     vTaskDelay(1);
       // lfDebug() << "Throttle: " << throttle.load();
    }

    // FATFS fs;
    // FATFS* fsPtr = &fs;
    // int result = SDCARD_Init();
    // MX_FATFS_Init();
    // auto res = f_mount(&fs, USERPath, 1);
    // if(res != FR_OK) {
    //     lfDebug() << "SD Card mount failed!";
    // }else{
    //     lfDebug() << "SD Card mount success!";
    // }

    // if(fs.fs_type != FS_FAT32){
    //     lfDebug() << "not fat32: formatting...";

    //     res = f_mkfs(USERPath, FM_FAT32, 0, fsbuffer, sizeof(fsbuffer));

    //     if(res == FR_OK){
    //         lfDebug() << "f_mkfs SUCCESS";
    //     }else{
    //         lfDebug() << "f_mkfs FAILED";
    //     }
    // }

    // DIR dir;
    // res = f_opendir(&dir, "/");
    // if(res != FR_OK) {
    //     lfDebug() << "f_opendir() failed";
    // }else{
    //     lfDebug() << "f_opendir() success";
    // }

    // FILINFO fileInfo;
    // for(;;) {
    //     res = f_readdir(&dir, &fileInfo);
    //     if((res != FR_OK) || (fileInfo.fname[0] == '\0')) {
    //         break;
    //     }

    //     if(fileInfo.fattrib & AM_DIR) {
    //         lfDebug() << "  DIR:";
    //         lfDebug() << fileInfo.fname;

    //     } else {
    //         lfDebug() << "  File:";
    //         lfDebug() << fileInfo.fname;
    //     }
    // }

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

    // PsIoStm32HalSpiPhy psIoPhy;

    // PsIoPort psPort;
    // psPort.phy = &psIoPhy;

    // PsPad pad;
    // pad.port = &psPort;

    // swapChain.init();

    // auto& backBuffer = swapChain.getBackBuffer();
    // TextureWriter writer;
    // writer.texture = &backBuffer;

    // Game_object go;
    // Camera camera(&go);
    // camera.SetZFar(30);
    // camera.SetAspect((float)SwapChain::Lcd::Width / (float)SwapChain::Lcd::Height);
    // //CameraController camController(&camera);

    // go.transform.SetLocalPosition(float3(0.0f, 0.0f, -10.0f));

    // StaticInputLayout<3> layout;
    // layout.elements = {
    // 	InputElement("POSITION", 0, RegType::float4, 0),
    // 	InputElement("NORMAL", 16, RegType::float3, 0),
    // 	InputElement("TEXCOORD", 28, RegType::float2, 0)
    // };

    // rasterizer.SetInputLayout(&layout);
    // rasterizer.SetPrimitiveType(PT_TRIANGLE_LIST);

    // rasterizer.setColorBuffer(&backBuffer);
    // rasterizer.setDepthBuffer(&depthBuffer);

    // cube.vertexBuffer = &cubeVb;
    // cube.submeshes[0] = &cubeIb;
    // CubeGenerator<Vertex, indices_t>::Generate(&cube);

    // auto vs = VSDefault();
    // auto ps = PSDefaultColor();

    // rasterizer.SetVertexShader(&vs);
    // rasterizer.SetPixelShader(&ps);

    // auto startTime = HAL_GetTick();

    // pad.psPadConfigEnter();
    // pad.psPadSetControllerMode(true, false);
    // pad.psPadConfigExit();

    // pad.psPadConfigEnter();
    // auto mask = DataMask::allDigitalAndAnalogJoy();
    // pad.psPadSetDataMask(mask.value.b0, mask.value.b1, mask.value.b2);
    // pad.psPadConfigExit();

    // int id = 0;

    // float rx = 0.0f;
    // float ry = 0.0f;
    // float rz = 0.0f;

    // float scale = 1.0f;

    // while(true){
    //     //clear render targets

    //     pad.psPadPollAnalog();

    //     PollResponse* response = reinterpret_cast<PollResponse*>(&psPort.rxBuffer.data[0]);

    //     float flx = intToFloatAxisValue(response->analogLX);
    //     float fly = intToFloatAxisValue(response->analogLY);

    //     float frx = intToFloatAxisValue(response->analogRX);
    //     float fry = intToFloatAxisValue(response->analogRY);

    //     rx += flx * 0.2f;
    //     ry += fly * 0.2f;
    //     rz += frx * 0.2f;

    //     float scaleSpeed = 0.95f;
    //     if(response->cross()){
    //         scale /= scaleSpeed;
    //     }

    //     if(response->triangle()){
    //         scale *= scaleSpeed;
    //     }

    // 	texture_utils::fill<uint16_t>(&backBuffer, 0x1f );//0x00232327
    // 	texture_utils::fill<float>(&depthBuffer, 1.0f);

    //     auto rotx = matrix4x4RotationQuaternion(Quaternion_f::angleAxis(rx, float3(1.0f,0.0f,0.0f)));
    //     auto roty = matrix4x4RotationQuaternion(Quaternion_f::angleAxis(ry, float3(0.0f,1.0f,0.0f)));
    //     auto rotz = matrix4x4RotationQuaternion(Quaternion_f::angleAxis(rz, float3(0.0f,0.0f,1.0f)));

    //     auto rot = lm::mul(rotz, lm::mul(roty, rotx));

    //     vs.mWorld = lm::mul(matrix4x4Scale<float>(scale, scale, scale), rot);
    // 	vs.mView = camera.world_to_camera_matrix();
    // 	vs.mProj = camera.GetProjection();

    //     //clear(writer, Color16::blue());
    //     //drawCircle(60, 60, 50, writer, Color16::green() );
    //     //writer.setPixel(id %320, 200, Color16::white());
    //     DrawMesh(&rasterizer, &cube);

    //     swapChain.present();

    //     if(id % 100 == 0){
    //         auto endTime = HAL_GetTick();
    //         auto dt = endTime - startTime;
    //         startTime = endTime;

    //         if(dt > 0){
    //             float frameTimeMs = (float)dt / 100.0f;
    //             float fps = 1000.0f / frameTimeMs;
    //             sprintf(buffer, "%f\r\n", fps);
    //             print(buffer);
    //         }

    //     }

    //     ++id;

    HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
    //}
}