#include "module/gripper_rm.hpp"

#include <cmath>

#include "task/user_task.h"

namespace arm {

GripperState gripper_state_dbg = GripperState::OPEN_HOLD;
bool gripper_target_close = false;
float gripper_output_dbg = 0.0f;

float gripper_out_open = -0.35f;
float gripper_out_close_fast = 0.55f;
float gripper_out_preload = 0.72f;
float gripper_out_hold = 0.46f;
float gripper_out_boost = 0.64f;

float gripper_contact_current_th = 4200.0f;
float gripper_contact_speed_th = 2.0f;
float gripper_slip_speed_th = 1.6f;

uint16_t gripper_contact_debounce_ms = 18;
uint16_t gripper_preload_ms = 180;
uint16_t gripper_boost_ms = 120;
uint16_t gripper_open_timeout_ms = 1200;
uint16_t gripper_close_timeout_ms = 1500;

static MOTOR_RM_Param_t g_gripper_rm_param = {};
static bool g_gripper_inited = false;

static uint16_t GripperMsToTicks(uint16_t ms) {
  const uint32_t freq = ARM_MAIN_FREQ;
  uint32_t ticks = ((uint32_t)ms * freq + 999u) / 1000u;
  if (ticks == 0u) {
    ticks = 1u;
  }
  return (uint16_t)ticks;
}

static void GripperSetOutput(float out_norm) {
  if (out_norm > 1.0f) out_norm = 1.0f;
  if (out_norm < -1.0f) out_norm = -1.0f;
  gripper_output_dbg = out_norm;
  (void)MOTOR_RM_SetOutput(&g_gripper_rm_param, out_norm);
}

static void GripperStateMachineStep(bool target_close) {
  static uint16_t state_ticks = 0;
  static uint16_t contact_ticks = 0;
  static uint16_t slip_ticks = 0;
  static uint16_t boost_ticks = 0;
  static bool prev_target_close = false;

  const bool target_changed = (target_close != prev_target_close);
  prev_target_close = target_close;

  MOTOR_RM_t* gm = MOTOR_RM_GetMotor(&g_gripper_rm_param);
  const bool online = (gm != nullptr) && gm->motor.header.online;
  const float fb_speed = online ? gm->feedback.rotor_speed : 0.0f;
  const float fb_current = online ? gm->feedback.torque_current : 0.0f;

  const bool contact_now = fabsf(fb_current) > gripper_contact_current_th &&
                           fabsf(fb_speed) < gripper_contact_speed_th;
  const bool slip_now = fabsf(fb_speed) > gripper_slip_speed_th;

  if (target_changed) {
    state_ticks = 0;
    contact_ticks = 0;
    slip_ticks = 0;
    boost_ticks = 0;
    gripper_state_dbg = target_close ? GripperState::CLOSING : GripperState::OPENING;
  }

  if (!online) {
    gripper_state_dbg = GripperState::FAULT;
    GripperSetOutput(0.0f);
    return;
  }

  ++state_ticks;

  switch (gripper_state_dbg) {
    case GripperState::OPENING:
      GripperSetOutput(gripper_out_open);
      if (target_close) {
        gripper_state_dbg = GripperState::CLOSING;
        state_ticks = 0;
        contact_ticks = 0;
      } else if (state_ticks >= GripperMsToTicks(gripper_open_timeout_ms)) {
        gripper_state_dbg = GripperState::OPEN_HOLD;
        state_ticks = 0;
      }
      break;

    case GripperState::OPEN_HOLD:
      GripperSetOutput(0.0f);
      if (target_close) {
        gripper_state_dbg = GripperState::CLOSING;
        state_ticks = 0;
        contact_ticks = 0;
      }
      break;

    case GripperState::CLOSING:
      GripperSetOutput(gripper_out_close_fast);
      if (!target_close) {
        gripper_state_dbg = GripperState::OPENING;
        state_ticks = 0;
        contact_ticks = 0;
        break;
      }

      contact_ticks = contact_now ? (uint16_t)(contact_ticks + 1u) : 0u;
      if (contact_ticks >= GripperMsToTicks(gripper_contact_debounce_ms)) {
        gripper_state_dbg = GripperState::PRELOAD;
        state_ticks = 0;
      } else if (state_ticks >= GripperMsToTicks(gripper_close_timeout_ms)) {
        // 稳夹优先：闭合超时后转入保持而不是松开
        gripper_state_dbg = GripperState::CLOSE_HOLD;
        state_ticks = 0;
      }
      break;

    case GripperState::PRELOAD:
      GripperSetOutput(gripper_out_preload);
      if (!target_close) {
        gripper_state_dbg = GripperState::OPENING;
        state_ticks = 0;
      } else if (state_ticks >= GripperMsToTicks(gripper_preload_ms)) {
        gripper_state_dbg = GripperState::CLOSE_HOLD;
        state_ticks = 0;
      }
      break;

    case GripperState::CLOSE_HOLD:
      if (!target_close) {
        gripper_state_dbg = GripperState::OPENING;
        state_ticks = 0;
        slip_ticks = 0;
        boost_ticks = 0;
        break;
      }

      if (boost_ticks > 0) {
        GripperSetOutput(gripper_out_boost);
        --boost_ticks;
      } else {
        GripperSetOutput(gripper_out_hold);
      }

      // 检测到可能打滑时短时增压，提高稳夹能力
      if (slip_now) {
        ++slip_ticks;
      } else {
        slip_ticks = 0;
      }
      if (slip_ticks >= GripperMsToTicks(30)) {
        boost_ticks = GripperMsToTicks(gripper_boost_ms);
        slip_ticks = 0;
      }
      break;

    case GripperState::FAULT:
    default:
      GripperSetOutput(0.0f);
      if (online) {
        gripper_state_dbg = target_close ? GripperState::CLOSING : GripperState::OPENING;
        state_ticks = 0;
      }
      break;
  }
}

int8_t Gripper_Init(const MOTOR_RM_Param_t& param) {
  g_gripper_rm_param = param;
  g_gripper_inited = (MOTOR_RM_Register(&g_gripper_rm_param) == DEVICE_OK);
  return g_gripper_inited ? DEVICE_OK : DEVICE_ERR;
}

void Gripper_Step(bool target_close) {
  if (!g_gripper_inited) {
    return;
  }

  gripper_target_close = target_close;
  (void)MOTOR_RM_Update(&g_gripper_rm_param);
  GripperStateMachineStep(target_close);
  (void)MOTOR_RM_Ctrl(&g_gripper_rm_param);
}

}  // namespace arm