/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016, Guan-Horng Liu.
 *  All rights reserved.
 *
 *  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.
 *   * Neither the name of the 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 OWNER 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.
 *
 * Author:  Guan-Horng Liu
 *********************************************************************/

#include "reeds_shepp.h"
#include <boost/math/constants/constants.hpp>

namespace
{
// The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.

const double pi = boost::math::constants::pi<double>();
const double twopi = 2. * pi;
const double RS_EPS = 1e-6;
const double ZERO = 10 * std::numeric_limits<double>::epsilon();

inline double mod2pi(double x)
{
  double v = fmod(x, twopi);
  if (v < -pi)
    v += twopi;
  else if (v > pi)
    v -= twopi;
  return v;
}
inline void polar(double x, double y, double& r, double& theta)
{
  r = sqrt(x * x + y * y);
  theta = atan2(y, x);
}
inline void tauOmega(double u, double v, double xi, double eta, double phi, double& tau, double& omega)
{
  double delta = mod2pi(u - v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
  double t1 = atan2(eta * A - xi * B, xi * A + eta * B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
  tau = (t2 < 0) ? mod2pi(t1 + pi) : mod2pi(t1);
  omega = mod2pi(tau - u + v - phi);
}

// formula 8.1 in Reeds-Shepp paper
inline bool LpSpLp(double x, double y, double phi, double& t, double& u, double& v)
{
  polar(x - sin(phi), y - 1. + cos(phi), u, t);
  if (t >= -ZERO)
  {
    v = mod2pi(phi - t);
    if (v >= -ZERO)
    {
      assert(fabs(u * cos(t) + sin(phi) - x) < RS_EPS);
      assert(fabs(u * sin(t) - cos(phi) + 1 - y) < RS_EPS);
      assert(fabs(mod2pi(t + v - phi)) < RS_EPS);
      return true;
    }
  }
  return false;
}
// formula 8.2
inline bool LpSpRp(double x, double y, double phi, double& t, double& u, double& v)
{
  double t1, u1;
  polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
  u1 = u1 * u1;
  if (u1 >= 4.)
  {
    double theta;
    u = sqrt(u1 - 4.);
    theta = atan2(2., u);
    t = mod2pi(t1 + theta);
    v = mod2pi(t - phi);
    assert(fabs(2 * sin(t) + u * cos(t) - sin(phi) - x) < RS_EPS);
    assert(fabs(-2 * cos(t) + u * sin(t) + cos(phi) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
    return t >= -ZERO && v >= -ZERO;
  }
  return false;
}
void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length(), L;
  if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
    Lmin = L;
  }
  if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
    Lmin = L;
  }
  if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
    Lmin = L;
  }
  if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
    Lmin = L;
  }
  if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
    Lmin = L;
  }
  if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
    Lmin = L;
  }
  if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
    Lmin = L;
  }
  if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
}
// formula 8.3 / 8.4  *** TYPO IN PAPER ***
inline bool LpRmL(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
  polar(xi, eta, u1, theta);
  if (u1 <= 4.)
  {
    u = -2. * asin(.25 * u1);
    t = mod2pi(theta + .5 * u + pi);
    v = mod2pi(phi - t + u);
    assert(fabs(2 * (sin(t) - sin(t - u)) + sin(phi) - x) < RS_EPS);
    assert(fabs(2 * (-cos(t) + cos(t - u)) - cos(phi) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t - u + v - phi)) < RS_EPS);
    return t >= -ZERO && u <= ZERO;
  }
  return false;
}
void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length(), L;
  if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
    Lmin = L;
  }
  if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
    Lmin = L;
  }
  if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
    Lmin = L;
  }
  if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
    Lmin = L;
  }

  // backwards
  double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
  if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
    Lmin = L;
  }
  if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
    Lmin = L;
  }
  if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
    Lmin = L;
  }
  if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
}
// formula 8.7
inline bool LpRupLumRm(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi * xi + eta * eta));
  if (rho <= 1.)
  {
    u = acos(rho);
    tauOmega(u, -u, xi, eta, phi, t, v);
    assert(fabs(2 * (sin(t) - sin(t - u) + sin(t - 2 * u)) - sin(phi) - x) < RS_EPS);
    assert(fabs(2 * (-cos(t) + cos(t - u) - cos(t - 2 * u)) + cos(phi) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t - 2 * u - v - phi)) < RS_EPS);
    return t >= -ZERO && v <= ZERO;
  }
  return false;
}
// formula 8.8
inline bool LpRumLumRp(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi * xi - eta * eta) / 16.;
  if (rho >= 0 && rho <= 1)
  {
    u = -acos(rho);
    if (u >= -.5 * pi)
    {
      tauOmega(u, u, xi, eta, phi, t, v);
      assert(fabs(4 * sin(t) - 2 * sin(t - u) - sin(phi) - x) < RS_EPS);
      assert(fabs(-4 * cos(t) + 2 * cos(t - u) + cos(phi) + 1 - y) < RS_EPS);
      assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
      return t >= -ZERO && v >= -ZERO;
    }
  }
  return false;
}
void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length(), L;
  if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
    Lmin = L;
  }
  if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
    Lmin = L;
  }
  if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
    Lmin = L;
  }
  if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
    Lmin = L;
  }

  if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
    Lmin = L;
  }
  if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
    Lmin = L;
  }
  if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
    Lmin = L;
  }
  if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
}
// formula 8.9
inline bool LpRmSmLm(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
  polar(xi, eta, rho, theta);
  if (rho >= 2.)
  {
    double r = sqrt(rho * rho - 4.);
    u = 2. - r;
    t = mod2pi(theta + atan2(r, -2.));
    v = mod2pi(phi - .5 * pi - t);
    assert(fabs(2 * (sin(t) - cos(t)) - u * sin(t) + sin(phi) - x) < RS_EPS);
    assert(fabs(-2 * (sin(t) + cos(t)) + u * cos(t) - cos(phi) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t + pi / 2 + v - phi)) < RS_EPS);
    return t >= -ZERO && u <= ZERO && v <= ZERO;
  }
  return false;
}
// formula 8.10
inline bool LpRmSmRm(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
  polar(-eta, xi, rho, theta);
  if (rho >= 2.)
  {
    t = theta;
    u = 2. - rho;
    v = mod2pi(t + .5 * pi - phi);
    assert(fabs(2 * sin(t) - cos(t - v) - u * sin(t) - x) < RS_EPS);
    assert(fabs(-2 * cos(t) - sin(t - v) + u * cos(t) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t + pi / 2 - v - phi)) < RS_EPS);
    return t >= -ZERO && u <= ZERO && v <= ZERO;
  }
  return false;
}
void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length() - .5 * pi, L;
  if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5 * pi, u, v);
    Lmin = L;
  }
  if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5 * pi, -u, -v);
    Lmin = L;
  }
  if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5 * pi, u, v);
    Lmin = L;
  }
  if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5 * pi, -u, -v);
    Lmin = L;
  }

  if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5 * pi, u, v);
    Lmin = L;
  }
  if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5 * pi, -u, -v);
    Lmin = L;
  }
  if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5 * pi, u, v);
    Lmin = L;
  }
  if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5 * pi, -u, -v);
    Lmin = L;
  }

  // backwards
  double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
  if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5 * pi, t);
    Lmin = L;
  }
  if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5 * pi, -t);
    Lmin = L;
  }
  if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5 * pi, t);
    Lmin = L;
  }
  if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5 * pi, -t);
    Lmin = L;
  }

  if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5 * pi, t);
    Lmin = L;
  }
  if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5 * pi, -t);
    Lmin = L;
  }
  if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5 * pi, t);
    Lmin = L;
  }
  if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5 * pi, -t);
}
// formula 8.11 *** TYPO IN PAPER ***
inline bool LpRmSLmRp(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
  polar(xi, eta, rho, theta);
  if (rho >= 2.)
  {
    u = 4. - sqrt(rho * rho - 4.);
    if (u <= ZERO)
    {
      t = mod2pi(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta));
      v = mod2pi(t - phi);
      assert(fabs(4 * sin(t) - 2 * cos(t) - u * sin(t) - sin(phi) - x) < RS_EPS);
      assert(fabs(-4 * cos(t) - 2 * sin(t) + u * cos(t) + cos(phi) + 1 - y) < RS_EPS);
      assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
      return t >= -ZERO && v >= -ZERO;
    }
  }
  return false;
}
void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length() - pi, L;
  if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path =
        ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5 * pi, u, -.5 * pi, v);
    Lmin = L;
  }
  if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5 * pi, -u, .5 * pi,
                                                -v);
    Lmin = L;
  }
  if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path =
        ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5 * pi, u, -.5 * pi, v);
    Lmin = L;
  }
  if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5 * pi, -u, .5 * pi,
                                                -v);
}

ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
{
  ReedsSheppStateSpace::ReedsSheppPath path;
  CSC(x, y, phi, path);
  CCC(x, y, phi, path);
  CCCC(x, y, phi, path);
  CCSC(x, y, phi, path);
  CCSCC(x, y, phi, path);
  return path;
}
}  // namespace

const ReedsSheppStateSpace::ReedsSheppPathSegmentType ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },         // 0
  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },        // 1
  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },       // 2
  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },       // 3
  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },    // 4
  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },   // 5
  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },    // 6
  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },   // 7
  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },   // 8
  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },    // 9
  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },   // 10
  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },    // 11
  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },     // 12
  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },     // 13
  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },      // 14
  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },    // 15
  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },  // 16
  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }   // 17
};

ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type, double t, double u,
                                                     double v, double w, double x)
  : type_(type)
{
  length_[0] = t;
  length_[1] = u;
  length_[2] = v;
  length_[3] = w;
  length_[4] = x;
  totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
}

double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
{
  return rho_ * reedsShepp(q0, q1).length();
}

ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
{
  double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
  double c = cos(q0[2]), s = sin(q0[2]);
  double x = c * dx + s * dy, y = -s * dx + c * dy;
  return ::reedsShepp(x / rho_, y / rho_, dth);
}

void ReedsSheppStateSpace::type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data)
{
  ReedsSheppPath path = reedsShepp(q0, q1);
  for (int i = 0; i < 5; ++i)
    cb(path.type_[i], user_data);
  return;
}

void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb,
                                  void* user_data)
{
  ReedsSheppPath path = reedsShepp(q0, q1);
  double dist = rho_ * path.length();

  for (double seg = 0.0; seg <= dist; seg += step_size)
  {
    double qnew[3] = {};
    interpolate(q0, path, seg / rho_, qnew);
    cb(qnew, user_data);
  }
  return;
}

void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath& path, double seg, double s[3])
{
  if (seg < 0.0)
    seg = 0.0;
  if (seg > path.length())
    seg = path.length();

  double phi, v;

  s[0] = s[1] = 0.0;
  s[2] = q0[2];

  for (unsigned int i = 0; i < 5 && seg > 0; ++i)
  {
    if (path.length_[i] < 0)
    {
      v = std::max(-seg, path.length_[i]);
      seg += v;
    }
    else
    {
      v = std::min(seg, path.length_[i]);
      seg -= v;
    }
    phi = s[2];
    switch (path.type_[i])
    {
      case RS_LEFT:
        s[0] += (sin(phi + v) - sin(phi));
        s[1] += (-cos(phi + v) + cos(phi));
        s[2] = phi + v;
        break;
      case RS_RIGHT:
        s[0] += (-sin(phi - v) + sin(phi));
        s[1] += (cos(phi - v) - cos(phi));
        s[2] = phi - v;
        break;
      case RS_STRAIGHT:
        s[0] += (v * cos(phi));
        s[1] += (v * sin(phi));
        break;
      case RS_NOP:
        break;
    }
  }

  s[0] = s[0] * rho_ + q0[0];
  s[1] = s[1] * rho_ + q0[1];
}


/*******************************************************************
Author: David Ge (dge893@gmail.com, aka Wei Ge)
Last modified: 11/16/2020
Allrights reserved by David Ge

********************************************************************/
#include "Space.h"
#include "Curl.h"
#include "../MathTools/Matrix.h"
#include "../FileUtil/fileutil.h"
#include <stdlib.h>

#include "../boostLib/HiMatrix.h"
#include <boost/multiprecision/cpp_dec_float.hpp>

Space::Space()
{
	ds = 0.0;
	nx = ny = nz = 0;
	nx1 = ny1 = nz1 = 0;
	cellCount = 0;
	A = NULL;
	smax = M = 0;
	fieldMemorySize = 0;
	curlH = curlE = NULL;
}

Space::~Space()
{
	cleanup();
}

void Space::cleanup()
{
	tss_free3Darray(A, M + 1, M);
	if (curlH != NULL)
	{
		delete curlH;
		curlH = NULL;
	}
	if (curlE != NULL)
	{
		delete curlE;
		curlE = NULL;
	}
}

void Space::setCalculationMethod(CURLMETHOD method)
{
	curlE->setCalculationMethod(method);
	curlH->setCalculationMethod(method);
}

inline size_t Space::Idx(unsigned i, unsigned j, unsigned k)
{
	return k + nz1 * (j + ny1 * i);
}

/*
	generate space derivative estimation matrixes
*/
int Space::generateMatrix()
{
	int ret = ERR_OK;
	using namespace boost::multiprecision;
	cpp_dec_float_100 *W = (cpp_dec_float_100 *)malloc(M*M*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *Ai = (cpp_dec_float_100 *)malloc(M*M*sizeof(cpp_dec_float_100));
	double *AI = (double *)malloc(M*M*sizeof(double));
	A = tss_allocate3Darray(M + 1, M, M);
	if (AI == NULL || W == NULL || Ai == NULL || A == NULL)
	{
		ret = ERR_OUTOFMEMORY;
	}
	if (ret == ERR_OK)
	{
		HiMatrixTools HiMatrix;
		unsigned int P = smax;
		unsigned int N = smax;
		unsigned int mi;
		cpp_dec_float_100 X = 1.0;
		cpp_dec_float_100 Dn = 1.0;
		cpp_dec_float_100 Dx;
		unsigned int i;
		unsigned int w = 0;
		size_t k;
		for (w = 0; w <= 2 * smax; w++)
		{
			if (w <= smax)
			{
				P = smax + w;
				N = smax - w;
			}
			else
			{
				P = smax - (w - smax);
				N = smax + (w - smax);
			}
			mi = 0;
			k = 0;
			i = 0;
			while (i < P)
			{
				Dn = ((double)i + 1.0);
				Dx = Dn;
				X = 1.0;
				for (unsigned int j = 0; j < M; j++)
				{
					X = X*((double)j + 1.0);
					W[k] = Dx / X;
					Dx = Dx * Dn;
					k++;
				}
				mi += M;
				i++;
			}
			i = 0;
			while (i < N)
			{
				Dn = -((double)i + 1.0);
				Dx = Dn;
				X = 1.0;
				for (unsigned int j = 0; j < M; j++)
				{
					X = X*((double)j + 1.0);
					W[k] = Dx / X;
					Dx = Dx * Dn;
					k++;
				}
				mi += M;
				i++;
			}
			if (HiMatrix.HIinverse(W, Ai, (int)M))
			{
				k = 0;
				for (i = 0; i < M; i++)
				{
					for (unsigned int j = 0; j < M; j++)
					{
						AI[k] = Ai[k].convert_to<double>();
						k++;
					}
				}
				copy2Darray(AI, &(A->a[w]), M);
			}
			else
			{
				ret = ERR_INVERSEMATRIX;
				break;
			}
		}
	}
	if(AI != NULL) free(AI);
	if (W != NULL) free(W);
	if (Ai != NULL) free(Ai);
	return ret;
}

int Space::initializeSpace(double spaceStep, unsigned int inx, unsigned int iny, unsigned int inz, unsigned int ismax, const char *matrixFile)
{
	int ret = ERR_OK;
	ds = spaceStep;
	nx = inx; ny = iny; nz = inz;
	nx1 = inx+1; ny1 = iny+1; nz1 = inz+1;
	smax = ismax;
	M = 2 * smax;
	if (ds <= 0.0 || nx == 0 || ny == 0 || nz == 0 || smax == 0)
	{
		ret = ERR_INVALID_PARAM;
	}
	else
	{
		if (matrixFile != NULL && fileexists(matrixFile))
		{
			A = LoadEstimationMatrixesFromFile(matrixFile, M, &ret);
		}
		else
		{
			ret = generateMatrix();
		}
	}
	if (ret == ERR_OK)
	{
		cellCount = (nx + 1)*(ny + 1)*(nz + 1);
		fieldMemorySize = cellCount*sizeof(Point3Dstruct);
	}
	if (ret == ERR_OK)
	{
		curlH = new Curl();
		ret = curlH->initialize(this);
		if (ret == ERR_OK)
		{
			curlE = new Curl();
			ret = curlE->initialize(this);
			if (ret == ERR_OK)
			{
				//hard coded here as default
				setCalculationMethod(ByFastMethodFirstOrder);
			}
		}
	}
	return ret;
}

////////3 functions for dFx/dx,dy,dz/////////////////////////////////////
double Space::dx_Fx(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - i;
	if (w > 0) //near or at the lower boundary: w=1,2,...,smax; i=smax-1, smax-2,...,0
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = i + smax - nx;
		if (w > 0) //near or at the upper boundary: w=1,2,...,smax; i=nx-smax+1, nx-smax+2,...,nx
		{
			P = smax - w;
			N = smax + w;
			w = w + smax; //index into the array
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i + m,j,k)].x - F[w0].x);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i - m,j,k)].x - F[w0].x);
	}
	return d;
}

double Space::dy_Fx(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - j;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = j + smax - ny;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j + m,k)].x - F[w0].x);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j - m,k)].x - F[w0].x);
	}
	return d;
}

double Space::dz_Fx(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - k;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = k + smax - nz;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j,k + m)].x - F[w0].x);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j,k - m)].x - F[w0].x);
	}
	return d;
}
/////////--end of dFx /dx.dy.dz/////////////////////////////////////////
/////////--start of dFy /dx.dy.dz/////////////////////////////////////////
double Space::dx_Fy(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - i;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = i + smax - nx;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i + m,j,k)].y - F[w0].y);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i - m,j,k)].y - F[w0].y);
	}
	return d;
}

double Space::dy_Fy(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - j;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = j + smax - ny;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j + m,k)].y - F[w0].y);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j - m,k)].y - F[w0].y);
	}
	return d;
}

double Space::dz_Fy(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - k;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = k + smax - nz;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j,k + m)].y - F[w0].y);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j,k - m)].y - F[w0].y);
	}
	return d;
}
/////////--end of dFy /dx.dy.dz/////////////////////////////////////////
/////////--start of dFz /dx.dy.dz/////////////////////////////////////////
double Space::dx_Fz(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - i;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = i + smax - nx;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i + m,j,k)].z - F[w0].z);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i - m,j,k)].z - F[w0].z);
	}
	return d;
}

double Space::dy_Fz(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - j;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = j + smax - ny;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j + m,k)].z - F[w0].z);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j - m,k)].z - F[w0].z);
	}
	return d;
}

double Space::dz_Fz(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - k;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = k + smax - nz;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j,k + m)].z - F[w0].z);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j,k - m)].z - F[w0].z);
	}
	return d;
}
/////////--end of dFz /dx.dy.dz/////////////////////////////////////////

void Space::SetFields(Point3Dstruct *fh, Point3Dstruct *fe)
{
	curlH->SetField(fh);
	curlE->SetField(fe);
}

int Space::CalculateNextCurlH(Point3Dstruct *pNext)
{
	return curlH->CalculateCurl(pNext);
}

int Space::CalculateNextCurlE(Point3Dstruct *pNext)
{
	return curlE->CalculateCurl(pNext);
}

Point3Dstruct *Space::GetCurrentCurlH()
{
	return curlH->GetCurrentCurl();
}

Point3Dstruct *Space::GetCurrentCurlE()
{
	return curlE->GetCurrentCurl();
}

int Space::writeMatrixesToFile(char *file)
{
	int ret = ERR_OK;
	FILE *retFileHandle = 0;
	ret = deleteFile(file);
	if (ret == ERR_OK)
	{
		ret = openTextfileWrite(file, &retFileHandle);
		if (ret == ERR_OK)
		{
			if (retFileHandle == 0)
			{
				ret = ERR_FILE_OPEN_WRIT_EINVAL;
			}
			else
			{
				unsigned int size = 1024;
				char buff[1024];
				Array3Dstruct *A = GetMaxtrixes(); //1+2smax,2smax,2smax
				sprintf_1(buff, size, "smax=%u, space estiation order = 2*smax = %u\r\n", smax, 2 * smax);
				writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
				for (unsigned int h = 0; h < 2 * smax + 1; h++)
				{
					if (h == 0)
					{
						sprintf_1(buff, size, "\r\nestimation matrix for spaces inside the boundary [%u x %u]=\n", 2 * smax, 2 * smax);
					}
					else if (h <= smax)
					{
						sprintf_1(buff, size, "\r\nestimation matrix for spaces near or at the lower boundary [%u x %u]=\n", 2 * smax, 2 * smax);
					}
					else
					{
						sprintf_1(buff, size, "\r\nestimation matrix for spaces near or at the upper boundary [%u x %u]=\n", 2 * smax, 2 * smax);
					}
					writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
					//
					for (unsigned int i = 0; i < 2 * smax; i++)
					{
						for (unsigned int j = 0; j < 2 * smax; j++)
						{
							if (j == 2 * smax - 1)
							{
								sprintf_1(buff, size, "%g\n", A->a[h].r[i].c[j]);
							}
							else
							{
								sprintf_1(buff, size, "%g,\t", A->a[h].r[i].c[j]);
							}
							writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
						}
					}
				}
				closefile(retFileHandle);
			}
		}
	}
	return ret;
}

int Space::saveMaxtrixToFile(char *file)
{
	int ret = ERR_OK;
	FILE *fh = 0;
	size_t w;
	size_t size = (M + 1)*M*M*sizeof(double);
	double *data = (double *)malloc(size);
	if (data == NULL)
	{
		ret = ERR_OUTOFMEMORY;
	}
	else
	{
		w = 0;
		for (unsigned int h = 0; h <= M; h++)
		{
			for (unsigned int i = 0; i < M; i++)
			{
				for (unsigned int j = 0; j < M; j++)
				{
					data[w] = A->a[h].r[i].c[j];
					w++;
				}
			}
		}
		ret = openfileWrite(file, &fh);
		if (ret == ERR_OK)
		{
			ret = writefile(fh, data, (unsigned int)size);
			closefile(fh);
		}
		free(data);
	}
	return ret;
}

//int Space::loadMaxtrixFromFile(const char *file)
Array3Dstruct *Space::LoadEstimationMatrixesFromFile(const char *file, unsigned int M, int *ret)
{
	*ret = ERR_OK;
	FILE *fh = 0;
	Array3Dstruct *A = NULL;
	size_t w;
	size_t size = (M + 1)*M*M*sizeof(double);
	double *data = (double *)malloc(size);
	if (data == NULL)
	{
		*ret = ERR_OUTOFMEMORY;
	}
	else
	{
		size_t fsize = fileSize(file);
		if (fsize != size)
		{
			*ret = ERR_INVALID_SIZE;
		}
	}
	if (*ret == ERR_OK)
	{
		*ret = openfileRead(file, &fh);
		if (*ret == ERR_OK)
		{
			*ret = readfile(fh, data, (unsigned int)size);
			closefile(fh);
		}
	}
	if (*ret == ERR_OK)
	{
		A = tss_allocate3Darray(M + 1, M, M);
		if (A == NULL)
		{
			*ret = ERR_OUTOFMEMORY;
		}
	}
	if (*ret == ERR_OK)
	{
		w = 0;
		for (unsigned int h = 0; h <= M; h++)
		{
			for (unsigned int i = 0; i < M; i++)
			{
				for (unsigned int j = 0; j < M; j++)
				{
					A->a[h].r[i].c[j] = data[w];
					w++;
				}
			}
		}
	}
	if (data != NULL)
		free(data);
	return A;
}

int Space::CreateMatrixFile(unsigned int smax0, char *file)
{
	int ret = ERR_OK;
	smax = smax0;
	M = 2 * smax;
	ret = generateMatrix();
	if (ret == ERR_OK)
	{
		ret = saveMaxtrixToFile(file);
	}
	return ret;
}


//
// Created by krab1k on 13.11.18.
//

#include <cmath>
#include <Eigen/LU>

#include "delre.h"
#include "../structures/molecule.h"
#include "../structures/bond.h"
#include "../parameters.h"

CHARGEFW2_METHOD(DelRe)

std::vector<double> DelRe::calculate_charges(const Molecule &molecule) const {

    const size_t n = molecule.atoms().size();
    const size_t m = molecule.bonds().size();

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n, n);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n);

    for (size_t i = 0; i < n; i++) {
        auto &atom_i = molecule.atoms()[i];
        b(i) = -parameters_->atom()->parameter(atom::delta)(atom_i);
        A(i, i) = -1.0;
    }

    for (const auto &bond: molecule.bonds()) {
        size_t i = bond.first().index();
        size_t j = bond.second().index();
        A(i, j) = parameters_->bond()->parameter(bond::gammaA)(bond);
        A(j, i) = parameters_->bond()->parameter(bond::gammaB)(bond);
    }

    Eigen::VectorXd d = A.partialPivLu().solve(b);
    std::vector<double> q(n, 0);

    for (size_t k = 0; k < m; k++) {
        const auto &bond = molecule.bonds()[k];
        size_t i = bond.first().index();
        size_t j = bond.second().index();
        double dq = (d(i) - d(j)) / (2 * parameters_->bond()->parameter(bond::eps)(bond));
        q[i] -= dq;
        q[j] += dq;
    }

    return q;
}


#pragma once

#include <Eigen/Core>
#include <limits>
#include <vector>

namespace collision2d {
template <typename N>
using Point = Eigen::Matrix<N, 2, 1>;

template <typename N>
using Polygon = std::vector<Point<N>>;

/**
 * Computes potential separating axes for a convex polygon.
 */
template <typename N>
void separatingAxes(const Polygon<N> &a, std::vector<Point<N>> &axes) {
  for (auto i = 0u; i < a.size(); ++i) {
    const auto current = a[i];
    const auto next = a[(i + 1) % a.size()];
    const auto edge = (next - current).normalized();
    // axis is the normal of an edge
    axes.emplace_back(Point<N>{-edge[1], edge[0]});
  }
}

/**
 * Projects the polygon onto the given axis and returns the maximum and minimum
 * coordinate on the axis. Note that the axis has to be normalized.
 */
template <typename N>
void project(const Polygon<N> &a, const Point<N> &axis, N &minProj,
             N &maxProj) {
  maxProj = -std::numeric_limits<N>::infinity();
  minProj = std::numeric_limits<N>::infinity();
  for (const Point<N> &v : a) {
    const N proj = axis.dot(v);
    if (proj < minProj) minProj = proj;
    if (proj > maxProj) maxProj = proj;
  }
}

/**
 * Check for collision between polygons a and b via the Separating Axis Theorem.
 */
template <typename N>
bool intersect(const Polygon<N> &a, const Polygon<N> &b) {
  // compute separating axes
  std::vector<Point<N>> axes;
  separatingAxes(a, axes);
  separatingAxes(b, axes);
  for (const auto &axis : axes) {
    N aMaxProj, aMinProj, bMaxProj, bMinProj;
    project(a, axis, aMinProj, aMaxProj);
    project(b, axis, bMinProj, bMaxProj);
    // check if projections overlap
    if (aMinProj > bMaxProj || bMinProj > aMaxProj) return false;
  }

  return true;
}

// /**
//  * Efficient test for a point to be in a convex polygon.
//  *
//  * Robert Nowak "An Efficient Test for a Point to Be in a Convex Polygon"
//  * http://demonstrations.wolfram.com/AnEfficientTestForAPointToBeInAConvexPolygon/
//  * Wolfram Demonstrations Project
//  * Published: March 7 2011
//  */
// template <typename N>
// bool intersect2(const Point<N> &point, const Polygon<N> &polygon,
//                 const N &epsilon = 1e-4) {
//   bool angle = false;  // stores the sign of the last angle
//   for (auto i = 0u; i < polygon.size(); ++i) {
//     const auto &a = polygon[i] - point;
//     const auto &b = polygon[(i + 1) % polygon.size()] - point;
//     const bool newAngle = b(0) * a(1) - a(0) * b(1) > -epsilon;
//     if (i > 0 && angle != newAngle) return false;
//     angle = newAngle;
//   }
//   return true;
// }

/**
 * Tests whether point intersects with convex polygon.
 * Source: https://stackoverflow.com/a/8721483
 */
template <typename N>
bool intersect(const Point<N> &point, const Polygon<N> &polygon,
               const N &epsilon = 1e-4) {
  bool result = false;
  std::size_t i, j;
  for (i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) {
    if ((polygon[i].y() > point.y()) != (polygon[j].y() > point.y()) &&
        (point.x() < (polygon[j].x() - polygon[i].x()) * (point.y() - polygon[i].y()) /
                          (polygon[j].y() - polygon[i].y() + epsilon) +
                      polygon[i].x())) {
      result = !result;
    }
  }
  return result;
}
}  // namespace collision2d


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/point_generators_3.h>

#include <CGAL/Side_of_triangle_mesh.h>

#include <vector>
#include <fstream>
#include <limits>
#include <boost/foreach.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_3 Point;
typedef CGAL::Polyhedron_3<K> Polyhedron;

double max_coordinate(const Polyhedron& poly)
{
  double max_coord = (std::numeric_limits<double>::min)();
  BOOST_FOREACH(Polyhedron::Vertex_handle v, vertices(poly))
  {
    Point p = v->point();
    max_coord = (std::max)(max_coord, p.x());
    max_coord = (std::max)(max_coord, p.y());
    max_coord = (std::max)(max_coord, p.z());
  }
  return max_coord;
}

int main(int argc, char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/eight.off";
  std::ifstream input(filename);

  Polyhedron poly;
  if (!input || !(input >> poly) || poly.empty()
             || !CGAL::is_triangle_mesh(poly))
  {
    std::cerr << "Not a valid input file." << std::endl;
    return 1;
  }

  CGAL::Side_of_triangle_mesh<Polyhedron, K> inside(poly);

  double size = max_coordinate(poly);

  unsigned int nb_points = 100;
  std::vector<Point> points;
  points.reserve(nb_points);
  CGAL::Random_points_in_cube_3<Point> gen(size);
  for (unsigned int i = 0; i < nb_points; ++i)
    points.push_back(*gen++);

  std::cout << "Test " << nb_points << " random points in cube "
    << "[-" << size << "; " << size <<"]" << std::endl;

  int nb_inside = 0;
  int nb_boundary = 0;
  for (std::size_t i = 0; i < nb_points; ++i)
  {
    CGAL::Bounded_side res = inside(points[i]);

    if (res == CGAL::ON_BOUNDED_SIDE) { ++nb_inside; }
    if (res == CGAL::ON_BOUNDARY) { ++nb_boundary; }
  }

  std::cerr << "Total query size: " << points.size() << std::endl;
  std::cerr << "  " << nb_inside << " points inside " << std::endl;
  std::cerr << "  " << nb_boundary << " points on boundary " << std::endl;
  std::cerr << "  " << points.size() - nb_inside - nb_boundary << " points outside " << std::endl;

  return 0;
}


/**
 * \file se_2_3_localization.cpp
 *
 *  Created on: Aug 03, 2020
 *     \author: prashanthr05
 *
 *  ---------------------------------------------------------
 *  This file is:
 *  (c) 2020 Prashanth Ramadoss @ DIC-IIT, Genova, Italy
 *
 *  adapted from the file se3_localization.cpp:
 *  (c) 2018 Joan Sola @ IRI-CSIC, Barcelona, Catalonia
 *
 *  This file is part of `manif`, a C++ template-only library
 *  for Lie theory targeted at estimation for robotics.
 *  Manif is:
 *  (c) 2018 Jeremie Deray @ IRI-UPC, Barcelona
 *  ---------------------------------------------------------
 *
 *  ---------------------------------------------------------
 *  Demonstration example:
 *
 *  3D Robot localization and linear velocity estimation based on strap-down IMU model and fixed beacons.
 *
 *  ---------------------------------------------------------
 *
 *  We consider a robot in 3D space surrounded by a small
 *  number of punctual landmarks or _beacons_.
 *  The robot is assumed to be mounted with an IMU whose
 *  measurements are fed as exogeneous inputs to the system.
 *  The robot is able to measure the location
 *  of the beacons w.r.t its own reference frame.
 *  We assume in this example that the IMU frame coincides with the robot frame.
 *
 *  The robot extended pose X is in SE_2(3) and the beacon positions b_k in R^3,
 *
 *      X = |    R   p  v|              // position, orientation and linear velocity
 *          |        1   |
 *          |           1|
 *
 *      b_k = (bx_k, by_k, bz_k)    // lmk coordinates in world frame
 *
 *      alpha_k = (alphax_k, alphay_k, alphaz_k) // linear accelerometer measurements in IMU frame
 *
 *      omega_k = (omegax_k, omegay_k, omegaz_k) // gyroscope measurements in IMU frame
 *
 *      g = (0, 0, -9.80665)  // acceleration due to gravity in world frame
 *
 * Consider robot coordinate frame B and world coordinate frame A.
 * - p is the position of the origin of the robot frame B with respect to the world frame A
 * - R is the orientation of the robot frame B with respect to the world frame A
 * - v is the velocity of the robot frame with respect to the world frame,
 *            expressed in a frame whose origin coincides with the robot frame, oriented similar to the world frame
 *            (it is equivalent to p_dot in continuous time. This is usually called mixed-frame representation
 *            and is denoted as (B[A] v_AB), where B[A] is the mixed frame as described above.
 *            For reference, please see "Multibody Dynamics Notation" by Silvio Traversaro and Alessandro Saccon.
 *            Link: https://research.tue.nl/en/publications/multibody-dynamics-notation-version-2)
 * - a is the frame acceleration in mixed-representation (equivalent to p_doubledot in continuous time).
 * - omega_b as the angular velocity of the robot expressed in the robot frame
 *
 * The kinematic equations (1) can be written as,
 * p <-- p + v dt + 0.5 a dt^2
 * R <-- R Exp_SO3(omega_b)
 * v <-- v + a dt
 *
 * However, we would like to express the kinematics equations in the form,
 * X <-- X * Exp(u)
 * where, X \in SE_2(3), u \in R^9 and u_hat \in se_2(3)
 * Note that here input vector u is expressed in the local frame (robot frame).
 * This can be seen as a motion integration on a manifold defined by the group SE_2(3).
 *
 * The exponential mapping of SE_2(3) is defined as,
 * for u = [u_p, u_w, u_v]
 * Exp(u) = | Exp_SO3(u_w)   JlSO3(u_w) u_p   JlSO3(u_w) u_v |
 *          | 0    0    0                 1                0 |
 *          | 0    0    0                 0                1 |
 * where, JlSO3 is the left Jacobian of the SO(3) group.
 *
 * Please see the Appendix C of the paper "A micro Lie theory for state estimation in robotics",
 * for the definition of the left Jacobian of SO(3).
 * Please see the Appendix D of the paper, for the definition of Exp map for SE(3).
 * The Exp map of SE_2(3) is a simple extension from the Exp map of SE(3).
 * Also, please refer to Example 7 of the paper to understand when and how the left Jacobian of SO(3)
 * appears in the definitions of Exp maps. The Example 7 illustrates the scenario for SE(3).
 * We use a direct extension here for SE_2(3).
 * One can arrive to such a definition by following the convergent Taylor's series expansion
 * for the matrix exponential of the Lie algebra element (Equation 16 of the paper).
 *
 * As a result of X <-- X * Exp(u), we get (2)
 * p <-- p + R JlSO3(u_w) u_p
 * R <-- R Exp_SO3(u_w)
 * v <-- v + R JlSO3(u_w) u_v
 *
 * It is important to notice the subtle difference between (1) and (2) here,
 * which is specifically the influence of the left Jacobian of SO(3) in (2).
 * The approach in (1) considers the motion integration is done by defining
 * the exponential map in R3xSO(3)xR3 instead of SE_2(3),
 * in the sense explored in Example 7 of the Micro Lie theory paper. It must be noted that
 * as dt tends to 0, both sets of equations (1) and (2) tend to be the same, since JlSO3 tends to identity.
 *
 * Since, (2) exploits the algebra of the SE_2(3) group properly,
 * we would like to draw a relationship between the sets of equations (2)
 * and the IMU measurements which will constitute the exogeneous input vector u \in se_2(3).
 *
 * Considering R.T as the transpose of R, the IMU measurements are modeled as,
 *    - linear accelerometer measurements alpha = R.T (a - g) + w_acc
 *    - gyroscope measurements omega = omega_b + w_omega
 * Note that the IMU measurements are expressed in the IMU frame (coincides with the robot frame - assumption).
 * The IMU measurements are corrupted by noise,
 *    - w_omega is the additive white noise affecting the gyroscope measurements
 *    - w_acc is the additive white noise affecting the linear accelerometer measurements
 * It must be noted that we do not consider IMU biases in the IMU measurement model in this example.
 *
 * Taking into account all of the above considerations, the exogenous input vector u (3) becomes,
 *   u = (u_p, u_w, u_v) where,
 *   u_w = omega dt
 *   u_p = (R.T v dt + 0.5 dt^2 (alpha + R.T g)
 *   u_v = (alpha + R.T g) dt
 *
 * This choice of input vector allows us to directly use measurements from the IMU
 * for an unified motion integration involving position, orientation and linear velocity of the robot using SE_2(3).
 * Equations (2) and (3) lead us to the following evolution equations,
 *
 * p <-- p + JlSO3 R.T v dt + 0.5 JlSO3 (alpha + R.T g) dt^2
 * R <-- R Exp_SO3(omega dt)
 * v <-- v + JlSO3 (alpha + R.T g) dt
 *
 * The system propagation noise covariance matrix becomes,
 *    U = diagonal(0, 0, 0, sigma_omegax^2, sigma_omegay^2, sigma_omegaz^2, sigma_accx^2, sigma_accy^2, sigma_accz^2).
 *
 *  At the arrival of a exogeneous input u, the robot pose is updated
 *  with X <-- X * Exp(u) = X + u.
 *
 *  Landmark measurements are of the range and bearing type,
 *  though they are put in Cartesian form for simplicity.
 *  Their noise n is zero mean Gaussian, and is specified
 *  with a covariances matrix R.
 *  We notice that the SE_2(3) action is the same as a
 *  rigid motion action of SE(3).
 *  This is the action of X \in SE_2(3) on a 3-d point b \in R^3 defined as,
 *  X b = R b + p
 *
 *  Thus, the landmark measurements can be expressed as a group action on 3d points,
 *  y = h(X,b) = X^-1 * b
 *
 *      y_k = (brx_k, bry_k, brz_k)    // lmk coordinates in robot frame
 *
 *  We consider the beacons b_k situated at known positions.
 *  We define the extended pose to estimate as X in SE_2(3).
 *  The estimation error dx and its covariance P are expressed
 *  in the tangent space at X.
 *
 *  All these variables are summarized again as follows
 *
 *    X   : robot's extended pose, SE_2(3)
 *    u   : robot control input, u = u(X, y_imu) \in se_2(3) with X as state and y_imu = [alpha, omega] as IMU readings, see Eq. (3)
 *    U   : control perturbation covariance
 *    b_k : k-th landmark position, R^3
 *    y   : Cartesian landmark measurement in robot frame, R^3
 *    R   : covariance of the measurement noise
 *
 *  The motion and measurement models are
 *
 *    X_(t+1) = f(X_t, u) = X_t * Exp ( u )     // motion equation
 *    y_k     = h(X, b_k) = X^-1 * b_k          // measurement equation
 *
 *  The algorithm below comprises first a simulator to
 *  produce measurements, then uses these measurements
 *  to estimate the state, using a Lie-based error-state Kalman filter.
 *
 *  This file has plain code with only one main() function.
 *  There are no function calls other than those involving `manif`.
 *
 *  Printing simulated state and estimated state together
 *  with an unfiltered state (i.e. without Kalman corrections)
 *  allows for evaluating the quality of the estimates.
 *
 * A side note: Besides the approach described here in this illustration example,
 * there are other interesting works like the paper,
 * The Invariant Extended Kalman filter as a stable observer (https://arxiv.org/pdf/1410.1465.pdf)
 * which assume a specific structure for the system propagation dynamics "f(X_t, u)" (group affine dynamics)
 * that simplifies the covariance propagation and enables error dynamics with stronger convergence properties.
 *
 */

#include "manif/SE_2_3.h"

#include <Eigen/Dense>

#include <vector>
#include <iostream>
#include <iomanip>

using std::cout;
using std::endl;

using namespace Eigen;

typedef Array<double, 3, 1> Array3d;
typedef Array<double, 6, 1> Array6d;
typedef Matrix<double, 6, 1> Vector6d;
typedef Matrix<double, 6, 6> Matrix6d;
typedef Array<double, 9, 1> Array9d;
typedef Matrix<double, 9, 1> Vector9d;
typedef Matrix<double, 9, 9> Matrix9d;

int main()
{
    std::srand((unsigned int) time(0));

    // START CONFIGURATION
    //
    //
    const int NUMBER_OF_LMKS_TO_MEASURE = 5;

    // Define the robot extended pose element and its covariance
    manif::SE_2_3d X, X_simulation, X_unfiltered;
    Matrix9d    P;

    X_simulation.setIdentity();
    X.setIdentity();
    X_unfiltered.setIdentity();
    P.setZero();
    P.block<3, 3>(0, 0) = 0.001*Eigen::Matrix3d::Identity();
    P.block<3, 3>(3, 3) = 0.01*Eigen::Matrix3d::Identity();
    P.block<3, 3>(6, 6) = 0.001*Eigen::Matrix3d::Identity();

    // acceleration due to gravity in world frame
    Vector3d g;
    g << 0, 0, -9.80665;
    const double dt = 0.01;

    // IMU measurements in IMU frame
    Vector3d alpha, alpha_const, omega, alpha_prev, omega_prev;
    alpha_const << 0.1, 0.01, 0.1; // constant acceleration in IMU frame without gravity compensation
    omega << 0.01, 0.1, 0; // constant angular velocity about x- and y-direction in IMU frame

    // Previous IMU measurements in IMU frame initialized to values expected when stationary
    alpha_prev = alpha = alpha_const - (X_simulation.rotation()).transpose()*g;
    omega_prev << 0, 0, 0;

    // Define a control vector and its noise and covariance
    manif::SE_2_3Tangentd  u_simu, u_est, u_unfilt;
    Vector9d            u_nom, u_noisy, u_noise;
    Array9d             u_sigmas;
    Matrix9d            U;

    u_sigmas << 0.0, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01;
    U        = (u_sigmas * u_sigmas).matrix().asDiagonal();

    // Declare the Jacobians of the motion wrt robot and control
    manif::SE_2_3d::Jacobian F;     // F = J_x_x + (J_x_u * J_u_x)
    manif::SE_2_3d::Jacobian J_x_x; // d(X * exp(u)) / dX
    manif::SE_2_3d::Jacobian J_x_u; // d(X * exp(u)) / du
    manif::SE_2_3d::Jacobian J_u_x; // du / dX, since u is a state-dependent vector

    // Define five landmarks in R^3
    Vector3d b0, b1, b2, b3, b4, b;
    b0 << 2.0,  0.0,  0.0;
    b1 << 3.0, -1.0, -1.0;
    b2 << 2.0, -1.0,  1.0;
    b3 << 2.0,  1.0,  1.0;
    b4 << 2.0,  1.0, -1.0;
    std::vector<Vector3d> landmarks;
    landmarks.push_back(b0);
    landmarks.push_back(b1);
    landmarks.push_back(b2);
    landmarks.push_back(b3);
    landmarks.push_back(b4);

    // Define the beacon's measurements
    Vector3d                y, y_noise;
    Array3d                 y_sigmas;
    Matrix3d                R;   /**< Beacon measurement noise covariance matrix **/
    std::vector<Vector3d>   measurements(landmarks.size());

    y_sigmas << 0.01, 0.01, 0.01;
    R        = (y_sigmas * y_sigmas).matrix().asDiagonal();

    // Declare the Jacobian of the measurements wrt the robot pose
    Matrix<double, 3, 9>    H;      // H = J_e_x

    // Declare some temporaries
    Vector3d                e, z;   // expectation, innovation
    Matrix3d                E, Z;   // covariances of the above
    Matrix<double, 9, 3>    K;      // Kalman gain
    manif::SE_2_3Tangentd      dx;     // optimal update step, or error-state
    manif::SE_2_3d::Jacobian   J_xi_x; // Jacobian is typedef Matrix
    Matrix<double, 3, 9>       J_e_xi; // Jacobian

    //
    //
    // CONFIGURATION DONE

    // DEBUG
    cout << std::fixed   << std::setprecision(3) << std::showpos << endl;
    cout << "X STATE     :    X      Y      Z    TH_x   TH_y   TH_z     V_x   V_y    V_z" << endl;
    cout << "---------------------------------------------------------------------------" << endl;
    cout << "X simulated : Pos: " << X_simulation.translation().transpose() <<
                             " RPY: " << X_simulation.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X_simulation.linearVelocity().transpose() << endl;
    cout << "X estimated : Pos: " << X.translation().transpose() <<
                             " RPY: " << X.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X.linearVelocity().transpose() << endl;
    cout << "X unfilterd : Pos: " << X_unfiltered.translation().transpose() <<
                             " RPY: " << X_unfiltered.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X_unfiltered.linearVelocity().transpose() << endl;
    cout << "---------------------------------------------------------------------------" << endl;
    // END DEBUG

    // START TEMPORAL LOOP
    //
    //

    // Make 10 steps. Measure up to three landmarks each time.
    for (double t = 0; t < 10; t+=dt)
    {
        //// I. Simulation ###############################################################################

        /// get current simulated state and measurements from previous step
        auto R_k = X_simulation.rotation();
        auto v_k = X_simulation.linearVelocity();
        auto acc_k = alpha_prev + R_k.transpose()*g;

        /// input vector
        u_nom << (dt*(R_k.transpose())*v_k + 0.5*dt*dt*acc_k),  dt*omega_prev, dt*acc_k;

        /// simulate noise
        u_noise = u_sigmas * Array9d::Random();             // control noise
        u_noisy = u_nom + u_noise;                          // noisy control

        u_simu   = u_nom;
        u_unfilt = u_noisy;

        /// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
        X_simulation = X_simulation + u_simu;               // overloaded X.rplus(u) = X * exp(u)
        /// update expected IMU measurements
        alpha = alpha_const - X_simulation.rotation().transpose()*g; // update expected IMU measurement after moving

        /// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -
        for (int i = 0; i < landmarks.size(); i++)
        {
            b = landmarks[i];                               // lmk coordinates in world frame

            /// simulate noise
            y_noise = y_sigmas * Array3d::Random();         // measurement noise

            y = X_simulation.inverse().act(b);              // landmark measurement, before adding noise
            y = y + y_noise;                                // landmark measurement, noisy
            measurements[i] << y;                     // store for the estimator just below
        }

        //// II. Estimation ###############################################################################

        /// get current state estimate to build the state-dependent control vector
        auto R_k_est = X.rotation();
        auto v_k_est = X.linearVelocity();
        auto acc_k_est = alpha_prev + R_k_est.transpose()*g;

        Eigen::Vector3d accLin = dt*(R_k_est.transpose())*v_k_est + 0.5*dt*dt*acc_k_est;
        Eigen::Vector3d gLin = R_k_est.transpose()*g*dt;
        Eigen::Matrix3d accLinCross = manif::skew(accLin);
        Eigen::Matrix3d gCross = manif::skew(gLin);

        u_est << accLin,  dt*omega_prev, dt*acc_k_est;
        u_est += u_noise;

        /// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -

        X = X.plus(u_est, J_x_x, J_x_u);                        // X * exp(u), with Jacobians

        // Prepare Jacobian of state-dependent control vector
        J_u_x.setZero();
        J_u_x.block<3, 3>(0, 3) = accLinCross;
        J_u_x.block<3, 3>(0, 6) = Eigen::Matrix3d::Identity()*dt;
        J_u_x.block<3, 3>(6, 3) = gCross;
        F = J_x_x + J_x_u*J_u_x;                                // chain rule for system model Jacobian

        P = F * P * F.transpose() + J_x_u * U * J_x_u.transpose();

        /// Then we correct using the measurements of each lmk - - - - - - - - -
        for (int i = 0; i < NUMBER_OF_LMKS_TO_MEASURE; i++)
        {
            // landmark
            b = landmarks[i];                               // lmk coordinates in world frame

           // measurement
            y = measurements[i];                            // lmk measurement, noisy

            // expectation
            e = X.inverse(J_xi_x).act(b, J_e_xi);           // note: e = R.tr * ( b - t ), for X = (R,t).
            H = J_e_xi * J_xi_x;                            // note: H = J_e_x = J_e_xi * J_xi_x
            E = H * P * H.transpose();

            // innovation
            z = y - e;
            Z = E + R;

            // Kalman gain
            K = P * H.transpose() * Z.inverse();            // K = P * H.tr * ( H * P * H.tr + R).inv

            // Correction step
            dx = K * z;                                     // dx is in the tangent space at X

            // Update
            X = X + dx;                                     // overloaded X.rplus(dx) = X * exp(dx)
            P = P - K * Z * K.transpose();
        }

        //// III. Unfiltered ##############################################################################

        // move also an unfiltered version for comparison purposes
        X_unfiltered = X_unfiltered + u_unfilt;

        alpha_prev = alpha;
        omega_prev = omega;

        //// IV. Results ##############################################################################

        // DEBUG
        cout << "X simulated : Pos: " << X_simulation.translation().transpose() <<
                             " RPY: " << X_simulation.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X_simulation.linearVelocity().transpose() << endl;
        cout << "X estimated : Pos: " << X.translation().transpose() <<
                             " RPY: " << X.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X.linearVelocity().transpose() << endl;
        cout << "X unfilterd : Pos: " << X_unfiltered.translation().transpose() <<
                             " RPY: " << X_unfiltered.rotation().eulerAngles(2, 1, 0).reverse().transpose() <<
                             " Vel: " << X_unfiltered.linearVelocity().transpose() << endl;
        cout << "---------------------------------------------------------------------------" << endl;

        cout << "X simulated : log: " << X_simulation.log() << endl;
        cout << "X estimated : log: " << X.log() << endl;
        cout << "X unfilterd : log: " << X_unfiltered.log() << endl;
        cout << "---------------------------------------------------------------------------" << endl;
        cout << "---------------------------------------------------------------------------" << endl;
        // END DEBUG

    }

    //
    //
    // END OF TEMPORAL LOOP. DONE.

    return 0;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Copyright 2012 The MITRE Corporation                                      *
 *                                                                           *
 * Licensed under the Apache License, Version 2.0 (the "License");           *
 * you may not use this file except in compliance with the License.          *
 * You may obtain a copy of the License at                                   *
 *                                                                           *
 *     http://www.apache.org/licenses/LICENSE-2.0                            *
 *                                                                           *
 * Unless required by applicable law or agreed to in writing, software       *
 * distributed under the License is distributed on an "AS IS" BASIS,         *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  *
 * See the License for the specific language governing permissions and       *
 * limitations under the License.                                            *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <Eigen/Dense>

#include <openbr/plugins/openbr_internal.h>
#include <openbr/core/opencvutils.h>
#include <openbr/core/eigenutils.h>

using namespace cv;

namespace br
{

/*!
 * \ingroup transforms
 * \brief Procrustes alignment of points
 * \author Scott Klum \cite sklum
 */
class ProcrustesTransform : public MetadataTransform
{
    Q_OBJECT

    Q_PROPERTY(bool warp READ get_warp WRITE set_warp RESET reset_warp STORED false)
    BR_PROPERTY(bool, warp, true)

    Eigen::MatrixXf meanShape;

    void train(const TemplateList &data)
    {
        QList< QList<QPointF> > normalizedPoints;

        // Normalize all sets of points
        foreach (br::Template datum, data) {
            QList<QPointF> points = datum.file.points();
            QList<QRectF> rects = datum.file.rects();

            if (points.empty() || rects.empty()) continue;

            // Assume rect appended last was bounding box
            points.append(rects.last().topLeft());
            points.append(rects.last().topRight());
            points.append(rects.last().bottomLeft());
            points.append(rects.last().bottomRight());

            // Center shape at origin
            Scalar mean = cv::mean(OpenCVUtils::toPoints(points).toVector().toStdVector());
            for (int i = 0; i < points.size(); i++) points[i] -= QPointF(mean[0],mean[1]);

            // Remove scale component
            float norm = cv::norm(OpenCVUtils::toPoints(points).toVector().toStdVector());
            for (int i = 0; i < points.size(); i++) points[i] /= norm;

            normalizedPoints.append(points);
        }

        if (normalizedPoints.empty()) qFatal("Unable to calculate normalized points");

        // Determine mean shape, assuming all shapes contain the same number of points
        meanShape = Eigen::MatrixXf(normalizedPoints[0].size(), 2);

        for (int i = 0; i < normalizedPoints[0].size(); i++) {
            double x = 0;
            double y = 0;

            for (int j = 0; j < normalizedPoints.size(); j++) {
                x += normalizedPoints[j][i].x();
                y += normalizedPoints[j][i].y();
            }

            x /= (double)normalizedPoints.size();
            y /= (double)normalizedPoints.size();

            meanShape(i,0) = x;
            meanShape(i,1) = y;
        }
    }

    void projectMetadata(const File &src, File &dst) const
    {
        QList<QPointF> points = src.points();
        QList<QRectF> rects = src.rects();

        if (points.empty() || rects.empty()) {
            dst = src;
            if (Globals->verbose) qWarning("Procrustes alignment failed because points or rects are empty.");
            return;
        }

        // Assume rect appended last was bounding box
        points.append(rects.last().topLeft());
        points.append(rects.last().topRight());
        points.append(rects.last().bottomLeft());
        points.append(rects.last().bottomRight());

        Scalar mean = cv::mean(OpenCVUtils::toPoints(points).toVector().toStdVector());
        for (int i = 0; i < points.size(); i++) points[i] -= QPointF(mean[0],mean[1]);

        Eigen::MatrixXf srcMat(points.size(), 2);
        float norm = cv::norm(OpenCVUtils::toPoints(points).toVector().toStdVector());
        for (int i = 0; i < points.size(); i++) {
            points[i] /= norm;
            srcMat(i,0) = points[i].x();
            srcMat(i,1) = points[i].y();
        }

        Eigen::JacobiSVD<Eigen::MatrixXf> svd(srcMat.transpose()*meanShape, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::MatrixXf R = svd.matrixU()*svd.matrixV().transpose();

        dst = src;

        // Store procrustes stats in the order:
        // R(0,0), R(1,0), R(1,1), R(0,1), mean_x, mean_y, norm
        QList<float> procrustesStats;
        procrustesStats << R(0,0) << R(1,0) << R(1,1) << R(0,1) << mean[0] << mean[1] << norm;
        dst.setList<float>("ProcrustesStats",procrustesStats);

        if (warp) {
            Eigen::MatrixXf dstMat = srcMat*R;
            for (int i = 0; i < dstMat.rows(); i++) {
                dst.appendPoint(QPointF(dstMat(i,0),dstMat(i,1)));
            }
        }
    }

    void store(QDataStream &stream) const
    {
        stream << meanShape;
    }

    void load(QDataStream &stream)
    {
        stream >> meanShape;
    }

};

BR_REGISTER(Transform, ProcrustesTransform)

} // namespace br

#include "metadata/procrustes.moc"


#include <iostream>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;

ublas::matrix<double> get_K(int N, double a0 = 2, double a1 = 2)
{
	ublas::identity_matrix<double> I(N, N);
	ublas::matrix<double> m1 = 2.0 * I;
	m1(0, 0) = a0;
	m1(N-1, N-1) = a1;

	ublas::symmetric_matrix<double, ublas::lower> m2(N, N);
	for (unsigned i = 0; i < m2.size1(); ++ i)
		for (unsigned j = 0; j <= i; ++ j)
			m2(i, j) = (j == i-1)? -1:0;
	return m1 + m2;
}

int main()
{
	int N = 3;
	double h = 1.0 / (N - 1);
	ublas::matrix<double> K = get_K(N);
	std::cout << K << std::endl;

	ublas::matrix<double> A = get_K(N, 1, 1);
	std::cout << A << std::endl;

	ublas::matrix<double> A1 = get_K(N, 2, 1);
	std::cout << A1 << std::endl;

	ublas::matrix<double> A2 = get_K(N, 1, 2);
	std::cout << A2 << std::endl;

	ublas::matrix<double> Kinv(N, N);
	Kinv.assign(ublas::identity_matrix<double> (K.size1()));
	ublas::permutation_matrix<size_t> pm(K.size1());
	ublas::lu_factorize(K, pm);
	ublas::lu_substitute(K, pm, Kinv);

	std::cout << Kinv << std::endl;

	double* tmp = (double*) Kinv.data().begin();
	int size = Kinv.data().size();

	for (int i = 0; i < size; ++i)
		std::cout << tmp[i] << std::endl;
	return 0;
}


#ifndef FAST_GICP_SO3_HPP
#define FAST_GICP_SO3_HPP

#include <Eigen/Core>

namespace fast_gicp {

inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) {
  Eigen::Matrix3f skew = Eigen::Matrix3f::Zero();
  skew(0, 1) = -x[2];
  skew(0, 2) = x[1];
  skew(1, 0) = x[2];
  skew(1, 2) = -x[0];
  skew(2, 0) = -x[1];
  skew(2, 1) = x[0];

  return skew;
}

}

#endif


/**
   \file powell_method.hpp
   \brief powerll optimization method
   \author Junhua Gu
 */

#ifndef POWELL_METHOD
#define POWELL_METHOD
#define OPT_HEADER
#include <core/optimizer.hpp>
//#include <blitz/array.h>
#include <limits>
#include <cassert>
#include <cmath>
#include "../linmin/linmin.hpp"
#include <algorithm>
#include <iostream>

namespace opt_utilities
{
  /**
     \brief Impliment of an optimization method
     \tparam rT return type of the object function
     \tparam pT parameter type of the object function
   */
  template <typename rT,typename pT>
  class powell_method
    :public opt_method<rT,pT>
  {
  public:
    typedef pT array1d_type;
    typedef rT T;
  private:
    func_obj<rT,pT>* p_fo;
    optimizer<rT,pT>* p_optimizer;
    volatile bool bstop;
    //typedef blitz::Array<rT,2> array2d_type;

    const char* do_get_type_name()const
    {
      return "powell method";
    }
  private:
    array1d_type start_point;
    array1d_type end_point;

  private:
    int ncom;
    array1d_type pcom_p;
    array1d_type xicom_p;
    rT threshold;
    T** xi;
    T* xi_1d;
  private:
    rT func(const pT& x)
    {
      assert(p_fo!=NULL_PTR);
      return p_fo->eval(x);
    }

  private:
    void clear_xi()
    {
      if(xi_1d!=NULL_PTR)
	{
	  delete[] xi_1d;
	}
      if(xi!=NULL_PTR)
	{
	  delete[] xi;
	}
    }

    void init_xi(int n)
    {
      clear_xi();
      xi_1d=new T[n*n];
      xi=new T*[n];
      for(int i=0;i!=n;++i)
	{
	  xi[i]=xi_1d+i*n;
	}
      for(int i=0;i!=n;++i)
	{
	  for(int j=0;j!=n;++j)
	    {
	      xi[i][j]=(j==i?1:0);
	    }
	}
    }

    void powell(array1d_type& p,const T ftol,
	   int& iter,T& fret)
    {
      const int ITMAX=200;
      const T TINY=std::numeric_limits<T>::epsilon();
      int i,j,ibig;
      T del,fp,fptt,t;
      int n=(int)get_size(p);
      array1d_type pt(n);
      array1d_type ptt(n);
      array1d_type xit(n);
      fret=p_fo->eval(p);

      for(j=0;j<n;++j)
	{
	  //get_element(pt,j)=get_element(p,j);
	  set_element(pt,j,get_element(p,j));
	}
      for(iter=0;!bstop;++iter)
	{
	  fp=fret;
	  ibig=0;
	  del=0.0;
	  for(i=0;i<n;++i)
	    {
#ifdef _OPENMP
#pragma omp parallel for
#endif
	      for(j=0;j<n;++j)
		{
		  //get_element(xit,j)=xi[j][i];
		  set_element(xit,j,xi[j][i]);
		}
	      fptt=fret;
	      linmin(p,xit,fret,(*p_fo));
	      if((fptt-fret)>del)
		{
		  del=fptt-fret;
		  ibig=i+1;
		}
	    }
	  if(T(2.)*(fp-fret)<=ftol*(tabs(fp)+tabs(fret))+TINY)
	    {
	      return;
	    }
	  if(iter==ITMAX)
	    {
	      std::cerr<<"powell exceeding maximun iterations."<<std::endl;
	      return;
	    }
#ifdef _OPENMP
#pragma omp parallel for
#endif
	  for(j=0;j<n;++j)
	    {
	      //get_element(ptt,j)=T(2.)*get_element(p,j)-get_element(pt,j);
	      set_element(ptt,j,T(2.)*get_element(p,j)-get_element(pt,j));
	      //get_element(xit,j)=
	      //get_element(p,j)-get_element(pt,j);
	      set_element(xit,j,get_element(p,j)-get_element(pt,j));
	      //get_element(pt,j)=get_element(p,j);
	      set_element(pt,j,get_element(p,j));
	    }
	  fptt=func(ptt);
	  if(fptt<fp)
	    {
	      t=T(2.)*(fp-T(2.)*fret+fptt)*sqr(T(fp-fret-del))-del*sqr(T(fp-fptt));
	      if(t<T(0.))
		{
		  linmin(p,xit,fret,*p_fo);
#ifdef _OPENMP
#pragma omp parallel for
#endif
		  for(j=0;j<n;++j)
		    {
		      xi[j][ibig-1]=xi[j][n-1];
		      xi[j][n-1]=get_element(xit,j);

		    }
		}
	    }
	}
    }

  public:

    powell_method()
      :threshold(1e-4),xi(NULL_PTR),xi_1d(NULL_PTR)
    {}

    virtual ~powell_method()
    {
      clear_xi();
    };

    powell_method(const powell_method<rT,pT>& rhs)
      :opt_method<rT,pT>(rhs),p_fo(rhs.p_fo),p_optimizer(rhs.p_optimizer),
       start_point(rhs.start_point),
       end_point(rhs.end_point),
       ncom(rhs.ncom),
       threshold(rhs.threshold),xi(NULL_PTR),xi_1d(NULL_PTR)
    {
    }

    powell_method<rT,pT>& operator=(const powell_method<rT,pT>& rhs)
    {
      threshold=rhs.threshold;
      xi=0;
      xi_1d=0;
      p_fo=rhs.p_fo;
      p_optimizer=rhs.p_optimizer;
      start_point=rhs.start_point;
      end_point=rhs.end_point;
      ncom=rhs.ncom;
      threshold=rhs.threshold;
    }

    opt_method<rT,pT>* do_clone()const
    {
      return new powell_method<rT,pT>(*this);
    }

    void do_set_start_point(const array1d_type& p)
    {
      resize(start_point,get_size(p));
      opt_eq(start_point,p);
    }

    array1d_type do_get_start_point()const
    {
      return start_point;
    }

    void do_set_lower_limit(const array1d_type&)
    {}

    void do_set_upper_limit(const array1d_type&)
    {}

    void do_set_precision(rT t)
    {
      threshold=t;
    }

    rT do_get_precision()const
    {
      return threshold;
    }

    void do_set_optimizer(optimizer<rT,pT>& o)
    {
      p_optimizer=&o;
      p_fo=p_optimizer->ptr_func_obj();
    }

    pT do_optimize()
    {
      bstop=false;
      init_xi((int)get_size(start_point));

      for(int i=0;i<(int)get_size(start_point);++i)
	{
	  for(int j=0;j<(int)get_size(start_point);++j)
	    {
	      xi[i][j]=(i==j)?1:0;
	    }
	}

      int iter=100;
      opt_eq(end_point,start_point);
      rT fret;
      powell(end_point,threshold,iter,fret);
      return end_point;
    }

    void do_stop()
    {
      bstop=true;
    }

  };

}

#endif
//EOF


#ifndef PRECONDITIONED_CONJUGATE_GRADIENT_H
#define PRECONDITIONED_CONJUGATE_GRADIENT_H
#include <Eigen/Dense>
#include "mtao/solvers/linear/linear.hpp"
#include "mtao/solvers/cholesky/ldlt.hpp"
#include <iostream>

namespace mtao::solvers::linear {
template <typename MatrixType, typename VectorType, typename Preconditioner>
    struct PCGSolver;

template <typename MatrixType, typename VectorType, typename Preconditioner>
struct solver_traits<PCGSolver< MatrixType, VectorType, Preconditioner> > {
    using Scalar = typename VectorType::Scalar;
    using Matrix = MatrixType;
    using Vector = VectorType;
};

template <typename MatrixType, typename VectorType, typename Preconditioner>
struct PCGSolver: public IterativeLinearSolver<PCGSolver< MatrixType, VectorType, Preconditioner> >
{
    typedef MatrixType Matrix;
    typedef VectorType Vector;
    typedef typename Vector::Scalar Scalar;
    using Base = IterativeLinearSolver<PCGSolver< MatrixType, VectorType, Preconditioner> >;
    using Base::A ;
    using Base::b ;
    using Base::x ;
    using Base::Base;
    void compute()
    {
        precond = Preconditioner(A());
        r = b()-A()*x();
        precond->solve(r,z);
        p = z;
        Ap = A()*p;
        rdz = r.dot(z);
    }
    Scalar error()
    {
        return r.template lpNorm<Eigen::Infinity>();
    }

    void step()
    {
        alpha = (rdz)/(p.dot(Ap));
        x()+=alpha * p;
        r-=alpha * Ap;
        precond->solve(r,z);
        beta=1/rdz;
        rdz = r.dot(z);
        beta*=rdz;
        p=z+beta*p;
        Ap=A()*p;
    }
private:
    Vector r;
    Vector z;
    Vector p;
    Vector Ap;
    Scalar rdz;
    Scalar alpha, beta;
    std::optional<Preconditioner> precond;

};

template <typename Preconditioner, typename Matrix, typename Vector>
void PCGSolve(const Matrix & A, const Vector & b, Vector & x)
{
    auto residual = (b-A*x).template lpNorm<Eigen::Infinity>();
    auto solver = PCGSolver<Matrix,Vector, Preconditioner>(5*A.rows(), 1e-5*residual);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
    solver.solve(A,b,x);
    x = solver.x();
}

template <typename Matrix, typename Vector>
void DenseCholeskyPCGSolve(const Matrix & A, const Vector & b, Vector & x)
{
    PCGSolve<cholesky::DenseLDLT_MIC0<std::decay_t<decltype(A)>> >(A,b,x);
}

template <typename Matrix, typename Vector>
void SparseCholeskyPCGSolve(const Matrix & A, const Vector & b, Vector & x)
{
    PCGSolve<cholesky::SparseLDLT_MIC0<Matrix,Vector> >(A,b,x);

}
template <typename Matrix, typename Vector>
void CholeskyPCGSolve(const Matrix & A, const Vector & b, Vector & x)
{
    PCGSolve<cholesky::LDLT_MIC0<Matrix,Vector> >(A,b,x);

}

}

#endif


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <boost/numeric/odeint.hpp>
#include <smooth/bundle.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/feedback/pid.hpp>
#include <smooth/se2.hpp>

#include <chrono>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

using namespace std::chrono_literals;
using namespace boost::numeric::odeint;

using Time = std::chrono::duration<double>;

int main()
{
  smooth::feedback::PIDParams prm{};
  smooth::feedback::PID<Time, smooth::SE2d> pid(prm);

  // set desired trajectory
  Eigen::Vector3d vdes(1, 0, 0.4);
  auto xdes = [&vdes](Time t) -> decltype(pid)::TrajectoryReturnT {
    return std::make_tuple(
      smooth::SE2d(smooth::SO2d(M_PI_2), Eigen::Vector2d(2.5, 0)) + (t.count() * vdes),
      vdes,
      Eigen::Vector3d::Zero());
  };

  pid.set_xdes(xdes);

  // input variable
  Eigen::Vector2d u;

  // prepare for integrating the closed-loop system
  using State = smooth::Bundle<smooth::SE2d, Eigen::Vector3d>;
  using Deriv = typename State::Tangent;
  runge_kutta4<State, double, Deriv, double, vector_space_algebra> stepper{};
  const auto ode = [&u](const State & x, Deriv & d, double) {
    d.template head<3>() = x.part<1>();
    d.template tail<3>() << u(0), 0, u(1);
  };

  State x(smooth::SE2d::Identity(), Eigen::Vector3d::Zero());
  std::vector<double> tvec, xvec, yvec, u1vec, u2vec;

  // integrate closed-loop system
  for (std::chrono::milliseconds t = 0s; t < 30s; t += 50ms) {
    // compute input
    Eigen::Vector3d a = pid(t, x.part<0>(), x.part<1>());

    // input allocation from desired acceleration
    u(0) = std::clamp<double>(a(0), -1, 1);               // throtte <- a_x
    u(1) = std::clamp<double>(a(2) + 0.3 * a(1), -1, 1);  // steering <- a_Yaw + 0.3 a_y

    // store data
    tvec.push_back(duration_cast<Time>(t).count());
    xvec.push_back(x.part<0>().r2().x());
    yvec.push_back(x.part<0>().r2().y());

    u1vec.push_back(u(0));
    u2vec.push_back(u(1));

    // step dynamics
    stepper.do_step(ode, x, 0, 0.05);
  }

#ifdef ENABLE_PLOTTING
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::title("Path");

  matplot::plot(xvec, yvec)->line_width(2);
  matplot::plot(
    matplot::transform(tvec, [&](auto t) { return std::get<0>(xdes(Time(t))).r2().x(); }),
    matplot::transform(tvec, [&](auto t) { return std::get<0>(xdes(Time(t))).r2().y(); }),
    "k--")
    ->line_width(2);
  matplot::legend({"actual", "desired"});

  matplot::figure();
  matplot::hold(matplot::on);
  matplot::title("Inputs");
  matplot::plot(tvec, u1vec)->line_width(2);
  matplot::plot(tvec, u2vec)->line_width(2);
  matplot::legend({"u1", "u2"});

  matplot::show();
#else
  std::cout << "TRAJECTORY:" << std::endl;
  for (auto i = 0u; i != tvec.size(); ++i) {
    std::cout << "t=" << tvec[i] << ": x=" << xvec[i] << ", y=" << yvec[i] << std::endl;
  }
#endif
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "kkt_inverse.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include "EPS.h"
#include <cstdio>

template <typename T>
IGL_INLINE void igl::kkt_inverse(
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& Aeq,
  const bool use_lu_decomposition,
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& S)
{
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Mat;
        // This threshold seems to matter a lot but I'm not sure how to
        // set it
  const T treshold = igl::FLOAT_EPS;
  //const T treshold = igl::DOUBLE_EPS;

  const int n = A.rows();
  assert(A.cols() == n);
  const int m = Aeq.rows();
  assert(Aeq.cols() == n);

  // Lagrange multipliers method:
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> LM(n + m, n + m);
  LM.block(0, 0, n, n) = A;
  LM.block(0, n, n, m) = Aeq.transpose();
  LM.block(n, 0, m, n) = Aeq;
  LM.block(n, n, m, m).setZero();

  Mat LMpinv;
  if(use_lu_decomposition)
  {
    // if LM is close to singular, use at your own risk :)
    LMpinv = LM.inverse();
  }else
  {
    // use SVD
    typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
    Vec singValues;
    Eigen::JacobiSVD<Mat> svd;
    svd.compute(LM, Eigen::ComputeFullU | Eigen::ComputeFullV );
    const Mat& u = svd.matrixU();
    const Mat& v = svd.matrixV();
    const Vec& singVals = svd.singularValues();

    Vec pi_singVals(n + m);
    int zeroed = 0;
    for (int i=0; i<n + m; i++)
    {
      T sv = singVals(i, 0);
      assert(sv >= 0);
                 // printf("sv: %lg ? %lg\n",(double) sv,(double)treshold);
      if (sv > treshold) pi_singVals(i, 0) = T(1) / sv;
      else
      {
        pi_singVals(i, 0) = T(0);
        zeroed++;
      }
    }

    printf("kkt_inverse : %i singular values zeroed (threshold = %e)\n", zeroed, treshold);
    Eigen::DiagonalMatrix<T, Eigen::Dynamic> pi_diag(pi_singVals);

    LMpinv = v * pi_diag * u.transpose();
  }
  S = LMpinv.block(0, 0, n, n + m);

  //// debug:
  //mlinit(&g_pEngine);
  //
  //mlsetmatrix(&g_pEngine, "A", A);
  //mlsetmatrix(&g_pEngine, "Aeq", Aeq);
  //mlsetmatrix(&g_pEngine, "LM", LM);
  //mlsetmatrix(&g_pEngine, "u", u);
  //mlsetmatrix(&g_pEngine, "v", v);
  //MatrixXd svMat = singVals;
  //mlsetmatrix(&g_pEngine, "singVals", svMat);
  //mlsetmatrix(&g_pEngine, "LMpinv", LMpinv);
  //mlsetmatrix(&g_pEngine, "S", S);

  //int hu = 1;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::kkt_inverse<double>(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, bool, Eigen::Matrix<double, -1, -1, 0, -1, -1>&);
#endif


#include "FEAnalyzer.h"

#include <math.h>
#include <algorithm>
#include <array>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <QQuickItem>
#include <QVariant>
#include <QVariantList>
#include <QQmlProperty>

struct InputNode
{
    Vec2f pos; // a vector2 with the (x , y) postion of the node in world space in m
    bool isSupport; // true if input node cannot move
};

struct InputMember
{
    float area; // cross sectional area of the member in m^2
    float youngsModulus; // the youngs modulus of the member in (N / m^2) or Pa
    unsigned nodeI; // the starting node index of the member
    unsigned nodeJ; // the end node index of the member
};

struct Output {
    std::vector<Vec2f> nodeOffsets;
    std::vector<float> memberStressIndicator;
    Eigen::VectorXf stressForces;
    Eigen::VectorXf error;
};

struct Node
{
    int id; // the nodes id#
    Vec2f pos; // a vector2 with the (x , y) postion of the node in world space in m
    bool isSupport; // true if input node cannot move
};

struct Member
{
    float area; // cross sectioal area of the memmber in m^2
    float youngsModulus; // the youngs modulus of the member in (N/m^2) or Pa
    Node nodeI; // the starting node of the member
    Node nodeJ; // the end node of the member
    float length; // the length of the member in m
    float theta; // the angle of the beam
    std::array<unsigned, 4> dofs; // global degrees of freedom
    // values above this line should be supplied when passing into ComputeDisplacements
    float stress; // will be the stress in the member in (N/m^2)
    Eigen::MatrixXf k_global; // the local stiffness matrix
    Eigen::MatrixXf L; // transformation Matrix
};

const float m_per_px = 1.0f;

float pixelToWorldX(float x) {
    return x * m_per_px;
}

float worldToPixelOffsetX(float x) {
    return x / m_per_px;
}

float pixelToWorldY(float y) {
    return (900 - y) * m_per_px;
}

float worldToPixelOffsetY(float y) {
    return -y / m_per_px;
}

Output computeDisplacements(std::vector<InputNode> inNodes,
                            std::vector<InputMember> inMembers,
                            Eigen::VectorXf externalForces) {

    std::vector<Node> nodes;
    for (unsigned i = 0; i < inNodes.size(); ++i) {
        InputNode in = inNodes[i];
        Node node;
        node.id = i;
        node.pos = in.pos;
        node.isSupport = in.isSupport;
        nodes.push_back(node);
    }

    std::vector<Member> members;
    for (unsigned i = 0; i < inMembers.size(); ++i) {
        InputMember in = inMembers[i];
        Member m;
        m.area = in.area;
        m.youngsModulus = in.youngsModulus;
        m.nodeI = nodes[in.nodeI];
        m.nodeJ = nodes[in.nodeJ];
        Vec2f memberVector = m.nodeJ.pos - m.nodeI.pos;
        m.length = magnitude(memberVector);
        m.theta = atan2f(memberVector.y, memberVector.x);
        m.dofs = {
            in.nodeI * 2,
            in.nodeI * 2 + 1,
            in.nodeJ * 2,
            in.nodeJ * 2 + 1
        };

        float C = cos(m.theta);
        float S = sin(m.theta);
        Eigen::MatrixXf matC(2,4);
        matC << C,S,0,0,
                0,0,C,S;
        // m.L = matC;
        //construct the member stiffness matrixes 'k' for each member
        Eigen::MatrixXf matA(2, 2);
        matA << C*C, C*S, C*S, S*S;
        Eigen::MatrixXf k(4, 4);
        k << matA, -matA, -matA, matA;
        // m.L.transpose();
        m.k_global = (m.area * m.youngsModulus / m.length) * k;
        members.push_back(m);
    }

    Eigen::MatrixXf K(2*nodes.size(), 2*nodes.size());
    K.setZero();  // new matrix size dof, dof, of floats is the struture stiffness matrix
    for (const Member& member : members) {
        //distribute the member stiffnesses to the structure stiffness matrix
        for (unsigned i = 0; i < 4; i++) {
            for (unsigned j = 0; j < 4; j++) {
                int A = member.dofs[i];
                int B = member.dofs[j];
                K(A, B) += member.k_global(i, j);
            }
        }
    }

    Eigen::VectorXf& F = externalForces;
    for (const Node& node : nodes) {
        if (node.isSupport) {
            K.row(node.id * 2).setZero();
            K.col(node.id * 2).setZero();
            K.row(node.id * 2 + 1).setZero();
            K.col(node.id * 2 + 1).setZero();

            F.row(node.id * 2).setZero();
            F.row(node.id * 2 + 1).setZero();
        }
    }

    // solve [K]{u}={F}
    Eigen::VectorXf u = K.fullPivLu().solve(F);

    for (unsigned i = 0; i < members.size(); ++i) {
        Member& m = members[i];
        Vec2f displacementI = { u(m.nodeI.id * 2), u(m.nodeI.id * 2 + 1) };
        Vec2f displacementJ = { u(m.nodeJ.id * 2), u(m.nodeJ.id * 2 + 1) };
        Vec2f totalDisplacement = displacementJ - displacementI;
        Vec2f memberVector = m.nodeJ.pos - m.nodeI.pos;
        Vec2f memberDirection = memberVector / magnitude(memberVector); // todo: ensure members are not 0-length
        float projectedDisplacement = dot(totalDisplacement, memberDirection);
        m.stress = projectedDisplacement * m.youngsModulus / m.length;
    }

    Output o;
    o.nodeOffsets.resize(nodes.size());
    for (unsigned i = 0; i < o.nodeOffsets.size(); ++i) {
        o.nodeOffsets[i] = { u(i * 2), u(i * 2 + 1) };
    }
    o.memberStressIndicator.resize(members.size());
    for (unsigned i = 0; i < o.memberStressIndicator.size(); ++i) {
        o.memberStressIndicator[i] = members[i].stress;
    }
    Eigen::VectorXf stressForces(2*nodes.size());
    stressForces.setZero();
    for (unsigned i = 0; i < members.size(); ++i) {
        const Member& member = members[i];
        float alongBeamForce = member.area * member.stress;
        float xForce = cosf(member.theta) * alongBeamForce;
        float yForce = sinf(member.theta) * alongBeamForce;
        stressForces(2*member.nodeI.id) += xForce;
        stressForces(2*member.nodeI.id+1) += yForce;
        stressForces(2*member.nodeJ.id) += -xForce;
        stressForces(2*member.nodeJ.id+1) += -yForce;
    }
    o.stressForces = stressForces;
    o.error = F - (K*u);
    return o;
}

template <class T>
int index_of(const std::vector<T>& v, const T& value) {
    auto it = std::find(v.cbegin(), v.cend(), value);
    if (it == v.cend()) {
        return -1;
    } else {
        return std::distance(v.cbegin(), it);
    }
}

void addInitialBeamWeightToNodes(Eigen::VectorXf& forces,
  const std::vector<InputNode>& inNodes,
  const std::vector<InputMember>& inMembers) {
  const float density = 7850.0f; // kg/m^3
  const float g = -9.81f; //gravity m/s^2 in the y axis
  for (unsigned i = 0; i < inMembers.size(); ++i) {
    Vec2f v = inNodes[inMembers[i].nodeJ].pos - inNodes[inMembers[i].nodeI].pos;
    float length = magnitude(v);
    float mass = density * length * inMembers[i].area;
    float force = mass * g;
    float force_per_node = force / 2;
    forces(2*inMembers[i].nodeI+1) += force_per_node;
    forces(2*inMembers[i].nodeJ+1) += force_per_node;
  }
}

struct Input {
    std::vector<InputNode> nodes;
    std::vector<InputMember> members;
};

Input extractInput(const QVariantList& nodes,
                   const QVariantList& beams) {
    std::vector<InputNode> inNodes;
    std::vector<QQuickItem*> items;
    foreach(QVariant v, nodes) {
        QQuickItem* node = qobject_cast<QQuickItem*>(v.value<QObject*>());
        items.push_back(node);
        InputNode n;
        QVariant support = QQmlProperty::read(node, QStringLiteral("structural"));
        n.isSupport = support.toBool();
        QVariant x = QQmlProperty::read(node, QStringLiteral("x"));
        n.pos.x = pixelToWorldX(x.toReal());
        QVariant y = QQmlProperty::read(node, QStringLiteral("y"));
        n.pos.y = pixelToWorldY(y.toReal());
        inNodes.push_back(n);
    }

    std::vector<InputMember> inMembers;
    foreach(QVariant v, beams) {
        QQuickItem* beam = qobject_cast<QQuickItem*>(v.value<QObject*>());

        QVariant lav = QQmlProperty::read(beam, QStringLiteral("leftAnchor"));
        QQuickItem* la = qobject_cast<QQuickItem*>(lav.value<QObject*>());
        int left_index = index_of(items, la);
        QVariant rav = QQmlProperty::read(beam, QStringLiteral("rightAnchor"));
        QQuickItem* ra = qobject_cast<QQuickItem*>(rav.value<QObject*>());
        int right_index = index_of(items, ra);
        InputMember m;
        m.area = 10100.f / 1e6f;
        m.youngsModulus = 200.f * 1e9f;
        m.nodeI = (unsigned)left_index;
        m.nodeJ = (unsigned)right_index;
        inMembers.push_back(m);
    }

    return { inNodes, inMembers };
}

FEAnalyzer::FEAnalyzer() {
    in_ = new Input;
    relaxation_ = 1.f;
}

FEAnalyzer::~FEAnalyzer() {
    delete in_;
}

void FEAnalyzer::applyOutputToInput(const Output& o) {
    for (unsigned i = 0; i < o.nodeOffsets.size(); ++i) {
        in_->nodes[i].pos += (o.nodeOffsets[i] * relaxation_);
    }
}

void FEAnalyzer::processBridge(const QVariantList& nodes,
                               const QVariantList& beams) {
    *in_ = extractInput(nodes, beams);
    Eigen::VectorXf forces(2 * nodes.size());
    forces.setZero();
    addInitialBeamWeightToNodes(forces, in_->nodes, in_->members);
    gravityForces_ = forces;
    Output o = computeDisplacements(in_->nodes, in_->members, forces);
    stressForces_ = o.stressForces;
    applyOutputToInput(o);
    emitCompleted(o);
}

void FEAnalyzer::emitCompleted(const Output& o) {
    QVariantList nodeOffsets;
    for (unsigned i = 0; i < o.nodeOffsets.size(); ++i) {
        QVariantMap offset;
        offset.insert("x", worldToPixelOffsetX(o.nodeOffsets[i].x));
        offset.insert("y", worldToPixelOffsetY(o.nodeOffsets[i].y));
        nodeOffsets.append(QVariant::fromValue(offset));
    }
    QVariantList beamStress;
    for (unsigned i = 0; i < o.memberStressIndicator.size(); ++i) {
        beamStress.append(QVariant::fromValue(o.memberStressIndicator[i]));
    }
    emit processingComplete(nodeOffsets, beamStress);

    // check for success or failure
    bool onlyLowError = std::all_of(
        o.error.data(),
        o.error.data() + o.error.rows(), [](float forceError){
        return fabsf(forceError) < 10.f;
    });
    bool onlyLowStress = std::all_of(
        o.memberStressIndicator.cbegin(),
        o.memberStressIndicator.cend(), [](float stress){
        return fabsf(stress) < 3.5e8;
    });
    bool onlySmallOffsets = std::all_of(
        o.nodeOffsets.cbegin(),
        o.nodeOffsets.cend(), [](Vec2f offset){
        return fabsf(offset.x) < 100.f && fabsf(offset.y) < 100.f;
    });

    if (!onlyLowError || !onlyLowStress || !onlySmallOffsets) {
        emit failed();
    } else {
        emit converged();
    }
}

void FEAnalyzer::step() {
    Eigen::VectorXf forces = gravityForces_ + stressForces_;
    Output o = computeDisplacements(in_->nodes, in_->members, forces);
    Eigen::VectorXf stressDifferences = stressForces_ - o.stressForces;
    stressForces_ = o.stressForces;
    applyOutputToInput(o);
    emitCompleted(o);
}

/*
int main()
{
	std::vector<InputNode> nodes = {
		{{0,40}, true},
	  {{20,40}, false},
		{{20,0}, true},
		{{50,0}, true},
	};
	std::vector<InputMember> members = {
		{ 1.5, 1e7, 0, 1 },
		{ 1.0, 1e7, 2, 1 },
		{ 3.0, 1e7, 3, 1 },
	};

	Eigen::VectorXf F(2 * nodes.size());
	F.setZero();
	F(2) = -4000;
	F(3) = -8000;
	Output o = computeDisplacements(nodes, members, F);
	std::cout << "displacements:" << std::endl;
	for (unsigned i = 0; i < o.nodeOffsets.size(); ++i) {
		Vec2f offset = o.nodeOffsets[i];
		std::cout << offset.x << "," << offset.y << std::endl;
	}
	return 0;
}
*/