/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file    stm32f1xx_it.c
 * @brief   Interrupt Service Routines.
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2024 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f1xx_it.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "pca9685.h"
#include "IR.h"

#include "usb_device.h"
#include "usbd_cdc_if.h"

#include "GlobalDefines.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */

/* USER CODE END TD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */

uint8_t myi;
extern uint16_t vi; // ������� ���������� ������ ��� �� 0..599
uint16_t v1[30];    // ������ ��������� (�������) [0] - ����� ���������, [1-29] - ������
uint16_t v2[30];

uint32_t vt1 = 0; // ����������� �������� (�� ������� ���������) (�������)
uint32_t vt2 = 0;

uint16_t vs1[4] = {0, 0, 0, 0}; // ������ ����������� �������� �� 3 ���������
uint16_t vs2[4] = {0, 0, 0, 0}; //[0] - ������� �������� ��������
                                //[1-3] - �������� �� vt

uint32_t vsk1 = 0; // ���������� ������� �������� (�������)
uint32_t vsk2 = 0;

uint16_t Vzad1 = 0; // ������� �������� �������� ������
uint16_t Vupr1 = 0; // ������� ��� �������� ������

uint16_t Vzad2 = 0; // ������� �������� ������� ������
uint16_t Vupr2 = 0; // ������� ��� ������� ������

extern uint8_t rejim[60];    // 0 - chislo rejimov, 1-6 - rejim1, 7-12 - rejim2...
extern uint8_t avto;         // vkl/otkl avtomaticheskoi raboti
extern uint8_t rejim_number; // nomer tekyshego rejima

extern uint8_t initcomlete;
extern uint16_t timing1;      // ���������� ��������
extern uint16_t timing2;      // ���������� ��������
extern uint8_t velosety[600]; // ����� � ������� ��� ��

extern int16_t Vz1;
extern int16_t Vz2;

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
int32_t computePID1(uint16_t input, uint16_t setpoint);
int32_t computePID2(uint16_t input, uint16_t setpoint);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
int32_t computePID1(uint16_t input, uint16_t setpoint)
{
  static int32_t integral = 0, prevErr = 0; // ������������ ������������ � ���������� ������

  int32_t err = setpoint - input; // 1.������

  integral = integral + err / 20; // 2.������������ ������������ dt = 0.01, ki = 5;

  if (integral > 4000)
  {
    integral = 4000;
  }
  if (integral < 0)
  {
    integral = 0;
  }

  int32_t D = (err - prevErr); // 3.���������������� ������������ dt = 0.01, kd = 0.01
  if (D > 4000)
  {
    D = 4000;
  }
  if (D < 0)
  {
    D = 0;
  }

  prevErr = err;
  //
  int32_t Rout = ((err * 3) >> 1) + integral + D; // 4.   �=1,5 + �=5 + �=0.01
  if (Rout > 4000)
  {
    Rout = 4000;
  } //
  if (Rout < 0)
  {
    Rout = 0;
  } //

  return Rout;
}

int32_t computePID2(uint16_t input, uint16_t setpoint)
{
  static int32_t integral = 0, prevErr = 0; // ������������ ������������ � ���������� ������
  int32_t err = setpoint - input;           // 1.������

  integral = integral + err / 20; // 2.������������ ������������ dt = 0.01, ki = 5;
  if (integral > 4000)
  {
    integral = 4000;
  }
  if (integral < 0)
  {
    integral = 0;
  }

  int32_t D = (err - prevErr); // 3.���������������� ������������ dt = 0.01, kd = 0.01
  if (D > 4000)
  {
    D = 4000;
  }
  if (D < 0)
  {
    D = 0;
  }

  prevErr = err;
  //
  int32_t Rout = ((err * 3) >> 1) + integral + D; // 4.   �=1,5 + �=5 + �=0.01
  if (Rout > 4000)
  {
    Rout = 4000;
  } //
  if (Rout < 0)
  {
    Rout = 0;
  } //

  return Rout;
}
/* USER CODE END 0 */

/* External variables --------------------------------------------------------*/
extern PCD_HandleTypeDef hpcd_USB_FS;
extern I2C_HandleTypeDef hi2c1;
extern TIM_HandleTypeDef htim3;
extern UART_HandleTypeDef huart3;
/* USER CODE BEGIN EV */
extern uint8_t uart_rx_buffer[UART_BUFFER_SIZE];
/* USER CODE END EV */

/******************************************************************************/
/*           Cortex-M3 Processor Interruption and Exception Handlers          */
/******************************************************************************/
/**
  * @brief This function handles Non maskable interrupt.
  */
void NMI_Handler(void)
{
  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */

  /* USER CODE END NonMaskableInt_IRQn 0 */
  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
  while (1)
  {
  }
  /* USER CODE END NonMaskableInt_IRQn 1 */
}

/**
  * @brief This function handles Hard fault interrupt.
  */
void HardFault_Handler(void)
{
  /* USER CODE BEGIN HardFault_IRQn 0 */

  /* USER CODE END HardFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
    /* USER CODE END W1_HardFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Memory management fault.
  */
void MemManage_Handler(void)
{
  /* USER CODE BEGIN MemoryManagement_IRQn 0 */

  /* USER CODE END MemoryManagement_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
    /* USER CODE END W1_MemoryManagement_IRQn 0 */
  }
}

/**
  * @brief This function handles Prefetch fault, memory access fault.
  */
void BusFault_Handler(void)
{
  /* USER CODE BEGIN BusFault_IRQn 0 */

  /* USER CODE END BusFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
    /* USER CODE END W1_BusFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Undefined instruction or illegal state.
  */
void UsageFault_Handler(void)
{
  /* USER CODE BEGIN UsageFault_IRQn 0 */

  /* USER CODE END UsageFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
    /* USER CODE END W1_UsageFault_IRQn 0 */
  }
}

/**
  * @brief This function handles System service call via SWI instruction.
  */
void SVC_Handler(void)
{
  /* USER CODE BEGIN SVCall_IRQn 0 */

  /* USER CODE END SVCall_IRQn 0 */
  /* USER CODE BEGIN SVCall_IRQn 1 */

  /* USER CODE END SVCall_IRQn 1 */
}

/**
  * @brief This function handles Debug monitor.
  */
void DebugMon_Handler(void)
{
  /* USER CODE BEGIN DebugMonitor_IRQn 0 */

  /* USER CODE END DebugMonitor_IRQn 0 */
  /* USER CODE BEGIN DebugMonitor_IRQn 1 */

  /* USER CODE END DebugMonitor_IRQn 1 */
}

/**
  * @brief This function handles Pendable request for system service.
  */
void PendSV_Handler(void)
{
  /* USER CODE BEGIN PendSV_IRQn 0 */

  /* USER CODE END PendSV_IRQn 0 */
  /* USER CODE BEGIN PendSV_IRQn 1 */

  /* USER CODE END PendSV_IRQn 1 */
}

/**
  * @brief This function handles System tick timer.
  */
void SysTick_Handler(void)
{
  /* USER CODE BEGIN SysTick_IRQn 0 */

  // ��������� ������� ��������� ��������� 1
  if (v1[0] > 0)
  { // ��������� ���� �� ���������� ������ � ������ ���������
    // ������� ������� ��������
    vt1 = 0;
    for (uint8_t i = 1; i <= v1[0]; i++)
    {
      vt1 += v1[i];
    }
    vt1 = vt1 / v1[0];
    // ���� �� ����� ������ ���, �� ����� ��������� ���������� ��������
    // ��������
  }
  else
  {
    vt1 = timing1;
  }
  v1[0] = 0; // �������� ������� ��������� ������� ���������
             // ������ ������ �������� ������

  // ��������� ������� ��������� ��������� 2
  if (v2[0] > 0)
  { // ��������� ���� �� ���������� ������ � ������ ���������
    // ������� ������� ��������
    vt2 = 0;
    for (uint8_t i = 1; i <= v2[0]; i++)
    {
      vt2 += v2[i];
    }
    vt2 = vt2 / v2[0];
    // ���� �� ����� ������ ���, �� ����� ��������� ���������� ��������
    // ��������
  }
  else
  {
    vt2 = timing2;
  }
  v2[0] = 0; // �������� ������� ��������� ������� ���������
             // ������ ������ �������� ������

  // ���������� ������� �� 3
  if (vs1[0] < 3)
  {
    vs1[vs1[0] + 1] = (uint16_t)vt1;
    vs1[0]++;
  }
  else
  {
    vs1[0] = 0;
    vs1[vs1[0] + 1] = (uint16_t)vt1;
    vs1[0]++;
  }

  vsk1 = (vs1[1] + vs1[2] + vs1[3]) / 3;

  if (vs2[0] < 3)
  {
    vs2[vs2[0] + 1] = (uint16_t)vt2;
    vs2[0]++;
  }
  else
  {
    vs2[0] = 0;
    vs2[vs2[0] + 1] = (uint16_t)vt2;
    vs2[0]++;
  }

  vsk2 = (vs2[1] + vs2[2] + vs2[3]) / 3;

  // ������� ������� ����� ���������� � �������� 60*1000000/(�*10)
  if (vsk1 != 0)
  {
    vsk1 = 6000000 / vsk1;
  }
  else
  {
    vsk1 = 0;
  }
  if (vsk1 > 10000) vsk1 = 10000;
  if (vsk1 < 150)   vsk1 = 0;

  if (vsk2 != 0)
  {
    vsk2 = 6000000 / vsk2;
  }
  else
  {
    vsk2 = 0;
  }
  if (vsk2 > 10000)
    vsk2 = 10000;
  if (vsk2 < 150)
    vsk2 = 0;

  // ������� ��������� �������� 0 - 200 � -8000 ... 8000 ��./���.
  if (Vz1 < 0)   {Vz1 = 0;}
  if (Vz1 > 200) {Vz1 = 200;}
  if (Vz2 < 0)   {Vz2 = 0;}
  if (Vz2 > 200) {Vz2 = 200;}

  if (Vz1 > 100)
  {
    Vzad1 = (Vz1 - 100) * 80;
  }
  else
  {
    Vzad1 = (100 - Vz1) * 80;
  }
  if (Vz2 > 100)
  {
    Vzad2 = (Vz2 - 100) * 80;
  }
  else
  {
    Vzad2 = (100 - Vz2) * 80;
  }

  // ���������
  Vupr1 = (uint16_t)computePID1(vsk1, Vzad1);
  Vupr2 = (uint16_t)computePID2(vsk2, Vzad2);

  if (Vz1 > 100)
  {
    TIM2->CCR2 = 0;
    TIM2->CCR1 = Vupr1;
  }
  else
  {
    TIM2->CCR1 = 0;
    TIM2->CCR2 = Vupr1;
  }

  if (Vz2 > 100)
  {
    TIM2->CCR3 = 0;
    TIM2->CCR4 = Vupr2;
  }
  else
  {
    TIM2->CCR4 = 0;
    TIM2->CCR3 = Vupr2;
  }


  // ������ ���������� �������� � ����� ��� ��
  if (vi < 600)
  {
    velosety[vi] = LOBYTE(timing1);
    velosety[vi + 1] = HIBYTE(timing1);
    velosety[vi + 2] = LOBYTE(vsk1);
    velosety[vi + 3] = HIBYTE(vsk1);
    velosety[vi + 4] = LOBYTE(vsk2);
    velosety[vi + 5] = HIBYTE(vsk2);
    vi = vi + 6;
  }

  //  else
  // {vi = 0;
  //	 velosety[vi] = LOBYTE(timing1);
  //	 velosety[vi+1] = HIBYTE(timing1);
  //  velosety[vi+2] = LOBYTE(vt);
  //  velosety[vi+3] = HIBYTE(vt);
  //	 velosety[vi+4] = LOBYTE(vsk);
  //  velosety[vi+5] = HIBYTE(vsk);
  //	 vi = vi + 6; }

  /* USER CODE END SysTick_IRQn 0 */
  HAL_IncTick();
  /* USER CODE BEGIN SysTick_IRQn 1 */

  /* USER CODE END SysTick_IRQn 1 */
}

/******************************************************************************/
/* STM32F1xx Peripheral Interrupt Handlers                                    */
/* Add here the Interrupt Handlers for the used peripherals.                  */
/* For the available peripheral interrupt handler names,                      */
/* please refer to the startup file (startup_stm32f1xx.s).                    */
/******************************************************************************/

/**
  * @brief This function handles EXTI line0 interrupt.
  */
void EXTI0_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI0_IRQn 0 */
  if (avto == 1)
  {
    if (rejim_number < rejim[0])
      rejim_number++;
    else
      rejim_number = 1;

    SetServo(0, rejim[1 + (rejim_number - 1) * 6]);
    SetServo(1, rejim[2 + (rejim_number - 1) * 6]);
    SetServo(2, rejim[3 + (rejim_number - 1) * 6]);
    Vz1 = rejim[4 + (rejim_number - 1) * 6];
    Vz2 = rejim[5 + (rejim_number - 1) * 6];
    TIM1->CCR1 = 0;
    TIM1->CCR2 = (uint16_t)(40 * rejim[6 + (rejim_number - 1) * 6]);
    vi = 0; // zapis grafika start
  }
  /* USER CODE END EXTI0_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(BALL_EXT_Pin);
  /* USER CODE BEGIN EXTI0_IRQn 1 */

  /* USER CODE END EXTI0_IRQn 1 */
}

/**
  * @brief This function handles EXTI line1 interrupt.
  */
void EXTI1_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI1_IRQn 0 */
  IR_handler();
  /* USER CODE END EXTI1_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(IR_EXT_Pin);
  /* USER CODE BEGIN EXTI1_IRQn 1 */

  /* USER CODE END EXTI1_IRQn 1 */
}

/**
  * @brief This function handles USB low priority or CAN RX0 interrupts.
  */
void USB_LP_CAN1_RX0_IRQHandler(void)
{
  /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 0 */

  /* USER CODE END USB_LP_CAN1_RX0_IRQn 0 */
  HAL_PCD_IRQHandler(&hpcd_USB_FS);
  /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 1 */

  /* USER CODE END USB_LP_CAN1_RX0_IRQn 1 */
}

/**
  * @brief This function handles TIM3 global interrupt.
  */
void TIM3_IRQHandler(void)
{
  /* USER CODE BEGIN TIM3_IRQn 0 */

  static uint32_t prev_capture1 = 0;
  static uint32_t prev_capture2 = 0;

  uint32_t current_capture1 = 0;
  uint32_t current_capture2 = 0;
  uint32_t elapsed_time1 = 0;
  uint32_t elapsed_time2 = 0;

  /* USER CODE BEGIN TIM3_IRQn 0 */

  // Обработка переполнения таймера
  if (TIM3->SR & TIM_SR_UIF)
  {
    // Сброс флага переполнения
    TIM3->SR &= ~TIM_SR_UIF;
  }

  // Обработка захвата сигнала для канала 1
  if (TIM3->SR & TIM_SR_CC1IF)
  {
    current_capture1 = TIM3->CCR1;

    if (current_capture1 >= prev_capture1)
    {
      elapsed_time1 = current_capture1 - prev_capture1;
    }
    else
    {
      // Учёт переполнения таймера
      elapsed_time1 = (65000 - prev_capture1) + current_capture1 + 1;
    }

    // Ограничение значения для сохранения в 16-битную переменную
    if (elapsed_time1 > 65535)
    {
      timing1 = 65535; // Максимальное значение
    }
    else
    {
      timing1 = (uint16_t)elapsed_time1;
    }

    prev_capture1 = current_capture1;
    TIM3->SR &= ~TIM_SR_CC1IF; // Сброс флага захвата сигнала
  }

  // Обработка захвата сигнала для канала 2
  if (TIM3->SR & TIM_SR_CC2IF)
  {
    current_capture2 = TIM3->CCR2;

    if (current_capture2 >= prev_capture2)
    {
      elapsed_time2 = current_capture2 - prev_capture2;
    }
    else
    {
      // Учёт переполнения таймера
      elapsed_time2 = (65000 - prev_capture2) + current_capture2 + 1;
    }

    // Ограничение значения для сохранения в 16-битную переменную
    if (elapsed_time2 > 65535)
    {
      timing2 = 65535; // Максимальное значение
    }
    else
    {
      timing2 = (uint16_t)elapsed_time2;
    }

    prev_capture2 = current_capture2;
    TIM3->SR &= ~TIM_SR_CC2IF; // Сброс флага захвата сигнала
  }

  /* USER CODE END TIM3_IRQn 0 */
  HAL_TIM_IRQHandler(&htim3);
  /* USER CODE BEGIN TIM3_IRQn 1 */

  /* USER CODE END TIM3_IRQn 1 */
}

/**
  * @brief This function handles I2C1 event interrupt.
  */
void I2C1_EV_IRQHandler(void)
{
  /* USER CODE BEGIN I2C1_EV_IRQn 0 */

  /* USER CODE END I2C1_EV_IRQn 0 */
  HAL_I2C_EV_IRQHandler(&hi2c1);
  /* USER CODE BEGIN I2C1_EV_IRQn 1 */

  /* USER CODE END I2C1_EV_IRQn 1 */
}

/**
  * @brief This function handles I2C1 error interrupt.
  */
void I2C1_ER_IRQHandler(void)
{
  /* USER CODE BEGIN I2C1_ER_IRQn 0 */

  /* USER CODE END I2C1_ER_IRQn 0 */
  HAL_I2C_ER_IRQHandler(&hi2c1);
  /* USER CODE BEGIN I2C1_ER_IRQn 1 */

  /* USER CODE END I2C1_ER_IRQn 1 */
}

/**
  * @brief This function handles USART3 global interrupt.
  */
void USART3_IRQHandler(void)
{
  /* USER CODE BEGIN USART3_IRQn 0 */
    // CDC_Transmit_FS(uart_rx_buffer, UART_BUFFER_SIZE);
    HAL_UART_Receive_IT(&huart3, uart_rx_buffer, UART_BUFFER_SIZE);

  /* USER CODE END USART3_IRQn 0 */
  HAL_UART_IRQHandler(&huart3);
  /* USER CODE BEGIN USART3_IRQn 1 */

  /* USER CODE END USART3_IRQn 1 */
}

/* USER CODE BEGIN 1 */

/* USER CODE END 1 */