#include "origincar_base/wall_kalman_filter.hpp"

#include <algorithm>
#include <cmath>

namespace origincar_wall
{

namespace
{
WallKalmanNoise highTrustNoise()
{
  return {0.015, 0.015, 0.025};
}

WallKalmanNoise mediumTrustNoise()
{
  return {0.045, 0.045, 0.060};
}

WallKalmanNoise weakTrustNoise()
{
  return {0.10, 0.10, 0.12};
}
}  // namespace

WallKalmanFilter::WallKalmanFilter(const Pose2D &initial_pose)
    : pose_(initial_pose),
      covariance_{0.10, 0.10, 0.10},
      process_noise_per_second_{0.04, 0.04, 0.06}
{
  pose_.theta = normalizeAngle(pose_.theta);
}

void WallKalmanFilter::reset(const Pose2D &pose)
{
  pose_ = pose;
  pose_.theta = normalizeAngle(pose_.theta);
  covariance_ = {0.10, 0.10, 0.10};
}

void WallKalmanFilter::predict(double vx, double vy, double yaw_rate, double dt)
{
  dt = std::max(0.0, dt);
  const auto cos_t = std::cos(pose_.theta);
  const auto sin_t = std::sin(pose_.theta);
  pose_.x += (vx * cos_t - vy * sin_t) * dt;
  pose_.y += (vx * sin_t + vy * cos_t) * dt;
  pose_.theta = normalizeAngle(pose_.theta + yaw_rate * dt);
  covariance_.x += process_noise_per_second_.x * dt;
  covariance_.y += process_noise_per_second_.y * dt;
  covariance_.theta += process_noise_per_second_.theta * dt;
}

void WallKalmanFilter::correct(const Pose2D &measurement, const WallKalmanNoise &measurement_noise)
{
  const auto noise_x = std::max(1e-6, measurement_noise.x);
  const auto noise_y = std::max(1e-6, measurement_noise.y);
  const auto noise_theta = std::max(1e-6, measurement_noise.theta);

  const auto kx = covariance_.x / (covariance_.x + noise_x);
  const auto ky = covariance_.y / (covariance_.y + noise_y);
  const auto kt = covariance_.theta / (covariance_.theta + noise_theta);

  pose_.x += kx * (measurement.x - pose_.x);
  pose_.y += ky * (measurement.y - pose_.y);
  pose_.theta = normalizeAngle(pose_.theta + kt * normalizeAngle(measurement.theta - pose_.theta));

  covariance_.x = (1.0 - kx) * covariance_.x;
  covariance_.y = (1.0 - ky) * covariance_.y;
  covariance_.theta = (1.0 - kt) * covariance_.theta;
}

Pose2D WallKalmanFilter::pose() const
{
  return pose_;
}

WallKalmanNoise WallKalmanFilter::covariance() const
{
  return covariance_;
}

WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose,
                                                const Pose2D &scan_pose,
                                                const LocalizationQuality &quality,
                                                bool stationary)
{
  WallScanUpdatePolicy policy{false, WallScanTrust::Reject, weakTrustNoise(), 0.0, ""};
  policy.innovation_distance = std::hypot(scan_pose.x - filter_pose.x, scan_pose.y - filter_pose.y);

  if (!quality.has_mean_error)
  {
    policy.reason = "missing mean error";
    return policy;
  }
  if (quality.wall_count < 3)
  {
    policy.reason = "too few walls";
    return policy;
  }
  if (quality.mean_error > 0.15)
  {
    policy.reason = "mean error too high";
    return policy;
  }

  if (stationary && policy.innovation_distance > 0.04)
  {
    policy.reason = "stationary innovation too large";
    return policy;
  }
  if (!stationary && policy.innovation_distance > 0.15)
  {
    policy.reason = "motion innovation too large";
    return policy;
  }

  if (quality.wall_count == 4 && quality.mean_error < 0.05)
  {
    policy.accept = true;
    policy.trust = stationary ? WallScanTrust::Medium : WallScanTrust::High;
    policy.noise = stationary ? mediumTrustNoise() : highTrustNoise();
    policy.reason = stationary ? "stationary high trust downgraded to medium" : "high trust";
    return policy;
  }

  if (quality.wall_count >= 3 && quality.mean_error < 0.10)
  {
    policy.accept = true;
    policy.trust = stationary ? WallScanTrust::Weak : WallScanTrust::Medium;
    policy.noise = stationary ? weakTrustNoise() : mediumTrustNoise();
    policy.reason = stationary ? "stationary medium trust downgraded to weak" : "medium trust";
    return policy;
  }

  if (quality.wall_count >= 3 && quality.mean_error <= 0.15)
  {
    if (stationary)
    {
      policy.reason = "stationary weak trust rejected";
      return policy;
    }
    policy.accept = true;
    policy.trust = WallScanTrust::Weak;
    policy.noise = weakTrustNoise();
    policy.reason = "weak trust";
    return policy;
  }

  policy.reason = "rejected by policy";
  return policy;
}

}  // namespace origincar_wall
