#ifndef MKT_PLUGINS_ONE_D_VELOCITY_ITERATOR_H_INCLUDED_
#define MKT_PLUGINS_ONE_D_VELOCITY_ITERATOR_H_INCLUDED_

#include <algorithm>
#include <cmath>
#include <robot/robot.h>

namespace mkt_plugins
{

  const double EPSILON = 5E-2;

  inline double exponentialFactor(double x, double k = 2.5)
  {
    x = std::clamp(x, 0.0, 1.0);
    return 1.0 - std::exp(-k * x); // tương đương (1 - e^{-kx})
  }

  /**
   * @brief Given initial conditions and a time, figure out the end velocity
   *
   * @param v0 Initial velocity
   * @param accel The acceleration rate
   * @param decel The decceleration rate
   * @param dt Delta time - amount of time to project into the future
   * @param target target velocity
   * @return The velocity dt seconds after v0.
   */
  inline double projectVelocity(double v0, double accel, double decel, double dt, double target)
  {
    double magnitude;
    if (fabs(v0) < EPSILON)
    {
      // Starting from standstill, always accelerate
      if (target >= 0.0)
      {
        magnitude = fabs(accel);
      }
      else
      {
        magnitude = -fabs(accel);
      }
    }
    else if (v0 > 0.0)
    {
      if (v0 < target)
      {
        // Acceleration (speed magnitude increases)
        magnitude = fabs(accel);
      }
      else
      {
        // Deceleration (speed magnitude decreases)
        magnitude = -fabs(decel);
      }
    }
    else
    {
      if (v0 < target)
      {
        // Deceleration (speed magnitude decreases)
        magnitude = fabs(decel);
      }
      else
      {
        // Acceleration (speed magnitude increases)
        magnitude = -fabs(accel);
      }
    }

    double delta_v = fabs(target - v0);
    double max_v = std::max(fabs(target), 1e-3);
    magnitude *= exponentialFactor(delta_v / max_v, 3.0);
    double v1 = v0 + magnitude * dt;

    if (magnitude > 0.0)
    {
      return std::min(target, v1);
    }
    else
    {
      return std::max(target, v1);
    }
  }

  /**
   * @class OneDVelocityIterator
   * @brief An iterator for generating a number of samples in a range
   *
   * In its simplest usage, this gives us N (num_samples) different velocities that are reachable
   * given our current velocity. However, there is some fancy logic around zero velocities and
   * the min/max velocities
   *
   * If the current velocity is 2 m/s, and the acceleration limit is 1 m/ss and the acc_time is 1 s,
   * this class would provide velocities between 1 m/s and 3 m/s.
   *
   *
   *
   */
  class OneDVelocityIterator
  {
  public:
    std::string name_;
    /**
     * @brief Constructor for the velocity iterator
     * @param current Current velocity of the robot
     * @param direct Direction multiplier (1 for forward, -1 for backward, 0 for stop)
     * @param min Minimum velocity allowable
     * @param max Maximum velocity allowable
     * @param acc_limit Maximum acceleration limit
     * @param decel_limit Maximum deceleration limit
     * @param acc_time Time delta for velocity projection
     * @param num_samples Number of velocity samples to generate
     * 
     * Creates an iterator that generates num_samples velocities reachable from
     * current velocity within acc_time seconds, respecting acceleration/deceleration limits.
     * The velocities range from min_vel_ to max_vel_ (projected from current velocity).
     */
    OneDVelocityIterator(
      std::string name, double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time, int num_samples)
     : name_(name)
    {
      // if (current < min)
      // {
      //   current = min;
      // }
      // else if (current > max)
      // {
      //   current = max;
      // }
      max_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, max);
      min_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, min);
      direct_ = direct;
      current_ = direct_ > 0 ? max_vel_ : min_vel_;
      reset();

      if (min_vel_ - max_vel_ < EPSILON)
      {
        increment_ = 1.0;
        return;
      }
      num_samples = std::max(2, num_samples);

      // e.g. for 4 samples, split distance in 3 even parts
      increment_ = (max_vel_ - min_vel_) / std::max(1, (num_samples - 1));
    }

    /**
     * @brief Get the current velocity value
     * @return Current velocity value, or 0.0 if return_zero_now_ is true
     * 
     * Returns the velocity at the current iterator position.
     * If the iterator is set to return zero (when crossing zero velocity),
     * returns 0.0 instead of the current value.
     */
    double getVelocity() const
    {
      if (return_zero_now_)
        return 0.0;
      return current_;
    }

    /**
     * @brief Increment the iterator to the next velocity sample
     * @return Reference to this iterator
     * 
     * Moves to the next velocity sample. If the iterator crosses zero velocity
     * (from negative to positive), it will return zero velocity on the next call
     * to getVelocity() before continuing with positive velocities.
     */
    OneDVelocityIterator &operator++()
    {
      if (return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 && current_ + increment_ <= max_vel_ + EPSILON)
      {
        return_zero_now_ = true;
        return_zero_ = false;
      }
      else
      {
        current_ += increment_;
        return_zero_now_ = false;
      }
      return *this;
    }

    /**
     * @brief Reset the iterator back to the first velocity
     * 
     * Resets the iterator to the starting velocity (max_vel_ if direct > 0,
     * min_vel_ if direct <= 0) and enables zero-velocity return if applicable.
     */
    void reset()
    {
      return_zero_ = true;
      return_zero_now_ = false;
    }

    /**
     * @brief Check if all velocities for this iteration have been returned
     * @return True if the iterator has passed beyond the limit velocity
     * 
     * Returns true when the current velocity has exceeded the maximum
     * (if direct > 0) or minimum (if direct <= 0) velocity limit.
     */
    bool isFinished() const
    {
      double limit_vel = direct_ > 0? max_vel_ : min_vel_;
      return fabs(current_) > fabs(limit_vel) + EPSILON;
    }

  private:
    bool return_zero_, return_zero_now_;
    double min_vel_, max_vel_;
    double current_;
    double direct_;
    double increment_;
  };
} // namespace mkt_plugins

#endif // MKT_PLUGINS_ONE_D_VELOCITY_ITERATOR_H_INCLUDED
