#include "Helper/delay_helper.h"
#include "stm32h563xx.h"

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


void TerminateHandler() {
    auto led = Config::kLed;
    while (true) {
        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();
}

void Setup() {
    GpioSetup();
    DelaySetup();
    CommonSetup();
}

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