#include "RemoteState.h"

void RemoteState::begin() {
  speed_.reset(speed_target_);
  tilt_blend_.reset(0);
}

void RemoteState::setMessage(const char* msg, uint32_t now_ms) {
  message_ = msg;
  message_until_ms_ = now_ms + STATUS_MESSAGE_MS;
}

// Disarming always clears the stand request: the robot arms into SIT, and
// re-arming must never make it stand up on its own.
void RemoteState::disarm() {
  armed_ = false;
  stand_ = false;
}

bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uint32_t now) {
  const StickFrame& sticks = stick_hw.frame();
  switch (e.cmd) {
    case UiCmd::None:
      return false;

    case UiCmd::Disarm:
      disarm();
      return true;

    case UiCmd::ToggleArm:
      if (armed_) { disarm(); return true; }
      if (calibrating_) { setMessage("Finish calibration first", now); return false; }
      if (!sticks.allCentered(ARM_CENTER_TOLERANCE)) { setMessage("Center both sticks to arm", now); return false; }
      armed_ = true;
      return true;

    case UiCmd::ToggleStand:
      if (!armed_) { setMessage("Arm first", now); return false; }
      stand_ = !stand_;
      return true;

    case UiCmd::ToggleGait:        gait_wave_    = !gait_wave_;    return true;
    case UiCmd::ToggleWalkMode:    mode_b_       = !mode_b_;       return true;
    case UiCmd::ToggleRStickXlate: rstick_xlate_ = !rstick_xlate_; return true;
    case UiCmd::ToggleRobotGyro:   robot_gyro_   = !robot_gyro_;   return true;

    case UiCmd::ToggleRemoteGyro:
      if (!remote_gyro_ && !imu.ok()) { setMessage("Remote IMU not detected", now); return false; }
      remote_gyro_ = !remote_gyro_;
      // Re-zero on enable: whatever angle you're holding the remote at when
      // you switch it on becomes "level", so the body doesn't lurch.
      if (remote_gyro_) imu.zeroLevel();
      return true;

    case UiCmd::SetSpeed:  speed_target_ = e.value; return true;
    case UiCmd::SpeedUp:   speed_target_ = (uint8_t)min(255, speed_target_ + SPEED_SCALE_STEP); return true;
    case UiCmd::SpeedDown: speed_target_ = (uint8_t)max(0, speed_target_ - SPEED_SCALE_STEP);   return true;

    case UiCmd::SetHeight:  height_ = (int16_t)constrain(((int)e.value - 100) * 10, -1000, 1000); return true;
    case UiCmd::HeightUp:   height_ = (int16_t)min(1000, height_ + HEIGHT_STEP);  return true;
    case UiCmd::HeightDown: height_ = (int16_t)max(-1000, height_ - HEIGHT_STEP); return true;

    case UiCmd::ZeroGyro:
      imu.zeroLevel();
      setMessage("Remote level zeroed", now);
      return true;

    case UiCmd::CalStartSave:
      if (!calibrating_) {
        if (armed_) { setMessage("Disarm to calibrate", now); return false; }
        stick_hw.startCalibration();
        calibrating_ = true;
        setMessage("Sweep all axes to limits, release, SAVE", now);
        return true;
      }
      if (!stick_hw.saveCalibration()) { setMessage("Sweep too small - keep sweeping", now); return false; }
      calibrating_ = false;
      setMessage("Calibration saved", now);
      return true;

    case UiCmd::NextionUpload:
      return false;   // not operator state - remote.ino handles it

    case UiCmd::CalCancel:
      if (!calibrating_) return false;
      stick_hw.cancelCalibration();
      calibrating_ = false;
      setMessage("Calibration cancelled", now);
      return true;
  }
  return false;
}

void RemoteState::update(float dt, bool link_ok, uint32_t now_ms) {
  // Mirror the robot's link failsafe: it disarms (servos relax) when packets
  // stop, so the remote disarms too and the operator must re-arm on purpose
  // once the link is back.
  if (!link_ok && link_was_ok_ && armed_) {
    disarm();
    setMessage("Link lost - disarmed", now_ms);
  }
  link_was_ok_ = link_ok;

  speed_.setTarget(speed_target_);
  speed_.update(dt);
  tilt_blend_.setTarget(remote_gyro_ ? 1.0f : 0.0f);
  tilt_blend_.update(dt);
}

void RemoteState::fillView(UiView& v, uint32_t now_ms) const {
  v.armed = armed_;
  v.stand = stand_;
  v.gait_wave = gait_wave_;
  v.mode_b = mode_b_;
  v.rstick_xlate = rstick_xlate_;
  v.remote_gyro = remote_gyro_;
  v.robot_gyro = robot_gyro_;
  v.calibrating = calibrating_;
  v.speed_target = speed_target_;
  v.height = height_;
  v.message = message(now_ms);
}