#include "stdio.h"
#include "stdarg.h"
#include "string.h"
#include "pwm_ctrol.h"
#include "bluetooth.h"
#include "stm32h7xx_it.h"
#include "stm32h7xx_hal.h"
#include "main.h"
#include "mainex.h"
#include "DBG.h"
#include "Uart.h"
#include "HIDO_Util.h"

#define STATE_WAIT_RISING   0
#define STATE_WAIT_FALLING  1
uint32_t rising_time = 0;
uint32_t falling_time = 0;
uint32_t pulse_width_us = 0;
uint8_t  capture_state = STATE_WAIT_RISING;;




// ÉèÖÃËùÓеç»úµ½Ö¸¶¨Õ¼¿Õ±È
void set_all_pwm(uint16_t duty)
{
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, duty);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, duty);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, duty);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, duty);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, duty);
}

// Ïò×ó£ºÇ°ÂÖ×óת£¬ºóÂÖÓÒת
void set_pwm_left()
{
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 1000); // Ç°×ó
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 2000); // Ç°ÓÒ
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 2000); // ºó×ó
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 1000); // ºóÓÒ
}

// ÏòÓÒ£ºÇ°ÂÖÓÒת£¬ºóÂÖ×óת
void set_pwm_right()
{
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 2000); // Ç°×ó
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 1000); // Ç°ÓÒ
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 1000); // ºó×ó
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 2000); // ºóÓÒ
}


// Ó³É亯Êý£º½« [-100,100] Ó³Éäµ½ [1000,2000]
uint32_t Map(int16_t input, int16_t in_min, int16_t in_max, uint32_t out_min, uint32_t out_max)
{
    return (input - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
// ÉèÖõç»ú PWM£¨Ç°½ø/ºóÍË£©
void Set_Motor_PWM(int16_t speed)
{
    static HIDO_UINT8 l_Motor[50];
    uint32_t pulse = Map(speed, -100, 100, 1000, 2000);  // -100~100 ¡ú 1000~2000
    __HAL_TIM_SetCompare(&MOTOR_TIM, MOTOR_CHANNEL, pulse);
	HIDO_UtilSnprintf((HIDO_CHAR *)l_Motor, sizeof(l_Motor), "Motor cortrol:speed=%d,pulse=%d\r\n", speed,pulse);
    Uart_Send(UART_ID_DBG, (HIDO_UINT8 *)l_Motor, strlen(l_Motor));
}

// ÉèÖÃתÏò PWM£¨×óת/ÓÒת£©
void Set_Steering_PWM(int16_t steer)
{
   static HIDO_UINT8 l_Steering[50];
    uint32_t pulse = Map(steer, -100, 100, 1000, 2000);  // -100~100 ¡ú 1000~2000
    __HAL_TIM_SetCompare(&STEERING_TIM, STEERING_CHANNEL, pulse);
	HIDO_UtilSnprintf((HIDO_CHAR *)l_Steering, sizeof(l_Steering), "Steering cortrol:steer=%d,pulse=%d\r\n", steer,pulse);
    Uart_Send(UART_ID_DBG, (HIDO_UINT8 *)l_Steering, strlen(l_Steering));
	
}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    static HIDO_UINT8 l_pulse_width[20];

    if (htim->Instance == TIM4)
    {
        uint32_t current_time = htim->Instance->CCR1;

        if (capture_state == STATE_WAIT_RISING)
        {
            // µ±Ç°ÊÇÉÏÉýÑØ ¡ú ¼Ç¼²¢Çл»µ½µÈ´ýϽµÑØ
            rising_time = current_time;
            // Çл»ÎªÏ½µÑØ´¥·¢
            htim->Instance->CCER &= ~TIM_CCER_CC1P; // ÉÏÉýÑØ
            htim->Instance->CCER |= TIM_CCER_CC1NP; // ¼ÓÉÏ NP ±íʾ·Ç·´Ïࣿʵ¼ÊӦʹÓü«ÐÔ¿ØÖƺ¯Êý

            // ¸üÍƼöʹÓà HAL º¯ÊýÉèÖü«ÐÔ
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);

            capture_state = STATE_WAIT_FALLING;
        }
        else if (capture_state == STATE_WAIT_FALLING)
        {
            // µ±Ç°ÊÇϽµÑØ ¡ú ¼ÆËã¸ßµçƽ¿í¶È
            uint32_t pulse_width = current_time - rising_time;

            if (pulse_width > 65535)  // ³¬¹ý×î´óºÏÀíÖµ
            {
                pulse_width = 0; // »òÕß±ê¼ÇΪÎÞЧ
            }

            //printf("High Pulse Width: %lu ¦Ìs\n", pulse_width);
            HIDO_UtilSnprintf((HIDO_CHAR *)l_pulse_width, sizeof(l_pulse_width), "pulse_width=%d\r\n", pulse_width);
            Uart_Send(UART_ID_DBG, (HIDO_UINT8 *)l_pulse_width, strlen(l_pulse_width));
            // ÇлØÉÏÉýÑØ´¥·¢
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
            capture_state = STATE_WAIT_RISING;
        }

        // Çå³ýÖжϱêÖ¾
        __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC1);
    }
}

#if 0
/**
  * @brief  Update Callback (for overflow protection)
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM4)
    {
        // Ö»ÔÚ³¤Ê±¼äÎÞÏìӦʱ²ÅÖØÖÃ״̬
        static uint32_t last_reset_ms = 0;

        if (HAL_GetTick() - last_reset_ms > 100)  // 100ms ³¬Ê±
        {
            capture_state = 0;
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
            last_reset_ms = HAL_Get_Tick();
            printf("TIM4 Overflow Reset (timeout)\r\n");
        }
    }
}

#endif
#if 0
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM4 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
    {
        uint32_t current_value = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);

        if (capture_state == 0)
        {
            // ÉÏÉýÑØ£º¼Ç¼Æðʼʱ¼ä
            rising_time = current_value;
            printf("Rising Edge: %lu\r\n", rising_time);

            // Çл»ÎªÏ½µÑؼì²â
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
            capture_state = 1;
        }
        else if (capture_state == 1)
        {
            // ϽµÑØ£º¼ÆËãÂö¿í
            falling_time = current_value;
            pulse_width_us = falling_time - rising_time;

            // ·ÀÖ¹¸ºÊý£¨·ÀÖ¹Òç³ö£©
            if (pulse_width_us > 65535)  // ³¬¹ý×î´óºÏÀíÖµ
            {
                pulse_width_us = 0; // »òÕß±ê¼ÇΪÎÞЧ
            }

            printf("Falling Edge: %lu\r\n", falling_time);
            printf("Pulse Width: %lu ¦Ìs\r\n", pulse_width_us);

            // ÖØÐÂÉèÖÃΪÉÏÉýÑØ£¬µÈ´ýÏÂÒ»¸öÖÜÆÚ
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
            capture_state = 0;
        }
    }
}
#endif