#include "mex.h"
#include "drake/drakeUtil.h"
#include <Eigen/Dense>

using namespace Eigen;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    if (nrhs != 4 || nlhs != 2) {
        mexErrMsgIdAndTxt("Drake:lqrmex:InvalidUsage","Usage: [K, S] = lqrmex(A, B, Q, R)");
    }

    const size_t A_rows = mxGetM(prhs[0]);
    const size_t A_cols = mxGetN(prhs[0]);

    const size_t B_rows = mxGetM(prhs[1]);
    const size_t B_cols = mxGetN(prhs[1]);

    const size_t Q_rows = mxGetM(prhs[2]);
    const size_t Q_cols = mxGetN(prhs[2]);

    const size_t R_rows = mxGetM(prhs[3]);
    const size_t R_cols = mxGetN(prhs[3]);

    assert(A_rows == A_cols);
    assert(Q_rows == Q_cols);
    assert(Q_rows == A_rows);
    assert(R_rows == R_cols);
    assert(R_rows == B_cols);

    Map<MatrixXd> A(mxGetPr(prhs[0]), A_rows, A_cols);
    Map<MatrixXd> B(mxGetPr(prhs[1]), B_rows, B_cols);
    Map<MatrixXd> Q(mxGetPr(prhs[2]), Q_rows, Q_cols);
    Map<MatrixXd> R(mxGetPr(prhs[3]), R_rows, R_cols);

    plhs[0] = mxCreateDoubleMatrix(R_rows, A_cols, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(A_rows, A_cols, mxREAL);

    Map<MatrixXd> K(mxGetPr(plhs[0]), R_rows, A_cols);
    Map<MatrixXd> S(mxGetPr(plhs[1]), A_rows, A_cols);

    lqr(A, B, Q, R, K, S);
}


/**
 * @file pfasst/encap/encapsulation.hpp
 * @since v0.1.0
 */
#ifndef _PFASST_ENCAPSULATED_HPP_
#define _PFASST_ENCAPSULATED_HPP_

#include <memory>
#include <vector>
using namespace std;

#include <Eigen/Dense>

template<typename scalar>
using Matrix = Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

#include "pfasst/interfaces.hpp"

namespace pfasst
{
  /**
   * Encapsulations (short _encaps_) are the central data type for all PFASST++ algorithms.
   * Encaps can represent the unknown variable or the result of evaluating the right hand side of
   * the problem equation(s).
   */
  namespace encap
  {
    typedef enum EncapType { solution, function } EncapType;

    /**
     * Data/solution encapsulation.
     *
     * An Encapsulation provides basic mathematical functionality for the user's data.
     *
     * @tparam time time precision; defaults to pfasst::time_precision
     */
    template<typename time = time_precision>
    class Encapsulation
    {
      public:
        //! @{
        virtual ~Encapsulation();
        //! @}

        //! @{
        /**
         * Zeroes out all values of this data structure.
         */
        virtual void zero();

        /**
         * Copies values from @p other into this data structure.
         *
         * @param[in] other other data structure to copy data from
         */
        virtual void copy(shared_ptr<const Encapsulation<time>> other);
        //! @}

        //! @{
        /**
         * Computes the \\( 0 \\)-norm of the data structure's values.
         *
         * @returns \\( 0 \\)-norm of this data structure
         */
        virtual time norm0() const;
        //! @}

        //! @{
        /**
         * Provides basic mathematical operation \\( y = ax + y \\).
         *
         * This is the main mathematical operation applied by PFASST on the data structures.
         * Here, \\( a \\) is a constant and \\( x \\) another data structure (usually of the same
         * type) and \\( y \\) is this data structure.
         *
         * @param[in] a time point to multiply
         * @param[in] x another data structure to scale-add onto this one
         */
        virtual void saxpy(time a, shared_ptr<const Encapsulation<time>> x);

        /**
         * Defines matrix-vector multiplication for this data type.
         *
         * This implements the matrix-vector multiplication of the form
         * \\( \\vec{y}=a M \\vec{x} \\).
         * Here, \\( a \\) is a time point to scale the matrix-vector multiplication with, \\( M \\)
         * is a matrix and \\( x \\) another data structure usually of the same type as this one.
         *
         * If @p zero is `true`, then @p dst is zeroed out before applying the matrix-vector
         * multiplication.
         * Elsewise the result of the matrix-vector multiplication is added onto @p dst.
         *
         * @param[in,out] dst  data structure to store the result in
         * @param[in]     a
         * @param[in]     mat
         * @param[in]     src
         * @param[in]     zero
         */
        virtual void mat_apply(vector<shared_ptr<Encapsulation<time>>> dst,
                               time a, Matrix<time> mat,
                               vector<shared_ptr<Encapsulation<time>>> src,
                               bool zero = true);
        //! @}

        //! @{
        /**
         * Prepare to receive a solution (MPI_IRecv).
         *
         * @param[in] comm     communicator managing the processes to post ??? to/from ???
         * @param[in] tag      tag to distinguish overlapping communication
         */
        virtual void post(ICommunicator* comm, int tag);

        /**
         * Send solution (MPI_Send).
         *
         * @param[in] comm     communicator managing the processes to send to
         * @param[in] tag      tag to distinguish overlapping communication
         * @param[in] blocking whether to use blocking or non-blocking send
         */
        virtual void send(ICommunicator* comm, int tag, bool blocking);

        /**
         * Receive solution (MPI_Recv).
         *
         * @param[in] comm     communicator managing the processes to receive from
         * @param[in] tag      tag to distinguish overlapping communication
         * @param[in] blocking whether to use blocking or non-blocking receive
         */
        virtual void recv(ICommunicator* comm, int tag, bool blocking);

        /**
         * Broadcast this data structure to all processes in @p comm.
         *
         * @param[in] comm communicator managing the processes to send this data structure to
         */
        virtual void broadcast(ICommunicator* comm);
        //! @}
    };

    /**
     * Abstract interface of factory for creating Encapsulation objects.
     *
     * This factory is intended to be instantiated once to create multiple Encapsulation objects
     * of the same type and with the same parameters later on through calls to
     * EncapFactory::create().
     *
     * @tparam time time precision; defaults to pfasst::time_precision
     */
    template<typename time = time_precision>
    class EncapFactory
    {
      public:
        /**
         * Actual method to create Encapsulation object of specific type.
         *
         * @param[in] type encapsulation type of the requested Encapsulation object
         */
        virtual shared_ptr<Encapsulation<time>> create(const EncapType type) = 0;
    };
  }  // ::pfasst::encap
} // ::pfasst

#include "pfasst/encap/encapsulation_impl.hpp"

#endif


/* @copyright The code is licensed under the MIT License
 *            <https://opensource.org/licenses/MIT>,
 *            Copyright (c) 2020 Christian Eskil Vaugelade Berg
 * @author Christian Eskil Vaugelade Berg
*/
#pragma once

#include <orient/axis.hpp>

#include <Eigen/Dense>

namespace orient {

// \brief Construct rotation matrix from scalar angle and a rotation axis
// \param angle Scalar angle
// \template-param axis Rotation axis
// \return Rotation matrix
template<Axis axis, typename Scalar>
Eigen::Matrix<Scalar,3,3> rotationMatrixFromEuler(Scalar angle);

// \brief Construct rotation matrix and partial derivatives from scalar angle and a rotation axis
// \param angle Scalar angle
// \template-param axis Rotation axis
// \return A pair containing first the rotation matrix and then secondly the Jacobian matrix
template<Axis axis, typename Scalar>
std::pair<Eigen::Matrix<Scalar,3,3>, Eigen::Matrix<Scalar,3,3>> rotationMatrixFromEulerWD(Scalar angle);

// \brief Calculate rotation matrix from Euler angles and a rotation order
//        The rotation order is expressed in instrisic rotations.
//        For example, the rotation matrix
//        R = Rz(yaw) * Ry(pitch) * Rx(roll)
//        is constructed with
//        auto R = rotationMatrixFromEuler<Axis::z, Axis::y, Axis::x>([yaw, pitch, roll]);
// \param anlges Source Euler angles
// \template-params A1,A2,A3 The intrinsic rotation order
// \return Rotation matrix
template<Axis A1, Axis A2, Axis A3, typename Scalar>
Eigen::Matrix<Scalar,3,3> rotationMatrixFromEuler(Eigen::Matrix<Scalar,3,1> const& angles);

// \brief Calculate rotation matrix and partial derivatves from Euler angles and a rotation order
//        See above for detail
// \param anlges Source Euler angles
// \template-params A1,A2,A3 The intrinsic rotation order
// \return A pair containing first the rotation matrix and then secondly the Jacobian matrix
template<Axis A1, Axis A2, Axis A3, typename Scalar>
std::pair<Eigen::Matrix<Scalar,3,3>, Eigen::Matrix<Scalar, 9, 3>> rotationMatrixFromEulerWD(Eigen::Matrix<Scalar,3,1> const& angles);

}

#include <orient/impl/from_euler.hpp>


#include <cmath>
#include <boost/foreach.hpp>
#include <boost/unordered_map.hpp>

#include "fastmath.hpp"
#include "nlopt/nlopt.h"
#include "tpbias.hpp"

typedef std::pair<pos_t, pos_t> TPDistPair;
typedef boost::unordered_map<pos_t, double> DistNormMap;

static double geometric_cdf_upper(double p, unsigned int k)
{
    return std::pow(1 - p, k);
}

static double geometric_logcdf_upper(double p, unsigned int k)
{
    return k * fastlog(1 - p);
}

static double tpbias_loglikelihood(double lp,
                                   pos_t maxtlen,
                                   DistNormMap& distnorm,
                                   const std::vector<TPDistPair>& xs)
{
    double p = exp(lp);
    double norm = 0.0;
    double ppower = 1.0;
    for (pos_t tlen = 0; tlen <= maxtlen; ++tlen) {
        ppower *= 1 - p;
        norm += ppower;

        DistNormMap::iterator i = distnorm.find(tlen);
        if (i != distnorm.end()) {
            i->second = fastlog(norm);
        }
    }

    double ll = 0.0;
    BOOST_FOREACH (const TPDistPair& x, xs)  {
        ll += geometric_logcdf_upper(p, x.first) - distnorm[x.second];
    }

    return ll;
}

struct TPBiasTrainData
{
    TPBiasTrainData(pos_t maxtlen,
                    DistNormMap& distnorm,
                    const std::vector<TPDistPair>& xs)
        : maxtlen(maxtlen)
        , distnorm(distnorm)
        , xs(xs)
    {
    }

    pos_t maxtlen;
    DistNormMap& distnorm;
    const std::vector<TPDistPair>& xs;
};

static double tpbias_nlopt_objective(unsigned int _n, const double* _x,
                                     double* _grad, void* data)
{
    UNUSED(_n);
    UNUSED(_grad);

    TPBiasTrainData* traindata =
        reinterpret_cast<TPBiasTrainData*>(data);

    return tpbias_loglikelihood(_x[0], traindata->maxtlen,
                                traindata->distnorm, traindata->xs);
}

TPBias::TPBias(const std::vector<std::pair<pos_t, pos_t> >& tpdists)
{
    // normalizing constants for fragment disributions given tlen
    boost::unordered_map<pos_t, double> distnorm;

    pos_t maxtlen = 0;
    BOOST_FOREACH (const TPDistPair& tpdist_tlen, tpdists) {
        distnorm[tpdist_tlen.second] = 0.0;
        maxtlen = std::max<pos_t>(maxtlen, tpdist_tlen.second);
    }

    TPBiasTrainData traindata(maxtlen, distnorm, tpdists);

#if 0
    FILE* out = fopen("tpdists.tsv", "w");
    fprintf(out, "pos\ttlen\n");
    BOOST_FOREACH (const TPDistPair& x, tpdists)  {
        fprintf(out, "%ld\t%ld\n", x.first, x.second);
    }
    fclose(out);
#endif

    nlopt_opt opt = nlopt_create(NLOPT_LN_SBPLX, 1);
    double lower_limit = -100;
    double upper_limit = -10.0;
    nlopt_set_lower_bounds(opt, &lower_limit);
    nlopt_set_upper_bounds(opt, &upper_limit);
    nlopt_set_max_objective(opt, tpbias_nlopt_objective,
                            reinterpret_cast<void*>(&traindata));
    //nlopt_set_ftol_abs(opt, 1e-4);

    double xtol_abs = 1e-3;
    nlopt_set_xtol_abs(opt, &xtol_abs);

    p = -12;
    double maxf;
    nlopt_result result = nlopt_optimize(opt, &p, &maxf);
    if (result < 0) {
        Logger::warn("Failed to fit 3' bias model.");
        p = 0.0;
    }

    p = exp(p);

    nlopt_destroy(opt);
}

double TPBias::get_bias(pos_t k)
{
    return geometric_cdf_upper(p, k);
}


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

#include <iostream>
#include <boost/math/tools/roots.hpp>

#include "NOpenMeshType.hpp"
//#include "NOpenMeshDesc.hpp"
#include "NOpenMeshBisect.hpp"

template <typename T>
NOpenMeshBisect<T>::NOpenMeshBisect( const SDF& sdf, const P& a, const P& b , float tolerance )
    :
    func(sdf, a, b ),
    tol(tolerance),
    invalid( func(0)*func(1) > 0 ),  // <-- sdf must have different signs at a and b : ie bracket zero
    degenerate( fabs(func(0)) < tolerance && fabs(func(1)) <  tolerance ),  // <-- no root to find, already there
    iterations(15)
{
}

template <typename T>
void NOpenMeshBisect<T>::bisect( P& frontier, float& t )
{
    std::pair<float, float> root = boost::math::tools::bisect(func, 0.f, 1.f, tol, iterations );

    t = (root.first + root.second)/2. ;

    func.position(frontier, t );
}

template <typename T>
NOpenMeshBisectFunc<T>::NOpenMeshBisectFunc( const SDF& sdf, const P& a, const P& b )
    :
    sdf(sdf),
    a(a),
    b(b)
{
}
template <typename T>
void NOpenMeshBisectFunc<T>::position(P& tp, const float t) const
{
    // parameterized position along a -> b line segment
    //    a(1-t)+t*b   t=0 -> a,   t=1 -> b

    const float s = 1.f - t ;
    tp[0] = a[0]*s + b[0]*t ;
    tp[1] = a[1]*s + b[1]*t ;
    tp[2] = a[2]*s + b[2]*t ;
}

template <typename T>
float NOpenMeshBisectFunc<T>::operator()(const float t) const
{
    // signed distance to CSG left/right/composite object from position on line segement
    P tp ;
    position(tp, t );

    float d = sdf( tp[0], tp[1], tp[2] ) ;

/*
    std::cout << "NOpenMeshBisectFunc<T>"
              << " t " << t
              << " tp " << NOpenMeshDesc<T>::desc_point(tp,8,2)
              << " d " << d
              << std::endl ;
*/

    return d ;
}

template struct NOpenMeshBisectFunc<NOpenMeshType> ;
template struct NOpenMeshBisect<NOpenMeshType> ;


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

 * 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


/* ----------------------------------------------------------------------------
 * 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 (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 <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 >&);
   */


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"
#include <math.h>

namespace caffe {

// 这些公式参考这些：https://blog.csdn.net/seven_first/article/details/47378697
template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_add<float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_sqrt<float>(const int n, const float* a, float* y) {
  vsSqrt(n, a, y);
}

template <>
void caffe_sqrt<double>(const int n, const double* a, double* y) {
  vdSqrt(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

/*********new added start***************************************************************************/

// 从这往后新加的代码
template <>
void caffe_clip<int>(const int n, int* y){
// NOT IMPLEMENTED
}

template <>
void caffe_clip<unsigned int>(const int n, unsigned int* y){
// NOT IMPLEMENTED
}

template <>
void caffe_clip<float>(const int n, float* y){
  for(int i=0; i<n; i++){
	const float x = y[i];
	y[i] = (x>1) - (x<1) + x * (x<=1 && x>=-1);
  }
}
/* 实现的功能：
       { -1, x<-1
f(x) = {  x, |x|<=1
       {  1, x>1
*/
template <>
void caffe_clip<double>(const int n, double* y){
  for(int i=0; i<n; i++){
	const double x = y[i];
	y[i] = (x>1) - (x<1) + x * (x<=1 && x>=-1);
  }
}

template <>
void caffe_quantize<int>(const int N, const int left, const int right, const int* X, int *Y){
// NOT IMPLEMENTED
}

template <>
void caffe_quantize<unsigned>(const int N, const unsigned left, const unsigned right,
const unsigned*X, unsigned* Y){
// NOT IMPLEMENTED
}

template <>
void caffe_quantize<float>(const int N, const float left, const float right, const float* X, float* Y){
  for(int i=0; i<N; i++){
	const float x = X[i];
	float idx = (x >= 0) ? floor(log2(x)) : floor(log2(-x));
	idx = (idx<left)*left + (idx>=left && idx<=right)*idx + (idx>right)*right;
	float sign = (x>=0) ? 1.0f : -1.0f;
	float p_up = sign * x / pow(2.0f, idx) - 1;
	float idx_rand = idx + (p_up >= 0.5);

	Y[i] = sign * pow(2.0f, idx_rand);
  }
}

template <>
void caffe_quantize<double>(const int N, const double left, const double right, const double* X, double* Y){
// 参数含义： N：元素个数 left：   right：     X：   Y：
  for(int i=0; i<N; i++){
	const double x = X[i];
	double idx = (x >= 0) ? floor(log2(x)) : floor(log2(-x)); // 看绝对值超过1，置1，否则为0
	idx = (idx<left)*left + (idx>=left && idx<=right)*idx + (idx>right)*right;
	double sign = (x>=0) ? 1.0d : -1.0d;
	double p_up = sign * x / pow(2.0d, idx) - 1;
	double idx_rand = idx + (p_up >= 0.5);

	Y[i] = sign * pow(2.0d, idx_rand);
  }
}

template<>
void caffe_cpu_ternary<int>(const int N, const int delta, const int* X, int* Y){
// NOT IMPLEMENTED
}

template<>
void caffe_cpu_ternary<unsigned>(const int N, const unsigned delta, const unsigned* X, unsigned* Y){
// NOT IMPLEMENTED
}

template<>
void caffe_cpu_ternary<float>(const int N, const float delta, const float* X, float* Y){
	for(int i=0; i<N; i++){
		float x = X[i];
		Y[i] = (x>delta) - (x<-delta);
	}
}

template<>
void caffe_cpu_ternary<double>(const int N, const double delta, const double* X, double* Y){
	for(int i=0; i<N; i++){
		double x = X[i];
		Y[i] = (x>delta) - (x<-delta);
	}
}

///*## 我写的量化公式 start ---- ####################################################################*/
///*
//bit_width:量化的位宽
//
//*/
//template<>
//void caffe_cpu_multi_bit_quantization<int>(const int N, const int bit_width, const int* X, int* Y){
//// NOT IMPLEMENTED
//}
//
//template<>
//void caffe_cpu_multi_bit_quantization<unsigned>(const int N, const int bit_width, const unsigned* X, unsigned* Y){
//// NOT IMPLEMENTED
//}
//
//template<>
//void caffe_cpu_multi_bit_quantization<float>(const int N, const int bit_width, const float* X, float* Y){
//	for(int i=0; i<N; i++){
//        caffe_clip(N, X);
//		float x = X[i];
//		Y[i] = round((pow(2,bit_width)-1.0)*x)/(pow(2,bit_width)-1.0);
//	}
//}
//
//template<>
//void caffe_cpu_multi_bit_quantization<double>(const int N, const int bit_width, const double* X, double* Y){
//	for(int i=0; i<N; i++){
//		caffe_clip(N, X);
//		double x = X[i];
//		Y[i] = round((pow(2,bit_width)-1.0)*x)/(pow(2,bit_width)-1.0);
//	}
//}
///*## 我写的量化公式 end ---- ####################################################################*/

/*********new added end********************************************************************************/
template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_uniform<float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int n, const double a, const double b,
                               double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int n, const double mu,
                                const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

/*
template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}*/

template <>
int caffe_cpu_dot<int>(const int n, const int* x, const int* y){
  // return caffe_cpu_strided_dot(n, x, 1, y, 1);
  return 0;
// NOT IMPLEMENTED
}

template <>
unsigned caffe_cpu_dot<unsigned>(const int n, const unsigned* x, const unsigned* y){
  // return caffe_cpu_strided_dot(n, x, 1, y, 1);
  return 0;
// NOT IMPLEMENTED
}

template <>
float caffe_cpu_dot<float>(const int n, const float* x, const float* y){
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template <>
double caffe_cpu_dot<double>(const int n, const double* x, const double* y){
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template <>
int caffe_cpu_asum<int>(const int n, const int* x){
return 0;
// NOT IMPLEMENTED YET
}

template <>
unsigned caffe_cpu_asum<unsigned>(const int n, const unsigned* x){
return 0;
// NOT IMPLEMENTED YET
}

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}
template <>
void caffe_cpu_scale<int>(const int n, const int alpha, const int* x, int* y){
// NOT IMPLEMENTED YET
}

template <>
void caffe_cpu_scale<unsigned>(const int n, const unsigned alpha, const unsigned* x, unsigned* y){
// NOT IMPLEMENTED YET
}
template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

}  // namespace caffe


/*
  High Performance Astrophysical Reconstruction and Processing (HARP)
  (c) 2014-2015, The Regents of the University of California,
  through Lawrence Berkeley National Laboratory.  See top
  level LICENSE file for details.
*/

#ifndef HARP_LINALG_HPP
#define HARP_LINALG_HPP

#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/storage.hpp>

#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/lapack.hpp>
#include <boost/numeric/bindings/views.hpp>

namespace harp {

  typedef enum {
    EIG_NONE,
    EIG_SQRT,
    EIG_INVSQRT,
    EIG_INV
  } eigen_op;

  typedef boost::numeric::ublas::matrix < double, boost::numeric::ublas::column_major > matrix_double;

  typedef boost::numeric::ublas::compressed_matrix < double, boost::numeric::ublas::row_major > matrix_double_sparse;

  typedef boost::numeric::ublas::vector < double > vector_double;

  typedef boost::numeric::ublas::matrix < uint8_t, boost::numeric::ublas::column_major > matrix_mask;

  typedef boost::numeric::ublas::vector < uint8_t > vector_mask;

  typedef boost::numeric::ublas::matrix < float, boost::numeric::ublas::column_major > matrix_float;

  typedef boost::numeric::ublas::compressed_matrix < float, boost::numeric::ublas::row_major > matrix_float_sparse;

  typedef boost::numeric::ublas::vector < float > vector_float;

  void check_column_major ( boost::numeric::ublas::column_major_tag );

  void check_column_major ( boost::numeric::ublas::row_major_tag );

  template < class M >
  void check_column_major ( boost::numeric::ublas::matrix_expression < M > const & matrix ) {
    typedef typename M::orientation_category orientation_category;
    check_column_major ( orientation_category() );
    return;
  }

  void eigen_decompose ( matrix_double const & invcov, vector_double & D, matrix_double & W, bool regularize );

  void eigen_compose ( eigen_op op, vector_double const & D, matrix_double const & W, matrix_double & out );

  void column_norm ( matrix_double const & mat, vector_double & S );

  void apply_norm ( vector_double const & S, matrix_double & mat );

  void apply_inverse_norm ( vector_double const & S, matrix_double & mat );

  void apply_norm ( vector_double const & S, vector_double & vec );

  void apply_inverse_norm ( vector_double const & S, vector_double & vec );

  void norm ( vector_double const & D, matrix_double const & W, vector_double & S );

  void sparse_mv_trans ( matrix_double_sparse const & AT, vector_double const & in, vector_double & out );

}

#endif