/*
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author: Tobias Kunz <tobias@gatech.edu>
 * Date: 05/2012
 *
 * Humanoid Robotics Lab      Georgia Institute of Technology
 * Director: Mike Stilman     http://www.golems.org
 *
 * Algorithm details and publications:
 * http://www.golems.org/node/1570
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include <limits>
#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
#include <vector>
#include "rclcpp/rclcpp.hpp"

namespace trajectory_processing
{
namespace
{
static const rclcpp::Logger LOGGER =
    rclcpp::get_logger("moveit_trajectory_processing.time_optimal_trajectory_generation");
constexpr double DEFAULT_TIMESTEP = 1e-3;
constexpr double EPS = 1e-6;
}  // namespace

class LinearPathSegment : public PathSegment
{
public:
  LinearPathSegment(const Eigen::VectorXd& start, const Eigen::VectorXd& end)
    : PathSegment((end - start).norm()), end_(end), start_(start)
  {
  }

  Eigen::VectorXd getConfig(double s) const override
  {
    s /= length_;
    s = std::max(0.0, std::min(1.0, s));
    return (1.0 - s) * start_ + s * end_;
  }

  Eigen::VectorXd getTangent(double /* s */) const override
  {
    return (end_ - start_) / length_;
  }

  Eigen::VectorXd getCurvature(double /* s */) const override
  {
    return Eigen::VectorXd::Zero(start_.size());
  }

  std::list<double> getSwitchingPoints() const override
  {
    return std::list<double>();
  }

  LinearPathSegment* clone() const override
  {
    return new LinearPathSegment(*this);
  }

private:
  Eigen::VectorXd end_;
  Eigen::VectorXd start_;
};

class CircularPathSegment : public PathSegment
{
public:
  CircularPathSegment(const Eigen::VectorXd& start, const Eigen::VectorXd& intersection, const Eigen::VectorXd& end,
                      double max_deviation)
  {
    if ((intersection - start).norm() < 0.000001 || (end - intersection).norm() < 0.000001)
    {
      length_ = 0.0;
      radius = 1.0;
      center = intersection;
      x = Eigen::VectorXd::Zero(start.size());
      y = Eigen::VectorXd::Zero(start.size());
      return;
    }

    const Eigen::VectorXd start_direction = (intersection - start).normalized();
    const Eigen::VectorXd end_direction = (end - intersection).normalized();
    const double start_dot_end = start_direction.dot(end_direction);

    // catch division by 0 in computations below
    if (start_dot_end > 0.999999 || start_dot_end < -0.999999)
    {
      length_ = 0.0;
      radius = 1.0;
      center = intersection;
      x = Eigen::VectorXd::Zero(start.size());
      y = Eigen::VectorXd::Zero(start.size());
      return;
    }

    const double angle = acos(start_dot_end);
    const double start_distance = (start - intersection).norm();
    const double end_distance = (end - intersection).norm();

    // enforce max deviation
    double distance = std::min(start_distance, end_distance);
    distance = std::min(distance, max_deviation * sin(0.5 * angle) / (1.0 - cos(0.5 * angle)));

    radius = distance / tan(0.5 * angle);
    length_ = angle * radius;

    center = intersection + (end_direction - start_direction).normalized() * radius / cos(0.5 * angle);
    x = (intersection - distance * start_direction - center).normalized();
    y = start_direction;
  }

  Eigen::VectorXd getConfig(double s) const override
  {
    const double angle = s / radius;
    return center + radius * (x * cos(angle) + y * sin(angle));
  }

  Eigen::VectorXd getTangent(double s) const override
  {
    const double angle = s / radius;
    return -x * sin(angle) + y * cos(angle);
  }

  Eigen::VectorXd getCurvature(double s) const override
  {
    const double angle = s / radius;
    return -1.0 / radius * (x * cos(angle) + y * sin(angle));
  }

  std::list<double> getSwitchingPoints() const override
  {
    std::list<double> switching_points;
    const double dim = x.size();
    for (unsigned int i = 0; i < dim; ++i)
    {
      double switching_angle = atan2(y[i], x[i]);
      if (switching_angle < 0.0)
      {
        switching_angle += M_PI;
      }
      const double switching_point = switching_angle * radius;
      if (switching_point < length_)
      {
        switching_points.push_back(switching_point);
      }
    }
    switching_points.sort();
    return switching_points;
  }

  CircularPathSegment* clone() const override
  {
    return new CircularPathSegment(*this);
  }

private:
  double radius;
  Eigen::VectorXd center;
  Eigen::VectorXd x;
  Eigen::VectorXd y;
};

Path::Path(const std::list<Eigen::VectorXd>& path, double max_deviation) : length_(0.0)
{
  if (path.size() < 2)
    return;
  std::list<Eigen::VectorXd>::const_iterator path_iterator1 = path.begin();
  std::list<Eigen::VectorXd>::const_iterator path_iterator2 = path_iterator1;
  ++path_iterator2;
  std::list<Eigen::VectorXd>::const_iterator path_iterator3;
  Eigen::VectorXd start_config = *path_iterator1;
  while (path_iterator2 != path.end())
  {
    path_iterator3 = path_iterator2;
    ++path_iterator3;
    if (max_deviation > 0.0 && path_iterator3 != path.end())
    {
      CircularPathSegment* blend_segment =
          new CircularPathSegment(0.5 * (*path_iterator1 + *path_iterator2), *path_iterator2,
                                  0.5 * (*path_iterator2 + *path_iterator3), max_deviation);
      Eigen::VectorXd end_config = blend_segment->getConfig(0.0);
      if ((end_config - start_config).norm() > 0.000001)
      {
        path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, end_config));
      }
      path_segments_.emplace_back(blend_segment);

      start_config = blend_segment->getConfig(blend_segment->getLength());
    }
    else
    {
      path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, *path_iterator2));
      start_config = *path_iterator2;
    }
    path_iterator1 = path_iterator2;
    ++path_iterator2;
  }

  // Create list of switching point candidates, calculate total path length and
  // absolute positions of path segments
  for (std::unique_ptr<PathSegment>& path_segment : path_segments_)
  {
    path_segment->position_ = length_;
    std::list<double> local_switching_points = path_segment->getSwitchingPoints();
    for (std::list<double>::const_iterator point = local_switching_points.begin();
         point != local_switching_points.end(); ++point)
    {
      switching_points_.push_back(std::make_pair(length_ + *point, false));
    }
    length_ += path_segment->getLength();
    while (!switching_points_.empty() && switching_points_.back().first >= length_)
      switching_points_.pop_back();
    switching_points_.push_back(std::make_pair(length_, true));
  }
  switching_points_.pop_back();
}

Path::Path(const Path& path) : length_(path.length_), switching_points_(path.switching_points_)
{
  for (const std::unique_ptr<PathSegment>& path_segment : path.path_segments_)
  {
    path_segments_.emplace_back(path_segment->clone());
  }
}

double Path::getLength() const
{
  return length_;
}

PathSegment* Path::getPathSegment(double& s) const
{
  std::list<std::unique_ptr<PathSegment>>::const_iterator it = path_segments_.begin();
  std::list<std::unique_ptr<PathSegment>>::const_iterator next = it;
  ++next;
  while (next != path_segments_.end() && s >= (*next)->position_)
  {
    it = next;
    ++next;
  }
  s -= (*it)->position_;
  return (*it).get();
}

Eigen::VectorXd Path::getConfig(double s) const
{
  const PathSegment* path_segment = getPathSegment(s);
  return path_segment->getConfig(s);
}

Eigen::VectorXd Path::getTangent(double s) const
{
  const PathSegment* path_segment = getPathSegment(s);
  return path_segment->getTangent(s);
}

Eigen::VectorXd Path::getCurvature(double s) const
{
  const PathSegment* path_segment = getPathSegment(s);
  return path_segment->getCurvature(s);
}

double Path::getNextSwitchingPoint(double s, bool& discontinuity) const
{
  std::list<std::pair<double, bool>>::const_iterator it = switching_points_.begin();
  while (it != switching_points_.end() && it->first <= s)
  {
    ++it;
  }
  if (it == switching_points_.end())
  {
    discontinuity = true;
    return length_;
  }
  discontinuity = it->second;
  return it->first;
}

std::list<std::pair<double, bool>> Path::getSwitchingPoints() const
{
  return switching_points_;
}

Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration,
                       double time_step)
  : path_(path)
  , max_velocity_(max_velocity)
  , max_acceleration_(max_acceleration)
  , joint_num_(max_velocity.size())
  , valid_(true)
  , time_step_(time_step)
  , cached_time_(std::numeric_limits<double>::max())
{
  trajectory_.push_back(TrajectoryStep(0.0, 0.0));
  double after_acceleration = getMinMaxPathAcceleration(0.0, 0.0, true);
  while (valid_ && !integrateForward(trajectory_, after_acceleration) && valid_)
  {
    double before_acceleration;
    TrajectoryStep switching_point;
    if (getNextSwitchingPoint(trajectory_.back().path_pos_, switching_point, before_acceleration, after_acceleration))
    {
      break;
    }
    integrateBackward(trajectory_, switching_point.path_pos_, switching_point.path_vel_, before_acceleration);
  }

  if (valid_)
  {
    double before_acceleration = getMinMaxPathAcceleration(path_.getLength(), 0.0, false);
    integrateBackward(trajectory_, path_.getLength(), 0.0, before_acceleration);
  }

  if (valid_)
  {
    // Calculate timing
    std::list<TrajectoryStep>::iterator previous = trajectory_.begin();
    std::list<TrajectoryStep>::iterator it = previous;
    it->time_ = 0.0;
    ++it;
    while (it != trajectory_.end())
    {
      it->time_ =
          previous->time_ + (it->path_pos_ - previous->path_pos_) / ((it->path_vel_ + previous->path_vel_) / 2.0);
      previous = it;
      ++it;
    }
  }
}

Trajectory::~Trajectory()
{
}

// Returns true if end of path is reached.
bool Trajectory::getNextSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
                                       double& before_acceleration, double& after_acceleration)
{
  TrajectoryStep acceleration_switching_point(path_pos, 0.0);
  double acceleration_before_acceleration, acceleration_after_acceleration;
  bool acceleration_reached_end;
  do
  {
    acceleration_reached_end =
        getNextAccelerationSwitchingPoint(acceleration_switching_point.path_pos_, acceleration_switching_point,
                                          acceleration_before_acceleration, acceleration_after_acceleration);
  } while (!acceleration_reached_end &&
           acceleration_switching_point.path_vel_ > getVelocityMaxPathVelocity(acceleration_switching_point.path_pos_));

  TrajectoryStep velocity_switching_point(path_pos, 0.0);
  double velocity_before_acceleration, velocity_after_acceleration;
  bool velocity_reached_end;
  do
  {
    velocity_reached_end = getNextVelocitySwitchingPoint(velocity_switching_point.path_pos_, velocity_switching_point,
                                                         velocity_before_acceleration, velocity_after_acceleration);
  } while (
      !velocity_reached_end && velocity_switching_point.path_pos_ <= acceleration_switching_point.path_pos_ &&
      (velocity_switching_point.path_vel_ > getAccelerationMaxPathVelocity(velocity_switching_point.path_pos_ - EPS) ||
       velocity_switching_point.path_vel_ > getAccelerationMaxPathVelocity(velocity_switching_point.path_pos_ + EPS)));

  if (acceleration_reached_end && velocity_reached_end)
  {
    return true;
  }
  else if (!acceleration_reached_end &&
           (velocity_reached_end || acceleration_switching_point.path_pos_ <= velocity_switching_point.path_pos_))
  {
    next_switching_point = acceleration_switching_point;
    before_acceleration = acceleration_before_acceleration;
    after_acceleration = acceleration_after_acceleration;
    return false;
  }
  else
  {
    next_switching_point = velocity_switching_point;
    before_acceleration = velocity_before_acceleration;
    after_acceleration = velocity_after_acceleration;
    return false;
  }
}

bool Trajectory::getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
                                                   double& before_acceleration, double& after_acceleration)
{
  double switching_path_pos = path_pos;
  double switching_path_vel;
  while (true)
  {
    bool discontinuity;
    switching_path_pos = path_.getNextSwitchingPoint(switching_path_pos, discontinuity);

    if (switching_path_pos > path_.getLength() - EPS)
    {
      return true;
    }

    if (discontinuity)
    {
      const double before_path_vel = getAccelerationMaxPathVelocity(switching_path_pos - EPS);
      const double after_path_vel = getAccelerationMaxPathVelocity(switching_path_pos + EPS);
      switching_path_vel = std::min(before_path_vel, after_path_vel);
      before_acceleration = getMinMaxPathAcceleration(switching_path_pos - EPS, switching_path_vel, false);
      after_acceleration = getMinMaxPathAcceleration(switching_path_pos + EPS, switching_path_vel, true);

      if ((before_path_vel > after_path_vel ||
           getMinMaxPhaseSlope(switching_path_pos - EPS, switching_path_vel, false) >
               getAccelerationMaxPathVelocityDeriv(switching_path_pos - 2.0 * EPS)) &&
          (before_path_vel < after_path_vel || getMinMaxPhaseSlope(switching_path_pos + EPS, switching_path_vel, true) <
                                                   getAccelerationMaxPathVelocityDeriv(switching_path_pos + 2.0 * EPS)))
      {
        break;
      }
    }
    else
    {
      switching_path_vel = getAccelerationMaxPathVelocity(switching_path_pos);
      before_acceleration = 0.0;
      after_acceleration = 0.0;

      if (getAccelerationMaxPathVelocityDeriv(switching_path_pos - EPS) < 0.0 &&
          getAccelerationMaxPathVelocityDeriv(switching_path_pos + EPS) > 0.0)
      {
        break;
      }
    }
  }

  next_switching_point = TrajectoryStep(switching_path_pos, switching_path_vel);
  return false;
}

bool Trajectory::getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
                                               double& before_acceleration, double& after_acceleration)
{
  bool start = false;
  path_pos -= DEFAULT_TIMESTEP;
  do
  {
    path_pos += DEFAULT_TIMESTEP;

    if (getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) >=
        getVelocityMaxPathVelocityDeriv(path_pos))
    {
      start = true;
    }
  } while ((!start || getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) >
                          getVelocityMaxPathVelocityDeriv(path_pos)) &&
           path_pos < path_.getLength());

  if (path_pos >= path_.getLength())
  {
    return true;  // end of trajectory reached
  }

  double before_path_pos = path_pos - DEFAULT_TIMESTEP;
  double after_path_pos = path_pos;
  while (after_path_pos - before_path_pos > EPS)
  {
    path_pos = (before_path_pos + after_path_pos) / 2.0;
    if (getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) >
        getVelocityMaxPathVelocityDeriv(path_pos))
    {
      before_path_pos = path_pos;
    }
    else
    {
      after_path_pos = path_pos;
    }
  }

  before_acceleration = getMinMaxPathAcceleration(before_path_pos, getVelocityMaxPathVelocity(before_path_pos), false);
  after_acceleration = getMinMaxPathAcceleration(after_path_pos, getVelocityMaxPathVelocity(after_path_pos), true);
  next_switching_point = TrajectoryStep(after_path_pos, getVelocityMaxPathVelocity(after_path_pos));
  return false;
}

// Returns true if end of path is reached
bool Trajectory::integrateForward(std::list<TrajectoryStep>& trajectory, double acceleration)
{
  double path_pos = trajectory.back().path_pos_;
  double path_vel = trajectory.back().path_vel_;

  std::list<std::pair<double, bool>> switching_points = path_.getSwitchingPoints();
  std::list<std::pair<double, bool>>::iterator next_discontinuity = switching_points.begin();

  while (true)
  {
    while ((next_discontinuity != switching_points.end()) &&
           (next_discontinuity->first <= path_pos || !next_discontinuity->second))
    {
      ++next_discontinuity;
    }

    double old_path_pos = path_pos;
    double old_path_vel = path_vel;

    path_vel += time_step_ * acceleration;
    path_pos += time_step_ * 0.5 * (old_path_vel + path_vel);

    if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first)
    {
      // Avoid having a TrajectoryStep with path_pos near a switching point which will cause an almost identical
      // TrajectoryStep get added in the next run (https://github.com/ros-planning/moveit/issues/1665)
      if (path_pos - next_discontinuity->first < EPS)
      {
        continue;
      }
      path_vel = old_path_vel +
                 (next_discontinuity->first - old_path_pos) * (path_vel - old_path_vel) / (path_pos - old_path_pos);
      path_pos = next_discontinuity->first;
    }

    if (path_pos > path_.getLength())
    {
      trajectory.push_back(TrajectoryStep(path_pos, path_vel));
      return true;
    }
    else if (path_vel < 0.0)
    {
      valid_ = false;
      RCLCPP_ERROR(LOGGER, "Error while integrating forward: Negative path velocity");
      return true;
    }

    if (path_vel > getVelocityMaxPathVelocity(path_pos) &&
        getMinMaxPhaseSlope(old_path_pos, getVelocityMaxPathVelocity(old_path_pos), false) <=
            getVelocityMaxPathVelocityDeriv(old_path_pos))
    {
      path_vel = getVelocityMaxPathVelocity(path_pos);
    }

    trajectory.push_back(TrajectoryStep(path_pos, path_vel));
    acceleration = getMinMaxPathAcceleration(path_pos, path_vel, true);

    if (path_vel > getAccelerationMaxPathVelocity(path_pos) || path_vel > getVelocityMaxPathVelocity(path_pos))
    {
      // Find more accurate intersection with max-velocity curve using bisection
      TrajectoryStep overshoot = trajectory.back();
      trajectory.pop_back();
      double before = trajectory.back().path_pos_;
      double before_path_vel = trajectory.back().path_vel_;
      double after = overshoot.path_pos_;
      double after_path_vel = overshoot.path_vel_;
      while (after - before > EPS)
      {
        const double midpoint = 0.5 * (before + after);
        double midpoint_path_vel = 0.5 * (before_path_vel + after_path_vel);

        if (midpoint_path_vel > getVelocityMaxPathVelocity(midpoint) &&
            getMinMaxPhaseSlope(before, getVelocityMaxPathVelocity(before), false) <=
                getVelocityMaxPathVelocityDeriv(before))
        {
          midpoint_path_vel = getVelocityMaxPathVelocity(midpoint);
        }

        if (midpoint_path_vel > getAccelerationMaxPathVelocity(midpoint) ||
            midpoint_path_vel > getVelocityMaxPathVelocity(midpoint))
        {
          after = midpoint;
          after_path_vel = midpoint_path_vel;
        }
        else
        {
          before = midpoint;
          before_path_vel = midpoint_path_vel;
        }
      }
      trajectory.push_back(TrajectoryStep(before, before_path_vel));

      if (getAccelerationMaxPathVelocity(after) < getVelocityMaxPathVelocity(after))
      {
        if (after > next_discontinuity->first)
        {
          return false;
        }
        else if (getMinMaxPhaseSlope(trajectory.back().path_pos_, trajectory.back().path_vel_, true) >
                 getAccelerationMaxPathVelocityDeriv(trajectory.back().path_pos_))
        {
          return false;
        }
      }
      else
      {
        if (getMinMaxPhaseSlope(trajectory.back().path_pos_, trajectory_.back().path_vel_, false) >
            getVelocityMaxPathVelocityDeriv(trajectory_.back().path_pos_))
        {
          return false;
        }
      }
    }
  }
}

void Trajectory::integrateBackward(std::list<TrajectoryStep>& start_trajectory, double path_pos, double path_vel,
                                   double acceleration)
{
  std::list<TrajectoryStep>::iterator start2 = start_trajectory.end();
  --start2;
  std::list<TrajectoryStep>::iterator start1 = start2;
  --start1;
  std::list<TrajectoryStep> trajectory;
  double slope;
  assert(start1->path_pos_ <= path_pos);

  while (start1 != start_trajectory.begin() || path_pos >= 0.0)
  {
    if (start1->path_pos_ <= path_pos)
    {
      trajectory.push_front(TrajectoryStep(path_pos, path_vel));
      path_vel -= time_step_ * acceleration;
      path_pos -= time_step_ * 0.5 * (path_vel + trajectory.front().path_vel_);
      acceleration = getMinMaxPathAcceleration(path_pos, path_vel, false);
      slope = (trajectory.front().path_vel_ - path_vel) / (trajectory.front().path_pos_ - path_pos);

      if (path_vel < 0.0)
      {
        valid_ = false;
        RCLCPP_ERROR(LOGGER, "Error while integrating backward: Negative path velocity");
        end_trajectory_ = trajectory;
        return;
      }
    }
    else
    {
      --start1;
      --start2;
    }

    // Check for intersection between current start trajectory and backward
    // trajectory segments
    const double start_slope = (start2->path_vel_ - start1->path_vel_) / (start2->path_pos_ - start1->path_pos_);
    const double intersection_path_pos =
        (start1->path_vel_ - path_vel + slope * path_pos - start_slope * start1->path_pos_) / (slope - start_slope);
    if (std::max(start1->path_pos_, path_pos) - EPS <= intersection_path_pos &&
        intersection_path_pos <= EPS + std::min(start2->path_pos_, trajectory.front().path_pos_))
    {
      const double intersection_path_vel =
          start1->path_vel_ + start_slope * (intersection_path_pos - start1->path_pos_);
      start_trajectory.erase(start2, start_trajectory.end());
      start_trajectory.push_back(TrajectoryStep(intersection_path_pos, intersection_path_vel));
      start_trajectory.splice(start_trajectory.end(), trajectory);
      return;
    }
  }

  valid_ = false;
  RCLCPP_ERROR(LOGGER, "Error while integrating backward: Did not hit start trajectory");
  end_trajectory_ = trajectory;
}

double Trajectory::getMinMaxPathAcceleration(double path_pos, double path_vel, bool max)
{
  Eigen::VectorXd config_deriv = path_.getTangent(path_pos);
  Eigen::VectorXd config_deriv2 = path_.getCurvature(path_pos);
  double factor = max ? 1.0 : -1.0;
  double max_path_acceleration = std::numeric_limits<double>::max();
  for (unsigned int i = 0; i < joint_num_; ++i)
  {
    if (config_deriv[i] != 0.0)
    {
      max_path_acceleration =
          std::min(max_path_acceleration, max_acceleration_[i] / std::abs(config_deriv[i]) -
                                              factor * config_deriv2[i] * path_vel * path_vel / config_deriv[i]);
    }
  }
  return factor * max_path_acceleration;
}

double Trajectory::getMinMaxPhaseSlope(double path_pos, double path_vel, bool max)
{
  return getMinMaxPathAcceleration(path_pos, path_vel, max) / path_vel;
}

double Trajectory::getAccelerationMaxPathVelocity(double path_pos) const
{
  double max_path_velocity = std::numeric_limits<double>::infinity();
  const Eigen::VectorXd config_deriv = path_.getTangent(path_pos);
  const Eigen::VectorXd config_deriv2 = path_.getCurvature(path_pos);
  for (unsigned int i = 0; i < joint_num_; ++i)
  {
    if (config_deriv[i] != 0.0)
    {
      for (unsigned int j = i + 1; j < joint_num_; ++j)
      {
        if (config_deriv[j] != 0.0)
        {
          double a_ij = config_deriv2[i] / config_deriv[i] - config_deriv2[j] / config_deriv[j];
          if (a_ij != 0.0)
          {
            max_path_velocity = std::min(max_path_velocity, sqrt((max_acceleration_[i] / std::abs(config_deriv[i]) +
                                                                  max_acceleration_[j] / std::abs(config_deriv[j])) /
                                                                 std::abs(a_ij)));
          }
        }
      }
    }
    else if (config_deriv2[i] != 0.0)
    {
      max_path_velocity = std::min(max_path_velocity, sqrt(max_acceleration_[i] / std::abs(config_deriv2[i])));
    }
  }
  return max_path_velocity;
}

double Trajectory::getVelocityMaxPathVelocity(double path_pos) const
{
  const Eigen::VectorXd tangent = path_.getTangent(path_pos);
  double max_path_velocity = std::numeric_limits<double>::max();
  for (unsigned int i = 0; i < joint_num_; ++i)
  {
    max_path_velocity = std::min(max_path_velocity, max_velocity_[i] / std::abs(tangent[i]));
  }
  return max_path_velocity;
}

double Trajectory::getAccelerationMaxPathVelocityDeriv(double path_pos)
{
  return (getAccelerationMaxPathVelocity(path_pos + EPS) - getAccelerationMaxPathVelocity(path_pos - EPS)) /
         (2.0 * EPS);
}

double Trajectory::getVelocityMaxPathVelocityDeriv(double path_pos)
{
  const Eigen::VectorXd tangent = path_.getTangent(path_pos);
  double max_path_velocity = std::numeric_limits<double>::max();
  unsigned int active_constraint;
  for (unsigned int i = 0; i < joint_num_; ++i)
  {
    const double this_max_path_velocity = max_velocity_[i] / std::abs(tangent[i]);
    if (this_max_path_velocity < max_path_velocity)
    {
      max_path_velocity = this_max_path_velocity;
      active_constraint = i;
    }
  }
  return -(max_velocity_[active_constraint] * path_.getCurvature(path_pos)[active_constraint]) /
         (tangent[active_constraint] * std::abs(tangent[active_constraint]));
}

bool Trajectory::isValid() const
{
  return valid_;
}

double Trajectory::getDuration() const
{
  return trajectory_.back().time_;
}

std::list<Trajectory::TrajectoryStep>::const_iterator Trajectory::getTrajectorySegment(double time) const
{
  if (time >= trajectory_.back().time_)
  {
    std::list<TrajectoryStep>::const_iterator last = trajectory_.end();
    last--;
    return last;
  }
  else
  {
    if (time < cached_time_)
    {
      cached_trajectory_segment_ = trajectory_.begin();
    }
    while (time >= cached_trajectory_segment_->time_)
    {
      ++cached_trajectory_segment_;
    }
    cached_time_ = time;
    return cached_trajectory_segment_;
  }
}

Eigen::VectorXd Trajectory::getPosition(double time) const
{
  std::list<TrajectoryStep>::const_iterator it = getTrajectorySegment(time);
  std::list<TrajectoryStep>::const_iterator previous = it;
  previous--;

  double time_step = it->time_ - previous->time_;
  const double acceleration =
      2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);

  time_step = time - previous->time_;
  const double path_pos =
      previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;

  return path_.getConfig(path_pos);
}

Eigen::VectorXd Trajectory::getVelocity(double time) const
{
  std::list<TrajectoryStep>::const_iterator it = getTrajectorySegment(time);
  std::list<TrajectoryStep>::const_iterator previous = it;
  previous--;

  double time_step = it->time_ - previous->time_;
  const double acceleration =
      2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);

  time_step = time - previous->time_;
  const double path_pos =
      previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
  const double path_vel = previous->path_vel_ + time_step * acceleration;

  return path_.getTangent(path_pos) * path_vel;
}

Eigen::VectorXd Trajectory::getAcceleration(double time) const
{
  std::list<TrajectoryStep>::const_iterator it = getTrajectorySegment(time);
  std::list<TrajectoryStep>::const_iterator previous = it;
  previous--;

  double time_step = it->time_ - previous->time_;
  const double acceleration =
      2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);

  time_step = time - previous->time_;
  const double path_pos =
      previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
  const double path_vel = previous->path_vel_ + time_step * acceleration;
  Eigen::VectorXd path_acc =
      (path_.getTangent(path_pos) * path_vel - path_.getTangent(previous->path_pos_) * previous->path_vel_);
  if (time_step > 0.0)
    path_acc /= time_step;
  return path_acc;
}

TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double path_tolerance, const double resample_dt,
                                                                 const double min_angle_change)
  : path_tolerance_(path_tolerance), resample_dt_(resample_dt), min_angle_change_(min_angle_change)
{
}

bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
                                                        const double max_velocity_scaling_factor,
                                                        const double max_acceleration_scaling_factor) const
{
  if (trajectory.empty())
    return true;

  const moveit::core::JointModelGroup* group = trajectory.getGroup();
  if (!group)
  {
    RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
    return false;
  }

  // Validate scaling
  double velocity_scaling_factor = 1.0;
  if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0)
  {
    velocity_scaling_factor = max_velocity_scaling_factor;
  }
  else if (max_velocity_scaling_factor == 0.0)
  {
    RCLCPP_DEBUG(LOGGER, "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
                 velocity_scaling_factor);
  }
  else
  {
    RCLCPP_WARN(LOGGER, "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
                max_velocity_scaling_factor, velocity_scaling_factor);
  }

  double acceleration_scaling_factor = 1.0;
  if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
  {
    acceleration_scaling_factor = max_acceleration_scaling_factor;
  }
  else if (max_acceleration_scaling_factor == 0.0)
  {
    RCLCPP_DEBUG(LOGGER, "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
                 acceleration_scaling_factor);
  }
  else
  {
    RCLCPP_WARN(LOGGER, "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
                max_acceleration_scaling_factor, acceleration_scaling_factor);
  }

  // This lib does not actually work properly when angles wrap around, so we need to unwind the path first
  trajectory.unwind();

  // This is pretty much copied from IterativeParabolicTimeParameterization::applyVelocityConstraints
  const std::vector<std::string>& vars = group->getVariableNames();
  const std::vector<int>& idx = group->getVariableIndexList();
  const moveit::core::RobotModel& rmodel = group->getParentModel();
  const unsigned num_joints = group->getVariableCount();
  const unsigned num_points = trajectory.getWayPointCount();

  // Get the limits (we do this at same time, unlike IterativeParabolicTimeParameterization)
  Eigen::VectorXd max_velocity(num_joints);
  Eigen::VectorXd max_acceleration(num_joints);
  for (size_t j = 0; j < num_joints; ++j)
  {
    const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]);

    // Limits need to be non-zero, otherwise we never exit
    max_velocity[j] = 1.0;
    if (bounds.velocity_bounded_)
    {
      if (bounds.max_velocity_ < std::numeric_limits<double>::epsilon())
      {
        RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0",
                     bounds.max_velocity_, vars[j].c_str());
        return false;
      }
      max_velocity[j] =
          std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor;
    }
    else
    {
      RCLCPP_WARN_STREAM_ONCE(
          LOGGER, "Joint velocity limits are not defined. Using the default "
                      << max_velocity[j] << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
    }

    max_acceleration[j] = 1.0;
    if (bounds.acceleration_bounded_)
    {
      if (bounds.max_acceleration_ < std::numeric_limits<double>::epsilon())
      {
        RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0",
                     bounds.max_acceleration_, vars[j].c_str());
        return false;
      }
      max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
                            acceleration_scaling_factor;
    }
    else
    {
      RCLCPP_WARN_STREAM_ONCE(LOGGER,
                              "Joint acceleration limits are not defined. Using the default "
                                  << max_acceleration[j]
                                  << " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
    }
  }

  // Have to convert into Eigen data structs and remove repeated points
  //  (https://github.com/tobiaskunz/trajectories/issues/3)
  std::list<Eigen::VectorXd> points;
  for (size_t p = 0; p < num_points; ++p)
  {
    moveit::core::RobotStatePtr waypoint = trajectory.getWayPointPtr(p);
    Eigen::VectorXd new_point(num_joints);
    // The first point should always be kept
    bool diverse_point = (p == 0);

    for (size_t j = 0; j < num_joints; ++j)
    {
      new_point[j] = waypoint->getVariablePosition(idx[j]);
      // If any joint angle is different, it's a unique waypoint
      if (p > 0 && std::fabs(new_point[j] - points.back()[j]) > min_angle_change_)
      {
        diverse_point = true;
      }
    }

    if (diverse_point)
      points.push_back(new_point);
  }

  // Return trajectory with only the first waypoint if there are not multiple diverse points
  if (points.size() == 1)
  {
    moveit::core::RobotState waypoint = moveit::core::RobotState(trajectory.getWayPoint(0));
    waypoint.zeroVelocities();
    waypoint.zeroAccelerations();
    trajectory.clear();
    trajectory.addSuffixWayPoint(waypoint, 0.0);
    return true;
  }

  // Now actually call the algorithm
  Trajectory parameterized(Path(points, path_tolerance_), max_velocity, max_acceleration, DEFAULT_TIMESTEP);
  if (!parameterized.isValid())
  {
    RCLCPP_ERROR(LOGGER, "Unable to parameterize trajectory.");
    return false;
  }

  // Compute sample count
  size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt_);

  // Resample and fill in trajectory
  moveit::core::RobotState waypoint = moveit::core::RobotState(trajectory.getWayPoint(0));
  trajectory.clear();
  double last_t = 0;
  for (size_t sample = 0; sample <= sample_count; ++sample)
  {
    // always sample the end of the trajectory as well
    double t = std::min(parameterized.getDuration(), sample * resample_dt_);
    Eigen::VectorXd position = parameterized.getPosition(t);
    Eigen::VectorXd velocity = parameterized.getVelocity(t);
    Eigen::VectorXd acceleration = parameterized.getAcceleration(t);

    for (size_t j = 0; j < num_joints; ++j)
    {
      waypoint.setVariablePosition(idx[j], position[j]);
      waypoint.setVariableVelocity(idx[j], velocity[j]);
      waypoint.setVariableAcceleration(idx[j], acceleration[j]);
    }

    trajectory.addSuffixWayPoint(waypoint, t - last_t);
    last_t = t;
  }

  return true;
}
}  // namespace trajectory_processing


// @file:     sim_anneal.cc
// @author:   Samuel
// @created:  2017.08.23
// @editted:  2019.02.01
// @license:  Apache License 2.0
//
// @desc:     Simulated annealing physics engine

#include "simanneal.h"
#include <ctime>
#include <algorithm>
#include <unordered_set>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

// thread CPU time for Linux
//#include <pthread.h>
//#include <time.h>

saglobal::TimeKeeper *saglobal::TimeKeeper::time_keeper=nullptr;
int saglobal::log_level = Logger::WRN;

using namespace phys;

// static variables
SimParams SimAnneal::sim_params;
std::mutex SimAnneal::result_store_mutex;
FPType SimAnneal::db_distance_scale = 1E-10;
AllChargeResults SimAnneal::charge_results;
AllEnergyResults SimAnneal::energy_results;
//AllCPUTimes SimAnneal::cpu_times;
SuggestedResults SimAnneal::suggested_gs_results;

// alias for the commonly used sim_params static variable
constexpr auto sparams = &SimAnneal::sim_params;

// SimParams implementation

void SimParams::setDBLocs(const std::vector<EuclCoord> &t_db_locs)
{
  db_locs = t_db_locs;
  if (db_locs.size() == 0) {
    throw "There must be 1 or more DBs when setting DBs for SimParams.";
  }
  n_dbs = db_locs.size();
  db_r.resize(n_dbs, n_dbs);
  v_ij.resize(n_dbs, n_dbs);
  v_ext.resize(n_dbs);
}

void SimParams::setDBLocs(const std::vector<LatCoord> &t_db_locs)
{
  std::vector<EuclCoord> db_locs;
  for (LatCoord lat_coord : t_db_locs) {
    assert(lat_coord.size() == 3);
    db_locs.push_back(latToEuclCoord(lat_coord[0], lat_coord[1], lat_coord[2]));
  }
  setDBLocs(db_locs);
}

EuclCoord SimParams::latToEuclCoord(const int &n, const int &m, const int &l)
{
  FPType x = n * constants::lat_a;
  FPType y = m * constants::lat_b + l * constants::lat_c;
  return std::make_pair(x, y);
}

// SimAnneal (master) Implementation

SimAnneal::SimAnneal(SimParams &sparams)
{
  sim_params = sparams;
  initialize();
}

void SimAnneal::invokeSimAnneal()
{
  Logger log(saglobal::log_level);
  log.debug() << "Setting up SimAnnealThreads..." << std::endl;

  // spawn all the threads
  for (int i=0; i<sim_params.num_instances; i++) {
    boost::random_device rd;
    std::uint64_t seed = rd();
    seed = (seed << 32) | rd();
    SimAnnealThread annealer(i, seed);
    std::thread th(&SimAnnealThread::run, annealer);
    anneal_threads.push_back(std::move(th));
  }

  log.debug() << "Wait for simulations to complete." << std::endl;

  // wait for threads to complete
  for (auto &th : anneal_threads) {
    th.join();
  }

  log.debug() << "All simulations complete." << std::endl;
}

FPType SimAnneal::systemEnergy(const ublas::vector<int> &n_in, bool qubo)
{
  assert(n_in.size() > 0);

  //FPType E = 0.5 * ublas::inner_prod(n_in, ublas::prod(sim_params.v_ij, n_in))
    //- ublas::inner_prod(n_in, sim_params.v_ext);
  FPType E = ublas::inner_prod(n_in, sim_params.v_ext)
    + 0.5 * ublas::inner_prod(n_in, ublas::prod(sim_params.v_ij, n_in));

  if (qubo) {
    for (int n_i : n_in) {
      E += n_i * sim_params.mu;
    }
  }

  return E;
}

bool SimAnneal::isMetastable(const ublas::vector<int> &n_in)
{
  assert(n_in.size() > 0);
  Logger log(saglobal::log_level);

  const FPType &muzm = sparams->mu;
  const FPType &mupz = sparams->mu - constants::eta;
  const FPType &zero_equiv = constants::RECALC_STABILITY_ERR;

  ublas::vector<FPType> v_local(n_in.size());
  log.debug() << "V_i and Charge State Config " << n_in << ":" << std::endl;
  for (unsigned int i=0; i<n_in.size(); i++) {
    // calculate v_i
    v_local[i] = - sim_params.v_ext[i];
    for (unsigned int j=0; j<n_in.size(); j++) {
      if (i == j) continue;
      v_local[i] -= sim_params.v_ij(i,j) * n_in[j];
    }
    log.debug() << "\tDB[" << i << "]: charge state=" << n_in[i]
      << ", v_local[i]=" << v_local[i] << " eV, and v_local[i]+muzm=" << v_local[i] + muzm << "eV" << std::endl;

    // return false if invalid
    if (!(   (n_in[i] == -1 && v_local[i] + muzm < zero_equiv)    // DB- valid condition
          || (n_in[i] == 1  && v_local[i] + mupz > - zero_equiv)  // DB+ valid condition
          || (n_in[i] == 0  && v_local[i] + muzm > - zero_equiv   // DB0 valid condition
                            && v_local[i] + mupz < zero_equiv))) {
      log.debug() << "config " << n_in << " has an invalid population, failed at index " << i << std::endl;
      log.debug() << "v_local[i]=" << v_local[i] << ", muzm=" << muzm << ", mupz=" << mupz << std::endl;
      return false;
    }
  }
  log.debug() << "config " << n_in << " has a valid population." << std::endl;

  auto hopDel = [v_local, n_in](const int &i, const int &j) -> FPType {
    int dn_i = (n_in[i]==-1) ? 1 : -1;
    int dn_j = - dn_i;
    return - v_local[i]*dn_i - v_local[j]*dn_j - sparams->v_ij(i,j);
  };

  for (unsigned int i=0; i<n_in.size(); i++) {
    // do nothing with DB+
    if (n_in[i] == 1)
      continue;

    for (unsigned int j=0; j<n_in.size(); j++) {
      // attempt hops from more negative charge states to more positive ones
      FPType E_del = hopDel(i, j);
      if ((n_in[j] > n_in[i]) && (E_del < -zero_equiv)) {
        log.debug() << "config " << n_in << " not stable since hopping from site "
          << i << " to " << j << " would result in an energy change of "
          << E_del << std::endl;
        return false;
      }
    }
  }
  log.debug() << "config " << n_in << " has a stable configuration." << std::endl;
  return true;
}

void SimAnneal::storeResults(SimAnnealThread *annealer, int thread_id)
{
  result_store_mutex.lock();

  charge_results[thread_id] = annealer->db_charges;
  energy_results[thread_id] = annealer->config_energies;
  //cpu_times[thread_id] = annealer->CPUTime();

  suggested_gs_results[thread_id] = annealer->suggestedConfig();

  result_store_mutex.unlock();
}

SuggestedResults SimAnneal::suggestedConfigResults(bool tidy)
{
  SuggestedResults filtered_results;
  if (tidy) {
    // deduplicate and recalculate energy
    std::unordered_set<std::string> config_set;
    //std::map< ublas::vector<int>, ChargeConfigResult > result_map;
    for (auto result : suggested_gs_results) {
      if (!result.initialized) {
        continue;
      }
      if (config_set.find(configToStr(result.config)) == config_set.end()) {
        config_set.insert(configToStr(result.config));
        if (isMetastable(result.config)) {
          result.system_energy = systemEnergy(result.config);
          filtered_results.push_back(result);
        }
      }
    }
  } else {
    // return every result that has been initialized
    for (auto result : suggested_gs_results)
      if (result.initialized)
        filtered_results.push_back(result);
  }
  return filtered_results;
}

// PRIVATE

void SimAnneal::initialize()
{
  Logger log(saglobal::log_level);
  SimParams &sp = sim_params;

  log.debug() << "Performing pre-calculations..." << std::endl;

  // set default values
  if (sp.v_freeze_init < 0)
    sp.v_freeze_init = fabs(sp.mu) / 2;
  if (sp.v_freeze_reset < 0)
    sp.v_freeze_reset = fabs(sp.mu);

  // apply schedule scaling
  sp.alpha = std::pow(std::exp(-1.), 1./(sp.T_e_inv_point * sp.anneal_cycles));
  sp.v_freeze_cycles = sp.v_freeze_end_point * sp.anneal_cycles;
  sp.v_freeze_step = sp.v_freeze_threshold / sp.v_freeze_cycles;

  log.debug() << "Anneal cycles: " << sp.anneal_cycles << ", alpha: "
    << sp.alpha << ", v_freeze_cycles: " << sp.v_freeze_cycles << std::endl;

  sp.result_queue_size = sp.anneal_cycles * sp.result_queue_factor;
  sp.result_queue_size = std::min(sp.result_queue_size, sp.anneal_cycles);
  sp.result_queue_size = std::max(sp.result_queue_size, 1);
  log.debug() << "Result queue size: " << sp.result_queue_size << std::endl;

  if (sp.preanneal_cycles > sp.anneal_cycles) {
    std::cerr << "Preanneal cycles > Anneal cycles";
    throw;
  }

  // phys
  sp.kT_min = constants::Kb * sp.T_min;
  sp.Kc = 1/(4 * constants::PI * sp.eps_r * constants::EPS0);

  // inter-db distances and voltages
  for (int i=0; i<sp.n_dbs; i++) {
    sp.db_r(i,i) = 0.;
    sp.v_ij(i,i) = 0.;
    for (int j=i+1; j<sp.n_dbs; j++) {
      sp.db_r(i,j) = db_distance_scale * distance(i,j);
      sp.v_ij(i,j) = interElecPotential(sp.db_r(i,j));
      sp.db_r(j,i) = sp.db_r(i,j);
      sp.v_ij(j,i) = sp.v_ij(i,j);

      log.debug() << "db_r[" << i << "][" << j << "]=" << sp.db_r(i,j)
        << ", v_ij[" << i << "][" << j << "]=" << sp.v_ij(i,j) << std::endl;
    }
  }

  log.debug() << "Pre-calculations complete" << std::endl << std::endl;

  // determine number of threads to run
  if (sp.num_instances == -1) {
    if (sp.n_dbs <= 9) {
      sp.num_instances = 8;
    } else if (sp.n_dbs <= 25) {
      sp.num_instances = 16;
    } else {
      sp.num_instances = 128;
    }
  }

  charge_results.resize(sp.num_instances);
  energy_results.resize(sp.num_instances);
  //cpu_times.resize(sp.num_instances);
  suggested_gs_results.resize(sp.num_instances);
}

FPType SimAnneal::distance(const int &i, const int &j)
{
  FPType x1 = sim_params.db_locs[i].first;
  FPType y1 = sim_params.db_locs[i].second;
  FPType x2 = sim_params.db_locs[j].first;
  FPType y2 = sim_params.db_locs[j].second;
  return sqrt(pow(x1-x2, 2.0) + pow(y1-y2, 2.0));
}

FPType SimAnneal::interElecPotential(const FPType &r)
{
  return constants::Q0 * sim_params.Kc * exp(-r/(sim_params.debye_length*1e-9)) / r;
}

FPType SimAnneal::hopEnergyDelta(ublas::vector<int> n_in, const int &from_ind,
    const int &to_ind)
{
  // TODO make an efficient implementation with energy delta implementation
  FPType orig_energy = systemEnergy(n_in);
  int from_state = n_in[from_ind];
  n_in[from_ind] = n_in[to_ind];
  n_in[to_ind] = from_state;
  return systemEnergy(n_in) - orig_energy;
}

// SimAnnealThread Implementation

SimAnnealThread::SimAnnealThread(const int t_thread_id, const std::uint64_t seed)
  : thread_id(t_thread_id), gener(seed), dis01(0,1)
{
  muzm = sparams->mu;
  mupz = sparams->mu - constants::eta;
}

void SimAnnealThread::run()
{
  // initialize variables & perform pre-calculation
  kT = sparams->T_init*constants::Kb;
  v_freeze = 0.;
  t = 0;
  t_freeze = 0;
  t_phys_validity_check = 0;
  pop_schedule_phase = PopulationUpdateMode;

  // resize vectors
  n.resize(sparams->n_dbs);
  v_local.resize(sparams->n_dbs);
  db_charges.resize(sparams->result_queue_size);
  config_energies.resize(sparams->result_queue_size);

  // SIM ANNEAL
  anneal();
}

void SimAnnealThread::anneal()
{
  typedef ublas::vector<int> OccListType;

  // Vars
  ublas::vector<int> dn(sparams->n_dbs);  // change of occupation for population update
  OccListType dbm_occ(sparams->n_dbs);                    // indices of DB- sites in n
  OccListType db0_occ(sparams->n_dbs);                    // indices of DB0 sites in n
  OccListType dbp_occ(sparams->n_dbs);                    // indices of DB+ sites in n
  OccListType::iterator from_occ, to_occ;
  int dbm_occ_count=0, db0_occ_count=0, dbp_occ_count=0;
  int hop_attempts, max_hop_attempts;
  int from_ind, to_ind;               // hopping from n[from_ind] to n[to_ind]
  FPType hop_E_del;
  bool pop_changed;

  auto rand_charged_db_ind = [this, &dbm_occ, &dbp_occ, &dbm_occ_count,
                              &dbp_occ_count]
                                (OccListType::iterator &occ_it) mutable -> int
  {
    if (dbm_occ_count == 0 && dbp_occ_count == 0)
      return -1;
    int r_ind = randInt(0, dbm_occ_count + dbp_occ_count - 1);
    occ_it = (r_ind < dbm_occ_count) ? dbm_occ.begin() + r_ind
                                        : dbp_occ.begin() + (r_ind - dbm_occ_count);
    return *occ_it;
  };

  auto rand_neutral_db_ind = [this, &db0_occ, &db0_occ_count]
                                (OccListType::iterator &occ_it) mutable -> int
  {
    if (db0_occ_count == 0)
      return -1;
    int r_ind = randInt(0, db0_occ_count - 1);
    occ_it = db0_occ.begin() + r_ind;
    return *occ_it;
  };

  E_sys = systemEnergy();
  v_local = - sparams->v_ext - ublas::prod(sparams->v_ij, n);

  Logger log(saglobal::log_level);

  // Run simulated annealing for predetermined time steps
  while(t < sparams->anneal_cycles) {
    //log.debug() << "Cycle " << t << ", kT=" << kT << ", v_freeze=" << v_freeze << std::endl;

    // Random population change, pop_changed is set to true if anything changes
    //log.debug() << "Before popgen: n=" << n << std::endl;
    genPopDelta(dn, pop_changed);
    if (pop_changed) {
      n += dn;
      E_sys += -1 * ublas::inner_prod(v_local, dn)
        + 0.5 * ublas::inner_prod(dn, ublas::prod(sparams->v_ij, dn));
      v_local -= ublas::prod(sparams->v_ij, dn);

      // Occupation lists update
      int dbm_ind=0, db0_ind=0, dbp_ind=0;
      for (int db_ind=0; db_ind<sparams->n_dbs; db_ind++) {
        if (n[db_ind]==-1) {
          dbm_occ[dbm_ind++] = db_ind;
        } else if (n[db_ind]==0) {
          db0_occ[db0_ind++] = db_ind;
        } else {
          dbp_occ[dbp_ind++] = db_ind;
        }
        dbm_occ_count = dbm_ind;
        db0_occ_count = db0_ind;
        dbp_occ_count = dbp_ind;
      }
    }

    // Hopping - randomly hop electrons from higher occupancy sites to lower
    // occupancy sites
    hop_attempts = 0;
    max_hop_attempts = 0;
    if (dbm_occ_count + dbp_occ_count < sparams->n_dbs
        && db0_occ_count < sparams->n_dbs) {
      max_hop_attempts = std::max(dbm_occ_count+dbp_occ_count, db0_occ_count);
      max_hop_attempts *= sparams->hop_attempt_factor;
    }

    while (hop_attempts < max_hop_attempts) {
      from_ind = rand_charged_db_ind(from_occ);
      to_ind = rand_neutral_db_ind(to_occ);
      if (from_ind == -1 || to_ind == -1) {
        std::cerr << "Invalid hop index, this shouldn't happen." << std::endl;
        throw;
      }
      hop_E_del = hopEnergyDelta(from_ind, to_ind);
      if (acceptHop(hop_E_del)) {
        performHop(from_ind, to_ind, E_sys, hop_E_del);
        // update occupation indices list
        if (n[from_ind] - n[to_ind] < 2) {
          // hopping from DB- or DB+ to DB0
          int orig_from_ind = *from_occ;
          *from_occ = *to_occ;
          *to_occ = orig_from_ind;
        }
      }
      hop_attempts++;
    }

    // push back the new arrangement
    db_charges.push_back(ChargeConfigResult(n,
          populationValid(), E_sys));
    config_energies.push_back(E_sys);

    // keep track of suggested ground state
    if (populationValid()) {
      if (E_sys < suggested_gs.system_energy || suggested_gs.config.empty()) {
        suggested_gs.initialized = true;
        suggested_gs.config = n;
        suggested_gs.system_energy = E_sys;
        suggested_gs.pop_likely_stable = true;
      }
    }

    //log.debug() << "db_charges = " << n << std::endl;

    // perform time-step if not pre-annealing
    timeStep();
  }

  log.debug() << "Final db_charges = " << n
    << ", delta-based system energy = " << E_sys
    << ", recalculated system energy=" << systemEnergy() << std::endl;

  SimAnneal::storeResults(this, thread_id);
}

void SimAnnealThread::genPopDelta(ublas::vector<int> &dn, bool &changed)
{
  // DB- and DB+ sites can be flipped to DB0, DB0 sites can be flipped to either
  // DB- or DB+ depending on which one is enegetically closer.
  FPType prob;
  FPType x;
  int change_dir;
  changed = false;
  for (unsigned i=0; i<n.size(); i++) {
    if (n[i] == -1) {
      // Probability from DB- to DB0
      x = - (v_local[i] + muzm) + v_freeze;
      change_dir = 1;
    } else if (n[i] == 1) {
      // Probability from DB+ to DB0
      x = v_local[i] + mupz + v_freeze;
      change_dir = -1;
    } else {
      if (fabs(v_local[i] + muzm) < fabs(v_local[i] + mupz)) {
        // Closer to DB(0/-) transition level, probability from DB0 to DB-
        x = v_local[i] + muzm + v_freeze;
        change_dir = -1;
      } else {
        // Closer to DB(+/0) transition level, probability from DB0 to DB+
        x = - (v_local[i] + mupz) + v_freeze;
        change_dir = 1;
      }
    }
    prob = 1. / (1 + exp(x / kT));

    if (evalProb(prob)) {
      dn[i] = change_dir;
      changed = true;
    } else {
      dn[i] = 0;
    }
  }
}

void SimAnnealThread::performHop(const int &from_ind, const int &to_ind,
    FPType &E_sys, const FPType &E_del)
{
  int dn_i = (n[from_ind]==-1) ? 1 : -1;
  int dn_j = - dn_i;

  n[from_ind] += dn_i;
  n[to_ind] += dn_j;

  E_sys += E_del;
  ublas::matrix_column<ublas::matrix<FPType>> v_i (sparams->v_ij, from_ind);
  ublas::matrix_column<ublas::matrix<FPType>> v_j (sparams->v_ij, to_ind);
  v_local -= v_i*dn_i + v_j*dn_j;
}

void SimAnnealThread::timeStep()
{
  Logger log(saglobal::log_level);

  // always progress annealing schedule
  t++;

  // if preannealing, stop here
  if (t < sparams->preanneal_cycles)
    return;

  // decide what to do with v_freeze schedule
  switch (pop_schedule_phase) {
    case PopulationUpdateMode:
    {
      if (t_freeze < sparams->v_freeze_cycles) {
        t_freeze++;
      } else {
        if (!sparams->strategic_v_freeze_reset
            || (sparams->anneal_cycles - t) < sparams->v_freeze_cycles + sparams->phys_validity_check_cycles) {
          // if there aren't enough cycles left for another full v_freeze cycles, then
          // stop playing with t_freeze
          pop_schedule_phase = PopulationUpdateFinished;
        } else {
          // initiate physical validity check for the next phys_validity_check_cycles
          pop_schedule_phase = PhysicalValidityCheckMode;
          t_phys_validity_check = 0;
          phys_valid_count = 0;
          phys_invalid_count = 0;
        }
      }
      break;
    }
    case PhysicalValidityCheckMode:
    {
      if (t_phys_validity_check < sparams->phys_validity_check_cycles) {
        populationValid() ? phys_valid_count++ : phys_invalid_count++;
        t_phys_validity_check++;
      } else if (t_phys_validity_check >= sparams->phys_validity_check_cycles) {
        pop_schedule_phase = PopulationUpdateMode;
        t_freeze = 0;
        if (phys_valid_count < phys_invalid_count) {
          log.debug() << "Thread " << thread_id << ": t=" << t
            << ", charge config is " << n
            << " which is physically invalid, resetting v_freeze." << std::endl;

          // reset v_freeze and temperature
          v_freeze = sparams->v_freeze_reset;
          if (sparams->reset_T_during_v_freeze_reset)
            kT = sparams->T_init*constants::Kb;
        }
      }
      break;
    }
    case PopulationUpdateFinished:
      break;
    default:
      std::cerr << "Invalid PopulationSchedulePhase.";
      throw;
  }

  // update parameters according to schedule
  if (sparams->T_schedule == ExponentialSchedule)
    kT = sparams->kT_min + (kT - sparams->kT_min) * sparams->alpha;
  else if (sparams->T_schedule == LinearSchedule)
    kT = std::max(sparams->kT_min, kT - sparams->alpha);

  // update v_freeze
  if (v_freeze < sparams->v_freeze_threshold)
    v_freeze += sparams->v_freeze_step;
}

bool SimAnnealThread::acceptHop(const FPType &v_diff)
{
  if (v_diff < 0)
    return true;

  // some acceptance function, acceptance probability falls off exponentially
  FPType prob = exp(-v_diff/kT);

  return evalProb(prob);
}

bool SimAnnealThread::evalProb(const FPType &prob)
{
  return prob >= dis01(gener);
}

int SimAnnealThread::randInt(const int &min, const int &max)
{
  RandIntDist dis(min,max);
  return dis(gener);
}

FPType SimAnnealThread::systemEnergy() const
{
  return ublas::inner_prod(n, sparams->v_ext)
    + 0.5 * ublas::inner_prod(n, ublas::prod(sparams->v_ij, n));
}

/*
FPType SimAnnealThread::totalCoulombPotential(ublas::vector<int> &config) const
{
  return 0.5 * ublas::inner_prod(config, ublas::prod(sparams->v_ij, config));
}
*/

FPType SimAnnealThread::hopEnergyDelta(const int &i, const int &j)
{
  //return v_local[i] - v_local[j] - sparams->v_ij(i,j);
  int dn_i = (n[i]==-1) ? 1 : -1;
  int dn_j = - dn_i;
  return - v_local[i]*dn_i - v_local[j]*dn_j - sparams->v_ij(i,j);
}

bool SimAnnealThread::populationValid() const
{
  // Check whether v_local at each site meets population validity constraints
  // Note that v_local components have flipped signs from E_sys
  bool valid;
  const FPType &zero_equiv = constants::POP_STABILITY_ERR;
  for (int i=0; i<sparams->n_dbs; i++) {
    valid = ((n[i] == -1 && v_local[i] + muzm < zero_equiv)   // DB- condition
          || (n[i] == 1  && v_local[i] + mupz > -zero_equiv)  // DB+ condition
          || (n[i] == 0  && v_local[i] + muzm > -zero_equiv
                         && v_local[i] + mupz < zero_equiv));
    if (!valid) {
      return false;
    }
  }
  return true;
}


//============================================================================//
//-------------------- pnt_integrity/AquisitionCheck.hpp -------*- C++ -*-----//
//============================================================================//
// BSD 3-Clause License
//
// Copyright (C) 2019 Integrated Solutions for Systems, Inc
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
// may be used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//----------------------------------------------------------------------------//
//
//  Class defined for the acquisition level checks
//  Josh Clanton <josh.clanton@is4s.com>
//  September 30, 2019
//============================================================================//
#include "pnt_integrity/AcquisitionCheck.hpp"
#include <Eigen/Dense>
#include <chrono>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <thread>

// #include "gnsscommon/GNSSConstants.hpp"

using namespace if_data_utils;

namespace pnt_integrity
{
//==============================================================================
//---------------------------- acquisitionSetup() ------------------------------
//==============================================================================
void AcquisitionCheck::acquisitionSetup()
{
  samplesPerIntPeriod_ = samplingFrequency_ * integrationPeriod_;
  samplesPerCode_ =
    std::round(samplingFrequency_ / (codeFrequencyBasis_ / codeLength_));

  generateCaCodeMap();
  generateFreqBins();

  std::stringstream setupMsg;
  setupMsg << "AcquisitionCheck::acquisitionSetup: "
           << "samps per int period = " << samplesPerIntPeriod_
           << ", num freq bins = " << freqBins_.size();
  logMsg_(setupMsg.str(), logutils::LogLevel::Warn);

  logMsg_("AcquisitionCheck::acquisitionSetup(): Code replicas initialized",
          logutils::LogLevel::Info);

  replicasInitialized_ = true;
}

//==============================================================================
//---------------------------- generateFreqBins() ------------------------------
//==============================================================================
void AcquisitionCheck::generateFreqBins()
{
  // numFreqBins_ = std::round(acquisitionSearchBand_ * 2) + 1;
  for (float curFreq = (intermediateFrequency_ - acquisitionSearchBand_);
       curFreq <= (intermediateFrequency_ + acquisitionSearchBand_);
       curFreq += searchStepSize_)
  {
    freqBins_.push_back(curFreq);
  }
}

//==============================================================================
//---------------------------- generateCaCodeMap()------------------------------
//==============================================================================
void AcquisitionCheck::generateCaCodeMap()
{
  prnList_.clear();
  for (int ii = 1; ii <= 32; ++ii)
  {
    prnList_.push_back(ii);

    caCodeMap_.insert(CodeMapEntry(ii,
                                   upsampleCaCode(generateCaCode(ii, 0),
                                                  samplingFrequency_,
                                                  codeFrequencyBasis_,
                                                  samplesPerIntPeriod_)));

    std::vector<std::complex<float> > caCodeFD;
    fftEngine_.fwd(caCodeFD, caCodeMap_[ii]);

    // convert to Eigen VectorXcf and take conjugate
    Eigen::Map<Eigen::VectorXcf> caFD_map(&caCodeFD[0], caCodeFD.size());
    caCodeMapFD_.insert(CodeFreqMapEntry(ii, caFD_map.conjugate()));
  }
}

//==============================================================================
//------------------------------ generateCacode() ------------------------------
//==============================================================================
std::vector<float> AcquisitionCheck::generateCaCode(signed int   _prn,
                                                    unsigned int _chip_shift)
{
  // initialize vector to hold result
  std::vector<float> ca_code;

  unsigned int G1[1023];
  unsigned int G2[1023];
  unsigned int G1_register[10], G2_register[10];
  unsigned int feedback1, feedback2;
  unsigned int lcv, lcv2;
  unsigned int delay;
  signed int   prn = _prn - 1;  // Move the PRN code to fit an array indices

  // G2 Delays as defined in IS-GPS-200E
  signed int delays[32] = {5,   6,   7,   8,   17,  18,  139, 140,
                           141, 251, 252, 254, 255, 256, 257, 258,
                           469, 470, 471, 472, 473, 474, 509, 512,
                           513, 514, 515, 516, 859, 860, 861, 862};
  // PRN sequences 33 through 37 are reserved for other uses (e.g. ground
  // transmitters)

  // A simple error check
  if ((prn < 0) || (prn > 32))
    return ca_code;

  for (lcv = 0; lcv < 10; lcv++)
  {
    G1_register[lcv] = 1;
    G2_register[lcv] = 1;
  }

  // Generate G1 & G2 Register
  for (lcv = 0; lcv < 1023; lcv++)
  {
    G1[lcv] = G1_register[0];
    G2[lcv] = G2_register[0];

    feedback1 = G1_register[7] ^ G1_register[0];
    feedback2 = (G2_register[8] + G2_register[7] + G2_register[4] +
                 G2_register[2] + G2_register[1] + G2_register[0]) &
                0x1;

    for (lcv2 = 0; lcv2 < 9; lcv2++)
    {
      G1_register[lcv2] = G1_register[lcv2 + 1];
      G2_register[lcv2] = G2_register[lcv2 + 1];
    }

    G1_register[9] = feedback1;
    G2_register[9] = feedback2;
  }

  // Set the delay
  delay = 1023 - delays[prn];
  delay += _chip_shift;
  delay %= 1023;
  // Generate PRN from G1 and G2 Registers
  for (lcv = 0; lcv < 1023; lcv++)
  {
    ca_code.push_back(G1[(lcv + _chip_shift) % 1023] ^ G2[delay]);
    if (ca_code[lcv] == 0.0)  // javi
    {
      ca_code[lcv] = -1.0;
    }
    delay++;
    delay %= 1023;
  }

  return ca_code;
}

//==============================================================================
//------------------------------ upsampleCaCode() ------------------------------
//==============================================================================
std::vector<float> AcquisitionCheck::upsampleCaCode(std::vector<float> codes,
                                                    double sampleFrequency,
                                                    double codeFrequency,
                                                    size_t samples,
                                                    double chip_shift)
{
  double codePhaseStep    = codeFrequency / sampleFrequency;
  int    samples_per_code = sampleFrequency / 1000;  // code is 1ms

  // if samples argument is set to 0, generate 1 ms of samples
  if (samples == 0)
    samples = samples_per_code;

  std::vector<float> digitized;

  // generate array that counts from 0 to 1023 and
  // has the same # of values as 1 period of the
  // sampled CA code

  int ca_idx = 0;
  for (size_t idx = 0; idx < samples; idx++)
  {
    // calculate C/A code index
    // add 1 to index to make it exactly match matlab code
    ca_idx = ceil((codePhaseStep * (idx + 1)) + chip_shift) - 1;
    // ca_idx = ((codePhaseStep * (idx)) + chip_shift) ;

    // wrap index into range of codes vector
    ca_idx = (ca_idx + 1023) % 1023;

    digitized.push_back(codes[ca_idx]);
  }

  return digitized;
}

//==============================================================================
//------------------------------ generateCarrier() -----------------------------
//==============================================================================
double AcquisitionCheck::generateCarrier(double           initPhase,
                                         double           phaseStep,
                                         size_t           length,
                                         Eigen::VectorXf& sine,
                                         Eigen::VectorXf& cosine)
{
  Eigen::VectorXf CarrArg(length);
  // TOOD: remove this if statement and test
  if (length == 1)
  {
    CarrArg(0) = initPhase + phaseStep;
  }
  else
  {
    CarrArg = Eigen::VectorXf::LinSpaced(
      length, initPhase + phaseStep, initPhase + phaseStep * (length));
  }

  sine   = CarrArg.array().sin();
  cosine = CarrArg.array().cos();

  return CarrArg(length - 1);
}

//==============================================================================
//--------------------------------- sinFast() ----------------------------------
//==============================================================================
float AcquisitionCheck::sinFast(float x)
{
  x = fmod(x + M_PI, M_PI * 2) - M_PI;  // restrict x so that -pi<x<pi

  if (x < 0)
  {
    return 1.27323954f * x + 0.405284735f * x * x;
  }
  else
  {
    return 1.27323954f * x - 0.405284735f * x * x;
  }
}

//==============================================================================
//------------------------- generateAcquisitionPlane() -------------------------
//==============================================================================
bool AcquisitionCheck::generateAcquisitionPlane(
  const Eigen::ArrayXcf& signalSamples)
{
  auto start = std::chrono::high_resolution_clock::now();

  // make sure CA tables have been initialized and settings set
  // before attempting acquisition
  if (!replicasInitialized_)
  {
    logMsg_("CA code replicas must be initialized before acquiring.",
            logutils::LogLevel::Error);
    return false;
  }

  // TODO: check that samples size and ca replica size matches
  size_t numSamples = signalSamples.size();

  // generate time vector for carrier replica
  Eigen::VectorXcf phasePoints(numSamples);
  for (size_t ii = 0; ii < numSamples; ii++)
  {
    phasePoints[ii] = twoGpsPi_ * (double)ii / samplingFrequency_;
  }

  // define PRN to search for
  std::vector<std::thread> fftThreads;

  Eigen::ArrayXXf results(freqBins_.size(), numSamples);

  for (PrnList::iterator prnIt = prnList_.begin(); prnIt != prnList_.end();
       ++prnIt)
  {
    // make sure PRN is between 1 and 32
    if ((*prnIt < 1) || (*prnIt > 32))
    {
      logMsg_("PRN must be between 1 and 32.", logutils::LogLevel::Error);
      return false;
    }
    // add the prn entry to the results map
    correlationResultsMap_.insert(
      std::pair<int, Eigen::ArrayXXf>(*prnIt, results));

    fftThreads.push_back(
      std::thread(std::bind(&AcquisitionCheck::acquisitionCorrelation,
                            this,
                            *prnIt,
                            signalSamples,
                            phasePoints)));
  }

  // close the FFT worker threads
  // size_t threadCount = 1;
  for (auto threadIt = fftThreads.begin(); threadIt != fftThreads.end();
       ++threadIt)
  {
    (*threadIt).join();
  }

  // publish the correlation data

  auto finish = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elapsed = finish - start;
  std::stringstream             timeMsg;
  timeMsg << "Elapsed time: " << elapsed.count() << " s";
  logMsg_(timeMsg.str(), logutils::LogLevel::Debug);

  if (publishAquisitionData_)
  {
    publishAquisitionData_(correlationResultsMap_);
  }
  if (publishPeakData_)
  {
    publishPeakData_(lastProcessTime_, peakResultsMap_);
  }

  return true;
}

//==============================================================================
//-------------------------- acquisitionCorrelation ----------------------------
//==============================================================================
void AcquisitionCheck::acquisitionCorrelation(
  const int&              prn,
  const Eigen::ArrayXcf&  signalSamples,
  const Eigen::VectorXcf& phasePoints)
{
  // initialize fft engine
  Eigen::FFT<float> fftEngine;

  size_t numSamples = signalSamples.size();

  std::vector<std::complex<float> > inputSignalDemod(numSamples);
  Eigen::Map<Eigen::ArrayXcf>       inputSignalDemodMap(&inputSignalDemod[0],
                                                  inputSignalDemod.size());

  std::vector<std::complex<float> > signalFft(numSamples);
  Eigen::Map<Eigen::ArrayXcf> signalFftMap(&signalFft[0], signalFft.size());

  std::vector<std::complex<float> > corrFreqDom(numSamples);
  Eigen::Map<Eigen::ArrayXcf>       corrFreqDomMap(&corrFreqDom[0],
                                             corrFreqDom.size());

  std::vector<std::complex<float> > corrTimeDom(numSamples);
  Eigen::Map<Eigen::ArrayXcf>       corrTimeDomMap(&corrTimeDom[0],
                                             corrTimeDom.size());

  // initialize arrays for peak
  auto samplesPerCodeChip =
    std::round(samplingFrequency_ / codeFrequencyBasis_);

  // set variables to save peak and it's location
  float                  peakValue      = 0.0;
  size_t                 peakFreqBinIdx = 0;
  Eigen::VectorXf::Index peakCodeIdx    = 0;

  // initialize array to hold results of acqusition tests
  size_t curBin = 0;
  for (auto freqIt = freqBins_.begin(); freqIt != freqBins_.end(); ++freqIt)
  {
    inputSignalDemodMap = (intermediateFrequency_ + *freqIt) * phasePoints *
                          std::complex<float>(0, 1);

    inputSignalDemodMap = inputSignalDemodMap.exp() * signalSamples;
    fftEngine.fwd(signalFft, inputSignalDemod);

    // multiply the complex conjugate of the CA replica with the demodulated
    // signal to get correlation in the frequency domain
    Eigen::Map<Eigen::ArrayXcf> caFftConj(&caCodeMapFD_[prn][0],
                                          caCodeMapFD_[prn].size());

    corrFreqDomMap = caFftConj * signalFftMap;

    fftEngine.inv(corrTimeDom, corrFreqDom);

    auto binResult                          = corrTimeDomMap.abs2();
    correlationResultsMap_[prn].row(curBin) = binResult;
    // resultsPtr->row(curBin)                 = binResult;

    // find the peak in this bin and the corresponding code idx
    Eigen::VectorXf::Index peakInBinCodeIdx;
    auto                   peakInBin = binResult.maxCoeff(&peakInBinCodeIdx);
    // if the bin peak is bigger than the previous, save it
    if (peakInBin > peakValue)
    {
      peakValue      = peakInBin;
      peakFreqBinIdx = curBin;
      peakCodeIdx    = peakInBinCodeIdx;
    }

    curBin++;
  }
  // define the exclusion zone around the peak
  auto excludeRangeLowIdx  = peakCodeIdx - samplesPerCodeChip;
  auto excludeRangeHighIdx = peakCodeIdx + samplesPerCodeChip;

  // pull out the frequency bin that has the max peak
  auto freqBinWithPeak = correlationResultsMap_[prn].row(peakFreqBinIdx);

  float secondPeakValue = 0.0;
  // auto secondPeakCodeIdx = 0;
  // find the second peak in this freqBin w.r.t. the exculsion zone
  for (auto codeIdx = 0; codeIdx < freqBinWithPeak.size(); codeIdx++)
  {
    // test for max everywehre but exclusion zone
    if ((codeIdx <= excludeRangeLowIdx) or (codeIdx >= excludeRangeHighIdx))
    {
      if (freqBinWithPeak(codeIdx) > secondPeakValue)
      {
        // secondPeakCodeIdx = codeIdx;
        secondPeakValue = freqBinWithPeak(codeIdx);
      }
    }
  }

  peakResultsMap_[prn] = std::pair<double, double>(peakValue, secondPeakValue);
}

//==============================================================================
//-------------------------------- runCheck ------------------------------------
//==============================================================================
bool AcquisitionCheck::runCheck()
{
  setPrnAssuranceLevels();
  calculateAssuranceLevel(lastProcessTime_);
  return true;
}

//==============================================================================
//-------------------------- setPrnAssuranceLevels ----------------------------
//==============================================================================
void AcquisitionCheck::setPrnAssuranceLevels()
{
  std::map<int, double> ratioMap;
  // look at the peak results map and make determinations bas
  for (auto prnIt = peakResultsMap_.begin(); prnIt != peakResultsMap_.end();
       ++prnIt)
  {
    double peakRatio       = (prnIt->second.first / prnIt->second.second);
    ratioMap[prnIt->first] = peakRatio;

    if (prnIt->second.first > highPowerThreshold_)
    {
      // power level is suspect, check ratio of 1st and 2nd peak

      if (peakRatio > peakRatioThreshold_)
      {
        prnAssuranceLevels_[prnIt->first] = data::AssuranceLevel::Unassured;
      }
      else
      {
        prnAssuranceLevels_[prnIt->first] = data::AssuranceLevel::Inconsistent;
      }
    }
    // check the first peak against the threshold
    else if (prnIt->second.first > acquisitionThreshold_)
    {
      prnAssuranceLevels_[prnIt->first] = data::AssuranceLevel::Assured;
    }
    // PRN is not visible
    else
    {
      prnAssuranceLevels_[prnIt->first] = data::AssuranceLevel::Unavailable;
    }
  }

  if (publishDiagnostics_)
  {
    diagnostics_.ratioMap = ratioMap;
  }
}

//==============================================================================
//---------------------------- setAssuranceLevel -------------------------------
//==============================================================================
void AcquisitionCheck::calculateAssuranceLevel(const double& checkTime)
{
  // go throuh the prn assurance map and determine how many have been flagged
  // to set the overall level
  int assuredCount      = 0;
  int unavailableCount  = 0;
  int unassuredCount    = 0;
  int inconsistentCount = 0;
  for (auto it = prnAssuranceLevels_.begin(); it != prnAssuranceLevels_.end();
       ++it)
  {
    if (it->second == data::AssuranceLevel::Unassured)
    {
      unassuredCount++;
    }
    else if (it->second == data::AssuranceLevel::Inconsistent)
    {
      inconsistentCount++;
    }
    else if (it->second == data::AssuranceLevel::Assured)
    {
      assuredCount++;
    }
    else if (it->second == data::AssuranceLevel::Unavailable)
    {
      unavailableCount++;
    }

    if (unassuredCount >= assuranceUnassuredThresh_)
    {
      changeAssuranceLevel(checkTime, data::AssuranceLevel::Unassured);
    }
    else if (inconsistentCount >= assuranceInconsistentThresh_)
    {
      changeAssuranceLevel(checkTime, data::AssuranceLevel::Inconsistent);
    }
    else if (assuredCount >= 4)
    {
      changeAssuranceLevel(checkTime, data::AssuranceLevel::Assured);
    }
    else
    {
      changeAssuranceLevel(checkTime, data::AssuranceLevel::Unavailable);
    }

  }  // end for loop

  if (publishDiagnostics_)
  {
    diagnostics_.highPowerThresh    = highPowerThreshold_;
    diagnostics_.peakRatioThresh    = peakRatioThreshold_;
    diagnostics_.acquisitionThresh  = acquisitionThreshold_;
    diagnostics_.inconsistentThresh = assuranceInconsistentThresh_;
    diagnostics_.unassuredThresh    = assuranceUnassuredThresh_;
    diagnostics_.unassuredCount     = unassuredCount;
    diagnostics_.inconsistentCount  = inconsistentCount;

    publishDiagnostics_(lastProcessTime_, diagnostics_);
  }
  // std::stringstream levelMsg;
  // levelMsg << std::endl;
  // levelMsg << "Unusable : " << unassuredCount << " prns " << std::endl;
  // levelMsg << "Unknown : " << inconsistentCount << " prns " << std::endl;
  // levelMsg << "Assured : " << assuredCount << " prns " << std::endl;
  // levelMsg << "Unavailable : " << unavailableCount << " prns ";
  // logMsg_(levelMsg.str(), "info");

  // std::stringstream msg;
  // msg << "AcquisitionCheck::calculateAssuranceLevel() : Calculated level : "
  //     << (int)assuranceLevel_;
  // logMsg_(msg.str(), "info");
}

}  // namespace pnt_integrity