#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stm32c0xx_hal.h>

// Handles UART
UART_HandleTypeDef huart2; // For USART1
UART_HandleTypeDef huart4; // For USART2

// Definition of buffers
#define BUFFER_LEN 4
uint8_t TX_BUFFER[BUFFER_LEN] = {1, 2, 3, 4}; // Initialized buffer
uint8_t RX_BUFFER[BUFFER_LEN]; // Buffer to receive values
uint8_t tx_index = 0;

// Function prototypes
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART2_UART_Init(void);
void Error_Handler(void);

// Logging function to send messages over UART2
void Log_UART2(const char* message) {
    HAL_UART_Transmit(&huart4, (uint8_t*)message, strlen(message), HAL_MAX_DELAY);
}


int main(void) {
    HAL_Init(); // Initialize HAL library
    SystemClock_Config(); // Configure system clock
    MX_GPIO_Init(); // Initialize GPIO
    MX_USART1_UART_Init(); // Initialize USART1
    MX_USART2_UART_Init(); // Initialize USART2

    // Start receiving data in interrupt mode
    HAL_UART_Receive_IT(&huart4, RX_BUFFER, 1);

    Log_UART2("System Initialized\n"); // Log initialization message

    while (1) {
        HAL_UART_Receive_IT(&huart4, RX_BUFFER, 1);
        char mmsg[5];
        sprintf(mmsg, "receiving %d\n", RX_BUFFER[0]);
        Log_UART2(mmsg);
        if (RX_BUFFER[0] != 0) {
            switch (RX_BUFFER[0]) {
                case 1: // Turn on LED PA0, turn off PA1
                    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_SET);
                    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
                    Log_UART2("LED PA0 ON, PA1 OFF\n");
                    break;
                case 2: // Turn off both LEDs
                    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_RESET);
                    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
                    Log_UART2("Both LEDs OFF\n");
                    break;
                case 3: // Turn on LED PA1, turn off PA0
                    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_RESET);
                    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_SET);
                    Log_UART2("LED PA1 ON, PA0 OFF\n");
                    break;
                case 4: // Toggle both LEDs
                    HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_0);
                    HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_1);
                    Log_UART2("Toggled both LEDs\n");
                    break;
                default:
                    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_RESET);
                    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
                    Log_UART2("Invalid Command, LEDs OFF\n");
                    break;
            }
            RX_BUFFER[0] = 0; // Clear the buffer
            HAL_UART_Receive_IT(&huart4, RX_BUFFER, 1); // Re-enable reception
        }

        if (tx_index < BUFFER_LEN) {
            HAL_UART_Transmit_IT(&huart2, &TX_BUFFER[tx_index], 1); // Transmit one byte
            Log_UART2("Transmitting: ");
            char msg[5];
            sprintf(msg, "%d\n", TX_BUFFER[tx_index]);
            Log_UART2(msg);
            tx_index++;
        }

        HAL_Delay(1000); // Delay for responsiveness
    }
}

// GPIO initialization
static void MX_GPIO_Init(void) {
    GPIO_InitTypeDef GPIO_Config;

    // Initialize LEDs on PA0 and PA1, and debug LED on PA5
    GPIO_Config.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_5;
    GPIO_Config.Mode = GPIO_MODE_OUTPUT_PP; // Push-Pull mode
    GPIO_Config.Pull = GPIO_NOPULL; // No pull-up or pull-down
    GPIO_Config.Speed = GPIO_SPEED_FREQ_LOW; // Low speed

    __HAL_RCC_GPIOA_CLK_ENABLE(); // Enable GPIOA clock
    HAL_GPIO_Init(GPIOA, &GPIO_Config); // Initialize GPIOA
}

// USART1 initialization
static void MX_USART1_UART_Init(void) {
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    __HAL_RCC_GPIOB_CLK_ENABLE(); // Enable GPIOB clock

    // Configure PB6 (TX) and PB7 (RX)
    GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; // Alternate Push-Pull
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF1_USART1; // Alternate function
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    // Configure USART1
    huart2.Instance = USART1;
    huart2.Init.BaudRate = 115200;
    huart2.Init.WordLength = UART_WORDLENGTH_8B;
    huart2.Init.StopBits = UART_STOPBITS_1;
    huart2.Init.Parity = UART_PARITY_NONE;
    huart2.Init.Mode = UART_MODE_TX_RX;
    huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart2.Init.OverSampling = UART_OVERSAMPLING_16;
    huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;

    if (HAL_UART_Init(&huart2) != HAL_OK) {
        Error_Handler();
    }
}

// USART2 initialization
static void MX_USART2_UART_Init(void) {
    GPIO_InitTypeDef GPIO_InitStruct;
    __HAL_RCC_GPIOA_CLK_ENABLE();

    // Configure PA2 (TX) and PA3 (RX)
    GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_3;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; // Alternate Push-Pull
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF1_USART2;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    // Configure USART2
    huart4.Instance = USART2;
    huart4.Init.BaudRate = 115200;
    huart4.Init.WordLength = UART_WORDLENGTH_8B;
    huart4.Init.StopBits = UART_STOPBITS_1;
    huart4.Init.Parity = UART_PARITY_NONE;
    huart4.Init.Mode = UART_MODE_TX_RX;
    huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart4.Init.OverSampling = UART_OVERSAMPLING_16;
    huart4.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;

    if (HAL_UART_Init(&huart4) != HAL_OK) {
        Error_Handler();
    }
}

// Error handler
void Error_Handler(void) {
    while (1) {
        HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_5); // Blink PA5 on error
        HAL_Delay(100);
    }
}

// Redirect printf to USART2
#define STDOUT_FILENO   1
#define STDERR_FILENO   2

int _write(int file, uint8_t *ptr, int len) {
    switch (file) {
        case STDOUT_FILENO:
        case STDERR_FILENO:
            HAL_UART_Transmit(&huart4, ptr, len, HAL_MAX_DELAY);
            break;
        default:
            return -1;
    }
    return len;
}

// System Clock Configuration (Example for STM32C0)
void SystemClock_Config(void) {
    // Implement your clock configuration here
    // This is platform-specific and depends on your STM32 variant
}