/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file  Pose2.cpp
 * @brief 2D Pose
 */

#include <gtsam/geometry/concepts.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/base/Testable.h>
#include <boost/foreach.hpp>
#include <cmath>
#include <iostream>
#include <iomanip>

using namespace std;

namespace gtsam {

/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose2);

/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2);

static const Matrix I3 = eye(3), Z12 = zeros(1,2);
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));

/* ************************************************************************* */
Matrix Pose2::matrix() const {
  Matrix R = r_.matrix();
  R = stack(2, &R, &Z12);
  Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0);
  return collect(2, &R, &T);
}

/* ************************************************************************* */
void Pose2::print(const string& s) const {
  cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
}

/* ************************************************************************* */
bool Pose2::equals(const Pose2& q, double tol) const {
  return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
}

/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi) {
  assert(xi.size() == 3);
  Point2 v(xi(0),xi(1));
  double w = xi(2);
  if (std::abs(w) < 1e-10)
    return Pose2(xi[0], xi[1], xi[2]);
  else {
    Rot2 R(Rot2::fromAngle(w));
    Point2 v_ortho = R_PI_2 * v; // points towards rot center
    Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
    return Pose2(R, t);
  }
}

/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
  const Rot2& R = p.r();
  const Point2& t = p.t();
  double w = R.theta();
  if (std::abs(w) < 1e-10)
    return Vector_(3, t.x(), t.y(), w);
  else {
    double c_1 = R.c()-1.0, s = R.s();
    double det = c_1*c_1 + s*s;
    Point2 p = R_PI_2 * (R.unrotate(t) - t);
    Point2 v = (w/det) * p;
    return Vector_(3, v.x(), v.y(), w);
  }
}

/* ************************************************************************* */
Pose2 Pose2::retract(const Vector& v) const {
#ifdef SLOW_BUT_CORRECT_EXPMAP
  return compose(Expmap(v));
#else
  assert(v.size() == 3);
  return compose(Pose2(v[0], v[1], v[2]));
#endif
}

/* ************************************************************************* */
Vector Pose2::localCoordinates(const Pose2& p2) const {
#ifdef SLOW_BUT_CORRECT_EXPMAP
  return Logmap(between(p2));
#else
  Pose2 r = between(p2);
  return Vector_(3, r.x(), r.y(), r.theta());
#endif
}

/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
Matrix Pose2::AdjointMap() const {
  double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
  return Matrix_(3,3,
      c,  -s,   y,
      s,   c,  -x,
      0.0, 0.0, 1.0
  );
}

/* ************************************************************************* */
Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
  if (H1) *H1 = -AdjointMap();
  return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}

/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  Point2 d = point - t_;
  Point2 q = r_.unrotate(d);
  if (!H1 && !H2) return q;
  if (H1) *H1 = Matrix_(2, 3,
      -1.0, 0.0,  q.y(),
      0.0, -1.0, -q.x());
  if (H2) *H2 = r_.transpose();
  return q;
}

/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  // TODO: inline and reuse?
  if(H1) *H1 = p2.inverse().AdjointMap();
  if(H2) *H2 = I3;
  return (*this)*p2;
}

/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  const Point2 q = r_ * p;
  if (H1 || H2) {
    const Matrix R = r_.matrix();
    const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
    if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
    if (H2) *H2 = R;                         // R
  }
  return q + t_;
}

/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  // get cosines and sines from rotation matrices
  const Rot2& R1 = r_, R2 = p2.r();
  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();

  // Assert that R1 and R2 are normalized
  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);

  // Calculate delta rotation = between(R1,R2)
  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
  Rot2 R(Rot2::atan2(s,c)); // normalizes

  // Calculate delta translation = unrotate(R1, dt);
  Point2 dt = p2.t() - t_;
  double x = dt.x(), y = dt.y();
  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);

  // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
  if (H1) {
    double dt1 = -s2 * x + c2 * y;
    double dt2 = -c2 * x - s2 * y;
    *H1 = Matrix_(3,3,
        -c,  -s,  dt1,
        s,  -c,  dt2,
        0.0, 0.0,-1.0);
  }
  if (H2) *H2 = I3;

  return Pose2(R,t);
}

/* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  Point2 d = transform_to(point, H1, H2);
  if (!H1 && !H2) return Rot2::relativeBearing(d);
  Matrix D_result_d;
  Rot2 result = Rot2::relativeBearing(d, D_result_d);
  if (H1) *H1 = D_result_d * (*H1);
  if (H2) *H2 = D_result_d * (*H2);
  return result;
}

/* ************************************************************************* */
Rot2 Pose2::bearing(const Pose2& point,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  Rot2 result = bearing(point.t(), H1, H2);
  if (H2) {
    Matrix H2_ = *H2 * point.r().matrix();
    *H2 = zeros(1, 3);
    insertSub(*H2, H2_, 0, 0);
  }
  return result;
}

/* ************************************************************************* */
double Pose2::range(const Point2& point,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  Point2 d = point - t_;
  if (!H1 && !H2) return d.norm();
  Matrix H;
  double r = d.norm(H);
  if (H1) *H1 = H * Matrix_(2, 3,
      -r_.c(),  r_.s(),  0.0,
      -r_.s(), -r_.c(),  0.0);
  if (H2) *H2 = H;
  return r;
}

/* ************************************************************************* */
double Pose2::range(const Pose2& pose2,
    boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  Point2 d = pose2.t() - t_;
  if (!H1 && !H2) return d.norm();
  Matrix H;
  double r = d.norm(H);
  if (H1) *H1 = H * Matrix_(2, 3,
      -r_.c(),  r_.s(),  0.0,
      -r_.s(), -r_.c(),  0.0);
  if (H2) *H2 = H * Matrix_(2, 3,
      pose2.r_.c(), -pose2.r_.s(),  0.0,
      pose2.r_.s(),  pose2.r_.c(),  0.0);
  return r;
}

/* *************************************************************************
 * New explanation, from scan.ml
 * It finds the angle using a linear method:
 * q = Pose2::transform_from(p) = t + R*p
 * We need to remove the centroids from the data to find the rotation
 * using dp=[dpx;dpy] and q=[dqx;dqy] we have
 *  |dqx|   |c  -s|     |dpx|     |dpx -dpy|     |c|
 *  |   | = |     |  *  |   |  =  |        |  *  | | = H_i*cs
 *  |dqy|   |s   c|     |dpy|     |dpy  dpx|     |s|
 * where the Hi are the 2*2 matrices. Then we will minimize the criterion
 * J = \sum_i norm(q_i - H_i * cs)
 * Taking the derivative with respect to cs and setting to zero we have
 * cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i)
 * The hessian is diagonal and just divides by a constant, but this
 * normalization constant is irrelevant, since we take atan2.
 * i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy)
 * The translation is then found from the centroids
 * as they also satisfy cq = t + R*cp, hence t = cq - R*cp
 */

boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {

  size_t n = pairs.size();
  if (n<2) return boost::none; // we need at least two pairs

  // calculate centroids
  Point2 cp,cq;
  BOOST_FOREACH(const Point2Pair& pair, pairs) {
    cp += pair.first;
    cq += pair.second;
  }
  double f = 1.0/n;
  cp *= f; cq *= f;

  // calculate cos and sin
  double c=0,s=0;
  BOOST_FOREACH(const Point2Pair& pair, pairs) {
    Point2 dq = pair.first  - cp;
    Point2 dp = pair.second - cq;
    c +=  dp.x() * dq.x() + dp.y() * dq.y();
    s +=  dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-(
  }

  // calculate angle and translation
  double theta = atan2(s,c);
  Rot2 R = Rot2::fromAngle(theta);
  Point2 t = cq - R*cp;
  return Pose2(R, t);
}

/* ************************************************************************* */
} // namespace gtsam


#include <catch.hpp>

#include <boost/concept_check.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/container_hash/hash.hpp>

#include <map>
#include <vector>
#include <exception>
#include <functional>

#define UNSAFE_FUNCTION_GRAPH

#include "functional_graph.h"
#include "EuclideanDistanceFunctor.h"
#include "map_property_map.h"

BOOST_CONCEPT_ASSERT((boost::BidirectionalGraphConcept<functional_graph<float>>));

namespace std {
template<typename A, typename B>
struct hash<std::pair<A, B>> {
    size_t operator()(std::pair<A, B> val) const {
        size_t seed = 0;
        boost::hash_combine(seed, val.first);
        boost::hash_combine(seed, val.second);
        return seed;
    }
};
}

TEST_CASE("Edge equality") {
    edge<int> a = {1, 2};
    edge<int> b = {1, 2};
    edge<int> c = {2, 1};
    REQUIRE(a == b);
    REQUIRE(b == a);
    REQUIRE(!(a == c));
    REQUIRE(!(c == a));
    REQUIRE(!(b == c));
    REQUIRE(!(c == b));
}

TEST_CASE("Edge inequality") {
    edge<int> a = {4, -1};
    edge<int> b = {4, -1};
    edge<int> c = {-11, -1};
    REQUIRE(a != c);
    REQUIRE(c != a);
    REQUIRE(b != c);
    REQUIRE(c != b);
    REQUIRE(!(a != b));
    REQUIRE(!(b != a));
}

using namespace boost;
typedef std::pair<float, float> V;
typedef functional_graph<V> G;
typedef edge<V> E;
typedef graph_traits<G>::out_edge_iterator OeItr;
typedef graph_traits<G>::in_edge_iterator IeItr;

std::vector<E> adj(V v) {
    return {
        {v, {v.first, v.second + 1}},
        {v, {v.first + 1, v.second}},
        {v, {v.first, v.second - 1}},
        {v, {v.first - 1, v.second}}
    };
};

std::vector<E> empty(V v) {
    return {};
}

std::vector<E> left(V v) {
    return { {v, {v.first - 1, v.second}} };
}

std::vector<E> right(V v) {
    return { {v, {v.first + 1, v.second}} };
}

TEST_CASE("Out edges") {
    G g(left, adj);
    V source = {0, 0};
    OeItr begin, end;
    tie(begin, end) = out_edges(source, g);
    edge<V> up = {source, {0, 1}};
    edge<V> down = {source, {0, -1}};
    edge<V> left = {source, {-1, 0}};
    edge<V> right = {source, {1, 0}};
    std::vector<edge<V>> edges(begin, end);
    REQUIRE(edges.size() == 4);
    REQUIRE(std::find(begin, end, up) != end);
    REQUIRE(std::find(begin, end, down) != end);
    REQUIRE(std::find(begin, end, left) != end);
    REQUIRE(std::find(begin, end, right) != end);
}

TEST_CASE("In edges") {
    G g(adj, left);
    V source = {0, 0};
    IeItr begin, end;
    tie(begin, end) = in_edges(source, g);
    edge<V> up = {source, {0, 1}};
    edge<V> down = {source, {0, -1}};
    edge<V> left = {source, {-1, 0}};
    edge<V> right = {source, {1, 0}};
    std::vector<edge<V>> edges(begin, end);
    REQUIRE(edges.size() == 4);
    REQUIRE(std::find(begin, end, up) != end);
    REQUIRE(std::find(begin, end, down) != end);
    REQUIRE(std::find(begin, end, left) != end);
    REQUIRE(std::find(begin, end, right) != end);
}

TEST_CASE("Source") {
    G g(adj, adj);
    V a = {0, 0};
    E edge = {a, {1, 1}};
    V v = source(edge, g);
    REQUIRE(v == a);
}

TEST_CASE("Target") {
    G g(adj, adj);
    V a = {0, 0};
    E edge = {{1, 1}, a};
    V v = target(edge, g);
    REQUIRE(v == a);
}

TEST_CASE("Out degree") {
    G g(adj, adj);
    V v = {0, 0};
    REQUIRE(out_degree(v, g) == 4);
}

TEST_CASE("In degree") {
    G g(adj, adj);
    V v = {0, 0};
    REQUIRE(in_degree(v, g) == 4);
}

TEST_CASE("Degree") {
    G g(adj, adj);
    V v = {0, 0};
    REQUIRE(degree(v, g) == 8);
}

struct goal_found {};

class goal_terminating_visitor : public default_astar_visitor {
private:
    const V _target;
public:
    goal_terminating_visitor(V v) : _target(v) {};
    void finish_vertex(V u, G g) {
        if (u == _target) {
            throw goal_found();
        }
    };
};

float dist(V src, V dst) {
    float dx = src.first - dst.first;
    float dy = src.second - dst.second;
    return sqrt(dx * dx + dy * dy);
}

TEST_CASE("A* over functional graph") {
    G g(adj, adj);
    V start = {0, 0};
    V goal = {10, 10};

    std::function<float(E)> w_map = [&g](E e){
        V src = source(e, g);
        V dst = target(e, g);
        return dist(src, dst);
    };

    map_property_map<V, V> pred_map;
    map_property_map<V, float> dist_map(INFINITY);
    dist_map[start] = 0.0f;
    map_property_map<V, float> r_map;
    map_property_map<V, unsigned int> vi_map;
    map_property_map<V, default_color_type> c_map;

    goal_terminating_visitor vis(goal);

    REQUIRE_THROWS_AS(
        astar_search_no_init(
            g,
            start,
            [&goal](V v) {
                float d = dist(v, goal);
                return d;
            },
            visitor(vis).
            predecessor_map(pred_map).
            rank_map(r_map).
            distance_map(dist_map).
            weight_map(make_function_property_map<E>(w_map)).
            color_map(c_map).
            vertex_index_map(vi_map)
        ),
        goal_found
    );
}


/* ----------------------------------------------------------------------------
 * Copyright 2020, Massachusetts Institute of Technology, * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Yulun Tian, et al. (see README for the full author list)
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

#include <DPGO/DPGO_utils.h>
#include <DPGO/DPGO_robust.h>
#include <Eigen/Geometry>
#include <Eigen/SPQRSupport>
#include <algorithm>
#include <fstream>
#include <iostream>
#include <random>
#include <cassert>
#include <boost/math/distributions/chi_squared.hpp>

namespace DPGO {

void writeMatrixToFile(const Matrix &M, const std::string &filename) {
  std::ofstream file;
  file.open(filename);
  if (!file.is_open()) {
    printf("Cannot write to specified file: %s\n", filename.c_str());
    return;
  }
  const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", "\n");
  file << M.format(CSVFormat);
  file.close();
}

void writeSparseMatrixToFile(const SparseMatrix &M, const std::string &filename) {
  std::ofstream file;
  file.open(filename);
  if (!file.is_open()) {
    printf("Cannot write to specified file: %s\n", filename.c_str());
    return;
  }

  for (int k = 0; k < M.outerSize(); ++k) {
    for (SparseMatrix::InnerIterator it(M, k); it; ++it) {
      file << it.row() << ",";
      file << it.col() << ",";
      file << it.value() << "\n";
    }
  }
  file.close();
}

/**
###############################################################
###############################################################
The following implementations are originally implemented in:

SE-Sync: https://github.com/david-m-rosen/SE-Sync.git

Cartan-Sync: https://bitbucket.org/jesusbriales/cartan-sync/src

###############################################################
###############################################################
*/

std::vector<RelativeSEMeasurement> read_g2o_file(const std::string &filename,
                                                 size_t &num_poses) {
  // Preallocate output vector
  std::vector<DPGO::RelativeSEMeasurement> measurements;

  // A single measurement, whose values we will fill in
  DPGO::RelativeSEMeasurement measurement;
  measurement.weight = 1.0;

  // A string used to contain the contents of a single line
  std::string line;

  // A string used to extract tokens from each line one-by-one
  std::string token;

  // Preallocate various useful quantities
  double dx, dy, dz, dtheta, dqx, dqy, dqz, dqw, I11, I12, I13, I14, I15, I16,
      I22, I23, I24, I25, I26, I33, I34, I35, I36, I44, I45, I46, I55, I56, I66;

  size_t i, j;

  // Open the file for reading
  std::ifstream infile(filename);

  num_poses = 0;

  while (std::getline(infile, line)) {
    // Construct a stream from the string
    std::stringstream strstrm(line);

    // Extract the first token from the string
    strstrm >> token;

    if (token == "EDGE_SE2") {
      // This is a 2D pose measurement

      /** The g2o format specifies a 2D relative pose measurement in the
       * following form:
       *
       * EDGE_SE2 id1 id2 dx dy dtheta, I11, I12, I13, I22, I23, I33
       *
       */

      // Extract formatted output
      strstrm >> i >> j >> dx >> dy >> dtheta >> I11 >> I12 >> I13 >> I22 >>
              I23 >> I33;

      // Fill in elements of this measurement

      // Pose ids
      measurement.r1 = 0;
      measurement.r2 = 0;
      measurement.p1 = i;
      measurement.p2 = j;

      // Raw measurements
      measurement.t = Eigen::Matrix<double, 2, 1>(dx, dy);
      measurement.R = Eigen::Rotation2Dd(dtheta).toRotationMatrix();

      Eigen::Matrix2d TranCov;
      TranCov << I11, I12, I12, I22;
      measurement.tau = 2 / TranCov.inverse().trace();

      measurement.kappa = I33;

    } else if (token == "EDGE_SE3:QUAT") {
      // This is a 3D pose measurement

      /** The g2o format specifies a 3D relative pose measurement in the
       * following form:
       *
       * EDGE_SE3:QUAT id1, id2, dx, dy, dz, dqx, dqy, dqz, dqw
       *
       * I11 I12 I13 I14 I15 I16
       *     I22 I23 I24 I25 I26
       *         I33 I34 I35 I36
       *             I44 I45 I46
       *                 I55 I56
       *                     I66
       */

      // Extract formatted output
      strstrm >> i >> j >> dx >> dy >> dz >> dqx >> dqy >> dqz >> dqw >> I11 >>
              I12 >> I13 >> I14 >> I15 >> I16 >> I22 >> I23 >> I24 >> I25 >> I26 >>
              I33 >> I34 >> I35 >> I36 >> I44 >> I45 >> I46 >> I55 >> I56 >> I66;

      // Fill in elements of the measurement

      // Pose ids
      measurement.r1 = 0;
      measurement.r2 = 0;
      measurement.p1 = i;
      measurement.p2 = j;

      // Raw measurements
      measurement.t = Eigen::Matrix<double, 3, 1>(dx, dy, dz);
      measurement.R = Eigen::Quaterniond(dqw, dqx, dqy, dqz).toRotationMatrix();

      // Compute precisions

      // Compute and store the optimal (information-divergence-minimizing) value
      // of the parameter tau
      Eigen::Matrix3d TranCov;
      TranCov << I11, I12, I13, I12, I22, I23, I13, I23, I33;
      measurement.tau = 3 / TranCov.inverse().trace();

      // Compute and store the optimal (information-divergence-minimizing value
      // of the parameter kappa

      Eigen::Matrix3d RotCov;
      RotCov << I44, I45, I46, I45, I55, I56, I46, I56, I66;
      measurement.kappa = 3 / (2 * RotCov.inverse().trace());

    } else if ((token == "VERTEX_SE2") || (token == "VERTEX_SE3:QUAT")) {
      // This is just initialization information, so do nothing
      continue;
    } else {
      std::cout << "Error: unrecognized type: " << token << "!" << std::endl;
      assert(false);
    }

    // Update maximum value of poses found so far
    size_t max_pair = std::max<double>(measurement.p1, measurement.p2);

    num_poses = ((max_pair > num_poses) ? max_pair : num_poses);
    measurements.push_back(measurement);
  }  // while

  infile.close();

  num_poses++;  // Account for the use of zero-based indexing

  return measurements;
}

void constructOrientedConnectionIncidenceMatrixSE(
    const std::vector<RelativeSEMeasurement> &measurements, SparseMatrix &AT,
    DiagonalMatrix &OmegaT) {
  // Deduce graph dimensions from measurements
  size_t d;  // Dimension of Euclidean space
  d = (!measurements.empty() ? measurements[0].t.size() : 0);
  size_t dh = d + 1;  // Homogenized dimension of Euclidean space
  size_t m;           // Number of measurements
  m = measurements.size();
  size_t n = 0;  // Number of poses
  for (const RelativeSEMeasurement &meas: measurements) {
    if (n < meas.p1) n = meas.p1;
    if (n < meas.p2) n = meas.p2;
  }
  n++;  // Account for 0-based indexing: node indexes go from 0 to max({i,j})

  // Define connection incidence matrix dimensions
  // This is a [n x m] (dh x dh)-block matrix
  size_t rows = (d + 1) * n;
  size_t cols = (d + 1) * m;

  // We use faster ordered insertion, as suggested in
  // https://eigen.tuxfamily.org/dox/group__TutorialSparse.html#TutorialSparseFilling
  Eigen::SparseMatrix<double, Eigen::ColMajor> A(rows, cols);
  A.reserve(Eigen::VectorXi::Constant(cols, 8));
  DiagonalMatrix Omega(cols);  // One block per measurement: (d+1)*m
  DiagonalMatrix::DiagonalVectorType &diagonal = Omega.diagonal();

  // Insert actual measurement values
  size_t i, j;
  for (size_t k = 0; k < m; k++) {
    const RelativeSEMeasurement &meas = measurements[k];
    i = meas.p1;
    j = meas.p2;

    /// Assign SE(d) matrix to block leaving node i
    /// AT(i,k) = -Tij (NOTE: NEGATIVE)
    // Do it column-wise for speed
    // Elements of rotation
    for (size_t c = 0; c < d; c++)
      for (size_t r = 0; r < d; r++)
        A.insert(i * dh + r, k * dh + c) = -meas.R(r, c);

    // Elements of translation
    for (size_t r = 0; r < d; r++)
      A.insert(i * dh + r, k * dh + d) = -meas.t(r);

    // Additional 1 for homogeneization
    A.insert(i * dh + d, k * dh + d) = -1;

    /// Assign (d+1)-identity matrix to block leaving node j
    /// AT(j,k) = +I (NOTE: POSITIVE)
    for (size_t r = 0; r < d + 1; r++) A.insert(j * dh + r, k * dh + r) = +1;

    /// Assign isotropic weights in diagonal matrix
    for (size_t r = 0; r < d; r++) diagonal[k * dh + r] = meas.weight * meas.kappa;

    diagonal[k * dh + d] = meas.weight * meas.tau;
  }

  A.makeCompressed();

  AT = A;
  OmegaT = Omega;
}

SparseMatrix constructConnectionLaplacianSE(
    const std::vector<RelativeSEMeasurement> &measurements) {
  SparseMatrix AT;
  DiagonalMatrix OmegaT;
  constructOrientedConnectionIncidenceMatrixSE(measurements, AT, OmegaT);
  return AT * OmegaT * AT.transpose();
}

void constructBMatrices(const std::vector<RelativeSEMeasurement> &measurements, SparseMatrix &B1,
                        SparseMatrix &B2, SparseMatrix &B3) {
  // Clear input matrices
  B1.setZero();
  B2.setZero();
  B3.setZero();

  size_t num_poses = 0;
  size_t d = (!measurements.empty() ? measurements[0].t.size() : 0);

  std::vector<Eigen::Triplet<double>> triplets;

  // Useful quantities to cache
  size_t d2 = d * d;
  size_t d3 = d * d * d;

  size_t i, j; // Indices for the tail and head of the given measurement
  double sqrttau;
  size_t max_pair;

  /// Construct the matrix B1 from equation (69a) in the tech report
  triplets.reserve(2 * d * measurements.size());

  for (size_t e = 0; e < measurements.size(); e++) {
    i = measurements[e].p1;
    j = measurements[e].p2;
    sqrttau = sqrt(measurements[e].tau);

    // Block corresponding to the tail of the measurement
    for (size_t l = 0; l < d; l++) {
      triplets.emplace_back(e * d + l, i * d + l,
                            -sqrttau); // Diagonal element corresponding to tail
      triplets.emplace_back(e * d + l, j * d + l,
                            sqrttau); // Diagonal element corresponding to head
    }

    // Keep track of the number of poses we've seen
    max_pair = std::max<size_t>(i, j);
    if (max_pair > num_poses)
      num_poses = max_pair;
  }
  num_poses++; // Account for zero-based indexing

  B1.resize(d * measurements.size(), d * num_poses);
  B1.setFromTriplets(triplets.begin(), triplets.end());

  /// Construct matrix B2 from equation (69b) in the tech report
  triplets.clear();
  triplets.reserve(d2 * measurements.size());

  for (size_t e = 0; e < measurements.size(); e++) {
    i = measurements[e].p1;
    sqrttau = sqrt(measurements[e].tau);
    for (size_t k = 0; k < d; k++)
      for (size_t r = 0; r < d; r++)
        triplets.emplace_back(d * e + r, d2 * i + d * k + r,
                              -sqrttau * measurements[e].t(k));
  }

  B2.resize(d * measurements.size(), d2 * num_poses);
  B2.setFromTriplets(triplets.begin(), triplets.end());

  /// Construct matrix B3 from equation (69c) in the tech report
  triplets.clear();
  triplets.reserve((d3 + d2) * measurements.size());

  for (size_t e = 0; e < measurements.size(); e++) {
    double sqrtkappa = std::sqrt(measurements[e].kappa);
    const Matrix &R = measurements[e].R;

    for (size_t r = 0; r < d; r++)
      for (size_t c = 0; c < d; c++) {
        i = measurements[e].p1; // Tail of measurement
        j = measurements[e].p2; // Head of measurement

        // Representation of the -sqrt(kappa) * Rt(i,j) \otimes I_d block
        for (size_t l = 0; l < d; l++)
          triplets.emplace_back(e * d2 + d * r + l, i * d2 + d * c + l,
                                -sqrtkappa * R(c, r));
      }

    for (size_t l = 0; l < d2; l++)
      triplets.emplace_back(e * d2 + l, j * d2 + l, sqrtkappa);
  }

  B3.resize(d2 * measurements.size(), d2 * num_poses);
  B3.setFromTriplets(triplets.begin(), triplets.end());
}

Matrix chordalInitialization(
    size_t dimension, size_t num_poses,
    const std::vector<RelativeSEMeasurement> &measurements) {
  SparseMatrix B1, B2, B3;
  constructBMatrices(measurements, B1, B2, B3);

  // Recover rotations
  size_t d = (!measurements.empty() ? measurements[0].t.size() : 0);
  unsigned int d2 = d * d;
  assert(dimension == d);
  assert(num_poses == (unsigned) B3.cols() / d2);

  SparseMatrix B3red = B3.rightCols((num_poses - 1) * d2);
  B3red.makeCompressed();  // Must be in compressed format to use
  // Eigen::SparseQR!

  // Vectorization of I_d
  Eigen::MatrixXd Id = Eigen::MatrixXd::Identity(d, d);
  Eigen::Map<Eigen::VectorXd> Id_vec(Id.data(), d2);

  Eigen::VectorXd cR = B3.leftCols(d2) * Id_vec;

  Eigen::VectorXd rvec;
  Eigen::SPQR<SparseMatrix> QR(B3red);
  rvec = -QR.solve(cR);

  Matrix Rchordal(d, d * num_poses);
  Rchordal.leftCols(d) = Id;
  Rchordal.rightCols((num_poses - 1) * d) =
      Eigen::Map<Eigen::MatrixXd>(rvec.data(), d, (num_poses - 1) * d);
  for (unsigned int i = 1; i < num_poses; i++)
    Rchordal.block(0, i * d, d, d) =
        projectToRotationGroup(Rchordal.block(0, i * d, d, d));

  // Recover translation
  Matrix tchordal = recoverTranslations(B1, B2, Rchordal);
  assert((unsigned) tchordal.rows() == dimension);
  assert((unsigned) tchordal.cols() == num_poses);

  // Assemble full pose
  Matrix Tchordal(d, num_poses * (d + 1));
  for (size_t i = 0; i < num_poses; i++) {
    Tchordal.block(0, i * (d + 1), d, d) = Rchordal.block(0, i * d, d, d);
    Tchordal.block(0, i * (d + 1) + d, d, 1) = tchordal.block(0, i, d, 1);
  }

  return Tchordal;
}

Matrix odometryInitialization(size_t dimension, size_t num_poses, const std::vector<RelativeSEMeasurement> &odometry) {
  size_t d = dimension;
  size_t n = num_poses;

  Matrix T(d, n * (d + 1));
  // Initialize first pose to be identity
  T.block(0, 0, d, d) = Matrix::Identity(d, d);
  T.block(0, d, d, 1) = Matrix::Zero(d, 1);
  for (size_t src = 0; src < odometry.size(); ++src) {
    size_t dst = src + 1;
    const RelativeSEMeasurement &m = odometry[src];
    assert(m.p1 == src);
    assert(m.p2 == dst);
    Matrix Rsrc = T.block(0, src * (d + 1), d, d);
    Matrix tsrc = T.block(0, src * (d + 1) + d, d, 1);
    Matrix Rdst = Rsrc * m.R;
    Matrix tdst = tsrc + Rsrc * m.t;
    T.block(0, dst * (d + 1), d, d) = Rdst;
    T.block(0, dst * (d + 1) + d, d, 1) = tdst;
  }
  return T;
}

Matrix recoverTranslations(const SparseMatrix &B1, const SparseMatrix &B2,
                           const Matrix &R) {
  unsigned int d = R.rows();
  unsigned int n = R.cols() / d;

  // Vectorization of R matrix
  Eigen::Map<Eigen::VectorXd> rvec((double *) R.data(), d * d * n);

  // Form the matrix comprised of the right (n-1) block columns of B1
  SparseMatrix B1red = B1.rightCols(d * (n - 1));

  Eigen::VectorXd c = B2 * rvec;

  // Solve
  Eigen::SPQR<SparseMatrix> QR(B1red);
  Eigen::VectorXd tred = -QR.solve(c);

  // Reshape this result into a d x (n-1) matrix
  Eigen::Map<Eigen::MatrixXd> tred_mat(tred.data(), d, n - 1);

  // Allocate output matrix
  Eigen::MatrixXd t = Eigen::MatrixXd::Zero(d, n);

  // Set rightmost n-1 columns
  t.rightCols(n - 1) = tred_mat;

  return t;
}

Matrix projectToRotationGroup(const Matrix &M) {
  // Compute the SVD of M
  Eigen::JacobiSVD<Matrix> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);

  double detU = svd.matrixU().determinant();
  double detV = svd.matrixV().determinant();

  if (detU * detV > 0) {
    return svd.matrixU() * svd.matrixV().transpose();
  } else {
    Eigen::MatrixXd Uprime = svd.matrixU();
    Uprime.col(Uprime.cols() - 1) *= -1;
    return Uprime * svd.matrixV().transpose();
  }
}

Matrix projectToStiefelManifold(const Matrix &M) {
  size_t r = M.rows();
  size_t d = M.cols();
  assert(r >= d);
  Eigen::JacobiSVD<Matrix> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
  return svd.matrixU() * svd.matrixV().transpose();
}

Matrix fixedStiefelVariable(unsigned d, unsigned r) {
  std::srand(1);
  ROPTLIB::StieVariable var(r, d);
  var.RandInManifold();
  return Eigen::Map<Matrix>((double *) var.ObtainReadData(), r, d);
}

double computeMeasurementError(const RelativeSEMeasurement &m,
                               const Matrix &R1, const Matrix &t1,
                               const Matrix &R2, const Matrix &t2) {
  double rotationErrorSq = (R1 * m.R - R2).squaredNorm();
  double translationErrorSq = (t2 - t1 - R1 * m.t).squaredNorm();
  return m.kappa * rotationErrorSq + m.tau * translationErrorSq;
}

double chi2inv(double quantile, size_t dof) {
  boost::math::chi_squared_distribution<double> chi2(dof);
  return boost::math::quantile(chi2, quantile);
}

double angular2ChordalSO3(double rad) {
  return 2 * sqrt(2) * sin(rad / 2);
}

void checkRotationMatrix(const Matrix &R) {
  const auto d = R.rows();
  assert(R.cols() == d);
  assert(abs(R.determinant() - 1.0) < 1e-8);
  assert((R.transpose() * R - Matrix::Identity(d, d)).norm() < 1e-8);
}

void singleTranslationAveraging(Vector &tOpt,
                                const std::vector<Vector> &tVec,
                                const Vector &tau) {
  const int n = (int) tVec.size();
  assert(n > 0);
  const auto d = tVec[0].rows();
  Vector tau_ = Vector::Ones(n);
  if (tau.rows() == n) {
    tau_ = tau;
  }
  Vector s = Vector::Zero(d);
  double w = 0;
  for (Eigen::Index i = 0; i < n; ++i) {
    s += tau_(i) * tVec[i];
    w += tau_(i);
  }
  tOpt = s / w;
}

void singleRotationAveraging(Matrix &ROpt,
                             const std::vector<Matrix> &RVec,
                             const Vector &kappa) {
  const int n = (int) RVec.size();
  assert(n > 0);
  const auto d = RVec[0].rows();
  Vector kappa_ = Vector::Ones(n);
  if (kappa.rows() == n) {
    kappa_ = kappa;
  }
  Matrix M = Matrix::Zero(d, d);
  for (Eigen::Index i = 0; i < n; ++i) {
    M += kappa_(i) * RVec[i];
  }
  ROpt = projectToRotationGroup(M);
}

void singlePoseAveraging(Matrix &ROpt, Vector &tOpt,
                         const std::vector<Matrix> &RVec,
                         const std::vector<Vector> &tVec,
                         const Vector &kappa,
                         const Vector &tau) {
  assert(!RVec.empty());
  assert(!tVec.empty());
  assert(RVec.size() == tVec.size());
  assert(RVec[0].rows() == tVec[0].rows());
  singleTranslationAveraging(tOpt, tVec, tau);
  singleRotationAveraging(ROpt, RVec, kappa);
}

void robustSingleRotationAveraging(Matrix &ROpt,
                                   std::vector<size_t> &inlierIndices,
                                   const std::vector<Matrix> &RVec,
                                   const Vector &kappa,
                                   double errorThreshold) {
  const double w_tol = 1e-8;
  const int n = (int) RVec.size();
  assert(n > 0);
  Vector kappa_ = Vector::Ones(n);
  Vector weights_ = Vector::Ones(n);
  if (kappa.rows() == n) {
    kappa_ = kappa;
  }
  for (const auto &Ri: RVec) {
    checkRotationMatrix(Ri);
  }
  // Initialize estimate
  singleRotationAveraging(ROpt, RVec, kappa_);
  Vector rSqVec = Vector::Zero(n);
  for (Eigen::Index i = 0; i < n; ++i) {
    rSqVec(i) = kappa_(i) * (ROpt - RVec[i]).squaredNorm();
  }
  // Initialize robust cost
  double barc = errorThreshold;
  double barcSq = barc * barc;
  double muInit = barcSq / (2 * rSqVec.maxCoeff() - barcSq);
  muInit = std::min(muInit, 1e-5);
  // Negative values of initial mu corresponds to small residual errors. In this case skip applying GNC.
  if (muInit > 0) {
    RobustCostParameters params;
    params.GNCBarc = barc;
    params.GNCMaxNumIters = 1000;
    params.GNCInitMu = muInit;
    RobustCost cost(RobustCostType::GNC_TLS, params);
    for (unsigned iter = 0; iter < params.GNCMaxNumIters; ++iter) {
      // Update solution
      singleRotationAveraging(ROpt, RVec, kappa_.cwiseProduct(weights_));
      // Update weight
      int nc = 0;
      for (Eigen::Index i = 0; i < n; ++i) {
        double rSq = kappa_(i) * (ROpt - RVec[i]).squaredNorm();
        double wi = cost.weight(sqrt(rSq));
        if (wi < w_tol || wi > 1 - w_tol) {
          nc++;
        }
        weights_(i) = wi;
      }
      if (nc == n) {
        break;
      }
      // Update GNC
      cost.update();
    }
  }
  // Retrieve inliers
  inlierIndices.clear();
  for (Eigen::Index i = 0; i < n; ++i) {
    double wi = weights_(i);
    if (wi > 1 - w_tol) {
      inlierIndices.push_back(i);
    }
  }
}

void robustSinglePoseAveraging(Matrix &ROpt, Vector &tOpt,
                               std::vector<size_t> &inlierIndices,
                               const std::vector<Matrix> &RVec,
                               const std::vector<Vector> &tVec,
                               const Vector &kappa,
                               const Vector &tau,
                               double errorThreshold) {
  const double w_tol = 1e-8;
  const int n = (int) RVec.size();
  assert(n > 0);
  assert(tVec.size() == n);
  Vector kappa_ = 10000 * Vector::Ones(n);
  Vector tau_ = 100 * Vector::Ones(n);
  Vector weights_ = Vector::Ones(n);
  if (kappa.rows() == n) {
    kappa_ = kappa;
  }
  if (tau.rows() == n) {
    tau_ = tau;
  }
  for (const auto &Ri: RVec) {
    checkRotationMatrix(Ri);
  }
  // Initialize estimate
  singlePoseAveraging(ROpt,
                      tOpt,
                      RVec,
                      tVec,
                      kappa_.cwiseProduct(weights_),
                      tau_.cwiseProduct(weights_));
  Vector rSqVec = Vector::Zero(n);
  for (Eigen::Index i = 0; i < n; ++i) {
    rSqVec(i) = kappa_(i) * (ROpt - RVec[i]).squaredNorm() + tau_(i) * (tOpt - tVec[i]).squaredNorm();
  }
  // Initialize robust cost
  double barc = errorThreshold;
  double barcSq = barc * barc;
  double muInit = barcSq / (2 * rSqVec.maxCoeff() - barcSq);
  muInit = std::min(muInit, 1e-5);
  // Negative values of initial mu corresponds to small residual errors. In this case skip applying GNC.
  if (muInit > 0) {
    RobustCostParameters params;
    params.GNCBarc = barc;
    params.GNCMaxNumIters = 10000;
    params.GNCInitMu = muInit;
    RobustCost cost(RobustCostType::GNC_TLS, params);
    unsigned iter = 0;
    for (iter = 0; iter < params.GNCMaxNumIters; ++iter) {
      // Update solution
      singlePoseAveraging(ROpt,
                          tOpt,
                          RVec,
                          tVec,
                          kappa_.cwiseProduct(weights_),
                          tau_.cwiseProduct(weights_));
      // Update weight
      int nc = 0;
      for (Eigen::Index i = 0; i < n; ++i) {
        double rSq = kappa_(i) * (ROpt - RVec[i]).squaredNorm() + tau_(i) * (tOpt - tVec[i]).squaredNorm();
        double wi = cost.weight(sqrt(rSq));
        if (wi < w_tol || wi > 1 - w_tol) {
          nc++;
        }
        weights_(i) = wi;
      }
      if (nc == n) {
        break;
      }
      // Update GNC
      cost.update();
    }
  }
  // Retrieve inliers
  inlierIndices.clear();
  for (Eigen::Index i = 0; i < n; ++i) {
    double wi = weights_(i);
    if (wi > 1 - w_tol) {
      inlierIndices.push_back(i);
    }
  }
}

}  // namespace DPGO


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Col;
using arma::as_scalar;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedOOColVec : public Expected {
    public:
      ExpectedOOColVec() {
        cout << "Compute ExpectedOOColVec(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
          InputClass::OOColVec
        });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _ooColVec = *static_cast<Col<double>*>(value.second);
                break;
            }
          }

          cout << "Using input: " << _fileSuffix << endl;

          expectedArmaAs_scalar();
        }

        cout << "done." << endl;
      }

    protected:
      Mat<double> _ooColVec;

      void expectedArmaAs_scalar() {
        cout << "- Compute expectedArmaAs_scalar() ... ";
        save<double>("Arma.as_scalar", Col<double>({as_scalar(_ooColVec)}));
        cout << "done." << endl;
      }
  };
}


#ifndef _SIM_NODE_NETWORK_HPP_
#define _SIM_NODE_NETWORK_HPP_ 1

#include <functional>
#include <vector>
#include <list>
#include <random>

#include <Eigen/Dense>

#include "sim_node.hpp"

class SimNodeNetwork {
 public:
  typedef std::vector<double> IterationEstimate;

  struct EstimateLog {
    SimNode::Target target;
    std::vector<IterationEstimate> estimates;

    EstimateLog() : target(0.0, {0.0, 0.0}), estimates() {};
  };

  static const double NO_READING;

  static std::vector<double> MaxDegreeConsensus(
    const SimNode::CoordinatePair& target_location,
    const SimNode& source_node,
    const std::vector<SimNode>& nodes_in_network);

  // static const std::function<double(int, int)> metropolis_consensus =
  //   [] (int p_source_vertex, int p_SimNode::Target_vertex) -> double {
  //       return 0.0;
  //   }

  SimNodeNetwork(
    const std::function<double(SimNode, SimNode)>& p_consensus_method,
    const double p_error_threshold);

  ~SimNodeNetwork();

  SimNodeNetwork& AddNode(
    const double p_sensing_range,
    const double p_communication_range,
    const SimNode::CoordinatePair& p_coordinates);

  std::vector<EstimateLog> BuildMapOfField(
    const std::vector<SimNode::Target>& targets);

  void PerformSensorSampling(
    const SimNode::Target& target);

  void PrepareNodesForConsensusFiltering(
    const SimNode::Target& target);

  EstimateLog PerformConsensusFiltering(
    const SimNode::Target& target);

  std::vector<SimNode> FindSubSetOfNodes(
    const std::map<int, SimNode>& sensor_nodes,
    const SimNode::CoordinatePair& target_location,
    const std::function<bool(
      const SimNode&,
      const SimNode::CoordinatePair&
    )>& CriteriaIsMet);

  std::vector<double> CollectEstimatesFromNodes(
    const SimNode::CoordinatePair& target_location);

  double ComputeAverageReading(
    const std::vector<SimNode> estimating_nodes,
    const SimNode::CoordinatePair& target_location);

  bool EstimatesHaveSufficientlyConverged(
    const std::vector<double>& estimates,
    const double average_reading) const;

  bool EstimatesHaveNotSufficientlyConverged(
    const std::vector<double>& estimates,
    const double average_reading) const;

 private:
  int next_node_id_;
  double error_threshold_;
  std::map<int, SimNode> sensor_nodes_;
  SimNode::CoordinatePair average_node_location_;
  std::vector<SimNode::CoordinatePair> target_locations_;
  std::map<SimNode::CoordinatePair, double> average_readings_; // aka convergence values
  std::function<double(SimNode, SimNode)> weight_strategy_;
};

#endif //_SIM_NODE_NETWORK_HPP_


////////////////////////////////////////////////////////////////////
//                                                                //
//    Copyright (c) 2019-20, UK Atomic Energy Authority (UKAEA)   //
//                                                                //
////////////////////////////////////////////////////////////////////

/*!
    @file
    Defines the core numerical operations for all data manipulation within of the library.

    @copyright UK Atomic Energy Authority (UKAEA) - 2019-20
*/
#ifndef CORE_NUMERICAL_HPP
#define CORE_NUMERICAL_HPP

#include <cmath>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/StdVector>

#include "common.hpp"
#include "core/numericalfunctions.hpp"
#include "core/numericalmacros.h"

PEAKINGDUCK_NAMESPACE_START(peakingduck)
PEAKINGDUCK_NAMESPACE_START(core)

    const int ArrayTypeDynamic = Eigen::Dynamic;

    template<typename Scalar, int Size=ArrayTypeDynamic>
    using Array1D = Eigen::Array<Scalar, Size, 1>;

    using Array1Di = Array1D<int>;
    using Array1Df = Array1D<float>;
    using Array1Dd = Array1D<double>;

    using DefaultType = double;

    /*!
       @brief Represents a 1-dimensional data structure (basically a 1D Eigen array)
        Dynamic array - most use cases will be determined at runtime (I am assuming).
        We don't want anyone to know we are using Eigen beyond this file, since (in theory)
        it should make it easier to change library if need be. We only really need the array
        datastructure from Eigen and not much else and instead of reinventing the wheel,
        we wrap Eigen array.

        We wrap this with private inheritance on the Eigen type but there
        are a lot of methods to expose, easy to add when/if we need them.

        Eigen array is pretty good, it has things like sqrt, exp on array coefficients, but
        we need to extend this to other functions, so we use CRTP to do this.

        For all of this, you may ask why not just use Eigen and use an alias?
        Well for one, we don't need all of Eigen just the array, and not all
        of the array type (we require a simpler interface). Additionally, at some
        point we may wish to use another data structure as std::array for example.
        In this case we just change the NumericalData class to wrap that instead.
        If we change the alias this could break existing interfaces and APIs, causing
        big changes later on. Since this datastructure is fundamental to everything
        we need to make sure that we have this sorted properly first!
    */
    template<typename T=DefaultType, int Size=ArrayTypeDynamic>
    struct NumericalData : private Array1D<T, Size>,
                           public NumericalFunctions<NumericalData<T, Size>>
    {
            using value_type = T;

            using BaseEigenArray = Array1D<value_type, Size>;

            // typedef Array1Dd Base;
            using BaseEigenArray::BaseEigenArray;

            // This constructor allows you to construct Derived type from Eigen expressions
            template<typename OtherDerived>
            explicit NumericalData(const Eigen::ArrayBase<OtherDerived>& other)
                : BaseEigenArray(other)
            { }

            // issue constructing with no arguments
            // this aims to fix it
            explicit NumericalData()
            {
                from_vector(std::vector<value_type>());
            }

            // This constructor allows you to construct from a std::vector
            explicit NumericalData(const std::vector<value_type>& other)
            {
                from_vector(other);
            }

            // This method allows you to assign Eigen expressions to Derived type
            template<typename OtherDerived>
            NumericalData& operator=(const Eigen::ArrayBase <OtherDerived>& other)
            {
                this->BaseEigenArray::operator=(other);
                return *this;
            }

            template<typename OtherDerived>
            NumericalData& operator=(const Eigen::EigenBase<OtherDerived> &other)
            {
                this->BaseEigenArray::operator=(other);
                return *this;
            }

            template<typename OtherDerived>
            NumericalData& operator=(const Eigen::ReturnByValue<OtherDerived> &other)
            {
                this->BaseEigenArray::operator=(other);
                return *this;
            }

            // This method allows you to assign Eigen expressions to Derived type
            NumericalData& operator=(const std::vector<T>& other)
            {
                from_vector(other);
                return *this;
            }

            // clang does not like this, but gcc does,
            // not sure why?
            // maybe private inheritance was not a good
            // option, composition is too much effort though!
            // using BaseEigenArray::Base;
            // using BaseEigenArray::ArrayBase;
            // using BaseEigenArray::DenseBase;
            using BaseEigenArray::Base::eval;

            // operations such as (x > 0).all()
            using BaseEigenArray::Base::all;
            using BaseEigenArray::Base::any;
            using BaseEigenArray::Base::count;

            using BaseEigenArray::Base::Zero;
            using BaseEigenArray::Base::Ones;

            // essential operations on arrays
            using BaseEigenArray::operator<<;
            using BaseEigenArray::operator>;
            using BaseEigenArray::operator<;
            using BaseEigenArray::operator=;
            using BaseEigenArray::operator==;
            using BaseEigenArray::operator*;
            using BaseEigenArray::operator*=;
            using BaseEigenArray::operator+;
            using BaseEigenArray::operator+=;
            using BaseEigenArray::operator-;
            using BaseEigenArray::operator-=;
            using BaseEigenArray::operator/;
            using BaseEigenArray::operator/=;
            using BaseEigenArray::operator new;
            using BaseEigenArray::operator delete;

            PEAKINGDUCK_NUMERICAL_OPERATOR_IMP_MACRO(NumericalData,BaseEigenArray,+)
            PEAKINGDUCK_NUMERICAL_OPERATOR_IMP_MACRO(NumericalData,BaseEigenArray,-)
            PEAKINGDUCK_NUMERICAL_OPERATOR_IMP_MACRO(NumericalData,BaseEigenArray,*)
            PEAKINGDUCK_NUMERICAL_OPERATOR_IMP_MACRO(NumericalData,BaseEigenArray,/)

            // entry access operations
            using BaseEigenArray::operator[];
            using BaseEigenArray::data;
            using BaseEigenArray::size;
            using BaseEigenArray::begin;
            using BaseEigenArray::end;
            using BaseEigenArray::segment;

            inline void from_vector(const std::vector<value_type>& raw){
                this->BaseEigenArray::operator=(BaseEigenArray::Map(raw.data(), raw.size()));
            }

            inline std::vector<value_type> to_vector() const{
                return std::vector<value_type>(this->data(), this->data() + this->size());
            }

            // some useful predefined methods
            // map
            using BaseEigenArray::exp;
            using BaseEigenArray::log;
            using BaseEigenArray::sqrt;
            using BaseEigenArray::square;
            using BaseEigenArray::pow;
            using BaseEigenArray::replicate;
            using BaseEigenArray::reverse;
            using BaseEigenArray::reverseInPlace;

            // reduce
            using BaseEigenArray::mean;
            using BaseEigenArray::sum;
            using BaseEigenArray::maxCoeff;
            using BaseEigenArray::minCoeff;

            // custom unary operations
            using BaseEigenArray::unaryExpr;

            // similar to python slicing,, but a bit more primative
            // arr = [1, 4, 5, 2, 10, 2, 2, -8, 2]
            // arr.slice(1, 3) == arr[1:3] == [4, 5]
            // arr.slice(1, -2) == arr[1:-2] == [4, 5, 2, 10, 2, 2]
            //
            // can we add templates when sindex and eindex are known
            // at compile time?
            NumericalData<value_type> slice(int sindex, int eindex) const
            {
                if(eindex >= 0){
                    return this->segment(sindex, eindex - sindex);
                }
                return this->segment(sindex, this->size() + eindex - sindex);
            }

            // uses the () operator with two indicies to do the slice
            // no way (overload comma operator?) to do this with square []
            // braces
            NumericalData<value_type> operator()(int sindex, int eindex) const
            {
                return slice(sindex, eindex);
            }

            // TODO: These really belong in the numerical functions
            // interface, but I am getting compilation problems due to template
            // parameter of derived. To not waste anymore time, we leave it here for now.

            /*!
                @brief A simple function for filtering values above a certain
                threshold (>=). This is useful to remove entries that are negative
                for example.

                Returns a new array
            */
            NumericalData ramp(const value_type& threshold) const
            {
                std::function<value_type(value_type)> imp = [&](const value_type& x){
                    return (x >= threshold) ? x : 0;
                };
                return this->unaryExpr(imp);
            }

            /*!
                @brief A simple function for filtering values above a certain
                threshold (>=). This is useful to remove entries that are negative
                for example.

                Mutates underlying array
            */
            NumericalData& rampInPlace(const value_type& threshold)
            {
                this->BaseEigenArray::operator=(this->ramp(threshold));
                return *this;
            }
    };

    /*!
        @brief Combine (concatenate) arrays into another.
    */
    template<typename T=DefaultType, int Size=ArrayTypeDynamic>
    NumericalData<T, Size> combine(const NumericalData<T, ArrayTypeDynamic>& one, const NumericalData<T, ArrayTypeDynamic>& two){
        NumericalData<T, Size> combined(one.size() + two.size());
        combined << one.eval(), two.eval();
        return combined;
    }

    /*!
       @brief Given a list of values take nouter points either side of
        the index given and ignore ninner points.

            Examples:

            1.
                values = [8, 2, 5, 2, 6, 6, 9, 23, 12]
                index = 4
                nouter = 3
                ninner = 0
                includeindex = True

                => [2, 5, 2, 6, 6, 9, 23]
            2.
                values = [8, 2, 5, 2, 6, 6, 9, 23, 12]
                index = 4
                nouter = 3
                ninner = 0
                includeindex = False

                => [2, 5, 2, 6, 9, 23]
            3.
                values = [8, 2, 5, 2, 6, 6, 9, 23, 12]
                index = 4
                nouter = 3
                ninner = 1
                includeindex = True

                => [2, 5, 6, 9, 23]
            4.
                values = [8, 2, 5, 2, 6, 6, 9, 23, 12]
                index = 4
                nouter = 3
                ninner = 1
                includeindex = False

                => [2, 5, 9, 23]

        Therefore:
            - ninner >= 0
            - ninner <= nouter
            - index >= nouter
            - index < values.size()

        It will clip at (0, len(values))

    */
    template<typename T=DefaultType, int InputSize=ArrayTypeDynamic, int WindowSize=ArrayTypeDynamic>
    NumericalData<T, WindowSize> window(const NumericalData<T, InputSize>& data,
        int centerindex, int nouter=5, int ninner=0, bool includeindex=true){

        // no funny business
        assert(ninner <= nouter);
        assert((centerindex >= 0) && (centerindex < static_cast<int>(data.size())));
        assert(data.size() > 0);

        const NumericalData<T, ArrayTypeDynamic> slicelower = data.slice(std::max(0, centerindex-nouter),
                                                                         std::max(0, centerindex-ninner));
        const NumericalData<T, ArrayTypeDynamic> sliceupper = data.slice(std::min(static_cast<int>(data.size()), centerindex+1+ninner),
                                                                         std::min(static_cast<int>(data.size()), centerindex+1+nouter));

        const size_t data_size = includeindex ? slicelower.size() + sliceupper.size() + 1 : slicelower.size() + sliceupper.size();
        NumericalData<T, WindowSize> combined(data_size);
        if(includeindex){
            combined << slicelower.eval(), data[centerindex], sliceupper.eval();
        }
        else{
            combined << slicelower.eval(), sliceupper.eval();
        }
        return combined;
    }

PEAKINGDUCK_NAMESPACE_END
PEAKINGDUCK_NAMESPACE_END

#endif // CORE_NUMERICAL_HPP


#include "stdafx.h"
#include "mscl/MicroStrain/MIP/Commands/GenericMipCommand.h"
#include "ExposedInertialTypes.h"
#include <boost/math/constants/constants.hpp>

namespace mscl
{
    //////////  Matrix  //////////

    Matrix_3x3::Matrix_3x3(float i00, float i01, float i02, float i10, float i11, float i12, float i20, float i21, float i22)
    {
        // Not sure why I need firstArray.  Just putting this list in the initialization below doesn't compile.
        std::array<float, 3> firstRow{ i00, i01, i02 };
        m_array = { firstRow,{ i10, i11, i12 },{ i20, i21, i22 } };
    }

    Matrix_3x3::Matrix_3x3(MipFieldValues data)
    {
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                int index = (i * 3) + j;
                m_array[i][j] = data[index].as_float();
            }
        }
    }

    Matrix_3x3::~Matrix_3x3()
    { }

    void Matrix_3x3::set(uint8 row, uint8 col, float value)
    {
        m_array[row][col] = value;
    }

    float Matrix_3x3::operator() (uint8 row, uint8 col) const
    {
        return m_array.at(row).at(col);
    }

    float Matrix_3x3::at(uint8 row, uint8 col) const
    {
        return (*this)(row, col);
    }

    std::string Matrix_3x3::str() const
    {
        std::stringstream result;
        result << "[";

        //for every value in the Matrix
        for (uint16 row = 0; row < 3; row++)
        {
            result << "[";
            for (uint16 col = 0; col < 3; col++)
            {
                result << m_array[row][col];

                //if this isn't the last column in the row
                if (col != 2)
                {
                    //add a separator
                    result << ",";
                }
            }
            result << "]";

            //if this isn't the last row
            if (row != 2)
            {
                //add a separator
                result << ",";
            }
        }

        result << "]";

        return result.str();
    }

    MipFieldValues Matrix_3x3::asMipFieldValues() const
    {
        MipFieldValues m;
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                m.push_back(Value::FLOAT(m_array[i][j]));
            }
        }

        return m;
    }

    //////////  Quaternion  //////////
    Quaternion::Quaternion() :
        Matrix(1, 4, ValueType::valueType_float, ByteStream())
    {
        m_data.append_float(0);
        m_data.append_float(0);
        m_data.append_float(0);
        m_data.append_float(0);
    }

    Quaternion::Quaternion(float q0, float q1, float q2, float q3) :
        Matrix(1, 4, ValueType::valueType_float, ByteStream())
    {
        m_data.append_float(q0);
        m_data.append_float(q1);
        m_data.append_float(q2);
        m_data.append_float(q3);
    }

    Quaternion::Quaternion(MipFieldValues data) :
        Matrix(1, 4, ValueType::valueType_float, ByteStream())
    {
        for (int i = 0; i < 4; i++)
        {
            m_data.append_float(data[i].as_float());
        }
    }

    float Quaternion::at(uint8 index) const
    {
        return as_floatAt(0, index);
    }

    void Quaternion::set(uint8 index, float val)
    {
        uint32 pos = getBytePos(0, index);
        ByteStream valB;
        valB.append_float(val);

        for (uint32 i = 0; i < 4; i++)
        {
            uint32 replaceIndex = pos + i;
            m_data.data()[replaceIndex] = valB[i];
        }
    }

    void Quaternion::normalize()
    {
        float magnitude = 0.0f;
        for (uint8 i = 0; i < 4; i++)
        {
            float val = at(i);
            magnitude += val * val;
        }

        magnitude = sqrt(magnitude);

        if (magnitude == 0)
        {
            return;
        }

        ByteStream b;
        for (uint8 i = 0; i < 4; i++)
        {
            float val = at(i);
            b.append_float(val / magnitude);
        }

        m_data = b;
    }

    MipFieldValues Quaternion::asMipFieldValues() const
    {
        MipFieldValues m;
        for (uint8 i = 0; i < 4; i++)
        {
            m.push_back(Value::FLOAT(at(i)));
        }

        return m;
    }

    //////////  GeometricVector  //////////

    GeometricVector::GeometricVector(float x_init, float y_init, float z_init, PositionVelocityReferenceFrame ref) :
        vec_0(x_init),
        vec_1(y_init),
        vec_2(z_init),
        referenceFrame(ref)
    { }

    GeometricVector::GeometricVector() :
        vec_0(0),
        vec_1(0),
        vec_2(0),
        referenceFrame(PositionVelocityReferenceFrame::ECEF)
    { }

    GeometricVector::~GeometricVector()
    { }

    //////////  TimeUpdate  //////////

    TimeUpdate::TimeUpdate(double timeOfWeek, uint16 weekNumber, float timeAccuracy):
        m_timeOfWeek(timeOfWeek),
        m_weekNumber(weekNumber),
        m_timeAccuracy(timeAccuracy)
    {
    }

    TimeUpdate::~TimeUpdate()
    {
    }

    //////////  HeadingUpdateOptions  //////////

    InertialTypes::HeadingUpdateEnableOption HeadingUpdateOptions::AsOptionId() const
    {
        if (useInternalMagnetometer)
        {
            if (useInternalGNSSVelocityVector)
            {
                if (useExternalHeadingMessages)
                    return InertialTypes::HeadingUpdateEnableOption::ENABLE_ALL;
                else
                    return InertialTypes::HeadingUpdateEnableOption::ENABLE_MAGNETOMETER_AND_GNSS;
            }
            else
            {
                if (useExternalHeadingMessages)
                    return InertialTypes::HeadingUpdateEnableOption::ENABLE_MAGNETOMETER_AND_EXTERNAL;
                else
                    return InertialTypes::HeadingUpdateEnableOption::ENABLE_INTERNAL_MAGNETOMETER;
            }
        }
        else
        {
            if (useInternalGNSSVelocityVector)
            {
                if (useExternalHeadingMessages)
                    return InertialTypes::HeadingUpdateEnableOption::ENABLE_GNSS_AND_EXTERNAL;
                else
                    return InertialTypes::HeadingUpdateEnableOption::ENABLE_INTERNAL_GNSS;
            }
            else
            {
                if (useExternalHeadingMessages)
                    return InertialTypes::HeadingUpdateEnableOption::ENABLE_EXTERNAL_MESSAGES;
                else
                    return InertialTypes::HeadingUpdateEnableOption::ENABLE_NONE;
            }
        }
    }

    //  This constructor converts a uint8 to a HeadingUpdateOptions object according to the Communications Protocol.
    HeadingUpdateOptions::HeadingUpdateOptions(const InertialTypes::HeadingUpdateEnableOption& headingUpdateOption)
    {
        switch (headingUpdateOption)
        {
        case InertialTypes::HeadingUpdateEnableOption::ENABLE_NONE:
            useInternalMagnetometer = false;
            useInternalGNSSVelocityVector = false;
            useExternalHeadingMessages = false;
            break;
        case InertialTypes::HeadingUpdateEnableOption::ENABLE_INTERNAL_MAGNETOMETER:
            useInternalMagnetometer = true;
            useInternalGNSSVelocityVector = false;
            useExternalHeadingMessages = false;
            break;
        case InertialTypes::HeadingUpdateEnableOption::ENABLE_INTERNAL_GNSS:
            useInternalMagnetometer = false;
            useInternalGNSSVelocityVector = true;
            useExternalHeadingMessages = false;
            break;
        case InertialTypes::HeadingUpdateEnableOption::ENABLE_EXTERNAL_MESSAGES:
            useInternalMagnetometer = false;
            useInternalGNSSVelocityVector = false;
            useExternalHeadingMessages = true;
            break;
        case InertialTypes::HeadingUpdateEnableOption::ENABLE_MAGNETOMETER_AND_GNSS:
            useInternalMagnetometer = true;
            useInternalGNSSVelocityVector = true;
            useExternalHeadingMessages = false;
            break;
        case InertialTypes::HeadingUpdateEnableOption::ENABLE_GNSS_AND_EXTERNAL:
            useInternalMagnetometer = false;
            useInternalGNSSVelocityVector = true;
            useExternalHeadingMessages = true;
            break;
        case InertialTypes::HeadingUpdateEnableOption::ENABLE_MAGNETOMETER_AND_EXTERNAL:
            useInternalMagnetometer = true;
            useInternalGNSSVelocityVector = false;
            useExternalHeadingMessages = true;
            break;
        case InertialTypes::HeadingUpdateEnableOption::ENABLE_ALL:
            useInternalMagnetometer = true;
            useInternalGNSSVelocityVector = true;
            useExternalHeadingMessages = true;
            break;
        default:
            throw Error_MipCmdFailed("An invalid option value was passed in to HeadingUpdateOptions.");
        }
    }

    //////////  EstimationControlOptions  //////////

    uint16 EstimationControlOptions::AsUint16() const {
        uint16 intValue = 0;

        if (enableGyroBiasEstimation) {
            intValue = intValue | InertialTypes::EstimationControlOption::ENABLE_GYRO_BIAS_ESTIMATION;
        }

        if (enableAccelBiasEstimation) {
            intValue = intValue | InertialTypes::EstimationControlOption::ENABLE_ACCEL_BIAS_ESTIMATION;
        }

        if (enableGyroScaleFactorEstimation) {
            intValue = intValue | InertialTypes::EstimationControlOption::ENABLE_GYRO_SCALE_FACTOR_ESTIMATION;
        }

        if (enableAccelScaleFactorEstimation) {
            intValue = intValue | InertialTypes::EstimationControlOption::ENABLE_ACCEL_SCALE_FACTOR_ESTIMATION;
        }

        if (enableGNSSAntennaOffsetEstimation) {
            intValue = intValue | InertialTypes::EstimationControlOption::ENABLE_GNSS_ANTENNA_OFFSET_ESTIMATION;
        }

        if (enableHardIronAutoCalibration) {
            intValue = intValue | InertialTypes::EstimationControlOption::ENABLE_HARD_IRON_AUTO_CALIBRATION;
        }

        if (enableSoftIronAutoCalibration) {
            intValue = intValue | InertialTypes::EstimationControlOption::ENABLE_SOFT_IRON_AUTO_CALIBRATION;
        }

        return intValue;
    }

    //  This constructor converts a uint16 to a EstimationControlOptions object according to the Communications Protocol.
    EstimationControlOptions::EstimationControlOptions(const mscl::uint16& estimationControlData) {
        enableGyroBiasEstimation = (estimationControlData & InertialTypes::EstimationControlOption::ENABLE_GYRO_BIAS_ESTIMATION) != 0;
        enableAccelBiasEstimation = (estimationControlData & InertialTypes::EstimationControlOption::ENABLE_ACCEL_BIAS_ESTIMATION) != 0;
        enableGyroScaleFactorEstimation = (estimationControlData & InertialTypes::EstimationControlOption::ENABLE_GYRO_SCALE_FACTOR_ESTIMATION) != 0;
        enableAccelScaleFactorEstimation = (estimationControlData & InertialTypes::EstimationControlOption::ENABLE_ACCEL_SCALE_FACTOR_ESTIMATION) != 0;
        enableGNSSAntennaOffsetEstimation = (estimationControlData & InertialTypes::EstimationControlOption::ENABLE_GNSS_ANTENNA_OFFSET_ESTIMATION) != 0;
        enableHardIronAutoCalibration = (estimationControlData & InertialTypes::EstimationControlOption::ENABLE_HARD_IRON_AUTO_CALIBRATION) != 0;
        enableSoftIronAutoCalibration = (estimationControlData & InertialTypes::EstimationControlOption::ENABLE_SOFT_IRON_AUTO_CALIBRATION) != 0;
    }

    DeviceStatusData::SystemState DeviceStatusData::systemState()
    {
        checkValue(m_systemState, "systemState");
        return *m_systemState;
    }

    void DeviceStatusData::systemState(SystemState val)
    {
        m_systemState = val;
    }

    bool DeviceStatusData::gnssPowerStateOn() const
    {
        checkValue(m_gnssPowerStateOn, "gnssPowerStateOn");
        return *m_gnssPowerStateOn;
    }

    void DeviceStatusData::gnssPowerStateOn(bool val)
    {
        m_gnssPowerStateOn = val;
    }

    PpsPulseInfo DeviceStatusData::gnss1PpsPulseInfo()
    {
        checkValue(m_gnss1PpsPulseInfo, "gnss1PpsPulseInfo");
        return *m_gnss1PpsPulseInfo;
    }

    void DeviceStatusData::gnss1PpsPulseInfo(PpsPulseInfo val)
    {
        m_gnss1PpsPulseInfo = val;
    }

    StreamInfo DeviceStatusData::imuStreamInfo()
    {
        checkValue(m_imuStreamInfo, "imuStreamInfo");
        return *m_imuStreamInfo;
    }

    void DeviceStatusData::imuStreamInfo(StreamInfo val)
    {
        m_imuStreamInfo = val;
    }

    StreamInfo DeviceStatusData::gnssStreamInfo()
    {
        checkValue(m_gnssStreamInfo, "gnssStreamInfo");
        return *m_gnssStreamInfo;
    }

    void DeviceStatusData::gnssStreamInfo(StreamInfo val)
    {
        m_gnssStreamInfo = val;
    }

    StreamInfo DeviceStatusData::estimationFilterStreamInfo()
    {
        checkValue(m_estimationFilterStreamInfo, "estimationFilterStreamInfo");
        return *m_estimationFilterStreamInfo;
    }

    void DeviceStatusData::estimationFilterStreamInfo(StreamInfo val)
    {
        m_estimationFilterStreamInfo = val;
    }

    DeviceMessageInfo DeviceStatusData::imuMessageInfo()
    {
        checkValue(m_imuMessageInfo, "imuMessageInfo");
        return *m_imuMessageInfo;
    }

    void DeviceStatusData::imuMessageInfo(DeviceMessageInfo val)
    {
        m_imuMessageInfo = val;
    }

    DeviceMessageInfo DeviceStatusData::gnssMessageInfo()
    {
        checkValue(m_gnssMessageInfo, "gnssMessageInfo");
        return *m_gnssMessageInfo;
    }

    void DeviceStatusData::gnssMessageInfo(DeviceMessageInfo val)
    {
        m_gnssMessageInfo = val;
    }

    PortInfo DeviceStatusData::comPortInfo()
    {
        checkValue(m_comPortInfo, "comPortInfo");
        return *m_comPortInfo;
    }

    void DeviceStatusData::comPortInfo(PortInfo val)
    {
        m_comPortInfo = val;
    }

    PortInfo DeviceStatusData::usbPortInfo()
    {
        checkValue(m_usbPortInfo, "usbPortInfo");
        return *m_usbPortInfo;
    }

    void DeviceStatusData::usbPortInfo(PortInfo val)
    {
        m_usbPortInfo = val;
    }

    bool DeviceStatusData::hasMagnetometer() const
    {
        checkValue(m_hasMagnetometer, "hasMagnetometer");
        return *m_hasMagnetometer;
    }

    void DeviceStatusData::hasMagnetometer(bool val)
    {
        m_hasMagnetometer = val;
    }

    bool DeviceStatusData::magnetometerInitializationFailed() const
    {
        checkValue(m_magnetometerInitializationFailed, "magnetometerInitializationFailed");
        return *m_magnetometerInitializationFailed;
    }

    void DeviceStatusData::magnetometerInitializationFailed(bool val)
    {
        m_magnetometerInitializationFailed = val;
    }

    bool DeviceStatusData::hasPressure() const
    {
        checkValue(m_hasPressure, "hasPressure");
        return *m_hasPressure;
    }

    void DeviceStatusData::hasPressure(bool val)
    {
        m_hasPressure = val;
    }

    bool DeviceStatusData::pressureInitializationFailed() const
    {
        checkValue(m_pressureInitializationFailed, "pressureInitializationFailed");
        return *m_pressureInitializationFailed;
    }

    void DeviceStatusData::pressureInitializationFailed(bool val)
    {
        m_pressureInitializationFailed = val;
    }

    bool DeviceStatusData::gnssReceiverInitializationFailed() const
    {
        checkValue(m_gnssReceiverInitializationFailed, "gnssReceiverInitializationFailed");
        return *m_gnssReceiverInitializationFailed;
    }

    void DeviceStatusData::gnssReceiverInitializationFailed(bool val)
    {
        m_gnssReceiverInitializationFailed = val;
    }

    bool DeviceStatusData::coldStartOnPowerOn() const
    {
        checkValue(m_coldStartOnPowerOn, "coldStartOnPowerOn");
        return *m_coldStartOnPowerOn;
    }

    void DeviceStatusData::coldStartOnPowerOn(bool val)
    {
        m_coldStartOnPowerOn = val;
    }

    TemperatureInfo DeviceStatusData::temperatureInfo()
    {
        checkValue(m_temperatureInfo, "temperatureInfo");
        return *m_temperatureInfo;
    }

    void DeviceStatusData::temperatureInfo(TemperatureInfo val)
    {
        m_temperatureInfo = val;
    }

    InertialTypes::PowerState DeviceStatusData::powerState() const
    {
        checkValue(m_powerState, "powerState");
        return *m_powerState;
    }

    void DeviceStatusData::powerState(InertialTypes::PowerState val)
    {
        m_powerState = val;
    }

    uint16 DeviceStatusData::gyroRange() const
    {
        checkValue(m_gyroRange, "gyroRange");
        return *m_gyroRange;
    }

    void DeviceStatusData::gyroRange(uint16 val)
    {
        m_gyroRange = val;
    }

    uint16 DeviceStatusData::accelRange() const
    {
        checkValue(m_accelRange, "accelRange");
        return *m_accelRange;
    }

    void DeviceStatusData::accelRange(uint16 val)
    {
        m_accelRange = val;
    }

    uint8 TareAxisValues::asUint8() const
    {
        return tarePitchAxis * InertialTypes::TARE_PITCH_AXIS
            | tareRollAxis * InertialTypes::TARE_ROLL_AXIS
            | tareYawAxis * InertialTypes::TARE_YAW_AXIS;
    }

    mscl::DeviceStatusMap DeviceStatusData::asMap() const
    {
        mscl::DeviceStatusMap statusMap;
        mscl::DeviceStatusValueMap m = asValueMap();

        for (auto kv : m)
        {
            statusMap[kv.first] = kv.second.as_string();
        }

        return statusMap;
    }

    mscl::DeviceStatusValueMap DeviceStatusData::asValueMap() const
    {
        mscl::DeviceStatusValueMap statusMap;
        statusMap[ModelNumber] = mscl::Value::UINT16(modelNumber);
        statusMap[StatusStructure_Value] = mscl::Value::UINT8(static_cast<uint8>(statusStructure));

        if (isSet(m_systemState))
        {
            statusMap[SystemState_Value] = mscl::Value::UINT16(static_cast<uint16>(m_systemState.get()));
        }

        if (isSet(m_gnss1PpsPulseInfo))
        {
            statusMap[gnss1PpsPulseInfo_Count] = mscl::Value::UINT32(m_gnss1PpsPulseInfo.get().count);
            statusMap[gnss1PpsPulseInfo_LastTimeinMS] = mscl::Value::UINT32(m_gnss1PpsPulseInfo.get().lastTimeinMS);
        }

        if (isSet(m_gnssPowerStateOn)) {
            statusMap[GnssPowerStateOn] = mscl::Value::BOOL(m_gnssPowerStateOn.get());
        }

        if (isSet(m_imuStreamInfo))
        {
            statusMap[ImuStreamInfo_Enabled] = mscl::Value::BOOL(m_imuStreamInfo.get().enabled);
            statusMap[ImuStreamInfo_PacketsDropped] = mscl::Value::UINT32(m_imuStreamInfo.get().outgoingPacketsDropped);
        }

        if (isSet(m_gnssStreamInfo))
        {
            statusMap[GnssStreamInfo_Enabled] = mscl::Value::BOOL(m_gnssStreamInfo.get().enabled);
            statusMap[GnssStreamInfo_PacketsDropped] = mscl::Value::UINT32(m_gnssStreamInfo.get().outgoingPacketsDropped);
        }

        if (isSet(m_estimationFilterStreamInfo))
        {
            statusMap[EstimationFilterStreamInfo_Enabled] = mscl::Value::BOOL(m_estimationFilterStreamInfo.get().enabled);
            statusMap[EstimationFilterStreamInfo_PacketsDropped] = mscl::Value::UINT32(m_estimationFilterStreamInfo.get().outgoingPacketsDropped);
        }

        if (isSet(m_comPortInfo))
        {
            statusMap[ComPortInfo_BytesRead] = mscl::Value::UINT32(m_comPortInfo.get().bytesRead);
            statusMap[ComPortInfo_BytesWritten] = mscl::Value::UINT32(m_comPortInfo.get().bytesWritten);
            statusMap[ComPortInfo_OverrunsOnRead] = mscl::Value::UINT32(m_comPortInfo.get().overrunsOnRead);
            statusMap[ComPortInfo_OverrunsOnWrite] = mscl::Value::UINT32(m_comPortInfo.get().overrunsOnWrite); // supported to features
        }

        if (isSet(m_imuMessageInfo))
        {
            statusMap[ImuMessageInfo_LastMessageReadinMS] = mscl::Value::UINT32(m_imuMessageInfo.get().lastMessageReadinMS);
            statusMap[ImuMessageInfo_MessageParsingErrors] = mscl::Value::UINT32(m_imuMessageInfo.get().messageParsingErrors);
            statusMap[ImuMessageInfo_MessagesRead] = mscl::Value::UINT32(m_imuMessageInfo.get().messagesRead);
        }

        if (isSet(m_gnssMessageInfo))
        {
            statusMap[GnssMessageInfo_LastMessageReadinMS] = mscl::Value::UINT32(m_gnssMessageInfo.get().lastMessageReadinMS);
            statusMap[GnssMessageInfo_MessageParsingErrors] = mscl::Value::UINT32(m_gnssMessageInfo.get().messageParsingErrors);
            statusMap[GnssMessageInfo_MessagesRead] = mscl::Value::UINT32(m_gnssMessageInfo.get().messagesRead);
        }

        if (isSet(m_temperatureInfo))
        {
            statusMap[TemperatureInfo_Error] = mscl::Value::UINT8(m_temperatureInfo.get().error);
            statusMap[TemperatureInfo_LastReadInMS] = mscl::Value::UINT32(m_temperatureInfo.get().lastReadInMS);
            statusMap[TemperatureInfo_OnBoardTemp] = mscl::Value::FLOAT(m_temperatureInfo.get().onBoardTemp);
        }

        if (isSet(m_powerState))
        {
            statusMap[PowerState] = mscl::Value::UINT8(static_cast<uint8>(m_powerState.get()));
        }

        if (isSet(m_gyroRange))
        {
            statusMap[GyroRange] = mscl::Value::UINT16(m_gyroRange.get());
        }

        if (isSet(m_accelRange))
        {
            statusMap[AccelRange] = mscl::Value::UINT16(m_accelRange.get());
        }

        if (isSet(m_hasMagnetometer))
        {
            statusMap[HasMagnetometer] = mscl::Value::BOOL(m_hasMagnetometer.get());
        }

        if (isSet(m_hasPressure))
        {
            statusMap[HasPressure] = mscl::Value::BOOL(m_hasPressure.get());
        }

        return statusMap;
    }

    uint8 RTKDeviceStatusFlags::state() const
    {
        return static_cast<uint8>(get(STATE));
    }

    void RTKDeviceStatusFlags::state(uint8 rtkState)
    {
        set(STATE, rtkState);
    }

    uint8 RTKDeviceStatusFlags::statusCode() const
    {
        return static_cast<uint8>(get(STATUS_CODE));
    }

    void RTKDeviceStatusFlags::statusCode(uint8 code)
    {
        set(STATUS_CODE, code);
    }

    bool RTKDeviceStatusFlags::correctionsTimedOut() const
    {
        return get(CORRECTIONS_TIMED_OUT) > 0;
    }

    void RTKDeviceStatusFlags::correctionsTimedOut(bool timedOut)
    {
        set(CORRECTIONS_TIMED_OUT, (timedOut ? 1 : 0));
    }

    bool RTKDeviceStatusFlags::serviceUnavailable() const
    {
        return get(SERVICE_UNAVAILABLE) > 0;
    }

    void RTKDeviceStatusFlags::serviceUnavailable(bool available)
    {
        set(SERVICE_UNAVAILABLE, (available ? 1 : 0));
    }

    RTKDeviceStatusFlags::ResetReason RTKDeviceStatusFlags::resetReason() const
    {
        return static_cast<ResetReason>(get(RESET_REASON));
    }

    void RTKDeviceStatusFlags::resetReason(RTKDeviceStatusFlags::ResetReason reason)
    {
        set(RESET_REASON, static_cast<uint32>(reason));
    }

    bool RTKDeviceStatusFlags::modemPowered() const
    {
        return get(MODEM_POWERED) > 0;
    }

    void RTKDeviceStatusFlags::modemPowered(bool powered)
    {
        set(MODEM_POWERED, (powered ? 1 : 0));
    }

    bool RTKDeviceStatusFlags::cellConnected()
    {
        return get(CELL_CONNECTED) > 0;
    }

    void RTKDeviceStatusFlags::cellConnected(bool connected)
    {
        set(CELL_CONNECTED, (connected ? 1 : 0));
    }

    bool RTKDeviceStatusFlags::serverConnected()
    {
        return get(SERVER_CONNECTED) > 0;
    }

    void RTKDeviceStatusFlags::serverConnected(bool connected)
    {
        set(SERVER_CONNECTED, (connected ? 1 : 0));
    }

    bool RTKDeviceStatusFlags::dataEnabled()
    {
        return get(DATA_ENABLED) > 0;
    }

    void RTKDeviceStatusFlags::dataEnabled(bool enabled)
    {
        set(DATA_ENABLED, (enabled ? 1 : 0));
    }

    uint8 RTKDeviceStatusFlags::rssi()
    {
        return static_cast<uint8>(get(RSSI));
    }

    void RTKDeviceStatusFlags::rssi(uint8 rtkRssi)
    {
        set(RSSI, rtkRssi);
    }

    uint8 RTKDeviceStatusFlags::signalQuality()
    {
        return static_cast<uint8>(get(SIGNAL_QUALITY));
    }

    void RTKDeviceStatusFlags::signalQuality(uint8 quality)
    {
        set(SIGNAL_QUALITY, quality);
    }

    GnssSignalConfiguration::GnssSignalConfiguration()
    {
        m_gpsSignals = Bitfield(0);
        m_glonassSignals = Bitfield(0);
        m_galileoSignals = Bitfield(0);
        m_beidouSignals = Bitfield(0);
    }

    void GnssSignalConfiguration::enableGpsSignal(GnssSignalConfiguration::GpsSignal signal, bool enable)
    {
        m_gpsSignals.set(signal, (enable ? 1 : 0));
    }

    bool GnssSignalConfiguration::gpsSignalEnabled(GnssSignalConfiguration::GpsSignal signal)
    {
        return m_gpsSignals.get(signal) > 0;
    }

    void GnssSignalConfiguration::enableGlonassSignal(GnssSignalConfiguration::GlonassSignal signal, bool enable)
    {
        m_glonassSignals.set(signal, (enable ? 1 : 0));
    }

    bool GnssSignalConfiguration::glonassSignalEnabled(GnssSignalConfiguration::GlonassSignal signal)
    {
        return m_glonassSignals.get(signal) > 0;
    }

    void GnssSignalConfiguration::enableGalileoSignal(GnssSignalConfiguration::GalileoSignal signal, bool enable)
    {
        m_galileoSignals.set(signal, (enable ? 1 : 0));
    }

    bool GnssSignalConfiguration::galileoSignalEnabled(GnssSignalConfiguration::GalileoSignal signal)
    {
        return m_galileoSignals.get(signal) > 0;
    }

    void GnssSignalConfiguration::enableBeiDouSignal(GnssSignalConfiguration::BeiDouSignal signal, bool enable)
    {
        m_beidouSignals.set(signal, (enable ? 1 : 0));
    }

    bool GnssSignalConfiguration::beidouSignalEnabled(GnssSignalConfiguration::BeiDouSignal signal)
    {
        return m_beidouSignals.get(signal) > 0;
    }

    OdometerConfiguration::Mode OdometerConfiguration::mode() const
    {
        return m_mode;
    }

    void OdometerConfiguration::mode(OdometerConfiguration::Mode m)
    {
        m_mode = m;
    }

    float OdometerConfiguration::scaling() const
    {
        return m_scaling;
    }

    void OdometerConfiguration::scaling(float scale)
    {
        if (scale == INFINITY)
        {
            m_scaling = 0;
            return;
        }

        m_scaling = scale;
    }

    void OdometerConfiguration::scaling(float resolution, float radius)
    {
        if (resolution == 0 || radius == 0)
        {
            m_scaling = 0;
        }

        float mPerRev = radius * 2 * boost::math::constants::pi<float>();
        m_scaling = resolution / mPerRev;
    }

    float OdometerConfiguration::uncertainty() const
    {
        return m_unc;
    }

    void OdometerConfiguration::uncertainty(float unc)
    {
        if (unc == INFINITY)
        {
            m_unc = 0.01f;
            return;
        }

        m_unc = unc;
    }
}  // namespace mscl


/*
 * 2018.5.4 kanda.motohiro@gmail.com C++ 練習
 * released under https://creativecommons.org/publicdomain/zero/1.0/legalcode.ja
 * based on https://www.boost.org/doc/libs/1_67_0/doc/html/date_time/examples.html
 */
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <chrono>
#include <thread>
#include "base.h"

int main()
{
    chrono::high_resolution_clock::time_point a = chrono::high_resolution_clock::now();
    this_thread::sleep_for(chrono::milliseconds{1});
    chrono::high_resolution_clock::time_point b = chrono::high_resolution_clock::now();
    chrono::nanoseconds c = chrono::duration_cast<chrono::nanoseconds>(b - a);
    p(c.count());

    boost::gregorian::date d(boost::gregorian::from_string("2018-5-4"));
    p(d);
    boost::gregorian::date e(2000, 1, 1);
    p(e);
    boost::gregorian::date_duration f = d - e;
    p(f);

    boost::posix_time::ptime g(d, boost::posix_time::seconds{0});
    p(g);
    boost::posix_time::ptime h = g + boost::posix_time::hours{22};
    p(h);
}


#include <math.h>
#include <iostream>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/SVD>

#include "fastQP.h"

#define _USE_MATH_DEFINES

extern "C"
{
	int MAX_ITER = -1; // default: #equality constraints
}

using namespace Eigen;
using namespace std;

//template <typename tA, typename tB, typename tC, typename tD, typename tE, typename tF, typename tG>
//int fastQPThatTakesQinv(vector< MatrixBase<tA>* > QinvblkDiag, const MatrixBase<tB>& f, const MatrixBase<tC>& Aeq, const MatrixBase<tD>& beq, const MatrixBase<tE>& Ain, const MatrixBase<tF>& bin, set<int>& active, MatrixBase<tG>& x)
int fastQPThatTakesQinv(vector< MatrixXd* > QinvblkDiag, const VectorXd& f, const MatrixXd& Aeq, const VectorXd& beq, const MatrixXd& Ain, const VectorXd& bin, set<int>& active, VectorXd& x)
{

	int max_iter = (MAX_ITER<0? (Aeq.rows()+Ain.rows()): MAX_ITER);
	int i,d;
	int iterCnt = 0;

	int M_in = bin.size();
	int M = Aeq.rows();
	int N = Aeq.cols();

	if (f.rows() != N) { cerr << "size of f (" << f.rows() << " by " << f.cols() << ") doesn't match cols of Aeq (" << Aeq.rows() << " by " << Aeq.cols() << ")" << endl; return -4; }
	if (beq.rows() !=M) { cerr << "size of beq doesn't match rows of Aeq" << endl; return -4; }
	if (Ain.cols() !=N) { cerr << "cols of Ain doesn't match cols of Aeq" << endl; return -4; };
	if (bin.rows() != Ain.rows()) { cerr << "bin rows doesn't match Ain rows" << endl; return -4; };
	if (x.rows() != N) { cerr << "x doesn't match Aeq" << endl; return -4; }
	int n_active = active.size();

	MatrixXd Aact = MatrixXd(n_active, N);
	VectorXd bact = VectorXd(n_active);

	MatrixXd QinvAteq(N,M);
	VectorXd minusQinvf(N);

	// calculate a bunch of stuff that is constant during each iteration
	int startrow=0;
	//  for (typename vector< MatrixBase<tA>* >::iterator iterQinv=QinvblkDiag.begin(); iterQinv!=QinvblkDiag.end(); iterQinv++) {
	//  	MatrixBase<tA> *thisQinv = *iterQinv;
	for (vector< MatrixXd* >::iterator iterQinv=QinvblkDiag.begin(); iterQinv!=QinvblkDiag.end(); iterQinv++) {
		MatrixXd *thisQinv = *iterQinv;
		int numRow = thisQinv->rows();
		int numCol = thisQinv->cols();

		if (numRow == 1 || numCol == 1) {  // it's a vector
			d = numRow*numCol;
			if (M>0) QinvAteq.block(startrow,0,d,M)= thisQinv->asDiagonal()*Aeq.block(0,startrow,M,d).transpose();  // Aeq.transpoODse().block(startrow,0,d,N)
			minusQinvf.segment(startrow,d) = -thisQinv->cwiseProduct(f.segment(startrow,d));
			startrow=startrow+d;
		} else { // potentially dense matrix
			d = numRow;
			if (numRow!=numCol) {
				cerr << "Q is not square! " << numRow << "x" << numCol << "\n";
				return -2;
			}

			if (M>0)
				QinvAteq.block(startrow,0,d,M) = thisQinv->operator*(Aeq.block(0,startrow,M,d).transpose());  // Aeq.transpose().block(startrow,0,d,N)

			minusQinvf.segment(startrow,d) = -thisQinv->operator*(f.segment(startrow,d));
			startrow=startrow+d;
		}
		if (startrow>N) {
			cerr << "Q is too big!" << endl;
			return -2;
		}
	}
	if (startrow!=N) { cerr << "Q is the wrong size.  Got " << startrow << "by" << startrow << " but needed " << N << "by" << N << endl; return -2; }

	MatrixXd A;
	VectorXd b;
	MatrixXd QinvAt;
	VectorXd lam, lamIneq;
	VectorXd violated(M_in);
	VectorXd violation;

	while(1) {
		iterCnt++;

		n_active = active.size();
		Aact.resize(n_active,N);
		bact.resize(n_active);

		i=0;
		for (set<int>::iterator iter=active.begin(); iter!=active.end(); iter++) {
			if (*iter<0 || *iter>=Ain.rows()) {
				return -3;  // active set is invalid.  exit quietly, because this is expected behavior in normal operation (e.g. it means I should immediately kick out to gurobi)
			}
			Aact.row(i) = Ain.row(*iter);
			bact(i++) = bin(*iter);
		}

		A.resize(Aeq.rows() + Aact.rows(),N);
		b.resize(beq.size() + bact.size());
		A << Aeq,Aact;
		b << beq,bact;

		if (A.rows() > 0) {
			//Solve H * [x;lam] = [-f;b] using Schur complements, H = [Q,At';A,0];
			QinvAt.resize(QinvAteq.rows(), QinvAteq.cols() + Aact.rows());

			if (n_active>0) {
				int startrow=0;
				for (vector< MatrixXd* >::iterator iterQinv=QinvblkDiag.begin(); iterQinv!=QinvblkDiag.end(); iterQinv++) {
					MatrixXd* thisQinv = (*iterQinv);
					d = thisQinv->rows();
					int numCol = thisQinv->cols();

					if (numCol == 1) {  // it's a vector
						QinvAt.block(startrow,0,d,M+n_active) << QinvAteq.block(startrow,0,d,M), thisQinv->asDiagonal()*Aact.block(0,startrow,n_active,d).transpose();
					} else { // it's a matrix
						QinvAt.block(startrow,0,d,M+n_active) << QinvAteq.block(startrow,0,d,M), thisQinv->operator*(Aact.block(0,startrow,n_active,d).transpose());
					}

					startrow=startrow+d;
				}
			} else {
				QinvAt = QinvAteq;
			}

			lam.resize(QinvAt.cols());
#if 1
			lam = -(A*QinvAt).ldlt().solve(b + (f.transpose()*QinvAt).transpose());
			//lam = -(A*QinvAt + MatrixXd::Identity(A.rows(),A.rows())*1e-4).ldlt().solve(b + (f.transpose()*QinvAt).transpose());
#else
			JacobiSVD<MatrixXd> svd(A*QinvAt , ComputeThinU | ComputeThinV);
			lam = -svd.solve(b + (f.transpose()*QinvAt).transpose());
			SingularValueType sigmas=svd.singularValues();
#endif
			x = minusQinvf - QinvAt*lam;
			lamIneq = lam.tail(lam.size() - M);
		} else {
			x = minusQinvf;
			lamIneq.resize(0);
		}

		if(Ain.rows() == 0) {
			active.clear();
			break;
		}

		set<int> new_active;

		violation = Ain*x - bin;
		for (i=0; i<M_in; i++)
			if (violation(i) >= 1e-6)
				new_active.insert(i);

		bool all_pos_mults = true;
		for (i=0; i<n_active; i++) {
			if (lamIneq(i)<0) {
				all_pos_mults = false;
				break;
			}
		}
		if (new_active.empty() && all_pos_mults) {
			// existing active was AOK
			break;
		}

		i=0;
		set<int>::iterator iter=active.begin(), tmp;
		while (iter!=active.end()) { // to accomodating inloop erase
			tmp = iter++;
			if (lamIneq(i++)<0) {
				active.erase(tmp);
			}
		}
		active.insert(new_active.begin(),new_active.end());

		if (iterCnt > max_iter) {
			return -1;
		}
	}
	return iterCnt;
}

//template <typename tA, typename tB, typename tC, typename tD, typename tE, typename tF, typename tG>
//int fastQP(vector< MatrixBase<tA>* > QblkDiag, const MatrixBase<tB>& f, const MatrixBase<tC>& Aeq, const MatrixBase<tD>& beq, const MatrixBase<tE>& Ain, const MatrixBase<tF>& bin, set<int>& active, MatrixBase<tG>& x)
int fastQP(vector< MatrixXd* > QblkDiag, const VectorXd& f, const MatrixXd& Aeq, const VectorXd& beq, const MatrixXd& Ain, const VectorXd& bin, set<int>& active, VectorXd& x)
{
	/* min 1/2 * x'QblkDiag'x + f'x s.t A x = b, Ain x <= bin
	 * using active set method.  Iterative solve a linearly constrained
	 * quadratic minimization problem where linear constraints include
	 * Ain(active,:)x == bin(active).  Quit if all dual variables associated
	 * with these equations are positive (i.e. they satisfy KKT conditions).
	 *
	 * Note:
	 * fails if QP is infeasible.
	 * active == initial rows of Ain to treat as equations.
	 * Frank Permenter - June 6th 2013
	 *
	 * @retval  if feasible then iterCnt, else -1 for infeasible, -2 for input error
	 */

	int N = f.rows();

	MatrixXd* Qinv = new MatrixXd[QblkDiag.size()];
	vector< MatrixXd* > Qinvmap;

#define REG 0.0
	// calculate a bunch of stuff that is constant during each iteration
	int startrow=0;
	//typedef typename vector< MatrixBase<tA> >::iterator Qiterator;

	int i=0;
	for (vector< MatrixXd* >::iterator iterQ=QblkDiag.begin(); iterQ!=QblkDiag.end(); iterQ++) {
		MatrixXd* thisQ = *iterQ;
		int numRow = thisQ->rows();
		int numCol = thisQ->cols();

		if (numCol == 1) {  // it's a vector
			VectorXd Qdiag_mod = thisQ->operator+(VectorXd::Constant(numRow,REG)); // regularize
			Qinv[i] = Qdiag_mod.cwiseInverse();
			Qinvmap.push_back( &Qinv[i] );
			startrow=startrow+numRow;
		}
		else
		{ // potentially dense matrix
			if (numRow!=numCol) {
				if (numRow==1)
					cerr << "diagonal Q's must be set as column vectors" << endl;
				else
					cerr << "Q is not square! " << numRow << "x" << numCol << endl;
				return -2;
			}

			MatrixXd Q_mod = thisQ->operator+(REG*MatrixXd::Identity(numRow,numRow));
			Qinv[i] = Q_mod.inverse();
			Qinvmap.push_back( &Qinv[i] );
			startrow=startrow+numRow;
		}
		//  	cout << "Qinv{" << i << "} = " << Qinv[i] << endl;
		if (startrow>N) {
			cerr << "Q is too big!" << endl;
			return -2;
		}
		i++;
	}
	if (startrow!=N) { cerr << "Q is the wrong size.  Got " << startrow << "by" << startrow << " but needed " << N << "by" << N << endl; return -2; }

	int info = fastQPThatTakesQinv(Qinvmap,f,Aeq,beq,Ain,bin,active,x);

	delete[] Qinv;
	return info;
}

/* Example call (allocate inequality matrix, call function, resize inequalites:
   VectorXd binBnd = VectorXd(2*N);
   AinBnd.setZero();
   int numIneq = boundToIneq(ub,lb,AinBnd,binBnd);
   AinBnd.resize(numIneq,N);
   binBnd.resize(numIneq);
   */
/*
   int boundToIneq(const VectorXd& uB,const VectorXd& lB, MatrixXd& Ain, VectorXd& bin)
   {
   int rCnt = 0;
   int cCnt = 0;

   if (uB.rows()+lB.rows() > A.rows() ) {
   cerr << "not enough memory allocated";
   }

   if (uB.rows()+lB.rows() > b.rows() ) {
   cerr << "not enough memory allocated";
   }

   for (int i = 0; i < lB.rows(); i++ ) {
   if (!isinf(lB(i))) {
   cout << lB(i);
   cout << i;
   Ain(rCnt,cCnt++) = -1;//lB(i);
   bin(rCnt++) = -lB(i);
   }
   }
   cCnt = 0;
   for (int i = 0; i < uB.rows(); i++ ) {
   if (!isinf(uB(i))) {
   Ain(rCnt,cCnt++) = 1;//uB(i);
   bin(rCnt++) = uB(i);
   }
   }

//resizing inside function all causes exception (why??)
//A.resize(rCnt,uB.rows());
return rCnt;
}
*/

/*
   template int fastQP(vector< MatrixBase<MatrixXd>* > QblkDiag, const MatrixBase< Map<VectorXd> >&, const MatrixBase< Map<MatrixXd> >&, const MatrixBase< Map<VectorXd> >&, const MatrixBase< Map<MatrixXd> >&, const MatrixBase< Map<VectorXd> >&, set<int>&, MatrixBase< Map<VectorXd> >&);
   template GRBmodel* gurobiQP(GRBenv *env, vector< MatrixBase<MatrixXd>* > QblkDiag, VectorXd& f, const MatrixBase< Map<MatrixXd> >& Aeq, const MatrixBase< Map<VectorXd> >& beq, const MatrixBase< Map<MatrixXd> >& Ain, const MatrixBase< Map<VectorXd> >&bin, VectorXd& lb, VectorXd& ub, set<int>&, VectorXd&);
   template GRBmodel* gurobiQP(GRBenv *env, vector< MatrixBase<MatrixXd>* > QblkDiag, VectorXd& f, const MatrixBase< MatrixXd >& Aeq, const MatrixBase< VectorXd >& beq, const MatrixBase< MatrixXd >& Ain, const MatrixBase< VectorXd >&bin, VectorXd&lb, VectorXd&ub, set<int>&, VectorXd&);
   */

/*
   template int fastQP(vector< MatrixBase< VectorXd > >, const MatrixBase< VectorXd >&, const MatrixBase< Matrix<double,-1,-1,RowMajor,1000,-1> >&, const MatrixBase< Matrix<double,-1,1,0,1000,1> >&, const MatrixBase< Matrix<double,-1,-1,RowMajor,1000,-1> >&, const MatrixBase< Matrix<double,-1,1,0,1000,1> >&, set<int>&, MatrixBase< VectorXd >&);
   */


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FRAC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FRAC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing frac capabilities

    This function returns the fractional part of the input

    @par Semantic:

    @code
    T r = frac(x);
    @endcode

    is similar to:

    @code
    T r =  x-trunc(x);
    @endcode

    @see trunc,  modf

  **/
  const boost::dispatch::functor<tag::frac_> frac = {};
} }
#endif

#include <boost/simd/function/scalar/frac.hpp>
#include <boost/simd/function/simd/frac.hpp>

#endif