#include "mod_wheelleg_chassis.hpp"

#include <cmath>
#include <tuple>

using namespace Module;
Wheelleg::Wheelleg(Param &param)
    : param_(param),
      roll_pid_(param.roll_pid_param, 333.0f),
      yaw_pid_(param.yaw_pid_param, 333.0f),
      yaccl_pid_(param.yaccl_pid_param, 333.0f),
      lowfilter_(333.0f, 50.0f),
      acclfilter_(333.0f, 30.0f),
      manfilter_(param.manfilter_param),

      ctrl_lock_(true)
{
  memset(&(this->cmd_), 0, sizeof(this->cmd_));

  this->hip_motor_.at(0) =
      new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
  this->hip_motor_.at(1) =
      new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
  this->hip_motor_.at(2) =
      new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
  this->hip_motor_.at(3) =
      new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");

  this->wheel_motor_.at(0) =
      new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
  this->wheel_motor_.at(1) =
      new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");

  this->leglength_pid_.at(0) =
      new Component::PID(param.leglength_pid_param.at(0), 333.0f);
  this->leglength_pid_.at(1) =
      new Component::PID(param.leglength_pid_param.at(1), 333.0f);

  this->theta_pid_.at(0) =
      new Component::PID(param.theta_pid_param.at(0), 333.0f);
  this->theta_pid_.at(1) =
      new Component::PID(param.theta_pid_param.at(1), 333.0f);

  this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
  this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);

  this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
  this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);

  auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg *chassis)
  {
    chassis->ctrl_lock_.Wait(UINT32_MAX);
    switch (event)
    {
    case SET_MODE_RELAX:
      chassis->SetMode(RELAX);
      break;
    case SET_MODE_STAND:
      chassis->SetMode(STAND);
      break;
    case SET_MODE_ROTOR:
      chassis->SetMode(ROTOR);
      break;
    case SET_MODE_RESET:
      chassis->SetMode(RESET);
      break;
    default:
      break;
    }
    chassis->ctrl_lock_.Post();
  };

  Component::CMD::RegisterEvent<Wheelleg *, ChassisEvent>(
      event_callback, this, this->param_.EVENT_MAP);

  auto chassis_thread = [](Wheelleg *chassis)
  {
    auto cmd_sub =
        Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
    auto eulr_sub =
        Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
    auto gyro_sub =
        Message::Subscriber<Component::Type::Vector3>("chassis_gyro");

    auto yaw_sub = Message::Subscriber<float>("chassis_yaw");

    auto accl_sub =
        Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
    // auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
    uint32_t last_online_time = bsp_time_get_ms();

    while (1)
    {
      eulr_sub.DumpData(chassis->eulr_); /* imu */
      gyro_sub.DumpData(chassis->gyro_); /* imu */
      accl_sub.DumpData(chassis->accl_);

      yaw_sub.DumpData(chassis->yaw_);
      cmd_sub.DumpData(chassis->cmd_);
      // yaw_sub.DumpData(chassis->yaw_);

      chassis->ctrl_lock_.Wait(UINT32_MAX);
      chassis->MotorSetAble();
      chassis->UpdateFeedback();
      chassis->Calculate();
      chassis->Control();
      chassis->ctrl_lock_.Post();

      /* 运行结束，等待下一次唤醒 */
      chassis->thread_.SleepUntil(3, last_online_time);
    }
  };
  this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
                       System::Thread::MEDIUM);
}

void Wheelleg::MotorSetAble()
{
  if (this->hip_motor_flag_ == 0)
  {
    this->hip_motor_[0]->Relax();
    this->hip_motor_[1]->Relax();
    this->hip_motor_[2]->Relax();
    this->hip_motor_[3]->Relax();
    this->dm_motor_flag_ = 0;
  }

  else
  {
    if (this->dm_motor_flag_ == 0)
    {
      for (int i = 0; i < 5; i++)
      {
        this->hip_motor_[0]->Enable();
      }
      for (int i = 0; i < 5; i++)
      {
        this->hip_motor_[1]->Enable();
      }
      for (int i = 0; i < 5; i++)
      {
        this->hip_motor_[2]->Enable();
      }
      for (int i = 0; i < 5; i++)
      {
        this->hip_motor_[3]->Enable();
      }

      this->dm_motor_flag_ = 1;
    }
  };
}

void Wheelleg::UpdateFeedback()
{
  this->now_ = bsp_time_get();
  this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
  this->last_wakeup_ = this->now_;

  this->wheel_motor_[0]->Update();
  this->wheel_motor_[1]->Update();

  this->leg_argu_[0].phi4_ =
      this->hip_motor_[0]->GetAngle() -
      this->param_.wheel_param.mech_zero[0]; // 前关节角度
  this->leg_argu_[0].phi1_ =
      this->hip_motor_[1]->GetAngle() -
      this->param_.wheel_param.mech_zero[1]; // 后关节角度

  this->leg_argu_[1].phi4_ =
      (-this->hip_motor_[2]->GetAngle() +
       this->param_.wheel_param.mech_zero[3]); // 前关节角度
  if (leg_argu_[1].phi4_ > M_PI)
  {
    this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
  }

  this->leg_argu_[1].phi1_ =
      (-this->hip_motor_[3]->GetAngle() +
       this->param_.wheel_param.mech_zero[2]); // 后关节角度
  if (leg_argu_[1].phi1_ > M_PI)
  {
    this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
  }

  this->pit_ = this->eulr_.pit;
  if (this->pit_ > M_PI)
  {
    this->pit_ = this->eulr_.pit - 2 * M_PI;
  }

  /*  注意极性 */
  std::tuple<float, float, float, float> result0 =
      this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
                              this->pit_, -this->gyro_.x, this->dt_);
  this->leg_argu_[0].L0 = std::get<0>(result0);
  this->leg_argu_[0].d_L0 = std::get<1>(result0);
  this->leg_argu_[0].theta = -std::get<2>(result0);
  this->leg_argu_[0].d_theta = -std::get<3>(result0);

  if (leg_argu_[0].theta > M_PI)
  {
    leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
  }
  if (leg_argu_[0].theta < -M_PI)
  {
    leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
  }

  std::tuple<float, float, float, float> result1 =
      this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
                              this->pit_, -this->gyro_.x, this->dt_);
  this->leg_argu_[1].L0 = std::get<0>(result1);
  this->leg_argu_[1].d_L0 = std::get<1>(result1);
  this->leg_argu_[1].theta = -std::get<2>(result1);
  this->leg_argu_[1].d_theta = -std::get<3>(result1);

  if (leg_argu_[1].theta > M_PI)
  {
    leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
  }
  if (leg_argu_[1].theta < -M_PI)
  {
    leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
  }

  /* 轮子转速近似于编码器速度 由此推测机体速度 */
  this->leg_argu_[0].single_x_dot =
      -wheel_motor_[0]->GetSpeed() * 2 * M_PI *
          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
      leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
      leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);

  this->leg_argu_[1].single_x_dot =
      wheel_motor_[1]->GetSpeed() * 2 * M_PI *
          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
      leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
      leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);

  this->move_argu_.last_x_dot = this->move_argu_.x_dot;
  this->move_argu_.x_dot =

      (this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
  this->move_argu_.x_dot =
      (-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
      this->param_.wheel_param.wheel_radius / 60.f / 15.765;
  this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;

  this->move_argu_.delta_speed =
      lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);

  this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);

  if (now_ > 1000000)
  {
    this->move_argu_.x_dot_hat =
        manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
                               this->move_argu_.last_x_dot, accl_.y * 9.8f,
                               dt_) -
        ((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
          leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
         (leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
          leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
            2;
    this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
  }
}

void Wheelleg::Calculate()
{
  switch (this->mode_)
  {
  case Wheelleg::RELAX:
    // if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
    //   if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
    //     move_argu_.target_dot_x -= 0.004;
    //   }
    //   if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
    //     move_argu_.target_dot_x += 0.004;
    //   }
    // } else {
    //   move_argu_.target_dot_x = cmd_.y * 1.5f;
    // }
    // move_argu_.target_x = move_argu_.target_dot_x * dt_ +
    // move_argu_.target_x;
    move_argu_.target_x = 0;
    move_argu_.target_dot_x = 0;
    break;
  case Wheelleg::STAND:

    /* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
    if (fabs(move_argu_.target_dot_x -
             cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
    {
      if (move_argu_.target_dot_x >
          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
      {
        move_argu_.target_dot_x -= 0.004;
      }
      if (move_argu_.target_dot_x <
          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
      {
        move_argu_.target_dot_x += 0.004;
      }
    }
    else
    {
      move_argu_.target_dot_x =
          cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
    }
    move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;

    this->move_argu_.target_yaw = 0.0f;

    /*双零点*/
    if (this->yaw_ > M_PI_2)
    {
      this->move_argu_.target_yaw = 3.14158f;
    }
    if (this->yaw_ < -M_PI_2)
    {
      this->move_argu_.target_yaw = 3.14158f;
    }
    break;

  case Wheelleg::ROTOR:
    if (fabs(move_argu_.target_dot_x -
             cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
    {
      if (move_argu_.target_dot_x >
          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
      {
        move_argu_.target_dot_x -= 0.004;
      }
      if (move_argu_.target_dot_x <
          cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
      {
        move_argu_.target_dot_x += 0.004;
      }
    }
    else
    {
      move_argu_.target_dot_x =
          cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
    }
    move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
    this->move_argu_.target_yaw = this->yaw_ + 0.01;

    break;
    // move_argu_.target_dot_x = cmd_.x;
    // move_argu_.target_x =
    //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
    // // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
    // cmd_.y);
    // // move_argu_.x_dot =
    // //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
    // // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
    // break;

  case Wheelleg::RESET:
    this->move_argu_.target_dot_x = 0;
    move_argu_.target_x = 0;
    this->move_argu_.target_yaw = this->eulr_.yaw;
    this->move_argu_.xhat = 0;

    //     move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
    // if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
    //   this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
    // }
    break;

  default:
    XB_ASSERT(false);
    return;
  }

  leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
  onground0_flag_ = false;
  if (leg_argu_[0].Fn > 30)
  {
    onground0_flag_ = true;
  }
  leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
  onground1_flag_ = false;
  if (leg_argu_[1].Fn > 30)
  {
    onground1_flag_ = true;
  }
  std::tuple<float, float> result3;
  std::tuple<float, float> result4;

  switch (this->mode_)
  {
  case Wheelleg::RELAX:
  case Wheelleg::ROTOR:
  case Wheelleg::STAND:

    for (int i = 0; i < 12; i++)
    {
      leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
          &this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
          leg_argu_[0].L0);
    }

    for (int i = 0; i < 12; i++)
    {
      leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
          &this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
          leg_argu_[1].L0);
    }
    if (now_ > 1000000)
      if (fabs(move_argu_.target_dot_x) > 0.5 ||
          fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
          ((!onground0_flag_) & (!onground1_flag_)))
      {
        leg_argu_[0].LQR_K[2] = 0;
        leg_argu_[1].LQR_K[2] = 0;
        this->move_argu_.xhat = 0;
        move_argu_.target_x = 0;
      }

    if (onground0_flag_)
    {
      leg_argu_[0].Tw =
          (leg_argu_[0].LQR_K[0] *
               (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
           leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
           leg_argu_[0].LQR_K[2] *
               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
           leg_argu_[0].LQR_K[3] *
               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
           leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
           leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
      leg_argu_[0].Tp =
          (leg_argu_[0].LQR_K[6] *
               (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
           leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
           leg_argu_[0].LQR_K[8] *
               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
           leg_argu_[0].LQR_K[9] *
               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
           leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
           leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
    }
    else
    {
      leg_argu_[0].Tw = 0;

      leg_argu_[0].Tp =
          (leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
           leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
    }
    if (onground1_flag_)
    {
      leg_argu_[1].Tw =
          (leg_argu_[1].LQR_K[0] *
               (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
           leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
           leg_argu_[1].LQR_K[2] *
               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
           leg_argu_[1].LQR_K[3] *
               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
           leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
           leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
      leg_argu_[1].Tp =
          (leg_argu_[1].LQR_K[6] *
               (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
           leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
           leg_argu_[1].LQR_K[8] *
               (-move_argu_.xhat + move_argu_.target_x - 0.56) +
           leg_argu_[1].LQR_K[9] *
               (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
           leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
           leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
    }
    else
    {
      leg_argu_[1].Tw = 0;
      leg_argu_[1].Tp =
          (leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
           leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
    }

    this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
                                  this->param_.wheel_param.static_L0[0] +
                                  +roll_pid_.Calculate(0, eulr_.rol, dt_);
    this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
                                  this->param_.wheel_param.static_L0[1] -
                                  roll_pid_.Calculate(0, eulr_.rol, dt_);

    leg_argu_[0].F0 =
        leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
        this->param_.wheel_param.static_F0[0] +
        leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
                                        this->leg_argu_[0].L0, this->dt_);
    leg_argu_[1].F0 =
        leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
        this->param_.wheel_param.static_F0[1] +
        leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
                                        this->leg_argu_[1].L0, this->dt_);

    this->leg_argu_[0].Delta_Tp =
        leg_argu_[0].L0 * 10.0f *
        tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
                                 this->leg_argu_[0].theta, this->dt_);
    this->leg_argu_[1].Delta_Tp =
        -leg_argu_[1].L0 * 10.0f *
        tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
                                 this->leg_argu_[0].theta, this->dt_);

    result3 = leg_[0]->VMCinserve(
        -leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
        -leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
    this->leg_argu_[0].T1 = std::get<0>(result3);
    this->leg_argu_[0].T2 = std::get<1>(result3);

    result4 = leg_[1]->VMCinserve(
        -leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
        -leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
    this->leg_argu_[1].T1 = -std::get<0>(result4);
    this->leg_argu_[1].T2 = -std::get<1>(result4);

    if (onground0_flag_ & onground1_flag_)
    {
      move_argu_.yaw_force =
          -this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
    }
    else
    {
      move_argu_.yaw_force = 0;
    }
    /*3508不带减速箱是Tw*3.2f*/
    /*2006带减速是Tw/1.8f*/
    /* 3508带15.7减速箱是Tw/4.9256 */

    this->wheel_motor_out_[0] =
        this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;

    this->wheel_motor_out_[1] =
        this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;

    this->hip_motor_out_[0] = this->leg_argu_[0].T1;
    this->hip_motor_out_[1] = this->leg_argu_[0].T2;
    this->hip_motor_out_[2] = this->leg_argu_[1].T1;
    this->hip_motor_out_[3] = this->leg_argu_[1].T2;

    this->hip_motor_flag_ = 1;
    break;

  case Wheelleg::RESET:
    if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
        fabs(leg_argu_[1].theta) > 1.57)
    {
      leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;

      if (fabs(pit_) > M_PI / 2)
      {
        if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03)
        {
          leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
        }
        if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03)
        {
          leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
        }
        leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
                                   0.65f, this->leg_argu_[0].L0, this->dt_);
        leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
                                   0.65f, this->leg_argu_[1].L0, this->dt_);
      }
      if (fabs(leg_argu_[0].theta) < 1.57)
      {
        leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
        leg_argu_[0].target_theta = leg_argu_[0].theta;
      }

      if (fabs(leg_argu_[1].theta) < 1.57)
      {
        leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
        leg_argu_[1].target_theta = leg_argu_[1].theta;
      }

      if (leg_argu_[0].target_theta > M_PI)
      {
        leg_argu_[0].target_theta -= 2 * M_PI;
      }
      if (leg_argu_[0].target_theta < -M_PI)
      {
        leg_argu_[0].target_theta += 2 * M_PI;
      }
      if (leg_argu_[1].target_theta > M_PI)
      {
        leg_argu_[1].target_theta -= 2 * M_PI;
      }
      if (leg_argu_[1].target_theta < -M_PI)
      {
        leg_argu_[1].target_theta += 2 * M_PI;
      }
      leg_argu_[0].Tp =
          500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
                                               leg_argu_[0].theta, dt_);
      leg_argu_[1].Tp =
          500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
                                               leg_argu_[1].theta, dt_);
    }
    else
    {
      leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
                                0.10f, this->leg_argu_[0].L0, this->dt_);
      leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
                                0.10f, this->leg_argu_[1].L0, this->dt_);

      if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20))
      {
        leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
                                    0.1, leg_argu_[0].theta, dt_);
        leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
                                    0.1, leg_argu_[1].theta, dt_);
        clampf(&leg_argu_[0].Tp, -10, 10);
        clampf(&leg_argu_[1].Tp, -10, 10);
      }
      else
      {
        leg_argu_[0].Tp = 0;
        leg_argu_[1].Tp = 0;
      }
    }

    result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
                                  -leg_argu_[0].Tp, leg_argu_[0].F0);
    this->leg_argu_[0].T1 = std::get<0>(result3);
    this->leg_argu_[0].T2 = std::get<1>(result3);

    result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
                                  -leg_argu_[1].Tp, leg_argu_[1].F0);
    this->leg_argu_[1].T1 = -std::get<0>(result4);
    this->leg_argu_[1].T2 = -std::get<1>(result4);

    this->hip_motor_out_[0] = this->leg_argu_[0].T1;
    this->hip_motor_out_[1] = this->leg_argu_[0].T2;
    this->hip_motor_out_[2] = this->leg_argu_[1].T1;
    this->hip_motor_out_[3] = this->leg_argu_[1].T2;

    this->hip_motor_flag_ = 1;
    break;
  }
}

void Wheelleg::Control()
{
  clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
  clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
  clampf(&hip_motor_out_[0], -20.0f, 20.0f);
  clampf(&hip_motor_out_[1], -20.0f, 20.0f);
  clampf(&hip_motor_out_[2], -20.0f, 20.0f);
  clampf(&hip_motor_out_[3], -20.0f, 20.0f);

  // if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
  //     fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
  //   wheel_motor_out_[0] = 0;
  //   wheel_motor_out_[1] = 0;

  //   if (fabs(this->pit_) > 0.5f) {
  //     this->hip_motor_flag_ = 0;
  //   }
  // }

  switch (this->mode_)
  {
  case Wheelleg::RELAX:

    this->wheel_motor_[0]->Relax();
    this->wheel_motor_[1]->Relax();
    hip_motor_flag_ = 0;
    hip_motor_[0]->SetMit(0.0f);
    hip_motor_[1]->SetMit(0.0f);
    hip_motor_[2]->SetMit(0.0f);
    hip_motor_[3]->SetMit(0.0f);
    break;

  case Wheelleg::STAND:
  case Wheelleg::ROTOR:
    // this->wheel_motor_[0]->Relax();
    // this->wheel_motor_[1]->Relax();
    this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
    this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
    hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
    hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
    hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
    hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
    // hip_motor_[0]->SetMit(0.0f);
    // hip_motor_[1]->SetMit(0.0f);
    // hip_motor_[2]->SetMit(0.0f);
    // hip_motor_[3]->SetMit(0.0f);
    break;

  case Wheelleg::RESET:

    this->wheel_motor_[0]->Relax();
    this->wheel_motor_[1]->Relax();

    hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
    hip_motor_[1]->SetMit(this->hip_motor_out_[1]);

    hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
    hip_motor_[3]->SetMit(this->hip_motor_out_[3]);

    break;
  }
}

void Wheelleg::SetMode(Wheelleg::Mode mode)
{
  if (mode == this->mode_)
  {
    return;
  }

  this->leg_[0]->Reset();
  this->leg_[1]->Reset();
  move_argu_.x = 0;
  move_argu_.x_dot = 0;
  move_argu_.last_x_dot = 0;
  move_argu_.target_x = move_argu_.xhat;
  move_argu_.target_yaw = this->eulr_.yaw;
  move_argu_.target_dot_x = 0;
  move_argu_.xhat = 0;
  move_argu_.x_dot_hat = 0;
  this->manfilter_.reset_comp();
  this->mode_ = mode;
}