#include "stm32f1xx_hal.h"
#include "stm32f1xx_hal_adc.h"  // Include ADC header
#include <stdio.h>
#include <string.h>
#include <stdarg.h>  // For UART_Printf

// I2C, UART, and ADC Handles
I2C_HandleTypeDef hi2c1;
UART_HandleTypeDef huart1;
ADC_HandleTypeDef hadc1;

#define MPU6050_ADDR 0xD0  // MPU6050 I2C address
#define MAX30102_ADDR 0xAE // MAX30102 I2C address

// Function Prototypes
void SystemClock_Config(void);
void GPIO_Init(void);
void I2C1_Init(void);
void UART1_Init(void);
void ADC1_Init(void);
void MPU6050_Init(void);
void MAX30102_Init(void);
void Read_MPU6050(int16_t *accel);
void Read_MAX30102(uint8_t *data);
uint16_t Read_Heart_Rate_Pulse(void);

// UART Print Helper Function
/*
void UART_Printf(const char *fmt, ...) {
    char buffer[100];
    va_list args;
    va_start(args, fmt);
    vsnprintf(buffer, sizeof(buffer), fmt, args);
    va_end(args);
    HAL_UART_Transmit(&huart1, (uint8_t *)buffer, strlen(buffer), HAL_MAX_DELAY);
}
*/

int main(void) {
    HAL_Init();
    SystemClock_Config();
    GPIO_Init();
    I2C1_Init();
    UART1_Init();
    ADC1_Init();
    MPU6050_Init();
    MAX30102_Init();

    //UART_Printf("System Initialized!\r\n");

    while (1) {
        int16_t accel[3];
        uint8_t max30102_data[6];
        //uint16_t pulse_value;

        Read_MPU6050(accel);
        Read_MAX30102(max30102_data);
        //pulse_value = Read_Heart_Rate_Pulse();

        //UART_Printf("ACC X: %d, Y: %d, Z: %d\r\n", accel[0], accel[1], accel[2]);
        //UART_Printf("Heart Rate Pulse ADC: %d\r\n", pulse_value);
        HAL_Delay(1000);  // Delay for 1 second
    }
}

void MPU6050_Init(void) {
    uint8_t data[2];
    data[0] = 0x6B;  // Power Management 1
    data[1] = 0x00;  // Wake up MPU6050
    HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, data, 2, HAL_MAX_DELAY);
}

void Read_MPU6050(int16_t *accel) {
    uint8_t data[6];
    HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, 0x3B, I2C_MEMADD_SIZE_8BIT, data, 6, HAL_MAX_DELAY);
    accel[0] = (int16_t)(data[0] << 8 | data[1]);
    accel[1] = (int16_t)(data[2] << 8 | data[3]);
    accel[2] = (int16_t)(data[4] << 8 | data[5]);
}

void MAX30102_Init(void) {
    uint8_t data[2];
    data[0] = 0x09;  // LED Mode Control Register
    data[1] = 0x03;  // SpO2 Mode
    HAL_I2C_Master_Transmit(&hi2c1, MAX30102_ADDR, data, 2, HAL_MAX_DELAY);
}

void Read_MAX30102(uint8_t *data) {
    HAL_I2C_Mem_Read(&hi2c1, MAX30102_ADDR, 0x07, I2C_MEMADD_SIZE_8BIT, data, 6, HAL_MAX_DELAY);
}

uint16_t Read_Heart_Rate_Pulse(void) {
    HAL_ADC_Start(&hadc1);                                  // Start ADC conversion
    HAL_ADC_PollForConversion(&hadc1, HAL_MAX_DELAY);       // Wait for conversion to complete
    return HAL_ADC_GetValue(&hadc1);                        // Get ADC value
}

void I2C1_Init(void) {
    hi2c1.Instance = I2C1;
    hi2c1.Init.ClockSpeed = 100000;
    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
    hi2c1.Init.OwnAddress1 = 0;
    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
    hi2c1.Init.DualAddressMode = DISABLE;
    hi2c1.Init.GeneralCallMode = DISABLE;
    hi2c1.Init.NoStretchMode = DISABLE;
    HAL_I2C_Init(&hi2c1);
}

void UART1_Init(void) {
    huart1.Instance = USART1;
    huart1.Init.BaudRate = 115200;
    huart1.Init.WordLength = UART_WORDLENGTH_8B;
    huart1.Init.StopBits = UART_STOPBITS_1;
    huart1.Init.Parity = UART_PARITY_NONE;
    huart1.Init.Mode = UART_MODE_TX_RX;
    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart1.Init.OverSampling = UART_OVERSAMPLING_16;
    HAL_UART_Init(&huart1);
}

void ADC1_Init(void) {
	__HAL_RCC_ADC1_CLK_ENABLE();  // Enable ADC clock

	hadc1.Instance = ADC1;

	// Configure ADC parameters
	hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;            // Disable scan mode
	hadc1.Init.ContinuousConvMode = DISABLE;               // Disable continuous mode
	hadc1.Init.DiscontinuousConvMode = DISABLE;            // Disable discontinuous mode
	hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;            // Right alignment of data
	hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;      // Software trigger conversion

	HAL_ADC_Init(&hadc1);

	// Configure ADC channel
	ADC_ChannelConfTypeDef sConfig = {0};
	sConfig.Channel = ADC_CHANNEL_0;                      // Use channel IN0 (adjust as necessary)
	sConfig.Rank = ADC_REGULAR_RANK_1;                    // Regular rank configuration
	sConfig.SamplingTime = ADC_SAMPLETIME_71CYCLES_5;     // Sampling time configuration

	HAL_ADC_ConfigChannel(&hadc1, &sConfig);              // Apply channel configuration
}

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

	RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
	RCC_OscInitStruct.HSEState = RCC_HSE_ON;
	RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
	RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
	RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;

	HAL_RCC_OscConfig(&RCC_OscInitStruct);

	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_DIV4;

	HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
}

void GPIO_Init(void) {
	__HAL_RCC_GPIOC_CLK_ENABLE();

	GPIO_InitTypeDef GPIO_InitStruct = {0};
	GPIO_InitStruct.Pin = GPIO_PIN_13;           // Onboard LED pin on Blue Pill
	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;  // Push-pull output mode
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

	HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);      // Initialize GPIO pin PC13
}