// --------------------------------------------------------
// Nucleo - STM32C031C6

// --------------------------------------------------------
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <float.h>
#include <stm32c0xx_hal.h>
#include <stm32c031xx.h>

// --------------------------------------------------------
// ST Nucleo Green user LED (PA5)
#define LED_PORT                GPIOA
#define LED_PIN                 GPIO_PIN_5
#define LED_PORT_CLK_ENABLE     __HAL_RCC_GPIOA_CLK_ENABLE

// --------------------------------------------------------
#define SERVO_PORT              GPIOB
#define SERVO_PIN               GPIO_PIN_3 // PB3 (PWM - TIM1-CH2)
#define SERVO_PORT_CLK_ENABLE   __HAL_RCC_GPIOB_CLK_ENABLE
// Servo Position
#define SERVO_OPEN_ANGLE 180
#define SERVO_CLOSE_ANGLE 0

// --------------------------------------------------------
#define __HAL_TIM_GET_PRESCALER(__HANDLE__)  ((__HANDLE__)->Instance->PSC)
#define __HAL_TIM_IS_RUNNING(__HANDLE__)    (((__HANDLE__)->Instance->CR1 & (TIM_CR1_CEN)) == (TIM_CR1_CEN))

#define __HAL_ADC_GET_ISRLREG(__HANDLE__)  ((__HANDLE__)->Instance->ISR)
#define __HAL_ADC_IS_FLAG(__HANDLE__, __FLAG__)     ((((__HANDLE__)->Instance->ISR) & (__FLAG__)) == (__FLAG__))
#define __HAL_ADC_IS_READY(__HANDLE__)     ((((__HANDLE__)->Instance->ISR) & (ADC_FLAG_RDY)) == (ADC_FLAG_RDY))

ADC_HandleTypeDef         hadc1;
TIM_HandleTypeDef         htim1;
UART_HandleTypeDef        huart2;

#define ADC_HANDLE 	      &hadc1
#define SERVO_TIM_HANDLE 	&htim1
#define SERVO_TIM_CHANNEL TIM_CHANNEL_2

// --------------------------------------------------------
#define SENSOR1_HADC 				ADC1
#define SENSOR2_HADC 				ADC1

typedef enum
{
  FALSE = 0,
  TRUE = 1,
} SENSOR_STAT_Def;

typedef enum
{
  SENSOR_ID_DUMMY  = -1,
  SENSOR_ID1   	  = 0,
  SENSOR_ID2,
  SENSORS_SIZE,
} SENSOR_ID_SizeDef;

typedef struct _SENSOR_IO {
  int8_t				      id;					// sensor id
  ADC_HandleTypeDef  *hAdc;				// ADC handler
} SENSOR_IO_t;

typedef struct _SENSOR_t {
  SENSOR_IO_t		sensor_io;
  uint16_t 		calib_val;
  volatile struct {
    uint8_t 	sensor_inUse: 1;		// in use flag
    uint8_t 	sensor_Init: 1;		// initializasion done
    uint8_t 	sensor_Cal: 1;		 // calibration done
  };
} SENSOR_t;

SENSOR_t sensor_drv[SENSORS_SIZE] __attribute__((at(0x20000000)));

// --------------------------------------------------------
uint8_t servoPosition = 0; // 0 = Closed, 1 = Open
uint32_t psc, arr;

// --------------------------------------------------------
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_TIM1_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_ADC1_Init(void);
static void initGPIO(void);

// --------------------------------------------------------
// Function Prototypes
void setServoAngle(uint8_t angle);
void openServo(void);
void closeServo(void);
int fraction_extractor (int integer_voltage, float voltage );
void adcVoltageRead(void);
void sensors_init(void);
SENSOR_t* getSensorPtr(uint8_t sensorNo);
void sensors_debug_parameters(SENSOR_t *_ps);
void pointer_info(void);
uint32_t HAL_millis(void);

// --------------------------------------------------------
// --------------------------------------------------------
static const  SENSOR_t  SENSORS_PARAMS[SENSORS_SIZE + 1] =     {
  { { .id = SENSOR_ID1,									    /* id */
      //(ADC_HandleTypeDef *)SENSOR1_HADC /* ADC handler */
      (ADC_HandleTypeDef *)ADC_HANDLE /* ADC handler */      
    },
    .calib_val = 123,
    .sensor_inUse = FALSE,
    .sensor_inUse = FALSE,
    .sensor_Cal = FALSE
  },
  { { .id = SENSOR_ID2,									    /* id */
      (ADC_HandleTypeDef *)SENSOR2_HADC /* ADC handler */
      //(ADC_HandleTypeDef *)ADC_HANDLE /* ADC handler */         
    },
    .calib_val = 456,
    .sensor_inUse = FALSE,
    .sensor_inUse = FALSE,
    .sensor_Cal = FALSE
  },
  { { .id = SENSOR_ID_DUMMY,									    /* id */
      (ADC_HandleTypeDef *)NULL /* ADC handler */
    },
    .calib_val = 0,
    .sensor_inUse = FALSE,
    .sensor_inUse = FALSE,
    .sensor_Cal = FALSE
  }
};
/* ********************************************************
 * ********************************************************/
void osSystickHandler(void)
{
  // 1 Hz blinking:
  if ((HAL_GetTick() % 500) == 0)
  {
    HAL_GPIO_TogglePin(LED_PORT, LED_PIN);
  }
}

/* ********************************************************
 * ********************************************************/
int main(void)
{
  HAL_Init();
  SystemClock_Config();

  initGPIO();
  MX_USART2_UART_Init();
  MX_ADC1_Init();
  MX_TIM1_Init();

  setvbuf(stdout, NULL, _IONBF, 0); 

  if (SysTick_Config(SystemCoreClock / 1000))
  {
    while (1) {  /* no error must happen here, otherwise this board is dead */ }
  }

  printf("\r\n[SYS] > [%8ld]:\tSys Core Clock: %ldMhz", millis(), SystemCoreClock / 1000000);
  printf("\r\n[MSG] > [%8ld]:\tHello, %s!", millis(), "Wokwi");


  HAL_GPIO_TogglePin(LED_PORT, LED_PIN);

  // Start PWM for Servo
  // HAL_TIM_PWM_Start(SERVO_TIM_HANDLE, SERVO_TIM_CHANNEL);
  // Close Servo initially
  // closeServo();

  psc = __HAL_TIM_GET_PRESCALER(SERVO_TIM_HANDLE);
  arr = __HAL_TIM_GET_AUTORELOAD(SERVO_TIM_HANDLE);
  printf("\r\n[TIM] > [%8ld]:\tTimer: PSC=%ld\tARR=%ld", millis(), psc, arr);

  sensors_init();

  pointer_info();

  while (1)
  {
    //printf("\r\n[MSG] > [%8ld]:\tTest!", millis(), psc, arr);
    /*
      if ((HAL_GetTick() % 5000) == 0)
      {
      servoPosition = !servoPosition;

      if (servoPosition) {
        openServo();
      } else {
        closeServo();
      }
      }
    */

    if ((HAL_GetTick() % 1000) == 0)
    {
      //adcVoltageRead();
      printf(".");
      fflush(stdout); // Will now print everything in the stdout buffer
    }
  };

  return 0;
}

/* ********************************************************
 * ********************************************************/
int fraction_extractor (int integer_voltage, float voltage )
{
  int fract_voltage = 1000 * (voltage - integer_voltage);
  return fract_voltage;
}

/* ********************************************************
 * ********************************************************/
void adcVoltageRead(void)
{
  float voltage;
  int analog_value, fractional_voltage;
  static int integer_voltage;

  HAL_ADC_Start(ADC_HANDLE);
  HAL_ADC_PollForConversion(ADC_HANDLE, 100);
  analog_value = HAL_ADC_GetValue(ADC_HANDLE);
  HAL_ADC_Stop(ADC_HANDLE);

  voltage = (3.3 / 4095) * analog_value;
  fractional_voltage = fraction_extractor (integer_voltage,  voltage );
  integer_voltage = voltage;
  printf("\r\n[ADC] > [%8ld]:\tADC: %d.%d V", millis(), integer_voltage, fractional_voltage);
}

/* ********************************************************
 * ********************************************************/
SENSOR_t* getSensorPtr(uint8_t sensorNo)
{
  SENSOR_t* _ps;
  _ps = (SENSOR_t *)&sensor_drv[sensorNo % SENSORS_SIZE];
  return _ps;
}

/* ********************************************************
 * ********************************************************/
void sensors_init(void)
{
  SENSOR_t* _ps;

  // reset servo
  for (uint8_t i = 0; i < SENSORS_SIZE; i++) {
    _ps = getSensorPtr(i);
    memcpy(_ps, &SENSORS_PARAMS[i], sizeof(SENSORS_PARAMS[i]));
    sensors_debug_parameters(_ps);
  }
}

/******************************************************************************
    @brief User Servo debug parameters function
    @retval int as status
  *****************************************************************************/
void sensors_debug_parameters(SENSOR_t *_ps)
{
  printf("\r\n[SNS] -----------------------------------------");
  printf("\r\n[SNS] Sensor [%d] id ... OK!", _ps->sensor_io.id + 1);
  printf("\r\n[SNS] In use         : %s", _ps->sensor_inUse ? "Yes" : "No");
  printf("\r\n[SNS] Initialization : %s", _ps->sensor_Init ? "Done" : "Not done");
  printf("\r\n[SNS] Calibration    : %s", _ps->sensor_Cal ? "Done" : "Not done");
  printf("\r\n[SNS] Calibration val: %d", _ps->calib_val);
  printf("\r\n[SNS] >");
}

/* ********************************************************
 * ********************************************************/
void pointer_info(void)
{
  SENSOR_t* _ps;
	ADC_TypeDef* _ai;

  _ps = getSensorPtr(0);
  
  ADC1->CHSELR = 8;
  
  printf("\r\n[IFO] -----------------------------------------");
  printf("\r\n[IFO] ADC1 (&(s->r)): addr: 0x%08lx, reg: 0x%08lx (CHSELR)", &(ADC1->CHSELR), ADC1->CHSELR);  
  printf("\r\n[IFO] ADC1          : addr: 0x%08lx, reg: 0x%08lx (CHSELR)", ADC1, ADC1->CHSELR);
  printf("\r\n[IFO] hAdc     (_ps): addr: 0x%08lx, reg: 0x%08lx (CHSELR)", _ps->sensor_io.hAdc, _ps->sensor_io.hAdc->Instance->CHSELR);
  printf("\r\n[IFO] hAdc    (&_ps): addr: 0x%08lx, reg: 0x%08lx (CHSELR)", &_ps->sensor_io.hAdc, _ps->sensor_io.hAdc->Instance->CHSELR);
  printf("\r\n[IFO] hAdc (*(&_ps)): addr: 0x%08lx, reg: 0x%08lx (CHSELR)", *(&_ps->sensor_io.hAdc), _ps->sensor_io.hAdc->Instance->CHSELR);

  //*(&ADC1->ISR + (0x28 / sizeof(int32_t))) |= 5;    // CHSELR is at address offset: 0x28
  //_ps->sensor_io.hAdc->Instance->CHSELR |= 5;
  *(&_ps->sensor_io.hAdc->Instance->ISR + (0x28 / sizeof(int32_t))) = 5;

  printf("\r\n[IFO] hAdc    (&_ps): addr: 0x%08lx, reg: 0x%08lx (CHSELR)", &_ps->sensor_io.hAdc->Instance->CHSELR, _ps->sensor_io.hAdc->Instance->CHSELR);

  _ai = _ps->sensor_io.hAdc->Instance;
  *(&_ai->ISR + (0x28 / sizeof(int32_t))) = 2;

  printf("\r\n[IFO] hAdc    (&_ai): addr: 0x%08lx, reg: 0x%08lx (CHSELR)", &_ai->CHSELR, _ai->CHSELR);

  *(&_ai->CHSELR) = 3;
  printf("\r\n[IFO] hAdc(&_ai+off): addr: 0x%08lx, reg: 0x%08lx (CHSELR)", (&_ai->ISR + (0x28 / sizeof(int32_t))), _ai->CHSELR);

  _ps = getSensorPtr(0);
  //printf("\r\n[IFO] Reg  ISR: 0x%08lx [@0x%08lx]", _ps->sensor_io.hAdc, &_ps->sensor_io.hAdc->Instance->ISR);
  //printf("\r\n[IFO] Reg   CR: 0x%08lx [@0x%08lx]", _ps->sensor_io.hAdc, &_ps->sensor_io.hAdc->Instance->CR);
  //printf("\r\n[IFO] _ps: %p, id[@0x%08lx]", _ps, &_ps->sensor_io.id);
  //printf("\r\n[IFO] _ps: %p, hAdc[@0x%08lx]", _ps, &_ps->sensor_io.hAdc);
  //printf("\r\n[IFO] _ps: %p, cal[@0x%08lx]", &_ps->calib_val, &_ps->calib_val);
  //_ps = getSensorPtr(1);
  //printf("\r\n[IFO] Reg  ISR: 0x%08lx [@0x%08lx]", _ps->sensor_io.hAdc, &_ps->sensor_io.hAdc->Instance->ISR);
  //printf("\r\n[IFO] Reg   CR: 0x%08lx [@0x%08lx]", _ps->sensor_io.hAdc, &_ps->sensor_io.hAdc->Instance->CR);
  //printf("\r\n[IFO] _ps: %p, id[@0x%08lx]", _ps, &_ps->sensor_io.id);
  //printf("\r\n[IFO] _ps: %p, cal[@0x%08lx]", &_ps->calib_val, &_ps->calib_val);

  printf("\r\n[IFO] >");
  printf("\r\n[IFO] >");
}

/* ********************************************************
 * ********************************************************/
void setServoAngle(uint8_t angle)
{
  uint32_t pulse = 1000 + (angle * 1000 / 180); // Map angle to PWM pulse (1000-2000us)
  __HAL_TIM_SET_COMPARE(SERVO_TIM_HANDLE, SERVO_TIM_CHANNEL, pulse);
}

/* ********************************************************
 * ********************************************************/
void openServo(void)
{
  setServoAngle(SERVO_OPEN_ANGLE);
  HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET); // LED ON
}

/* ********************************************************
 * ********************************************************/
void closeServo(void)
{
  setServoAngle(SERVO_CLOSE_ANGLE);
  HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_RESET); // LED OFF
}

/* ********************************************************
  @brief System Clock Configuration
  @retval None
 * ********************************************************/
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** 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.HSIDiv = RCC_HSI_DIV1;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  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_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
  RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV1;

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

/* ********************************************************
 * ********************************************************/
static void initGPIO(void)
{
  GPIO_InitTypeDef GPIO_Config = {0};

  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

  GPIO_Config.Pin = LED_PIN;
  GPIO_Config.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_Config.Pull = GPIO_NOPULL;
  GPIO_Config.Speed = GPIO_SPEED_FREQ_HIGH;
  LED_PORT_CLK_ENABLE();
  HAL_GPIO_Init(LED_PORT, &GPIO_Config);

  GPIO_Config.Pin = SERVO_PIN;
  GPIO_Config.Mode = GPIO_MODE_AF_PP;
  GPIO_Config.Pull = GPIO_NOPULL;
  GPIO_Config.Speed = GPIO_SPEED_FREQ_LOW;
  GPIO_Config.Alternate = GPIO_AF1_TIM1;
  SERVO_PORT_CLK_ENABLE();
  HAL_GPIO_Init(SERVO_PORT, &GPIO_Config);
}

/* ********************************************************
    @brief USART2 Initialization Function
    @param None
    @retval None
 * ********************************************************/
static void MX_USART2_UART_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  __HAL_RCC_GPIOA_CLK_ENABLE();
  // PA2     ------> USART2_TX
  // PA3     ------> USART2_RX
  GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_3;
  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  GPIO_InitStruct.Alternate = GPIO_AF1_USART2;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  huart2.Instance = USART2;
  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.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart2.Init.ClockPrescaler = UART_PRESCALER_DIV1;
  huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* ********************************************************
 * ********************************************************/
static void MX_TIM1_Init(void)
{
  TIM_OC_InitTypeDef sConfigOC = {0};

  __HAL_RCC_TIM1_CLK_ENABLE();

  htim1.Instance = TIM1;
  htim1.Init.Prescaler = 48 - 1; // 1 MHz timer frequency
  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim1.Init.Period = 20000 - 1; // 20 ms period (50 Hz)
  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  HAL_TIM_PWM_Init(&htim1);

  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2);
}

/* ********************************************************
    @brief ADC1 Initialization Function
    @param None
    @retval None
 * ********************************************************/
static void MX_ADC1_Init(void)
{
  /* USER CODE BEGIN ADC1_Init 0 */

  /* USER CODE END ADC1_Init 0 */

  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC1_Init 1 */

  /* USER CODE END ADC1_Init 1 */

  /** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
  */
  hadc1.Instance = ADC1;
  hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV1;
  hadc1.Init.Resolution = ADC_RESOLUTION_10B;
  hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;
  hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  hadc1.Init.LowPowerAutoWait = DISABLE;
  hadc1.Init.LowPowerAutoPowerOff = DISABLE;
  hadc1.Init.ContinuousConvMode = DISABLE;
  hadc1.Init.NbrOfConversion = 1;
  hadc1.Init.DiscontinuousConvMode = DISABLE;
  hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc1.Init.DMAContinuousRequests = DISABLE;
  hadc1.Init.Overrun = ADC_OVR_DATA_PRESERVED;
  hadc1.Init.SamplingTimeCommon1 = ADC_SAMPLETIME_1CYCLE_5;
  hadc1.Init.SamplingTimeCommon2 = ADC_SAMPLETIME_1CYCLE_5;
  hadc1.Init.OversamplingMode = DISABLE;
  hadc1.Init.TriggerFrequencyMode = ADC_TRIGGER_FREQ_HIGH;
  if (HAL_ADC_Init(&hadc1) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_0;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLINGTIME_COMMON_1;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC1_Init 2 */

  /* USER CODE END ADC1_Init 2 */

}

/* ********************************************************
 * ********************************************************/
void Error_Handler(void)
{
  /* User can add his own implementation to report the HAL error return state */
}

/* ********************************************************
 * ********************************************************/
// The following makes printf() write to USART2:

#define STDOUT_FILENO   1
#define STDERR_FILENO   2

int _write(int file, uint8_t *ptr, int len)
{
  switch (file)
  {
    case STDOUT_FILENO:
      HAL_UART_Transmit(&huart2, ptr, len, HAL_MAX_DELAY);
      break;

    case STDERR_FILENO:
      HAL_UART_Transmit(&huart2, ptr, len, HAL_MAX_DELAY);
      break;

    default:
      return -1;
  }

  return len;
}

/* ********************************************************
 * ********************************************************/
uint32_t HAL_millis(void)
{
  return HAL_GetTick();		// (osKernelGetTickCount() /or osKernelSysTick())
}