#include "origincar_base/wall_kalman_filter.hpp"

#include <algorithm>
#include <cmath>

namespace origincar_wall
{

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_;
}

}  // namespace origincar_wall
