#include "Helper/delay_helper.h"
#include "stm32h563xx.h"
#include <string>

#include "Center/common_center.hpp"
#include "Common/serial_port.hpp"
#include "Helper/gpio_helper.hpp"
#include "config.hpp"


void TerminateHandler() {
    auto led              = Config::kLed;
    auto& debugSerialPort = CommonCenter::GetDebugSerialPort();
    std::string message;
    std::exception_ptr exceptionPtr = std::current_exception();
    if (exceptionPtr) {
        try {
            std::rethrow_exception(exceptionPtr);
        } catch (const std::exception& e) {
            message = e.what();
        } catch (...) {
            message = "Unknown exception type";
        }
    } else {
        message = "No active exception, but terminate() was called";
    }

    while (true) {
        debugSerialPort.WriteLineBlocking("FATAL ERROR: %s", message.c_str());
        led.Toggle();
        DelayS(1);
    }
}

void GpioSetup() {
    GpioHelper::EnableAllGpioPeripheral();
    GpioHelper::GpioInit(GPIOB, GPIO_PIN_0, GPIO_MODE_OUTPUT_PP, GPIO_NOPULL, GPIO_PIN_RESET);
}

void CommonSetup() {
    CommonCenter::GetUltrasonic();
    CommonCenter::GetDfPlayer();
    CommonCenter::GetCanMv();
    CommonCenter::GetDebugSerialPort();
    CommonCenter::GetHc05();
}

void Setup() {
    GpioSetup();
    DelaySetup();
    CommonSetup();
    std::set_terminate(TerminateHandler);
}

extern "C" void AppStart() {
    Setup();
    auto& ultrasonic      = CommonCenter::GetUltrasonic();
    auto& debugSerialPort = CommonCenter::GetDebugSerialPort();
    while (true) {
        // Example usage of the classes
        auto disatnce = ultrasonic.GetDistance();
        debugSerialPort.WriteLineBlocking("Distance: %.2f cm", disatnce);
        DelayS(1); // Delay for 1 second
    }
}