/* USER CODE BEGIN PFP */
void program_start(void);
void RA_engine(uint32_t period_RA);
void RA_engine_stop(void);
uint32_t right_ascension(uint32_t *period_RA);
uint32_t declination(uint32_t *period_Dec);
void Dec_engine(uint32_t period_Dec);
void Dec_engine_stop(void);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
uint32_t period_RA = 0;
int RA_on_off = 0;
int previous_state_RA = 0; 
int previous_state_speedRA = 0;
int clickcount_RA = 1;
float speed_RA = 2;
int state_slewing_RA = 0;

void RA_engine(uint32_t period_RA){
 	  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, period_RA/2);
 	  htim4.Init.Period = period_RA;
 	  HAL_TIM_Base_Init(&htim4);
 	  HAL_TIM_Base_Start(&htim4);
      HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
}


void RA_engine_stop(){
     HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_1);  // Stop PWM

}

uint32_t right_ascension(uint32_t *period_RA) {
    int button_state_RA = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_3);
    int button_state_speedRA = HAL_GPIO_ReadPin(GPIOC, GPIO_PIN_0);

    // Toggle RA on/off
    if (button_state_RA == GPIO_PIN_SET && previous_state_RA == GPIO_PIN_RESET) {
        if (RA_on_off == 0) {
            HAL_GPIO_WritePin(GPIOF, GPIO_PIN_5, GPIO_PIN_SET);
            HAL_GPIO_WritePin(GPIOF, GPIO_PIN_10, GPIO_PIN_SET);
            RA_on_off = 1;
        } else {
            HAL_GPIO_WritePin(GPIOF, GPIO_PIN_5, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(GPIOF, GPIO_PIN_10, GPIO_PIN_RESET);
            RA_engine_stop();
            RA_on_off = 0;
        }
        present_state_lcd();
    }
    previous_state_RA = button_state_RA;

    // Change RA speed
    if (RA_on_off == 1) {
        if (button_state_speedRA == GPIO_PIN_SET && previous_state_speedRA == GPIO_PIN_RESET) {
            clickcount_RA++;
            speed_RA = (1 * pow(2, clickcount_RA));
            if (speed_RA > 32) {
                speed_RA = 2;
                clickcount_RA = 1;
            }
            present_state_lcd();
        }
    }
    previous_state_speedRA = button_state_speedRA;

    // Adjust RA direction and speed
    if (RA_on_off == 1) {
        if (HAL_GPIO_ReadPin(GPIOC, GPIO_PIN_3) == GPIO_PIN_SET) {
            *period_RA = (250 / pow(2, clickcount_RA)); // Increase speed
            HAL_GPIO_WritePin(GPIOD, GPIO_PIN_13, GPIO_PIN_RESET);
            lcd_put_cur(1, 0);
            lcd_send_string("Slewing H.A+ ...     ");
            state_slewing_RA = 1;
        } else if (HAL_GPIO_ReadPin(GPIOF, GPIO_PIN_3) == GPIO_PIN_SET) {
            *period_RA = (250 / pow(2, clickcount_RA)); // Increase speed
            HAL_GPIO_WritePin(GPIOD, GPIO_PIN_13, GPIO_PIN_SET);
            lcd_put_cur(1, 0);
            lcd_send_string("Slewing H.A- ...     ");
            state_slewing_RA = 1;
        } else {
            HAL_GPIO_WritePin(GPIOD, GPIO_PIN_13, GPIO_PIN_RESET);
            *period_RA = 990; // This is 0.25 arcmin/s - Earth's rotation speed
            if (state_slewing_RA == 1) {
                char speedStr[20];
                lcd_put_cur(1, 0);
                lcd_send_string("                    ");
                sprintf(speedStr, "Speed: %.f arcmin/s ", speed_RA);
                lcd_put_cur(1, 0);
                lcd_send_string(speedStr);
            }
            state_slewing_RA = 0;
        }
        RA_engine(*period_RA);
    }

    return *period_RA;
}


/* USER CODE END Header_StartTask1 */
void StartTask1(void *argument)
{
  /* USER CODE BEGIN 5 */
	period_RA = right_ascension(&period_RA);
  /* Infinite loop */
  for(;;)
  {
	right_ascension(&period_RA);

    osDelay(1);
  }
  /* Infinite loop */
  /* USER CODE END 5 */
}