/* USER CODE BEGIN Includes */
#include <stdio.h>
#include "uartRingBufDMA.h"
#include "stdbool.h"
#include "stdlib.h"
#include "wheelslib.h"
#include "string.h"
#include "pid.h"

extern uint16_t Head, Tail;
extern uint8_t RxBuf[RxBuf_SIZE];
extern uint8_t MainBuf[MainBuf_SIZE];

#define MAX_BUTTONS 8
#define Enc_A_Pin GPIO_PIN_0
#define Enc_B_Pin GPIO_PIN_1
#define Enc_C_Pin GPIO_PIN_8
#define Enc_D_Pin GPIO_PIN_9

volatile int32_t encA_count = 0;
volatile int32_t encB_count = 0;
volatile int32_t encC_count = 0;
volatile int32_t encD_count = 0;

typedef struct {
    volatile int32_t *counter;
    uint16_t pin;
} EncSensor;

EncSensor sensors[4] = {
    {&encA_count, Enc_A_Pin},
    {&encB_count, Enc_B_Pin},
    {&encC_count, Enc_C_Pin},
    {&encD_count, Enc_D_Pin}
};

uint8_t pwmIndex = 0;
char buffer[100];
uint32_t pwmData_CH1[10] = {0};
uint32_t pwmData_CH2[10] = {0};
uint32_t pwmData_CH3[10] = {0};
uint32_t pwmData_CH4[10] = {0};

// Структуры данных для очереди
typedef enum {
    PACKET_TYPE_BUTTON = 0xF0,
    PACKET_TYPE_JOYSTICK = 0xF1
} PacketType;

typedef struct {
    PacketType type;
    union {
        struct {
            uint8_t id;
            uint8_t state;
        } button;
        struct {
            uint8_t x;
            uint8_t y;
            uint8_t power;
        } joystick;
    } data;
} PacketMessage;

// Дескриптор очереди пакетов
osMessageQueueId_t packetQueueHandle;
#define PACKET_QUEUE_SIZE 16

/**************************************/
// Для отладки
volatile uint32_t total_bytes_received = 0;
volatile uint32_t valid_packets_received = 0;
volatile uint32_t button_packets_received = 0;
volatile uint32_t joystick_packets_received = 0;
volatile uint32_t invalid_packets_detected = 0;
volatile uint32_t dropped_packets = 0;
volatile uint8_t last_joystick_data[3] = {0};
volatile uint8_t last_button_data[2] = {0};

int16_t joystickPower = 0;
char joystickDir = 'F';
/* USER CODE END Includes */

/* USER CODE BEGIN PV */
// флаг для управления режимом работы
typedef enum {
    CONTROL_MODE_DIRECT,
    CONTROL_MODE_PID
} ControlMode;

ControlMode currentControlMode = CONTROL_MODE_DIRECT;

// Структура для хранения желаемых скоростей (setpoints)
typedef struct {
    double speedFL;
    double speedFR;
    double speedBL;
    double speedBR;
    bool isActive;
} TargetSpeeds;

TargetSpeeds targetSpeeds = {0, 0, 0, 0, false};

// PID структуры для каждого колеса
PID_TypeDef pidFL, pidFR, pidBL, pidBR;

// Заданные скорости для каждого колеса (setpoints)
double setpointFL = 0, setpointFR = 0, setpointBL = 0, setpointBR = 0;
double currentSpeedFL = 0, currentSpeedFR = 0, currentSpeedBL = 0, currentSpeedBR = 0;
double pidOutputFL = 0, pidOutputFR = 0, pidOutputBL = 0, pidOutputBR = 0;

// Константы PID - делаем более консервативными
#define KP 1.0f      // Вместо 0.2f
#define KI 0.05f     // Вместо 0.01f
#define KD 0.01f     // Можно оставить
#define MOTOR_DEADZONE_MIN 1
#define MOTOR_DEADZONE_MAX 5
#define PID_UPDATE_PERIOD 10

bool pidDebugEnabled = true;
int8_t ticks_per_rev = 20; // Количество тактов кулера в один оборот.
float rpm = 0.0;

// Для расчета скорости - добавляем более детальную диагностику
volatile int32_t lastEncA = 0, lastEncB = 0, lastEncC = 0, lastEncD = 0;
float ticksToSpeed = 10.0f;

// Добавляем диагностические переменные
volatile uint32_t pidComputeCount = 0;
volatile uint32_t pidLastComputeTime = 0;
volatile bool pidInitialized = false;

MotorsState motors;
/* USER CODE END PV */

/* USER CODE BEGIN PFP */
int _write(int file, char *ptr, int len) {
	int i;
	HAL_UART_Transmit(&huart6, (uint8_t*) ptr, len, 50);
	for (i = 0; i < len; i++) {
		ITM_SendChar(*ptr++);
	}
	return len;
}

void process_uart_data(void);
/* USER CODE END PFP */

/* USER CODE BEGIN 0 */
// Определение состояний FSM для обработки данных из UART
typedef enum {
    STATE_WAIT_HEADER,
    STATE_COLLECT_BUTTON,
    STATE_COLLECT_JOYSTICK
} RxState;

void UART_ProcessByte(uint8_t byte) {
    static RxState state = STATE_WAIT_HEADER;
    static PacketMessage packet;
    static uint8_t byteCounter = 0;

    total_bytes_received++;

    switch (state) {
        case STATE_WAIT_HEADER:
            if (byte == PACKET_TYPE_BUTTON) {
                packet.type = PACKET_TYPE_BUTTON;
                state = STATE_COLLECT_BUTTON;
                byteCounter = 0;
            } else if (byte == PACKET_TYPE_JOYSTICK) {
                packet.type = PACKET_TYPE_JOYSTICK;
                state = STATE_COLLECT_JOYSTICK;
                byteCounter = 0;
            } else {
                invalid_packets_detected++;
            }
            break;

        case STATE_COLLECT_BUTTON:
            if (byteCounter == 0) {
                packet.data.button.id = byte;
                byteCounter++;
            } else if (byteCounter == 1) {
                packet.data.button.state = byte;
                if (osMessageQueuePut(packetQueueHandle, &packet, 0, 0) == osOK) {
                    button_packets_received++;
                    valid_packets_received++;
                    last_button_data[0] = packet.data.button.id;
                    last_button_data[1] = packet.data.button.state;
                } else {
                    dropped_packets++;
                }
                state = STATE_WAIT_HEADER;
            }
            break;

        case STATE_COLLECT_JOYSTICK:
            if (byteCounter == 0) {
                packet.data.joystick.x = byte;
                byteCounter++;
            } else if (byteCounter == 1) {
                packet.data.joystick.y = byte;
                byteCounter++;
            } else if (byteCounter == 2) {
                packet.data.joystick.power = byte;
                if (osMessageQueuePut(packetQueueHandle, &packet, 0, 0) == osOK) {
                    joystick_packets_received++;
                    valid_packets_received++;
                    last_joystick_data[0] = packet.data.joystick.x;
                    last_joystick_data[1] = packet.data.joystick.y;
                    last_joystick_data[2] = packet.data.joystick.power;
                } else {
                    dropped_packets++;
                }
                state = STATE_WAIT_HEADER;
            }
            break;
    }
}

void InitializePID(void) {
    printf("=== INITIALIZING PID CONTROLLERS ===\r\n");

    // Инициализация PID-регуляторов для каждого колеса
    printf("Initializing PID for FL wheel...\r\n");
    PID(&pidFL, &currentSpeedFL, &pidOutputFL, &setpointFL, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT);
    PID(&pidFR, &currentSpeedFR, &pidOutputFR, &setpointFR, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT);
    PID(&pidBL, &currentSpeedBL, &pidOutputBL, &setpointBL, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT);
    PID(&pidBR, &currentSpeedBR, &pidOutputBR, &setpointBR, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT);

    // ИСПРАВЛЕНИЕ: Устанавливаем правильные ограничения выхода
    printf("Setting output limits to ±15...\r\n");  // Исправляем лог
    PID_SetOutputLimits(&pidFL, -15, 15);  // Уменьшаем с ±80 до ±15
    PID_SetOutputLimits(&pidFR, -15, 15);
    PID_SetOutputLimits(&pidBL, -15, 15);
    PID_SetOutputLimits(&pidBR, -15, 15);

    // Включаем автоматический режим
    PID_SetMode(&pidFL, _PID_MODE_AUTOMATIC);
    PID_SetMode(&pidFR, _PID_MODE_AUTOMATIC);
    PID_SetMode(&pidBL, _PID_MODE_AUTOMATIC);
    PID_SetMode(&pidBR, _PID_MODE_AUTOMATIC);

    // Устанавливаем время семплирования
    PID_SetSampleTime(&pidFL, PID_UPDATE_PERIOD);
    PID_SetSampleTime(&pidFR, PID_UPDATE_PERIOD);
    PID_SetSampleTime(&pidBL, PID_UPDATE_PERIOD);
    PID_SetSampleTime(&pidBR, PID_UPDATE_PERIOD);

    pidInitialized = true;
    printf("=== PID CONTROLLERS INITIALIZED SUCCESSFULLY ===\r\n");
    printf("PID Parameters: Kp=%.2f, Ki=%.2f, Kd=%.2f\r\n", KP, KI, KD);
    printf("Output limits: ±15, PWM range: %d-%d\r\n", MOTOR_DEADZONE_MIN, MOTOR_DEADZONE_MAX);
}

uint32_t CompensateDeadzone(double pidOutput) {
    double absOutput = fabs(pidOutput);
    if (absOutput < 1.0) return 0;
    if (absOutput > 15.0) absOutput = 15.0;

    // Линейное масштабирование без сложных расчетов
    return (uint32_t)(absOutput * (35.0 / 15.0));
}

// Добавляем функцию для детального логирования PID
void LogPIDState(const char* wheelName, PID_TypeDef* pid, double setpoint, double input, double output) {
    printf("SETPOINT: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n",
           setpointFL, setpointFR, setpointBL, setpointBR);
    printf("CURRENT:  FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n",
           currentSpeedFL, currentSpeedFR, currentSpeedBL, currentSpeedBR);
    printf("PID_OUT:  FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n",
           pidOutputFL, pidOutputFR, pidOutputBL, pidOutputBR);
}
/* USER CODE END 0 */

  /* USER CODE BEGIN 2 */
  printf("Hi, from machine! \r\n");

  // Инициализация переменных перед PID
  setpointFL = setpointFR = setpointBL = setpointBR = 0.0;
  currentSpeedFL = currentSpeedFR = currentSpeedBL = currentSpeedBR = 0.0;
  pidOutputFL = pidOutputFR = pidOutputBL = pidOutputBR = 0.0;

  InitializePID();
  /* USER CODE END 2 */

/* USER CODE BEGIN 4 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    for (int i = 0; i < 4; i++) {
        if (GPIO_Pin == sensors[i].pin) {
            (*(sensors[i].counter))++;
            // Добавляем логирование энкодеров при отладке
            if (pidDebugEnabled && (*(sensors[i].counter)) % 100 == 0) {
                printf("ENC_%d: %ld\r\n", i, *(sensors[i].counter));
            }
            break;
        }
    }
}
/* USER CODE END 4 */

void Start_Motor_Task(void *argument) {
	/* USER CODE BEGIN Start_Motor_Task */
 #define PACKET_TIMEOUT 50
 #define JOYSTICK_UPDATE_RATE 20
 #define PWM_UPDATE_INTERVAL 50

 // Инициализация GPIO и PWM
 HAL_GPIO_WritePin(Mot_A2_GPIO_Port, Mot_A2_Pin, GPIO_PIN_RESET);
 HAL_GPIO_WritePin(GPIOD, Mot_D2_Pin | Mot_C2_Pin | Mot_B2_Pin, GPIO_PIN_RESET);

 __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
 __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
 __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
 __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);

// uint32_t now = 0;
 Ringbuf_Init();
 Wheels_Init();

 // Инициализация очереди для пакетов
 packetQueueHandle = osMessageQueueNew(PACKET_QUEUE_SIZE, sizeof(PacketMessage), NULL);
 if (packetQueueHandle == NULL) {
     while (1) {
         printf("Queue init failed!\r\n");
         osDelay(1000);
     }
 }

// typedef struct {
//     uint8_t buttonId;
//     uint8_t stableState;
//     uint32_t lastStableEventTime;
// } ButtonState;

// ButtonState buttons[MAX_BUTTONS] = {0};
// for (int i = 0; i < MAX_BUTTONS; i++) {
//     buttons[i].buttonId = i;
// }

 uint32_t lastPwmUpdateTime = 0;
 bool pwmRunning = false;
 static uint32_t lastPwmValues[4] = {0};

 // Инициализация массивов PWM
 memset(pwmData_CH1, 0, sizeof(pwmData_CH1));
 memset(pwmData_CH2, 0, sizeof(pwmData_CH2));
 memset(pwmData_CH3, 0, sizeof(pwmData_CH3));
 memset(pwmData_CH4, 0, sizeof(pwmData_CH4));

 // Запуск PWM с DMA
 HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_1, (uint32_t*) pwmData_CH1, 10);
 HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_2, (uint32_t*) pwmData_CH2, 10);
 HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_3, (uint32_t*) pwmData_CH3, 10);
 HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_4, (uint32_t*) pwmData_CH4, 10);

 pwmRunning = true;
 Wheels_Stop();

 // ВАЖНО: Начинаем с прямого режима
 currentControlMode = CONTROL_MODE_DIRECT;
 printf("Started in DIRECT MODE\r\n");
	/* Infinite loop */
	for (;;) {
	      // Обработка UART данных
	        if (Tail != Head) {
	            osMutexAcquire(myMutexHandle, osWaitForever);
//	            now = HAL_GetTick();

	            uint16_t bytesToProcess = (Head >= Tail) ? (Head - Tail) : (MainBuf_SIZE - Tail + Head);
	            uint16_t maxBytes = 16;
	            if (bytesToProcess > maxBytes)
	                bytesToProcess = maxBytes;

	            uint8_t tempBuffer[16];
	            uint16_t tempTail = Tail;

	            for (uint16_t i = 0; i < bytesToProcess; i++) {
	                tempBuffer[i] = MainBuf[tempTail];
	                tempTail = (tempTail + 1) % MainBuf_SIZE;
	            }

	            Tail = tempTail;
	            osMutexRelease(myMutexHandle);

	            for (uint16_t i = 0; i < bytesToProcess; i++) {
	                UART_ProcessByte(tempBuffer[i]);
	            }
	        }

	        // Обновление PWM
	        if (pwmRunning && (HAL_GetTick() - lastPwmUpdateTime) >= PWM_UPDATE_INTERVAL) {
	            lastPwmUpdateTime = HAL_GetTick();

	            uint32_t newValues[4] = {0};
	            bool motorsActive = false;

	            // УЛУЧШЕННАЯ логика выбора режима
	            if (currentControlMode == CONTROL_MODE_PID) {
	                if (targetSpeeds.isActive) {
	                    motorsActive = true;
	                    newValues[0] = CompensateDeadzone(pidOutputFL);
	                    newValues[1] = CompensateDeadzone(pidOutputFR);
	                    newValues[2] = CompensateDeadzone(pidOutputBL);
	                    newValues[3] = CompensateDeadzone(pidOutputBR);

	                    HAL_GPIO_WritePin(Mot_A2_GPIO_Port, Mot_A2_Pin, (pidOutputFL < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
	                    HAL_GPIO_WritePin(GPIOD, Mot_B2_Pin, (pidOutputFR < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
	                    HAL_GPIO_WritePin(GPIOD, Mot_C2_Pin, (pidOutputBL < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
	                    HAL_GPIO_WritePin(GPIOD, Mot_D2_Pin, (pidOutputBR < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
	                }
	            } else {
	                MotorsState currentState = Wheels_Get_State();
	                if (currentState.isActive) {
	                    motorsActive = true;
	                    newValues[0] = currentState.powerFL;
	                    newValues[1] = currentState.powerFR;
	                    newValues[2] = currentState.powerBL;
	                    newValues[3] = currentState.powerBR;
	                }
	            }

	            // Проверяем изменились ли значения
	            bool valuesChanged = false;
	            for (int i = 0; i < 4; i++) {
	                if (lastPwmValues[i] != newValues[i]) {
	                    valuesChanged = true;
	                    lastPwmValues[i] = newValues[i];
	                }
	            }

	            // Обновляем PWM значения
	            for (int i = 0; i < 10; i++) {
	                pwmData_CH1[i] = newValues[0];
	                pwmData_CH2[i] = newValues[1];
	                pwmData_CH3[i] = newValues[2];
	                pwmData_CH4[i] = newValues[3];
	            }

	            // УПРОЩЕННОЕ логирование для избежания переполнения буфера printf
	            if (valuesChanged) {
	                if (currentControlMode == CONTROL_MODE_PID) {
	                    printf("[PID] PWM: %lu,%lu,%lu,%lu\r\n", newValues[0], newValues[1], newValues[2], newValues[3]);
	                } else {
	                    printf("[DIR] P=%d PWM: %lu\r\n", joystickPower, newValues[0]);
	                }
	            }
	        }
	        osDelay(1);
	}
	/* USER CODE END Start_Motor_Task */
}

void Start_PID_Task(void *argument)
{
  /* USER CODE BEGIN Start_PID_Task */
    uint32_t lastUpdateTime = 0;
    uint32_t lastLogTime = 0;
    const uint32_t LOG_INTERVAL = 15000; // Увеличил интервал логирования

    printf("=== PID TASK STARTED ===\r\n");
    printf("Update period: %d ms\r\n", PID_UPDATE_PERIOD);
	/* Infinite loop */
  for(;;)
  {
      uint32_t currentTime = HAL_GetTick();

      // Работаем только в режиме PID и по истечении периода обновления
		if (currentControlMode == CONTROL_MODE_PID && currentTime - lastUpdateTime >= PID_UPDATE_PERIOD) {

          // Чтение текущих значений энкодеров с защитой от прерываний
          int32_t currentEncA, currentEncB, currentEncC, currentEncD;
          osMutexAcquire(myMutexHandle, osWaitForever);
          currentEncA = encA_count;
          currentEncB = encB_count;
          currentEncC = encC_count;
          currentEncD = encD_count;
          osMutexRelease(myMutexHandle);

          // Вычисление изменений
          int32_t deltaEncA = currentEncA - lastEncA;
          int32_t deltaEncB = currentEncB - lastEncB;
          int32_t deltaEncC = currentEncC - lastEncC;
          int32_t deltaEncD = currentEncD - lastEncD;

          // Обновление предыдущих значений
          lastEncA = currentEncA;
          lastEncB = currentEncB;
          lastEncC = currentEncC;
          lastEncD = currentEncD;

			if (targetSpeeds.isActive && pidDebugEnabled) {
			    static uint32_t debugCounter = 0;
			    if (++debugCounter % 100 == 0) {  // Каждые 5 секунд при 50ms периоде
			        printf("PID_DEBUG: ENC_DELTA: A=%ld B=%ld C=%ld D=%ld\r\n",
			               deltaEncA, deltaEncB, deltaEncC, deltaEncD);
			        printf("PID_DEBUG: SPEEDS: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n",
			               currentSpeedFL, currentSpeedFR, currentSpeedBL, currentSpeedBR);
			        printf("PID_DEBUG: OUTPUTS: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n",
			               pidOutputFL, pidOutputFR, pidOutputBL, pidOutputBR);
			    }
			}

          // Расчет скоростей
//          double timeFactor = 1000.0f / PID_UPDATE_PERIOD;
//          double ticksToSpeedFactor = 0.2f;
          currentSpeedFL = (double)deltaEncA * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0);
          currentSpeedFR = (double)deltaEncB * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0);
          currentSpeedBL = (double)deltaEncC * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0);
          currentSpeedBR = (double)deltaEncD * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0);

          // Установка setpoint'ов
          if (targetSpeeds.isActive) {
              static double rampSpeed = 0;
              rampSpeed += 0.2f;
              if(rampSpeed > 1.0f) rampSpeed = 1.0f;

              setpointFL = targetSpeeds.speedFL * rampSpeed;
              setpointFR = targetSpeeds.speedFR * rampSpeed;
              setpointBL = targetSpeeds.speedBL * rampSpeed;
              setpointBR = targetSpeeds.speedBR * rampSpeed;
          } else {
              setpointFL = setpointFR = setpointBL = setpointBR = 0;
          }

          // Проверка необходимости вычислений
          bool shouldCompute = targetSpeeds.isActive ||
                             (fabs(currentSpeedFL) > 0.1 || fabs(currentSpeedFR) > 0.1 ||
                              fabs(currentSpeedBL) > 0.1 || fabs(currentSpeedBR) > 0.1);

          if (shouldCompute) {
              bool computeFL = PID_Compute(&pidFL);
              bool computeFR = PID_Compute(&pidFR);
              bool computeBL = PID_Compute(&pidBL);
              bool computeBR = PID_Compute(&pidBR);
              printf("PID_COMPUTE: [%d,%d,%d,%d] Ticks: %ld,%ld,%ld,%ld\r\n",
                      computeFL, computeFR, computeBL, computeBR,
                      deltaEncA, deltaEncB, deltaEncC, deltaEncD);

              printf("PID_VALUES: SP=[%.1f,%.1f,%.1f,%.1f] IN=[%.1f,%.1f,%.1f,%.1f] OUT=[%.1f,%.1f,%.1f,%.1f]\r\n",
                      setpointFL, setpointFR, setpointBL, setpointBR,
                      currentSpeedFL, currentSpeedFR, currentSpeedBL, currentSpeedBR,
                      pidOutputFL, pidOutputFR, pidOutputBL, pidOutputBR);

              // ЛОГИРУЕМ ТОЛЬКО РАЗ В 5 СЕКУНД И ТОЛЬКО ЕСЛИ ЕСТЬ АКТИВНОСТЬ
              static uint32_t lastQuickLog = 0;
              if (targetSpeeds.isActive && (currentTime - lastQuickLog) > 5000) {
					printf("PID: SP=%.1f CUR=%.1f OUT=%.1f\r\n", setpointFL, currentSpeedFL, pidOutputFL);
                  lastQuickLog = currentTime;
              }
          } else {
              // Пропускаем цикл вычислений
              osDelay(1);
              continue;
          }

          lastUpdateTime = currentTime;
          pidLastComputeTime = currentTime;
      }

      // Периодическое логирование состояния
      if (pidDebugEnabled && currentTime - lastLogTime >= LOG_INTERVAL) {
          lastLogTime = currentTime;

			printf("\n=== PID STATUS REPORT ===\r\n");
			printf("Control Mode: %s\r\n", (currentControlMode == CONTROL_MODE_PID) ? "PID" : "DIRECT");
			printf("Target Active: %s\r\n", targetSpeeds.isActive ? "YES" : "NO");
			printf("Last Compute: %lu ms ago\r\n", currentTime - pidLastComputeTime);
			printf("Encoder Values: A=%ld B=%ld C=%ld D=%ld\r\n", encA_count, encB_count, encC_count, encD_count);
			printf("=========================\r\n");
      }
      osDelay(1);
  } 
  /* USER CODE END Start_PID_Task */
}

void Start_Pkt_Proc_Task(void *argument) {
	/* USER CODE BEGIN Start_Pkt_Proc_Task */
	   osDelay(100);
	    PacketMessage msg;
//	    uint32_t lastJoystickUpdateTime = 0;
//    uint32_t joystickTimeout = 500; // Таймаут в мс - если не получали команд от джойстика за это время, останавливаем моторы	/* Infinite loop */
	for (;;) {
//		uint32_t currentTime = HAL_GetTick();
		// Проверка таймаута джойстика - если давно не получали команд, останавливаем моторы
//	      if ((currentTime - lastJoystickUpdateTime) > joystickTimeout && joystickPower != 0) {
//	          // Останавливаем моторы, если последняя команда была не остановка
//	          Wheels_Stop();
//	          joystickPower = 0;
//	          joystickDir = 'F';
//	          printf("[AUTO STOP] Joystick timeout - motors stopped\r\n");
//	      }
	       if (osMessageQueueGet(packetQueueHandle, &msg, NULL, 10) == osOK) {
	            switch (msg.type) {
	            case PACKET_TYPE_BUTTON:
	                if (msg.data.button.id < MAX_BUTTONS) {
	                    uint8_t buttonId = msg.data.button.id;
	                    uint8_t newState = (msg.data.button.state == 0x7F) ? 1 : 0;

	                    if (buttonId == 4 && newState == 1) { // STOP
	                        printf("=== EMERGENCY STOP PRESSED ===\r\n");
	                        Wheels_Stop();
	                        joystickPower = 0;
	                        joystickDir = 'F';
	                        targetSpeeds.isActive = false;
	                        targetSpeeds.speedFL = 0;
	                        targetSpeeds.speedFR = 0;
	                        targetSpeeds.speedBL = 0;
	                        targetSpeeds.speedBR = 0;
	                        printf("All motors stopped, PID targets cleared\r\n");

	                    } else if (buttonId == 5 && newState == 1) { // Переключение режимов
	                        printf("=== MODE SWITCH PRESSED ===\r\n");
	                        if (currentControlMode == CONTROL_MODE_DIRECT) {
	                            // Сначала останавливаем моторы
	                            Wheels_Stop();
	                            osDelay(100); // Увеличил задержку

	                            currentControlMode = CONTROL_MODE_PID;
	                            printf("SWITCHED TO: PID MODE\r\n");
	                        } else {
	                            // Сначала останавливаем PID
	                            targetSpeeds.isActive = false;
	                            targetSpeeds.speedFL = 0;
	                            targetSpeeds.speedFR = 0;
	                            targetSpeeds.speedBL = 0;
	                            targetSpeeds.speedBR = 0;
	                            osDelay(100); // Увеличил задержку

	                            currentControlMode = CONTROL_MODE_DIRECT;
	                            printf("SWITCHED TO: DIRECT MODE\r\n");
	                        }

	                    } else if (buttonId == 6 && newState == 1) {
	                        pidDebugEnabled = !pidDebugEnabled;
	                        printf("PID Debug: %s\r\n", pidDebugEnabled ? "ENABLED" : "DISABLED");
	                    }
	                }
	                break;

	            case PACKET_TYPE_JOYSTICK: {
	                int8_t x = (int8_t)(msg.data.joystick.x - 127);
	                int8_t y = (int8_t)(msg.data.joystick.y - 127);
	                uint8_t power = msg.data.joystick.power;

	                // ТОЛЬКО ОСНОВНАЯ ИНФОРМАЦИЯ
	                printf("JS: x=%d y=%d p=%d\r\n", x, y, power);

//	                lastJoystickUpdateTime = currentTime;
	                joystickPower = power;
	                joystickDir = (y >= 0) ? 'F' : 'R';

	                if (power <= 100) {
	                    if (power == 0) {
	                        Wheels_Stop();
	                        targetSpeeds.isActive = false;
	                        targetSpeeds.speedFL = 0;
	                        targetSpeeds.speedFR = 0;
	                        targetSpeeds.speedBL = 0;
	                        targetSpeeds.speedBR = 0;
	                    } else {
	                    	if (currentControlMode == CONTROL_MODE_PID) {
	                    	    if (power > 30) {  // Уменьшаем максимальную мощность с 40 до 30
	                    	        power = 30;
	                    	    }
	                    	    targetSpeeds.isActive = true;

	                    	    // ИСПРАВЛЕНИЕ: Еще больше уменьшаем speedScale
	                    	    float speedScale = 3.0f; // Было 10.0f - уменьшаем до 3.0f
	                    	    float baseSpeed = (power / 100.0f) * speedScale;

	                    	    float normY = y / 127.0f;
	                    	    float normX = x / 127.0f;

	                    	    float forwardSpeed = normY * baseSpeed;
	                    	    float turnSpeed = normX * baseSpeed * 0.5f;  // Увеличиваем коэффициент поворота

	                    	    targetSpeeds.speedFL = forwardSpeed + turnSpeed;
	                    	    targetSpeeds.speedFR = forwardSpeed - turnSpeed;
	                    	    targetSpeeds.speedBL = forwardSpeed + turnSpeed;
	                    	    targetSpeeds.speedBR = forwardSpeed - turnSpeed;

	                    	    printf("PID_TGT: %.1f,%.1f,%.1f,%.1f\r\n",
	                    	           targetSpeeds.speedFL, targetSpeeds.speedFR,
	                    	           targetSpeeds.speedBL, targetSpeeds.speedBR);
	                    	} else {
	                            Wheels_Process_Joystick(x, y, power);
	                        }
	                    }
	                }
	            }
	                break;
	            }
	        }
	        osDelay(1);
	}
	/* USER CODE END Start_Pkt_Proc_Task */
}