#include "main.h"

#include <cstdint>
#include <memory>
#include <string>

#include "gpio.h"
#include "i2c.hpp"
#include "stm32_hal_legacy.h"
#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_def.h"
#include "stm32f4xx_hal_i2c.h"
#include "usart.hpp"

// needs to be global to be accessible in error handler
std::unique_ptr<driver::usart::Usart> usart2{nullptr};

void SystemClock_Config(void);
void checkHalStatus(HAL_StatusTypeDef);

int main(void) {

    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
    HAL_Init();

    /* Configure the system clock */
    SystemClock_Config();

    /* Initialize all configured peripherals */
    MX_GPIO_Init();

    usart2 = std::make_unique<driver::usart::Usart>(
        driver::usart::Usart(USART2, 115200, UART_WORDLENGTH_8B, UART_STOPBITS_1, UART_PARITY_NONE,
                             UART_MODE_TX_RX, UART_HWCONTROL_NONE, UART_OVERSAMPLING_16));

    usart2->init();
    usart2->println("");
    usart2->println("\r\nWeight cell init.");

    auto i2c1 = std::make_unique<driver::i2c::I2c>(
        I2C1, 100000, I2C_DUTYCYCLE_2, 0x00, I2C_ADDRESSINGMODE_7BIT, I2C_DUALADDRESS_DISABLED,
        0x00, I2C_GENERALCALL_DISABLED, I2C_NOSTRETCH_DISABLED);

    if (!i2c1->init()) {
        usart2->println("Error intializing I2C1!");
        usart2->println("Last Error: " + std::to_string(i2c1->getLastError()));
        Error_Handler();
    } else {
        usart2->println("I2C1 intialized successful");
    }
    // init NAU7802
    {
        std::vector<uint8_t> data = {0x01};
        //  reset NAU7802
        checkHalStatus(i2c1->write(0x2A, 0x00, data));
        usart2->println("Reset nau");
        HAL_Delay(100);

        // power up digital logic
        data[0] = 0x02;
        i2c1->write(0x2A, 0x00, data);
        HAL_Delay(100);
        usart2->println("PUP digi");

        // power up analog logic
        data[0] = 0x06;
        i2c1->write(0x2A, 0x00, data);
        HAL_Delay(100);

        usart2->println("PUP analog");

        // use internal LDO as reference
        data[0] = 0x86;
        checkHalStatus(i2c1->write(0x2A, 0x00, data));
        HAL_Delay(100);

        // print status back to usart
        auto ret = i2c1->read(0x2A, 0x00);
        checkHalStatus(ret.first);
        usart2->println("NAU7802 reports state " + std::to_string(ret.second) + " at 0x00");

        // REG_CHPS CLK_CHP off
        data[0] = 0x30;
        checkHalStatus(i2c1->write(0x2A, 0x15, data));
        HAL_Delay(100);
    }

    uint32_t measurement = 0;
    auto val             = i2c1->read(0x2A, 0x12);
    measurement |= (val.second << 16);

    val = i2c1->read(0x2A, 0x13);
    measurement |= (val.second << 8);

    val = i2c1->read(0x2A, 0x14);
    measurement |= val.second << 16;

    while (1) {
        measurement = 0;
        val         = i2c1->read(0x2A, 0x12);
        measurement |= (val.second << 16);

        val = i2c1->read(0x2A, 0x13);
        measurement |= (val.second << 8);

        val = i2c1->read(0x2A, 0x14);
        measurement |= val.second << 16;

        usart2->println("Measurement " + std::to_string(measurement) + " counts");

        HAL_Delay(1000);
    }
}

void SystemClock_Config(void) {
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

    /** Configure the main internal regulator output voltage
  */
    __HAL_RCC_PWR_CLK_ENABLE();
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);

    /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
    RCC_OscInitStruct.OscillatorType      = RCC_OSCILLATORTYPE_HSI;
    RCC_OscInitStruct.HSIState            = RCC_HSI_ON;
    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
    RCC_OscInitStruct.PLL.PLLState        = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource       = RCC_PLLSOURCE_HSI;
    RCC_OscInitStruct.PLL.PLLM            = 16;
    RCC_OscInitStruct.PLL.PLLN            = 336;
    RCC_OscInitStruct.PLL.PLLP            = RCC_PLLP_DIV4;
    RCC_OscInitStruct.PLL.PLLQ            = 7;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
        Error_Handler();
    }

    /** Initializes the CPU, AHB and APB buses clocks */
    RCC_ClkInitStruct.ClockType =
        RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource   = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider  = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) {
        Error_Handler();
    }
}

void checkHalStatus(HAL_StatusTypeDef status) {
    if (status == HAL_OK) {
        return;
    }
    if (usart2 == nullptr) {
        Error_Handler();
        return;  // never reached
    }
    usart2->println("HAL Status not okay! Calling Error Handler");
}

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void) {
    /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    if (usart2 != nullptr) {
        usart2->println("=== ERROR HANDLER ===");
        usart2->println("entering infinite loop...");
    }
    __disable_irq();
    while (1) {}
    /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t* file, uint32_t line) {
    /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
    /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */