/*
 * STM32 Wearable Stress Detection Simulation - Fixed Version
 * - PPG simulated via Potentiometer on PA0
 * - GSR simulated via Potentiometer on PA1
 * - OLED SSD1306 via I2C (PB6: SCL, PB7: SDA)
 * - Includes I2C scanner for debugging
 */

#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#include <stm32f1xx_hal.h>
#include "ssd1306.h"
#include "fonts.h"

#define ADC_RESOLUTION 4096
#define VREF 3.3f

I2C_HandleTypeDef hi2c1;
ADC_HandleTypeDef hadc1;

uint32_t lastPeakTime = 0;
bool peakDetected = false;
float hrBPM = 0.0f;
float gsrVolt = 0.0f;
int stressLevel = 0;

void SystemClock_Config(void);
void GPIO_Init(void);
void ADC1_Init(void);
void MX_I2C1_Init(void);
void Error_Handler(void);
uint16_t ADC_Read(uint32_t channel);
void ScanI2CDevices(void);
float computeHR(uint32_t rrIntervalMs);
int determineStressLevel(float hr, float gsr);
void OLED_Display(float hr, float gsr, int stressLevel);

int main(void) {
    HAL_Init();
    SystemClock_Config();
    GPIO_Init();
    ADC1_Init();
    MX_I2C1_Init();
    
    // Initialize OLED
    ssd1306_Init();
    ssd1306_Fill(Black);
    ssd1306_UpdateScreen();
    
    // Debug: Scan I2C devices
    ScanI2CDevices();
    HAL_Delay(2000);
    
    // Main loop
    while (1) {
        uint32_t currentMillis = HAL_GetTick();
        
        // Read GSR (simulated)
        uint16_t gsrRaw = ADC_Read(ADC_CHANNEL_1);
        gsrVolt = (gsrRaw * VREF) / ADC_RESOLUTION;
        
        // Read PPG (simulated via potentiometer)
        uint16_t ppgRaw = ADC_Read(ADC_CHANNEL_0);
        
        // Simulate PPG peak detection
        if (ppgRaw > 2500 && !peakDetected) {
            uint32_t rrInterval = currentMillis - lastPeakTime;
            lastPeakTime = currentMillis;
            peakDetected = true;
            
            if (rrInterval >= 300 && rrInterval <= 2000) {
                hrBPM = computeHR(rrInterval);
            }
        }
        if (ppgRaw < 2000) {
            peakDetected = false;
        }
        
        // Calculate stress level
        stressLevel = determineStressLevel(hrBPM, gsrVolt);
        
        // Update display
        OLED_Display(hrBPM, gsrVolt, stressLevel);
        
        HAL_Delay(100);
    }
}

void ScanI2CDevices(void) {
    uint8_t found = 0;
    char msg[32];
    ssd1306_Fill(Black);
    
    for (uint8_t addr = 1; addr < 127; addr++) {
        if (HAL_I2C_IsDeviceReady(&hi2c1, addr << 1, 3, 10) == HAL_OK) {
            sprintf(msg, "I2C: 0x%02X found", addr);
            ssd1306_SetCursor(0, 0);
            ssd1306_WriteString(msg, Font_7x10, White);
            ssd1306_UpdateScreen();
            found = 1;
            HAL_Delay(1000);
        }
    }
    
    if (!found) {
        ssd1306_SetCursor(0, 0);
        ssd1306_WriteString("No I2C devices!", Font_7x10, White);
        ssd1306_UpdateScreen();
    }
}

float computeHR(uint32_t rrIntervalMs) {
    return 60000.0f / rrIntervalMs;
}

int determineStressLevel(float hr, float gsr) {
    int level_hr = (hr > 120) ? 2 : (hr > 90) ? 1 : 0;
    int level_gsr = (gsr > 2.5f) ? 2 : (gsr >= 1.5f) ? 1 : 0;
    return (level_hr > level_gsr) ? level_hr : level_gsr;
}

void OLED_Display(float hr, float gsr, int stressLevel) {
    char line1[20], line2[20], line3[20];
    sprintf(line1, "HR: %.1f BPM", hr);
    sprintf(line2, "GSR: %.2f V", gsr);
    sprintf(line3, "Stress: %d", stressLevel);

    ssd1306_Fill(Black);
    ssd1306_SetCursor(0, 0);
    ssd1306_WriteString(line1, Font_7x10, White);
    ssd1306_SetCursor(0, 12);
    ssd1306_WriteString(line2, Font_7x10, White);
    ssd1306_SetCursor(0, 24);
    ssd1306_WriteString(line3, Font_7x10, White);
    ssd1306_UpdateScreen();
}

void GPIO_Init(void) {
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_AFIO_CLK_ENABLE();

    // Configure I2C pins (PB6: SCL, PB7: SDA)
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    // Configure analog inputs (PA0: PPG, PA1: GSR)
    GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1;
    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}

void ADC1_Init(void) {
    __HAL_RCC_ADC1_CLK_ENABLE();
    
    hadc1.Instance = ADC1;
    hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;
    hadc1.Init.ContinuousConvMode = DISABLE;
    hadc1.Init.DiscontinuousConvMode = DISABLE;
    hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
    hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
    hadc1.Init.NbrOfConversion = 1;
    HAL_ADC_Init(&hadc1);
    
    HAL_ADCEx_Calibration_Start(&hadc1);
}

void MX_I2C1_Init(void) {
    __HAL_RCC_I2C1_CLK_ENABLE();
    
    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 = I2C_DUALADDRESS_DISABLE;
    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
    
    if (HAL_I2C_Init(&hi2c1) != HAL_OK) {
        Error_Handler();
    }
}

uint16_t ADC_Read(uint32_t channel) {
    ADC_ChannelConfTypeDef sConfig = {0};
    sConfig.Channel = channel;
    sConfig.Rank = ADC_REGULAR_RANK_1;
    sConfig.SamplingTime = ADC_SAMPLETIME_55CYCLES_5;
    
    HAL_ADC_ConfigChannel(&hadc1, &sConfig);
    HAL_ADC_Start(&hadc1);
    HAL_ADC_PollForConversion(&hadc1, HAL_MAX_DELAY);
    uint16_t val = HAL_ADC_GetValue(&hadc1);
    HAL_ADC_Stop(&hadc1);
    return val;
}

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.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
    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_DIV1;
    HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
}

void Error_Handler(void) {
    while(1);
}

#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line) {
    while(1);
}
#endif