#include "tracking.h"

uint8_t TrackingController::baseSpeed = 50;
uint8_t TrackingController::turnSpeed = 50;
uint8_t TrackingController::rotateSensitive = 3;
float TrackingController::lastOffset = 0;
bool TrackingController::isSearching = false;
unsigned long TrackingController::allHighStartTime = 0;
bool TrackingController::isAllHighState = false;

void TrackingController::init()
{
  Serial.println("Tracking Init");

  // 从存储中读取参数
  Storage::getTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
}

void TrackingController::setSpeed(uint8_t base, uint8_t turn)
{
  baseSpeed = base;
  turnSpeed = turn;

  // 保存参数到存储
  Storage::setTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
}

void TrackingController::setRotateSensitive(uint8_t sensitive)
{
  rotateSensitive = sensitive;

  // 保存参数到存储
  Storage::setTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
}

void TrackingController::update()
{
  IR::update();

  const IRData &irData = IR::data;
  float offset = calculateOffset(irData);

  // 检查是否丢线
  if (offset == LOST_LINE)
  {
    // 如果是因为全高超时导致的停止
    if (isAllHighState)
    {
      Serial.println("Vehicle likely lifted, stopping all motors");
      MotorController::motorXYRControl(0, 0, 0);
      return;
    }

    // 正常丢线处理逻辑
    Serial.println("Lost line! Rotating to search...");
    float rotateDirection = (lastOffset != 0) ? (lastOffset > 0 ? 1.0f : -1.0f) : 1.0f;

    isSearching = true;
    MotorController::rotate(lastOffset < 0 ? ROTATE_CLOCKWISE : ROTATE_ANTICLOCKWISE, baseSpeed * rotateSensitive);
    return;
  }

  // 如果之前在搜索模式，现在找到线了，打印日志
  if (isSearching)
  {
    Serial.println("Line found!");
    isSearching = false;
  }

  // 添加防抖：如果偏移量很小，认为是直线
  if (abs(offset) < 0.5)
  {
    offset = 0;
  }

  // 更新最后的偏移量，用于丢线时的方向判断
  lastOffset = offset;

  // 计算基础速度和转向调整
  float leftSpeed = baseSpeed;
  float rightSpeed = baseSpeed;
  
  if (offset != 0) {
    // 在大偏移时降低基础速度
    float speedFactor = 1.0f;
    if (abs(offset) > 1.0f) {
      speedFactor = 0.7f;
      leftSpeed *= speedFactor;
      rightSpeed *= speedFactor;
    }

    // 根据偏移量调整左右轮速度
    float speedDiff = baseSpeed * (abs(offset) / 2.0f) * speedFactor;  // 差速也要相应降低
    
    // 增加最小速度差以确保能够转向
    float minSpeedDiff = baseSpeed * 0.5f * speedFactor; // 最小速度差也要相应降低
    if (speedDiff < minSpeedDiff)
    {
      speedDiff = minSpeedDiff;
    }

    if (offset < 0)
    {
      // 向左偏，左轮加速，右轮减速
      leftSpeed += speedDiff;
      rightSpeed -= speedDiff;
    }
    else
    {
      // 向右偏，右轮加速，左轮减速
      leftSpeed -= speedDiff;
      rightSpeed += speedDiff;
    }

    // 确保速度不超过限制
    float maxSpeed = baseSpeed * speedFactor;  // 最大速度也要相应降低
    leftSpeed = constrain(leftSpeed, -maxSpeed, maxSpeed);
    rightSpeed = constrain(rightSpeed, -maxSpeed, maxSpeed);
  }

  // 调试信息
  Serial.print("Offset: ");
  Serial.print(offset);
  Serial.print(" Left: ");
  Serial.print(leftSpeed);
  Serial.print(" Right: ");
  Serial.println(rightSpeed);

  // 设置电机速度
  MotorController::motorControl('A', leftSpeed);  // 左前
  MotorController::motorControl('B', leftSpeed);  // 左后
  MotorController::motorControl('C', rightSpeed); // 右后
  MotorController::motorControl('D', rightSpeed); // 右前
}

// 计算偏移量，返回范围 [-2, 2]
float TrackingController::calculateOffset(const IRData &irData)
{
  float offset = 0;
  bool allHigh = true;
  bool allLow = true;

  Serial.print("IR Data: ");
  for (int i = 0; i < IR_COUNT; i++)
  {
    Serial.print(irData.data[i]);
    Serial.print(" ");
  }
  Serial.println();

  // 检查是否所有传感器都是高电平或低电平
  for (int i = 0; i < IR_COUNT; i++)
  {
    if (irData.data[i])
      allLow = false;
    else
      allHigh = false;
  }

  // 全高状态处理
  if (allHigh)
  {
    // 如果刚进入全高状态
    if (!isAllHighState)
    {
      isAllHighState = true;
      allHighStartTime = millis();
      Serial.println("All sensors high, continue moving forward");
      return 0; // 返回0表示直行
    }

    // 检查是否超过超时时间
    if (millis() - allHighStartTime >= ALL_HIGH_TIMEOUT)
    {
      Serial.println("All sensors high for too long, stopping (possibly lifted)");
      return LOST_LINE; // 触发停止
    }

    return 0; // 继续直行
  }

  if (isAllHighState)
  {
    isAllHighState = false;
  }

  // 全低说明全离线
  if (allLow)
    return LOST_LINE;

  // 寻找最长的连续激活区域
  int maxConsecutiveCount = 0;
  int maxConsecutiveStart = -1;
  int currentConsecutiveCount = 0;
  int currentConsecutiveStart = -1;

  for (int i = 0; i < IR_COUNT; i++)
  {
    if (irData.data[i])
    {
      if (currentConsecutiveCount == 0)
      {
        currentConsecutiveStart = i;
      }
      currentConsecutiveCount++;

      if (currentConsecutiveCount > maxConsecutiveCount)
      {
        maxConsecutiveCount = currentConsecutiveCount;
        maxConsecutiveStart = currentConsecutiveStart;
      }
    }
    else
    {
      currentConsecutiveCount = 0;
    }
  }

  // 如果找到连续区域
  if (maxConsecutiveCount > 0)
  {
    // 计算连续区域的中心位置
    float centerPos = maxConsecutiveStart + (maxConsecutiveCount - 1) / 2.0f;
    // 将中心位置转换为偏移量
    offset = (centerPos - (IR_COUNT - 1) / 2.0f) * (4.0f / (IR_COUNT - 1));

    // 修改权重计算逻辑
    float weight = 1.0;
    if (maxConsecutiveCount == 1)
    {
      // 单点检测到，说明可能在边缘，需要较大的修正
      weight = 2.0;
    }
    else if (maxConsecutiveCount == 2)
    {
      // 两点检测到，需要中等程度的修正
      weight = 1.5;
    }
    else if (maxConsecutiveCount >= 3)
    {
      // 多点检测到，可能接近中心，使用较小的修正
      weight = 1.0;
    }

    // 根据检测点的位置调整权重
    if (maxConsecutiveStart == 0 || maxConsecutiveStart + maxConsecutiveCount >= IR_COUNT - 1)
    {
      // 如果在边缘位置，增加权重以加快回中
      weight *= 1.5;
    }

    offset *= weight;

    Serial.print("Center at sensor: ");
    Serial.print(centerPos);
    Serial.print(" Count: ");
    Serial.print(maxConsecutiveCount);
    Serial.print(" Weight: ");
    Serial.println(weight);
  }
  else
  {
    // 如果没有找到连续区域，使用所有激活点的加权平均
    int activeCount = 0;
    for (int i = 0; i < IR_COUNT; i++)
    {
      if (irData.data[i])
      {
        float positionOffset = (i - (IR_COUNT - 1) / 2.0f) * (4.0f / (IR_COUNT - 1));
        offset += positionOffset;
        activeCount++;
      }
    }
    if (activeCount > 0)
    {
      offset /= activeCount;
    }
  }

  lastOffset = offset;
  return offset;
}