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

/* ================== ХЭНДЛЫ ПЕРИФЕРИИ ================== */

UART_HandleTypeDef huart2;
TIM_HandleTypeDef htim3;

/* ================== КОНСТАНТЫ СЕРВОПРИВОДА ================== */

#define SERVO_MIN_PULSE  500    /* 0.5мс → 0°   */
#define SERVO_MAX_PULSE  2500   /* 2.5мс → 180° */
#define SERVO_ANGLE_MAX  180
#define ANGLE_STEP       2

/* Кнопки: PB0 (D3) = увеличить, PB1 (D6) = уменьшить */
#define BTN_UP_PORT   GPIOA
#define BTN_UP_PIN    GPIO_PIN_0
#define BTN_DOWN_PORT GPIOA
#define BTN_DOWN_PIN  GPIO_PIN_1  

int angle = 90;

/* ================== ПРОТОТИПЫ ================== */

void SystemClock_Config(void);
void MX_USART2_UART_Init(void);
void MX_TIM1_PWM_Init(void);
void MX_GPIO_Init(void);
void setAngle(int ang);

/* ================== printf → USART2 ================== */

int _write(int file, uint8_t *ptr, int len) {
  HAL_UART_Transmit(&huart2, ptr, len, HAL_MAX_DELAY);
  return len;
}

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

/* ================== УПРАВЛЕНИЕ СЕРВОПРИВОДОМ ================== */

void setAngle(int ang) {
  if (ang < 0) ang = 0;
  if (ang > SERVO_ANGLE_MAX) ang = SERVO_ANGLE_MAX;
  uint32_t ccr = SERVO_MIN_PULSE +
    (uint32_t)ang * (SERVO_MAX_PULSE - SERVO_MIN_PULSE) / SERVO_ANGLE_MAX;
  __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, ccr);
}

/* ================== ИНИЦИАЛИЗАЦИЯ ================== */

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

  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSIDiv = RCC_HSI_DIV1;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  HAL_RCC_OscConfig(&RCC_OscInitStruct);

  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;
  HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1);
}


void MX_GPIO_Init(void) {
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

  GPIO_InitTypeDef g = {0};
  g.Pin = GPIO_PIN_0 | GPIO_PIN_1;
  g.Mode = GPIO_MODE_INPUT;
  g.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(GPIOA, &g);
}


void MX_USART2_UART_Init(void) {
  __HAL_RCC_USART2_CLK_ENABLE();

  GPIO_InitTypeDef g = {0};
  g.Pin = GPIO_PIN_2 | GPIO_PIN_3;
  g.Mode = GPIO_MODE_AF_PP;
  g.Pull = GPIO_NOPULL;
  g.Speed = GPIO_SPEED_FREQ_LOW;
  g.Alternate = GPIO_AF1_USART2;
  HAL_GPIO_Init(GPIOA, &g);

  huart2.Instance = USART2;
  huart2.Init.BaudRate = 9600;
  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;
  HAL_UART_Init(&huart2);
}

void MX_TIM3_PWM_Init(void) {
  __HAL_RCC_TIM3_CLK_ENABLE();

  GPIO_InitTypeDef g = {0};
  g.Pin = GPIO_PIN_7;
  g.Mode = GPIO_MODE_AF_PP;
  g.Pull = GPIO_NOPULL;
  g.Speed = GPIO_SPEED_FREQ_HIGH;
  g.Alternate = GPIO_AF1_TIM3;
  HAL_GPIO_Init(GPIOA, &g);

  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 47;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 19999;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  HAL_TIM_PWM_Init(&htim3);

  TIM_OC_InitTypeDef oc = {0};
  oc.OCMode = TIM_OCMODE_PWM1;
  oc.Pulse = 1500;
  oc.OCPolarity = TIM_OCPOLARITY_HIGH;
  oc.OCFastMode = TIM_OCFAST_DISABLE;
  HAL_TIM_PWM_ConfigChannel(&htim3, &oc, TIM_CHANNEL_2);
}

/* ================== MAIN ================== */

int main(void) {
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_USART2_UART_Init();
  MX_TIM3_PWM_Init();

  /* Запуск ШИМ */
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);

  /* Начальный угол 90° */
  angle = 90;
  setAngle(angle);

  printf("Servo started at %d deg\r\n", angle);

  while (1) {
    uint8_t up = HAL_GPIO_ReadPin(BTN_UP_PORT, BTN_UP_PIN);
    uint8_t down = HAL_GPIO_ReadPin(BTN_DOWN_PORT,  BTN_DOWN_PIN);

    printf("UP:%d DOWN:%d angle:%d\r\n", up, down, angle);

    if (up == GPIO_PIN_RESET) {
      if (angle <= SERVO_ANGLE_MAX - ANGLE_STEP) {
        angle += ANGLE_STEP;
        setAngle(angle);
      }
      while (HAL_GPIO_ReadPin(BTN_UP_PORT, BTN_UP_PIN) == GPIO_PIN_RESET);
      HAL_Delay(50);
    }

    if (down == GPIO_PIN_RESET) {
      if (angle >= ANGLE_STEP) {
        angle -= ANGLE_STEP;
        setAngle(angle);
      }
      while (HAL_GPIO_ReadPin(BTN_DOWN_PORT, BTN_DOWN_PIN) == GPIO_PIN_RESET);
      HAL_Delay(50);
    }

    HAL_Delay(200);
  }
}