#include "encoder.h"

volatile int32_t tick_L = 0,tick_R = 0;
volatile int16_t rpm_L = 0,rpm_R = 0;

extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim8;

static TX_THREAD enc_thread;
static UCHAR enc_stack[512]; 

static void enc_thread_entry(ULONG);

UINT Encoder_ThreadCreate(void)
{
	UINT status;
	
	HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
	HAL_TIM_Encoder_Start(&htim8,TIM_CHANNEL_ALL);
	
	__HAL_TIM_SET_COUNTER(&htim1,0);
	__HAL_TIM_SET_COUNTER(&htim8,0);
	
    status = tx_thread_create(&enc_thread, "Encoder", enc_thread_entry, 0,
                     enc_stack, sizeof(enc_stack),
                     6, 6, TX_NO_TIME_SLICE, TX_AUTO_START);
					 
	return status;
}


static void enc_thread_entry(ULONG arg)
{
	const float msPerMin = 6000.0f;
	const float ticksPerRev = 1024.0f; // ÏßÊý  * 4
	uint16_t lastL = 0, lastR = 0;
	
    while (1)
    {
        uint16_t cntL = __HAL_TIM_GET_COUNTER(&htim1);
        uint16_t cntR = __HAL_TIM_GET_COUNTER(&htim8);

        int16_t dL = (int16_t)(cntL - lastL);
        int16_t dR = (int16_t)(cntR - lastR);

        lastL = cntL;
        lastR = cntR;

        tick_L += dL;
        tick_R += dR;

        rpm_L = (int16_t)(dL * msPerMin / (ticksPerRev * 10));
        rpm_R = (int16_t)(dR * msPerMin / (ticksPerRev * 10));

        tx_thread_sleep(10);
    }
	
}


//

//ControlThread
//

static TX_THREAD control_thread;
static UCHAR control_stack[512];

volatile int16_t target_rpm_L = 0;
volatile int16_t target_rpm_R = 0;

static void control_thread_entry(ULONG arg);


UINT ControlThreadCreate(void)
{
	UINT status;
    status = tx_thread_create(&control_thread, "Control", control_thread_entry, 0,
                     control_stack, sizeof(control_stack),
                     7, 7, TX_NO_TIME_SLICE, TX_AUTO_START);
					 
	return status;
}


static void control_thread_entry(ULONG arg)
{
    int16_t pwmL = 0, pwmR = 0;
    float integralL = 0, integralR = 0;
    float Kp = 1.0f, Ki = 0.1f;

    while (1)
    {
        int16_t errorL = target_rpm_L - rpm_L;
        int16_t errorR = target_rpm_R - rpm_R;

        integralL += errorL;
        integralR += errorR;

        pwmL += (int16_t)(Kp * errorL + Ki * integralL);
        pwmR += (int16_t)(Kp * errorR + Ki * integralR);

        if (pwmL < 0) pwmL = 0;
        if (pwmL > 255) pwmL = 255;
        if (pwmR < 0) pwmR = 0;
        if (pwmR > 255) pwmR = 255;

        DriveBOTH(pwmL, pwmR);

        tx_thread_sleep(10);
    }
}

int16_t map_speed_to_rpm(int speed)
{
    if (speed < 60) return 0;
    return (speed - 60) * 1.5;  // ¾ÙÀý
}