/*
        Olalekan Ogunmolu.
        June 01, 2017

        See: A Model for Registration of 3D shapes,
             Paul Besl and Neil D. McKay

             Eqs 23 - 27
*/

#include "ros/ros.h"
#include <ros/spinner.h>
#include "std_msgs/String.h"
#include "tf_conversions/tf_eigen.h"
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>  //for bt Quaternion
#include <geometry_msgs/Transform.h>

#include <mutex>
#include <vector>
#include <thread>
#include <typeinfo>

#include <vicon_bridge/Markers.h>
#include <geometry_msgs/Point.h>

#include <Eigen/Eigenvalues>
// for sender to RIO
#include <ensenso/boost_sender.h>

using namespace Eigen;
#define OUT(__o__) std::cout<< __o__ << std::endl;

//used to retrieve value from the ros param server
double get(
    const ros::NodeHandle& n,
    const std::string& name) {
    double value;
    n.getParam(name, value);
    return value;
}

class Receiver
{
private:
    int count, num_points; //iterator and number of markers on object
    std::vector<geometry_msgs::Point> headMarkersVector,
                                    firstHeadMarkersVector;
    std::vector<Vector3d> face_vec, first_face_vec;
    // identity matrix involved in Q matrix
    Matrix3d I3;

    // covariance matrix
    Matrix<double, 3, 3> sigma_px, temp, rotation_matrix;
    Matrix<double, 3, 3> A_Mat;

    // Form Q from which we compute the rotation quaternion
    Matrix4d Q;
    // Delta
    Vector3d Delta;
    // will contain rotationand translation of the head
    geometry_msgs::Pose pose_info;

    ros::NodeHandle nm_;
    std::mutex mutex;
    bool updatePose, running, print_;
    ros::AsyncSpinner spinner;
    unsigned long const hardware_threads;

    // pose vector
    ros::Subscriber sub_markers;
    ros::Publisher pose_pub; //publisher for translation and euler angles

    std::thread rotoTransThread;
    double roll, pitch, yaw;
    Vector3d mu_p, mu_x;  // average of points
    geometry_msgs::Point translation_vec_optim; // optimal translation vector
    double x, y, z;
    double q1, q2, q3, q4;

    // sender objects
    boost::asio::io_service io_service;
    const std::string multicast_address;

public:
    Receiver(const bool& print)
    :  hardware_threads(std::thread::hardware_concurrency()),
       spinner(2), count(0), num_points(4), updatePose(false), print_(print),
       multicast_address("235.255.0.1")
    {
       I3.setIdentity(3, 3);
    }

    ~Receiver()
    {
        // rotoTransThread.detach();
    }

    Receiver(Receiver const&) =delete;
    Receiver& operator=(Receiver const&) = delete;

    void run()
    {
      spawn();
      unspawn();
    }
private:
    void spawn()
    {
        if(spinner.canStart())
            spinner.start();
        running = true;
        pose_pub = nm_.advertise<geometry_msgs::Pose>("/mannequine_head/pose", 1000);

        sub_markers = nm_.subscribe("/vicon/markers", 10, &Receiver::callback, this);
        while(!updatePose) {
            if(!ros::ok()) {
              return;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
        // spawn the threads
        rotoTransThread = std::thread(&Receiver::processRotoTrans, this);
        if(rotoTransThread.joinable())
            rotoTransThread.join();
    }

    void unspawn()
    {
        spinner.stop();
        rotoTransThread.detach();
        running = false;
    }

    void callback(const vicon_bridge::MarkersConstPtr& markers_msg)
    {
        // solve all vicon markers here
        std::vector<geometry_msgs::Point> headMarkersVector;
        headMarkersVector.resize(num_points);
        for(auto i=0; i < num_points; ++i){
            headMarkersVector[i] = markers_msg -> markers[i].translation;   // fore
        }

        std::lock_guard<std::mutex> lock(mutex);
        this->headMarkersVector = headMarkersVector;
        updatePose          = true;
        ++count;
    }

    void remove_mean(std::vector<geometry_msgs::Point> && vec, Vector3d&& mu)    {
        double mu_x = 0, mu_y = 0, mu_z = 0;
        // std::cout << "mu_x: " << mu_x << " mu_y: " << mu_y << " mu_z: " << mu_z << std::endl;
        for(auto i = 0; i < num_points; ++i)        {
            mu_x += vec[i].x;
            mu_y += vec[i].y;
            mu_z += vec[i].z;
        }

        mu_x /= num_points;
        mu_y /= num_points;
        mu_z /= num_points;

        mu << mu_x, mu_y, mu_z;

        for(auto i = 0; i < num_points; ++i)        {
            vec[i].x -= mu_x;
            vec[i].y -= mu_y;
            vec[i].z -= mu_z;
            // ROS_INFO("vec[%d].z: %.3f", i, headMarkersVector[i].z);
        }
    }

    void point_to_eigen(std::vector<geometry_msgs::Point>&& pt, std::vector<Vector3d>&& face_vec)    {
        for(auto i=0; i < num_points; ++i){
            face_vec[i] << pt[i].x, pt[i].y, pt[i].z;
        }
    }

    // this closely follows pg 243 of the ICP paper by Besl and McKay
    void processRotoTrans()    {
        std::vector<geometry_msgs::Point> headMarkersVector;
        headMarkersVector.resize(num_points);
        firstHeadMarkersVector.resize(num_points);
        first_face_vec.resize(num_points);
        bool use_hard_coded = get(nm_, "/vicon_icp/Utils/use_hard_coded");

        if(use_hard_coded){
            // seems better we hardcode these values in a yaml file for now
            firstHeadMarkersVector[0].x = get(nm_, "/vicon_icp/BasePose/Marker_Fore/x");
            firstHeadMarkersVector[0].y = get(nm_, "/vicon_icp/BasePose/Marker_Fore/y");
            firstHeadMarkersVector[0].z = get(nm_, "/vicon_icp/BasePose/Marker_Fore/z");

            firstHeadMarkersVector[1].x = get(nm_, "/vicon_icp/BasePose/Marker_Left/x");
            firstHeadMarkersVector[1].y = get(nm_, "/vicon_icp/BasePose/Marker_Left/y");
            firstHeadMarkersVector[1].z = get(nm_, "/vicon_icp/BasePose/Marker_Left/z");

            firstHeadMarkersVector[2].x = get(nm_, "/vicon_icp/BasePose/Marker_Right/x");
            firstHeadMarkersVector[2].y = get(nm_, "/vicon_icp/BasePose/Marker_Right/y");
            firstHeadMarkersVector[2].z = get(nm_, "/vicon_icp/BasePose/Marker_Right/z");

            firstHeadMarkersVector[3].x = get(nm_, "/vicon_icp/BasePose/Marker_Chin/x");
            firstHeadMarkersVector[3].y = get(nm_, "/vicon_icp/BasePose/Marker_Chin/y");
            firstHeadMarkersVector[3].z = get(nm_, "/vicon_icp/BasePose/Marker_Chin/z");
        }
        else{
            if(count==7){
                firstHeadMarkersVector = this->headMarkersVector;
            }
        }

        this->mu_p.resize(3);
        this->mu_x.resize(3);
        remove_mean(std::move(firstHeadMarkersVector), std::move(this->mu_x));  // mu_x is the model point set
        //convert from geometry points to eigen
        point_to_eigen(std::move(firstHeadMarkersVector), std::move(first_face_vec));

        for(; running && ros::ok() ;)
        {

            if(updatePose)
            {

                // ROS_INFO_STREAM("model point set: " << this->mu_x(0) << " | " <<  this->mu_x(1) << " | " << this->mu_x(2));
                // ROS_INFO_STREAM("measured point set: " << this->mu_p(0) << " | " <<  this->mu_p(1) << " | " << this->mu_p(2));
                {
                    std::lock_guard<std::mutex> lock(mutex);
                    headMarkersVector = this->headMarkersVector;
                    updatePose = false;
                }

                //compute center of mass of model and measured point set
                remove_mean(std::move(headMarkersVector), std::move(this->mu_p));  // mu_p is the measured point set
                //convert from geometry points to eigen
                face_vec.resize(num_points);
                point_to_eigen(std::move(headMarkersVector), std::move(face_vec));

                // for(auto elem: face_vec)
                //     ROS_INFO_STREAM("face_vec: " << elem.transpose());
                // for(auto elem: first_face_vec)
                //     ROS_INFO_STREAM("first_face_vec: " << elem.transpose());
                //compute the cross covariance matrix of the points sets P and X
                sigma_px.resize(3, 3); // sigma_px will be 3x3 after the multiplication below

                sigma_px =  first_face_vec[0] * face_vec[0].transpose() +
                            first_face_vec[1] * face_vec[1].transpose() +
                            first_face_vec[2] * face_vec[2].transpose() +
                            first_face_vec[3] * face_vec[3].transpose() ;
                sigma_px /= num_points;
                // ROS_INFO_STREAM("\nsigma_px: \n" << sigma_px);

                // A will be 3x3 skew symmetric
                A_Mat.resize(3, 3);
                A_Mat = sigma_px - sigma_px.transpose();
                // ROS_INFO_STREAM("\nA_Mat: \n" << A_Mat);

                //collect cyclic components of skew symmetric matrix
                Delta << A_Mat(1,2), A_Mat(2, 0), A_Mat(0, 1); // will be of size 3x1
                // ROS_INFO_STREAM("\nDelta: \n" << Delta.transpose());

                temp.resize(3, 3);  // will be 3x3
                temp = sigma_px + sigma_px.transpose() - (sigma_px.trace() * I3);
                // ROS_INFO_STREAM("\ntemp: \n" << temp);

                // Form the symmetric 4x4 Q matrix
                Q(0, 0) =  sigma_px.trace();                  Q.block<1, 3>(0, 1) = Delta.transpose().eval(); //top row, last three entries
                Q.block<3, 1>(1, 0) = Delta;                  Q.bottomRightCorner<3, 3>() = temp;
                // ROS_INFO_STREAM("\nQ: \n" << Q);

                // we now find the maximum eigen value of the matrix Q
                EigenSolver<Matrix4d> eig(Q);

                // Note that eigVal and eigVec are std::complex types. To access their
                // real or imaginary parts, call real or imag
                EigenSolver< Matrix4d >::EigenvalueType eigVals = eig.eigenvalues();
                EigenSolver< Matrix4d >::EigenvectorsType eigVecs = eig.eigenvectors();

                // ROS_INFO_STREAM("eigVals: " << eigVals[0].real() << ", " << eigVals[1].real() << ", " << eigVals[2].real() << ", " << eigVals[3].real());
                findQuaternion(std::move(eigVals), std::move(eigVecs));
            }
        }
    }

    inline void rad2deg(double&& rad) {
        rad = (rad * 180.0)/M_PI;
    }

    void findQuaternion(EigenSolver< Matrix4d >::EigenvalueType&& eigVals, EigenSolver< Matrix4d >::EigenvectorsType && eigVecs)
    {
        //create a look-up table of eig vectors and values
        std::vector<double> valueVectors {eigVals[0].real(), eigVals[1].real(), eigVals[2].real(), eigVals[3].real()};

        auto max = valueVectors[0];
        int magicIdx = 0;
        for(int i = 0; i < valueVectors.size(); ++i)        {
            if(valueVectors[i] > max) {
              max = valueVectors[i];
              magicIdx = i;
            }
        }
        // find the eigen vector with the largest eigen value, This would be the optimal rotation quaternion
        auto optimalEigVec = eigVecs.col(magicIdx);
        // ROS_INFO_STREAM("\neigVec: \n" << eigVecs);
        // ROS_INFO("optimEigVec: %.4f, %.4f, %.4f, %.4f ", optimalEigVec[0].real(), optimalEigVec[1].real(), optimalEigVec[2].real(), optimalEigVec[3].real());
        // Form optimal rotation quaternion components
        double q0 = optimalEigVec[0].real();
        double q1 = optimalEigVec[1].real();
        double q2 = optimalEigVec[2].real();
        double q3 = optimalEigVec[3].real();

        // tf::Quaternion quart(q0, q1, q2, q3);
        // tf::Matrix3x3 Rot(quart);
        // Rot.getRPY(roll, pitch, yaw);
        // tf::matrixTFToEigen (Rot, rotation_matrix);

        // calculate rotation matrix
        rotation_matrix.resize(3, 3);
        rotation_matrix(0, 0) = std::pow(q0, 2) + std::pow(q1, 2) - std::pow(q2, 2) - std::pow(q3, 2);
        rotation_matrix(0, 1) = 2 * (q1*q2 - q0*q3);
        rotation_matrix(0, 2) = 2 * (q1*q3 + q0*q2);
        rotation_matrix(1, 0) = 2 * (q1*q2 + q0*q3);
        rotation_matrix(1, 1) = std::pow(q0, 2) + std::pow(q2, 2) - std::pow(q1, 2) - std::pow(q3, 2);
        rotation_matrix(1, 2) = 2 * (q2*q3 - q0*q1);
        rotation_matrix(2, 0) = 2 * (q1*q3 - q0*q2);
        rotation_matrix(2, 1) = 2 * (q2*q3 + q0*q1);
        rotation_matrix(2, 2) = std::pow(q0, 2) + std::pow(q3, 2) - std::pow(q1, 2) - std::pow(q2, 2);

        // from https://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gac3d90b12b21e1aaa2a9de9b0e45e6b7c
        // in Vector3f ea = mat.eulerAngles(2, 0, 2), for instance
        // "2" represents the z axis and "0" the x axis, etc
        // This corresponds to the right-multiply conventions (with right hand side frames).
        // The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
        Eigen::Vector3d rpy = rotation_matrix.eulerAngles(0, 1, 2);
        roll    = rpy(0);   // roll is about axis x
        pitch   = rpy(1);   // pitch is about axis z
        yaw     = rpy(1);   // yaw about axis y

        // see this: http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf
        // theta =rot about x== roll
        // psi = rot about y == pitch
        // phi = rot about z == yaw
        // double roll2, pitch2, yaw2;
        // if (rotation_matrix(2,0) != 1.0 || rotation_matrix(2,0) != -1.0){
        //     roll = -std::asin( rotation_matrix(2,0) );
        //     pitch = std::atan2( rotation_matrix(2,1)/std::cos(roll), rotation_matrix(2,2)/std::cos(roll) );
        //     yaw   = std::atan2( rotation_matrix(1,0)/std::cos(roll), rotation_matrix(0,0)/std::cos(roll) );

        //     roll2  = M_PI - roll;
        //     pitch2 = std::atan2( rotation_matrix(2,1)/std::cos(roll2), rotation_matrix(2,2)/std::cos(roll2) );
        //     yaw2   = std::atan2( rotation_matrix(1,0)/std::cos(roll2), rotation_matrix(0,0)/std::cos(roll2) );
        // }
        // else{
        //     yaw = 0;
        //     if (rotation_matrix(2,0) == -1.0){
        //         roll = M_PI/2.0;
        //         pitch = yaw + std::atan2(rotation_matrix(0,1), rotation_matrix(0,2));
        //     }
        //     else{
        //         roll = -M_PI/2.0;
        //         pitch = -yaw + std::atan2(-rotation_matrix(0,1), -rotation_matrix(0,2));
        //     }
        // }
        // convert rads to degrees
        rad2deg(std::move(roll));
        rad2deg(std::move(pitch));
        rad2deg(std::move(yaw));
        // ROS_INFO("[(roll, roll2), (pitch, pitch2): , (yaw, yaw2)]: [(%.4f, %.4f), (%.4f, %.4f), (%.4f, %.4f)]", roll, roll2, pitch,pitch2, yaw, yaw2);

        Vector3d optimal_trans = (this->mu_p - /*rotation_matrix **/ this->mu_x);
        pose_info.position.x = optimal_trans(0);
        pose_info.position.y = optimal_trans(1);
        pose_info.position.z = this->mu_p(2)-942;
        // ROS_INFO_STREAM("optimal trans check: " << this->mu_x - rotation_matrix * this->mu_p);

        pose_info.orientation.x = roll;
        pose_info.orientation.y = pitch;
        pose_info.orientation.z = yaw;
        pose_info.orientation.w = 1;

        // publish the head pose
        pose_pub.publish(pose_info);
        if(print_){
            ROS_INFO("z: %.3f | roll: %.3f | pitch: %.3f | yaw: %.3f \n", pose_info.position.z, \
                                                            pose_info.orientation.x, pose_info.orientation.y, pose_info.orientation.z);
        }
        ros::Rate looper(30);
        looper.sleep();

        // ROS_INFO_STREAM("\nrotation matrix\n" << rotation_matrix);

        // define tf matrix to hold xalculated eigen matrix
        // if(std::fabs(rotation_matrix(0,0)) < 0.001 & std::fabs(rotation_matrix(1, 0)) < .001){
        //     //singularity
        //     roll  = 0 ;
        //     pitch = std::atan2(-rotation_matrix(2,0), rotation_matrix(0,0));
        //     yaw   = std::atan2(-rotation_matrix(1,2), rotation_matrix(1,1));
        // }
        // else{
        //     roll = std::atan2(rotation_matrix(1,0), rotation_matrix(0,0));
        //     pitch = std::atan2(-rotation_matrix(2,0),
        //                         std::cos(roll) * rotation_matrix(0,0) + std::sin(roll) * rotation_matrix(1,0));
        //     yaw = std::atan2(std::sin(roll) * rotation_matrix(0,2) - std::cos(roll) * rotation_matrix(1,2),
        //                     std::cos(roll)*rotation_matrix(1,1) - std::sin(roll)*rotation_matrix(0,1));
        // }

        // form quaternion from euler angles
        // tf::Quaternion quat = tf::createQuaternionFromRPY(roll, pitch, yaw);
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "vicon_icp_node");

    if(!ros::ok())
      return EXIT_SUCCESS;

    ROS_INFO_STREAM("Started node " << ros::this_node::getName().c_str());

    bool print;
    if(!ros::param::get("/vicon_icp/Utils/print", print))
        ROS_DEBUG("could not retrieve print param value from ros parameter server");

    Receiver rcvr(print);
    rcvr.run();

    ros::shutdown();
    return EXIT_SUCCESS;
}


/* Copyright (C) 2017 IBM Corp.
 * 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.
 */
/* makeRandomBP.cpp - generate random read-once BPs
 */
#include <fstream>
#include <iostream>
#include <string>

#include <NTL/ZZ.h>
#include <NTL/matrix.h>

#include "../utils/tools.h"
#include "../utils/argmap.h"

NTL_CLIENT

/** Usage example:
 *
 * % ./makeRandomBP_x name=myProgram dim=5 L=4 sig=4
 *
 * This will generate a length-5 BP over 4-symbol alphabet
 * with 5x5 transition matrices
 **/
int main(int argc, char *argv[])
{
    extern bool bGGH15threading;

    ArgMapping amap;

    std::string dirName = "test";
     amap.arg("testDir", dirName, "where to write the obfuscated program");

    std::string name = "P";
    amap.arg("name", name, "Name of BP");
    long dim=2;
    amap.arg("dim", dim, "dimension of transition matrices");
    long L=3;
    amap.arg("L", L, "langth of BP");
    long sig= 2;
    amap.arg("sig", sig, "alphabet size");

    amap.parse(argc, argv); // parses and overrides initail values

    bGGH15threading = 0; //off

    // For each step i=0,1,...,L-1 and each symbol sigma=0,1,...,nSym-1,
    // choose a zero-matrix with probability 1/L, random 0/1 matrix otherwise

    Mat< Mat<long> > trans(INIT_SIZE, L, sig);
    for (long i=0; i<L; i++) for (long iSig=0; iSig<sig; iSig++) {
        Mat<long>& M = trans[i][iSig];
        M.SetDims(dim,dim);
        if (NTL::RandomBnd(L)==0) clear(M);
        else
	  for (long row=0; row<dim; row++) for (long col=0; col<dim; col++) {
              M[row][col] = NTL::RandomBnd(2);
	  }
      }

    // Write the transitions matrices to file

    std::string filename = dirName + "/" + name + "_dim" + ToString(dim)
                                + "_sig" + ToString(sig)
                                + "_L" + ToString(L)
                                + ".txt";

    std::fstream fs;
    fs.open(filename.c_str(), std::ios::out); // E.g., "BPs/P_dim2_sig2_L3.txt"
    if (!fs.is_open())
    {
      std::cout << "Cannot open input file "<< filename << endl;
      exit(0);
    }
    fs << trans << endl;
    fs.close();

#ifdef CodeBlocks
    cin.get();
#endif
}


#include <cstdlib>
#include <iostream>
#include <set>
#include <climits>
#include <algorithm>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/graph/graphviz.hpp>

#include <vector>
#include <utility>
#include <queue>
#include <ctime>

#include "naive.hh"

//using namespace std;

//Treewidth is always positive, so we can use this in place of -Inf
const int NO_WIDTH = -1;

int naiveTW(std::set<Vertex> S, Graph G)
{
    if (S.empty())
    {
        return NO_WIDTH;
    }
    else
    {
        int minSoFar = INT_MAX;

        for (auto iter = S.begin(); iter != S.end(); iter++)
        {
            Vertex v = *iter;
            std::set<Vertex> S2(S);

            S2.erase(v);

            int subTW = naiveTW(S2, G);
            int qVal = sizeQ(S2, v, G);

            //std::cout << "S2 is " << showSet(S2) << std::endl;
            //std::cout << "subTW " << subTW << std::endl;
            //std::cout << "qVal " << qVal << std::endl;

            minSoFar = std::min(minSoFar, std::max(subTW, qVal ) );
        }
        return minSoFar;

    }
}

int naiveMain()
{

    Graph g;
    boost::mt19937 rng(time (NULL));
    boost::generate_random_graph(g, 8, 20, rng, true, true);

    boost::write_graphviz(std::cout, g);

    auto iterInfo = boost::vertices(g);
    std::set<Vertex> S;
    for (auto iter = iterInfo.first; iter != iterInfo.second; iter++ )
    {
        S.insert(*iter);
    }

    std::cout << "Treewidth: " << naiveTW(S, g) << std::endl;

    return EXIT_SUCCESS;

}


/*
   ____    _ __           ____               __    ____
  / __/___(_) /  ___ ____/ __ \__ _____ ___ / /_  /  _/__  ____
 _\ \/ __/ / _ \/ -_) __/ /_/ / // / -_|_-</ __/ _/ // _ \/ __/
/___/\__/_/_.__/\__/_/  \___\_\_,_/\__/___/\__/ /___/_//_/\__(_)

Copyright 2012 SciberQuest Inc.
*/
#include "vtkSQFTLE.h"

#include "vtkCell.h"
#include "vtkCellData.h"
#include "vtkDataSet.h"
#include "vtkDoubleArray.h"
#include "vtkGenericCell.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkTensor.h"
#include "vtkPVXMLElement.h"
#include "XMLUtils.h"
#include "vtkSQLog.h"

#include <cmath>
#include <string>
using std::string;
#include <algorithm>
using std::max;

#include "SQEigenWarningSupression.h"
#include <Eigen/Eigenvalues>
using namespace Eigen;

// ****************************************************************************
static
void ComputeVectorGradient(
      vtkAlgorithm *alg,
      double prog0,
      double prog1,
      vtkDataSet *input,
      vtkIdType nCells,
      vtkDataArray *V,
      vtkDoubleArray *gradV)
{
  const vtkIdType nProgSteps=10;
  const vtkIdType progInt=max(nCells/nProgSteps,vtkIdType(1));
  const double progInc=(prog1-prog0)/nProgSteps;
  double prog=prog0;

  gradV->SetNumberOfComponents(9);
  gradV->SetNumberOfTuples(nCells);
  string name="grad-";
  name+=V->GetName();
  gradV->SetName(name.c_str());
  double *pGradV=gradV->GetPointer(0);

  vtkDoubleArray *cellV=vtkDoubleArray::New();
  cellV->SetNumberOfComponents(3);
  cellV->Allocate(3*VTK_CELL_SIZE);

  vtkGenericCell *cell=vtkGenericCell::New();

  // for each cell
  for (vtkIdType cellId=0; cellId<nCells; ++cellId)
    {
    if (!(cellId%progInt))
      {
      alg->UpdateProgress(prog);
      prog+=progInc;
      }

    input->GetCell(cellId,cell);

    double coords[3];
    cell->GetParametricCenter(coords);

    V->GetTuples(cell->PointIds,cellV);

    double *pCellV=cellV->GetPointer(0);

    double grad[9];
    cell->Derivatives(0,coords,pCellV,3,grad);

    for (int i=0; i<9; ++i)
      {
      pGradV[i]=grad[i];
      }
    pGradV+=9;
    }

  cell->Delete();
  cellV->Delete();
}

// ****************************************************************************
static
void ComputeFTLE(
      vtkAlgorithm *alg,
      double prog0,
      double prog1,
      vtkIdType nCells,
      vtkDoubleArray *gradV,
      double timeInterval,
      vtkDoubleArray *ftleV)
{
  const vtkIdType nProgSteps=10;
  const vtkIdType progInt=max(nCells/nProgSteps,vtkIdType(1));
  const double progInc=(prog1-prog0)/nProgSteps;
  double prog=prog0;

  double *pGradV=gradV->GetPointer(0);

  ftleV->SetNumberOfComponents(1);
  ftleV->SetNumberOfTuples(nCells);
  double *pFtleV=ftleV->GetPointer(0);

  // for each cell
  for (vtkIdType cellId=0; cellId<nCells; ++cellId)
    {
    if (!(cellId%progInt))
      {
      alg->UpdateProgress(prog);
      prog+=progInc;
      }

    Matrix<double,3,3> J;
    J <<
      pGradV[0], pGradV[1], pGradV[2],
      pGradV[3], pGradV[4], pGradV[5],
      pGradV[6], pGradV[7], pGradV[8];

    Matrix<double,3,3> JJT;
    JJT=J*J.transpose();

    // compute eigen values
    Matrix<double,3,1> e;
    SelfAdjointEigenSolver<Matrix<double,3,3> >solver(JJT,false);
    e=solver.eigenvalues();

    double lam;
    lam=max(e(0,0),e(1,0));
    lam=max(lam,e(2,0));
    lam=max(lam,1.0);

    pFtleV[0]=log(sqrt(lam))/timeInterval;

    pFtleV+=1;
    pGradV+=9;
    }
}

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkSQFTLE);

//-----------------------------------------------------------------------------
vtkSQFTLE::vtkSQFTLE()
      :
  PassInput(0),
  TimeInterval(1.0),
  LogLevel(0)
{
  #ifdef SQTK_DEBUG
  pCerr() << "=====vtkSQFTLE::vtkSQFTLE" << endl;
  #endif

  this->SetNumberOfInputPorts(1);
  this->SetNumberOfOutputPorts(1);
}

//-----------------------------------------------------------------------------
int vtkSQFTLE::Initialize(vtkPVXMLElement *root)
{
  #ifdef SQTK_DEBUG
  pCerr() << "=====vtkSQFTLE::Initialize" << endl;
  #endif

  vtkPVXMLElement *elem=0;
  elem=GetOptionalElement(root,"vtkSQFTLE");
  if (elem==0)
    {
    return -1;
    }

  // input arrays, optional but must be set somewhwere
  vtkPVXMLElement *nelem;
  nelem=GetOptionalElement(elem,"input_arrays");
  if (nelem)
    {
    ExtractValues(nelem->GetCharacterData(),this->InputArrays);
    }

  int passInput=0;
  GetOptionalAttribute<int,1>(elem,"pass_input",&passInput);
  if (passInput>0)
    {
    this->SetPassInput(passInput);
    }

  double timeInterval=0.0;
  GetOptionalAttribute<double,1>(elem,"time_interval",&timeInterval);
  if (timeInterval>0.0)
    {
    this->SetTimeInterval(timeInterval);
    }

  vtkSQLog *log=vtkSQLog::GetGlobalInstance();
  int globalLogLevel=log->GetGlobalLevel();
  if (this->LogLevel || globalLogLevel)
    {
    log->GetHeader()
      << "# ::vtkSQFTLE" << "\n"
      << "#   pass_input=" << this->PassInput << "\n"
      << "#   time_interval=" << this->TimeInterval << "\n"
      << "#   input_arrays=";

    set<string>::iterator it=this->InputArrays.begin();
    set<string>::iterator end=this->InputArrays.end();
    for (; it!=end; ++it)
      {
      log->GetHeader() << *it << " ";
      }
    log->GetHeader() << "\n";
    }

  return 0;
}

//-----------------------------------------------------------------------------
void vtkSQFTLE::AddInputArray(const char *name)
{
  #ifdef SQTK_DEBUG
  pCerr()
    << "=====vtkSQFTLE::AddInputArray"
    << "name=" << name << endl;
  #endif

  if (this->InputArrays.insert(name).second)
    {
    this->Modified();
    }
}

//-----------------------------------------------------------------------------
void vtkSQFTLE::ClearInputArrays()
{
  #ifdef SQTK_DEBUG
  pCerr()
    << "=====vtkSQFTLE::ClearInputArrays" << endl;
  #endif

  if (this->InputArrays.size())
    {
    this->InputArrays.clear();
    this->Modified();
    }
}

//-----------------------------------------------------------------------------
int vtkSQFTLE::RequestData(
      vtkInformation *vtkNotUsed(request),
      vtkInformationVector **inInfos,
      vtkInformationVector *outInfos)
{
  #ifdef SQTK_DEBUG
  pCerr() << "=====vtkSQFTLE::RequestData" << endl;
  #endif

  vtkSQLog *log=vtkSQLog::GetGlobalInstance();
  int globalLogLevel=log->GetGlobalLevel();
  if (this->LogLevel || globalLogLevel)
    {
    log->StartEvent("vtkSQFTLE::RequestData");
    }

  vtkInformation *inInfo = inInfos[0]->GetInformationObject(0);
  vtkInformation *outInfo = outInfos->GetInformationObject(0);

  vtkDataSet *input
     = vtkDataSet::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));
  if (!input)
    {
    vtkErrorMacro("Null input.");
    return 1;
    }

  vtkDataSet *output
     = vtkDataSet::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));
  if (!output)
    {
    vtkErrorMacro("Null output.");
    return 1;
    }

  output->CopyStructure(input);
  if (this->PassInput)
    {
    output->CopyAttributes(input);
    }

  vtkIdType nCells=input->GetNumberOfCells();
  if (nCells>0)
    {
    set<string>::iterator it;
    set<string>::iterator begin=this->InputArrays.begin();
    set<string>::iterator end=this->InputArrays.end();
    for (it=begin; it!=end; ++it)
      {
      vtkDataArray *V=input->GetPointData()->GetArray((*it).c_str());
      if (V==0)
        {
        vtkErrorMacro(
          << "Array " << (*it).c_str()
          << " was requested but is not present");
        continue;
        }

      if (V->GetNumberOfComponents()!=3)
        {
        vtkErrorMacro(
          << "Array " << (*it).c_str() << " is not a vector.");
        continue;
        }

      // Gradient.
      vtkDoubleArray *gradV=vtkDoubleArray::New();
      ComputeVectorGradient(this,0.0,0.4,input,nCells,V,gradV);

      // FTLE
      string name;
      name+="ftle-";
      name+=V->GetName();

      vtkDoubleArray *ftleV=vtkDoubleArray::New();
      ftleV->SetName(name.c_str());

      ComputeFTLE(this,0.5,1.0,nCells,gradV,this->TimeInterval,ftleV);

      output->GetCellData()->AddArray(ftleV);

      ftleV->Delete();
      gradV->Delete();
      }
    }

  if (this->LogLevel || globalLogLevel)
    {
    log->EndEvent("vtkSQFTLE::RequestData");
    }

  return 1;
}

//-----------------------------------------------------------------------------
void vtkSQFTLE::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);
}

/*
vtk code for eigen values.

  double ux=pV[0];
  double uy=pV[1];
  double uz=pV[2];
  double vx=pV[3];
  double vy=pV[4];
  double vz=pV[5];
  double wx=pV[6];
  double wy=pV[7];
  double wz=pV[8];

  double JJT0[3], JJT1[3], JJT2[3];
  double *JJT[3]={JJT0, JJT1, JJT2};

  JJT[0][0] = ux*ux+vx*vx+wx*wx;
  JJT[0][1] = ux*uy+vx*vy+wx*wy;
  JJT[0][2] = ux*uz+vx*vz+wx*wz;
  JJT[1][0] = JJT[0][1];
  JJT[1][1] = uy*uy+vy*vy+uz*wz;
  JJT[1][2] = uy*uz+vy*vz+wy*wz;
  JJT[2][0] = JJT[0][2];
  JJT[2][1] = JJT[1][2];
  JJT[2][2] = uz*uz+vz*vz+wz*wz;
*/


/*
 *  (C) Copyright Nick Thompson 2018.
 *  Use, modification and distribution are subject to the
 *  Boost Software License, Version 1.0. (See accompanying file
 *  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#ifndef BOOST_INTEGER_EXTENDED_EUCLIDEAN_HPP
#define BOOST_INTEGER_EXTENDED_EUCLIDEAN_HPP
#include <limits>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <boost/core/swap.hpp>
#include <boost/core/enable_if.hpp>

namespace boost { namespace integer {

// From "The Joy of Factoring", Algorithm 2.7, with a small optimization to remove tmps from Wikipedia.
// Solves mx + ny = gcd(m,n). Returns tuple with (gcd(m,n), x, y).

template<class Z>
struct euclidean_result_t
{
    Z gcd;
    Z x;
    Z y;
};

template<class Z>
typename boost::enable_if_c< std::numeric_limits< Z >::is_signed, euclidean_result_t< Z > >::type
extended_euclidean(Z m, Z n)
{
    if (m < 1 || n < 1)
    {
        BOOST_THROW_EXCEPTION(std::domain_error("extended_euclidean: arguments must be strictly positive"));
    }

    bool swapped = false;
    if (m < n)
    {
        swapped = true;
        boost::swap(m, n);
    }
    Z u0 = m;
    Z u1 = 1;
    Z u2 = 0;
    Z v0 = n;
    Z v1 = 0;
    Z v2 = 1;
    Z w0;
    Z w1;
    Z w2;
    while(v0 > 0)
    {
        Z q = u0/v0;
        w0 = u0 - q*v0;
        w1 = u1 - q*v1;
        w2 = u2 - q*v2;
        u0 = v0;
        u1 = v1;
        u2 = v2;
        v0 = w0;
        v1 = w1;
        v2 = w2;
    }

    euclidean_result_t< Z > result;
    result.gcd = u0;
    if (!swapped)
    {
        result.x = u1;
        result.y = u2;
    }
    else
    {
        result.x = u2;
        result.y = u1;
    }

    return result;
}

}}
#endif


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__MPC__DLTV_OCP_SOLVER_HPP_
#define CBR_CONTROL__MPC__DLTV_OCP_SOLVER_HPP_

#include <Eigen/Dense>

#include <cbr_utils/utils.hpp>

#include <utility>
#include <array>

#include "cbr_control/osqp-cpp.hpp"
#include "ocp_common.hpp"

namespace cbr
{

namespace ocp_detail
{
template<std::size_t nx, std::size_t nu, std::size_t nPts>
constexpr auto DltvOcpSolverCostSparsity()  // objective
{
  constexpr std::size_t nz = (nx + nu) * (nPts - 1);
  std::array<std::size_t, nz> out{};

  std::array<std::size_t, nx> etaX{};
  std::array<std::size_t, nx> etaU{};

  // Generate original vectors
  for (std::size_t i = 0; i < nx; i++) {
    etaX[i] = i + 1;
  }
  for (std::size_t i = 0; i < nu; i++) {
    etaU[i] = i + 1;
  }

  // populate LHS values
  for (std::size_t j = 0; j < nPts - 1; j++) {
    for (std::size_t i = 0; i < nx; i++) {
      out[(j * nx) + i] = etaX[i];
    }
  }

  std::size_t oft = nx * (nPts - 1);
  // populate RHS values
  for (std::size_t j = 0; j < nPts - 1; j++) {
    for (std::size_t i = 0; i < nu; i++) {
      out[oft + (j * nu) + i] = etaU[i];
    }
  }

  return out;
}

template<std::size_t nx, std::size_t nu, std::size_t nPts>
constexpr auto DltvOcpSolverCstrSparsity()  // constraint
{
  constexpr std::size_t nz = (nx + nu) * (nPts - 1);
  std::array<std::size_t, nz> out{};

  std::array<std::size_t, nx> etaX{};
  std::array<std::size_t, nx> etaU{};

  // Generate original vectors
  for (std::size_t i = 0; i < nx; i++) {
    etaX[i] = nx + 2;
  }

  for (std::size_t i = 0; i < nu; i++) {
    etaU[i] = nx + 1;
  }

  // populate LHS values
  for (std::size_t j = 0; j < nPts - 1; j++) {
    for (std::size_t i = 0; i < nx; i++) {
      if (j == nPts - 2) {
        out[(j * nx) + i] = 2;
      } else {
        out[(j * nx) + i] = etaX[i];
      }
    }
  }

  std::size_t oft = nx * (nPts - 1);
  // populate RHS values
  for (std::size_t j = 0; j < nPts - 1; j++) {
    for (std::size_t i = 0; i < nu; i++) {
      out[oft + (j * nu) + i] = etaU[i];
    }
  }

  return out;
}

}  // namespace ocp_detail

/* ---------------------------------------------------------------------------------------------- */
/*                Discrete Time Linear Time Varying Optimal Control Problem Solver                */
/* ---------------------------------------------------------------------------------------------- */

struct DltvOcpSolverParams
{
  osqp::OsqpSettings osqp_settings{};
};

// return code
enum class DltvOcpSolverCode
{
  no_run,
  success,
  failure,
};

/**
 * @brief Solve a linear optimal control problem of type
 *
 *   min_{x, u}   \sum_{k=0}^{K-1} [(1/2) x_k' Q x_k + q' x+k  + (1/2) u_k' R u_k + r' u_k ]  +  (1/2) x_K' QT Q_K
 *
 *    s.t.        x_{k+1} = A_k x_k + B_k u_k + E_k
 *
 * The optimal control problem is defined by implementing get_xxx methods as seen below
 *
 * The variable vector is
 *
 *  [x_1, x_2, \ldots, x_K, u_0, u_1, \ldots, u_{K-1}]
 *
 * @tparam dltv_pb_t
 */
template<typename dltv_pb_t>
class DltvOcpSolver
{
public:
  // Must be defined in dtlv_pb problem
  constexpr static std::size_t nx = dltv_pb_t::nx;
  constexpr static std::size_t nu = dltv_pb_t::nu;
  constexpr static std::size_t nPts = dltv_pb_t::nPts;

  using problem_t = dltv_pb_t;

  // Variable sizes
  constexpr static std::size_t nX = (nPts - 1) * nx;
  constexpr static std::size_t nU = (nPts - 1) * nu;
  constexpr static std::size_t nZ = nX + nU;

  // Constraint matrix sizes
  constexpr static std::size_t nIneqCstr = nZ;
  constexpr static std::size_t nEqCstr = nX;
  constexpr static std::size_t nCstr = nIneqCstr + nEqCstr;

  // QP sparsity
  constexpr static std::array<std::size_t, nZ> costSparsity =
    ocp_detail::DltvOcpSolverCostSparsity<nx, nu, nPts>();
  constexpr static std::array<std::size_t, nZ> cstrSparsity =
    ocp_detail::DltvOcpSolverCstrSparsity<nx, nu, nPts>();

  // Create some useful aliases
  using state_t = Eigen::Matrix<double, nx, 1>;
  using input_t = Eigen::Matrix<double, nu, 1>;
  using A_t = Eigen::Matrix<double, nx, nx>;
  using B_t = Eigen::Matrix<double, nx, nu>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;
  using state_traj_t = Eigen::Matrix<double, nx, nPts>;
  using input_traj_t = Eigen::Matrix<double, nu, nPts>;
  using z_t = Eigen::Matrix<double, nZ, 1>;

  // Get return type of problem functions
  using Ar_t = std::result_of_t<decltype(&dltv_pb_t::get_A)(dltv_pb_t, std::size_t)>;
  using Br_t = std::result_of_t<decltype(&dltv_pb_t::get_B)(dltv_pb_t, std::size_t)>;
  using Qr_t = std::result_of_t<decltype(&dltv_pb_t::get_Q)(dltv_pb_t, std::size_t)>;
  using QTr_t = std::result_of_t<decltype(&dltv_pb_t::get_QT)(dltv_pb_t)>;
  using Rr_t = std::result_of_t<decltype(&dltv_pb_t::get_R)(dltv_pb_t, std::size_t)>;

  /* -------------------------------------------------------------------------- */
  /*                                  Optionals                                 */
  /* -------------------------------------------------------------------------- */

  // Check existance of get_E function
  constexpr static bool has_E_approx = std::experimental::is_detected_v<ocp_detail::has_E_discrete,
      dltv_pb_t>;
  constexpr static bool has_E = std::experimental::is_detected_exact_v<state_t,
      ocp_detail::has_E_discrete, dltv_pb_t>||
    std::experimental::is_detected_exact_v<const state_t &, ocp_detail::has_E_discrete, dltv_pb_t>;
  using Er_t = std::experimental::detected_or_t<state_t, ocp_detail::has_E_discrete, dltv_pb_t>;
  static_assert(
    !(has_E_approx && !has_E),
    "Detected get_E function doesn't have a correct return type. "
    "It must be an nx*1 Eigen::Matrix (or a const reference to one)");

  // Check existance of get_q function
  constexpr static bool has_q_approx = std::experimental::is_detected_v<ocp_detail::has_q_discrete,
      dltv_pb_t>;
  constexpr static bool has_q = std::experimental::is_detected_exact_v<state_t,
      ocp_detail::has_q_discrete, dltv_pb_t>||
    std::experimental::is_detected_exact_v<const state_t &, ocp_detail::has_q_discrete, dltv_pb_t>;
  using qr_t = std::experimental::detected_or_t<state_t, ocp_detail::has_q_discrete, dltv_pb_t>;
  static_assert(
    !(has_q_approx && !has_q),
    "Detected get_q function doesn't have a correct return type. "
    "It must be an nx*1 Eigen::Matrix (or a const reference to one)");

  // Check existance of get_qT function
  constexpr static bool has_qT_approx = std::experimental::is_detected_v<
    ocp_detail::has_qT_discrete, dltv_pb_t>;
  constexpr static bool has_qT = std::experimental::is_detected_exact_v<state_t,
      ocp_detail::has_qT_discrete, dltv_pb_t>||
    std::experimental::is_detected_exact_v<const state_t &, ocp_detail::has_qT_discrete, dltv_pb_t>;
  using qTr_t = std::experimental::detected_or_t<state_t, ocp_detail::has_qT_discrete, dltv_pb_t>;
  static_assert(
    !(has_qT_approx && !has_qT),
    "Detected get_qT function doesn't have a correct return type. "
    "It must be an nx*1 Eigen::Matrix (or a const reference to one)");

  // Check existance of get_r function
  constexpr static bool has_r_approx = std::experimental::is_detected_v<ocp_detail::has_r_discrete,
      dltv_pb_t>;
  constexpr static bool has_r = std::experimental::is_detected_exact_v<input_t,
      ocp_detail::has_r_discrete, dltv_pb_t>||
    std::experimental::is_detected_exact_v<const input_t &, ocp_detail::has_r_discrete, dltv_pb_t>;
  using rr_t = std::experimental::detected_or_t<input_t, ocp_detail::has_r_discrete, dltv_pb_t>;
  static_assert(
    !(has_r_approx && !has_r),
    "Detected get_r function doesn't have a correct return type. "
    "It must be an nu*1 Eigen::Matrix (or a const reference to one)");

  // Check problem dimensions
  static_assert(nx > 0, "Number of states must be > 0.");
  static_assert(nu > 0, "Number of inputs must be > 0.");
  static_assert(nPts > 1, "Number of trajectory points must be > 1.");

  // Check return type of problem functions
  static_assert(
    std::is_same_v<std::decay_t<Ar_t>, A_t>,
    "The get_A method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Br_t>, B_t>,
    "The get_B method of the problem must return an nx*nu Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Qr_t>, Q_t>,
    "The get_Q method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<qr_t>, state_t>,
    "The get_q method of the problem must return an nx*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<QTr_t>, Q_t>,
    "The get_QT method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<qTr_t>, state_t>,
    "The get_qT method of the problem must return an nx*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Rr_t>, R_t>,
    "The get_R method of the problem must return an nu*nu Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<rr_t>, input_t>,
    "The get_r method of the problem must return an nu*1 Eigen::Matrix (or a reference to one).");

public:
  // define structure SOLUTION for the output of the SOLVER
  struct Solution
  {
    DltvOcpSolverCode rc = DltvOcpSolverCode::no_run;
    state_traj_t x = state_traj_t::Zero();
    input_traj_t u = input_traj_t::Zero();
    z_t z = z_t::Zero();
  };

public:
  DltvOcpSolver() = delete;
  DltvOcpSolver(const DltvOcpSolver &) = default;
  DltvOcpSolver(DltvOcpSolver &&) = default;
  DltvOcpSolver & operator=(const DltvOcpSolver &) = default;
  DltvOcpSolver & operator=(DltvOcpSolver &&) = default;

  explicit DltvOcpSolver(const dltv_pb_t & pb)
  : dltv_pb_(pb) {}

  explicit DltvOcpSolver(dltv_pb_t && pb)
  : dltv_pb_(std::move(pb)) {}

  template<typename T1, typename T2>
  DltvOcpSolver(T1 && pb, T2 && prm)
  : dltv_pb_(std::forward<T1>(pb)),
    prm_(std::forward<T2>(prm)) {}

public:
  /**
   * @brief Read problem information and build constraint matrices
   */
  void init()
  {
    osqp::OsqpInstance osqp_instance;

    // Resize osqp_instance matrices
    osqp_instance.objective_matrix.resize(nZ, nZ);
    osqp_instance.objective_vector.resize(nZ);
    osqp_instance.constraint_matrix.resize(nCstr, nZ);
    osqp_instance.lower_bounds.resize(nCstr);
    osqp_instance.upper_bounds.resize(nCstr);

    // Set sparsity structure
    const Eigen::Map<const Eigen::Matrix<std::size_t, nZ, 1>> costSparsityVec(costSparsity.data());
    const Eigen::Map<const Eigen::Matrix<std::size_t, nZ, 1>> cstrSparsityVec(cstrSparsity.data());
    osqp_instance.objective_matrix.reserve(costSparsityVec);
    osqp_instance.constraint_matrix.reserve(cstrSparsityVec);

    // Get inital state
    dltv_pb_.get_x0(sol_.x.col(0));

    /* -------------------------------------------------------------------------- */
    /*                         Set state and input bounds                         */
    /* -------------------------------------------------------------------------- */

    for (std::size_t i = 0; i < nPts - 1; i++) {
      // state bounds
      dltv_pb_.get_state_lb(i + 1, osqp_instance.lower_bounds.segment<nx>(i * nx));
      dltv_pb_.get_state_ub(i + 1, osqp_instance.upper_bounds.segment<nx>(i * nx));
      // input bounds
      dltv_pb_.get_input_ub(i, osqp_instance.upper_bounds.segment<nu>(nX + i * nu));
      dltv_pb_.get_input_lb(i, osqp_instance.lower_bounds.segment<nu>(nX + i * nu));
    }

    // Set equality constraints bounds A(0)*x(0)
    osqp_instance.lower_bounds.segment<nx>(nZ) = dltv_pb_.get_A(0) * sol_.x.col(0);

    if constexpr (has_E) {
      osqp_instance.lower_bounds.segment<nx>(nZ) += dltv_pb_.get_E(0);

      for (std::size_t i = 1; i < nPts - 1; i++) {
        osqp_instance.lower_bounds.segment<nx>(nZ + (i * nx)) = dltv_pb_.get_E(i);
      }
    } else {
      osqp_instance.lower_bounds.segment<nx * (nPts - 2)>(nZ + nx) =
        Eigen::Matrix<double, nx *(nPts - 2), 1>::Zero();
    }

    // Duplicate lower bound constraints onto upper bounds to make equalities.
    osqp_instance.upper_bounds.segment<nEqCstr>(nZ) =
      osqp_instance.lower_bounds.segment<nEqCstr>(nZ);

    /* -------------------------------------------------------------------------- */
    /*                            Set contraint matrix A                          */
    /* -------------------------------------------------------------------------- */

    // Set bound constraints matrix
    for (std::size_t i = 0; i < nZ; i++) {
      osqp_instance.constraint_matrix.insert(i, i) = 1.;
    }

    // Fill up Go_eq Bottom upper half: I_(nx+nu*nx+nu)
    for (std::size_t i = nZ; i < nCstr; i++) {
      osqp_instance.constraint_matrix.insert(i, i - nZ) = 1.;
    }

    // Fill up Go_eq Bottom Left subDiagonal: -A (note i = 1 starting point)
    for (std::size_t i = 1; i < nPts - 1; i++) {
      const std::size_t i_col = (i - 1) * nx;
      const std::size_t i_row = nZ + i * nx;
      const Ar_t A = dltv_pb_.get_A(i);
      for (std::size_t c = 0; c < nx; ++c) {
        for (std::size_t r = 0; r < nx; ++r) {
          osqp_instance.constraint_matrix.insert(i_row + r, i_col + c) = -A(r, c);
        }
      }
    }

    // Fill up Go_eq Bottom Right Block Diagonal: -B
    for (std::size_t i = 0; i < nPts - 1; i++) {
      const std::size_t i_col = nX + i * nu;
      const std::size_t i_row = nZ + i * nx;
      const Br_t B = dltv_pb_.get_B(i);
      for (std::size_t c = 0; c < nu; ++c) {
        for (std::size_t r = 0; r < nx; ++r) {
          osqp_instance.constraint_matrix.insert(i_row + r, i_col + c) = -B(r, c);
        }
      }
    }

    /* -------------------------------------------------------------------------- */
    /*                   Set objective Vector q' = [qx qu]'                       */
    /* -------------------------------------------------------------------------- */

    //  fill in q
    if constexpr (has_q) {
      for (std::size_t i = 0; i < (nPts - 2); i++) {
        osqp_instance.objective_vector.segment<nx>(i * nx) = dltv_pb_.get_q(i + 1);
      }
    } else {
      osqp_instance.objective_vector.segment<nx * (nPts - 2)>(0).setZero();
    }
    //  fill in qT
    if constexpr (has_qT) {
      osqp_instance.objective_vector.segment<nx>((nPts - 2) * nx) = dltv_pb_.get_qT();
    } else {
      osqp_instance.objective_vector.segment<nx>((nPts - 2) * nx).setZero();
    }

    //  fill in r
    if constexpr (has_r) {
      for (std::size_t i = 0; i < (nPts - 1); i++) {
        osqp_instance.objective_vector.segment<nu>(nX + (i * nu)) = dltv_pb_.get_r(i);
      }
    } else {
      osqp_instance.objective_vector.segment<nu * (nPts - 1)>(nX).setZero();
    }

    /* -------------------------------------------------------------------------- */
    /*                     Set Objective Matrix P = [Q/QT/R]                      */
    /* -------------------------------------------------------------------------- */

    // Fill up Diagonal: Q
    for (std::size_t i = 0; i < nPts - 2; i++) {
      const std::size_t i_col = i * nx;
      const std::size_t i_row = i * nx;
      const Qr_t Q = dltv_pb_.get_Q(i + 1);
      for (std::size_t c = 0; c < nx; ++c) {
        for (std::size_t r = 0; r < c; ++r) {
          osqp_instance.objective_matrix.insert(i_row + r, i_col + c) = (Q(r, c) + Q(c, r)) / 2;
        }
        osqp_instance.objective_matrix.insert(i_row + c, i_col + c) = Q(c, c);
      }
    }

    // Fill up Diagonal: QT
    const std::size_t i_col = (nPts - 2) * nx;
    const std::size_t i_row = (nPts - 2) * nx;
    const QTr_t QT = dltv_pb_.get_QT();
    for (std::size_t c = 0; c < nx; ++c) {
      for (std::size_t r = 0; r < c; ++r) {
        osqp_instance.objective_matrix.insert(i_row + r, i_col + c) = (QT(r, c) + QT(c, r)) / 2;
      }
      osqp_instance.objective_matrix.insert(i_row + c, i_col + c) = QT(c, c);
    }

    // Fill up Diagonal: R
    for (std::size_t i = 0; i < nPts - 1; i++) {
      const std::size_t i_col = nX + i * nu;
      const std::size_t i_row = nX + i * nu;
      const Rr_t R = dltv_pb_.get_R(i);
      for (std::size_t c = 0; c < nu; ++c) {
        for (std::size_t r = 0; r < c; ++r) {
          osqp_instance.objective_matrix.insert(i_row + r, i_col + c) = (R(r, c) + R(c, r)) / 2;
        }
        osqp_instance.objective_matrix.insert(i_row + c, i_col + c) = R(c, c);
      }
    }

    /* ------------------------ compress Sparse Matrices ------------------------ */
    osqp_instance.objective_matrix.makeCompressed();
    osqp_instance.constraint_matrix.makeCompressed();

    const auto status = osqp_solver_.Init(osqp_instance, prm_.osqp_settings, true);

    if (!status.ok()) {
      throw std::runtime_error("Osqp initialization failed.");
    }

    osqp_instance_ = std::move(osqp_instance);
  }

  /**
   * @brief Set initial guess for problem solution
   *
   * @param x nx x nPts matrix of state values
   * @param u nu x nPts matrix of input values
   */
  template<typename Derived1, typename Derived2>
  void set_ic(const Eigen::MatrixBase<Derived1> & x, const Eigen::MatrixBase<Derived2> & u)
  {
    static_assert(
      Derived1::RowsAtCompileTime == nx,
      "Number of states inconsistant with the problem.");
    static_assert(
      Derived2::RowsAtCompileTime == nu,
      "Number of inputs inconsistant with the problem.");
    static_assert(
      Derived1::ColsAtCompileTime == nPts,
      "Number of state initial conditions inconsistent with the problem.");
    static_assert(
      Derived2::ColsAtCompileTime == nPts,
      "Number of input initial conditions inconsistent with the problem.");

    Eigen::Map<Eigen::Matrix<double, nx, nPts - 1>> varX(sol_.z.data());
    varX = x.template rightCols<nPts - 1>();

    Eigen::Map<Eigen::Matrix<double, nu, nPts - 1>> varU(sol_.z.data() + nX);
    varU = u.template leftCols<nPts - 1>();

    osqp_solver_.SetPrimalWarmStart(sol_.z);
  }

  /**
   * @brief Update solver parameters
   */
  template<typename T>
  void update_params(T && p)
  {
    prm_ = std::forward<T>(p);

    const auto status = osqp_solver_.Init(osqp_instance_, prm_.osqp_settings, true);

    if (!status.ok()) {
      throw std::runtime_error("Osqp initialization failed.");
    }
  }

  /**
   * @brief Solve the problem and return solution
   *
   * Note that init() must have been called prior to this function
   */
  Solution solve()
  {
    osqp::OsqpExitCode exit_code = osqp_solver_.Solve();

    if (exit_code == osqp::OsqpExitCode::kOptimal) {
      sol_.rc = DltvOcpSolverCode::success;
    } else {
      sol_.rc = DltvOcpSolverCode::failure;
    }

    sol_.z = osqp_solver_.primal_solution();

    // Create X
    const Eigen::Map<const Eigen::Matrix<double, nx, nPts - 1>> mX(sol_.z.data());
    sol_.x.template rightCols<nPts - 1>() = mX;

    // Create U
    const Eigen::Map<const Eigen::Matrix<double, nu, nPts - 1>> mU(sol_.z.data() + nX);

    sol_.u.col(nPts - 1) = mU.template rightCols<1>();
    sol_.u.template leftCols<nPts - 1>() = mU;

    return sol_;
  }

  Solution solution() const
  {
    return sol_;
  }

  dltv_pb_t & problem()
  {
    return dltv_pb_;
  }

protected:
  dltv_pb_t dltv_pb_{};
  DltvOcpSolverParams prm_{};
  Solution sol_{};
  osqp::OsqpInstance osqp_instance_{};
  osqp::OsqpSolver osqp_solver_{};
};

// Class template argument deduction guides
template<typename T>
DltvOcpSolver(T)->DltvOcpSolver<T>;

template<typename T1, typename T2>
DltvOcpSolver(T1, T2)->DltvOcpSolver<T1>;

}  // namespace cbr

#endif  // CBR_CONTROL__MPC__DLTV_OCP_SOLVER_HPP_


#include "sharp_edges.h"
#include <igl/unique_edge_map.h>
#include <igl/per_face_normals.h>
#include <igl/PI.h>
#include <Eigen/Geometry>

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedSE,
  typename DerivedE,
  typename DeriveduE,
  typename DerivedEMAP,
  typename uE2Etype,
  typename sharptype>
IGL_INLINE void igl::sharp_edges(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const typename DerivedV::Scalar angle,
  Eigen::PlainObjectBase<DerivedSE> & SE,
  Eigen::PlainObjectBase<DerivedE> & E,
  Eigen::PlainObjectBase<DeriveduE> & uE,
  Eigen::PlainObjectBase<DerivedEMAP> & EMAP,
  std::vector<std::vector<uE2Etype> > & uE2E,
  std::vector< sharptype > & sharp)
{
  typedef typename DerivedSE::Scalar Index;
  typedef typename DerivedV::Scalar Scalar;
  typedef Eigen::Matrix<Index,Eigen::Dynamic,2> MatrixX2I;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,3> MatrixX3S;
  typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
  typedef Eigen::Matrix<Index,Eigen::Dynamic,1> VectorXI;

  unique_edge_map(F,E,uE,EMAP,uE2E);
  MatrixX3S N;
  per_face_normals(V,F,N);
  // number of faces
  const Index m = F.rows();
  // Dihedral angles
  //std::vector<Eigen::Triplet<Scalar,int> > DIJV;
  sharp.clear();
  // Loop over each unique edge
  for(int u = 0;u<uE2E.size();u++)
  {
    bool u_is_sharp = false;
    // Consider every pair of incident faces
    //
    // if there are 3 faces (non-manifold) it appears to follow that the edge
    // must be sharp if angle<60. Could skip those (they're likely small number
    // anyway).
    for(int i = 0;i<uE2E[u].size();i++)
    for(int j = i+1;j<uE2E[u].size();j++)
    {
      const int ei = uE2E[u][i];
      const int fi = ei%m;
      const int ej = uE2E[u][j];
      const int fj = ej%m;
      const RowVector3S ni = N.row(fi);
      const RowVector3S nj = N.row(fj);
      // Edge vector
      // normalization might not be necessary
      const RowVector3S ev = (V.row(E(ei,1)) - V.row(E(ei,0))).normalized();
      const Scalar dij =
        igl::PI - atan2((ni.cross(nj)).dot(ev),ni.dot(nj));
      //DIJV.emplace_back(fi,fj,dij);
      if(std::abs(dij-igl::PI) > angle)
      {
        u_is_sharp = true;
      }
    }
    if(u_is_sharp)
    {
      sharp.push_back(u);
    }
  }
  SE.resize(sharp.size(),2);
  for(int i = 0;i<SE.rows();i++)
  {
    SE(i,0) = uE(sharp[i],0);
    SE(i,1) = uE(sharp[i],1);
  }
}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedSE>
IGL_INLINE void igl::sharp_edges(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const typename DerivedV::Scalar angle,
  Eigen::PlainObjectBase<DerivedSE> & SE
  )
{
  typedef typename DerivedSE::Scalar Index;
  typedef typename DerivedV::Scalar Scalar;
  typedef Eigen::Matrix<Index,Eigen::Dynamic,2> MatrixX2I;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,3> MatrixX3S;
  typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
  typedef Eigen::Matrix<Index,Eigen::Dynamic,1> VectorXI;
  MatrixX2I E,uE;
  VectorXI EMAP;
  std::vector<std::vector<Index> > uE2E;
  std::vector<int>  sharp;
  return sharp_edges(V,F,angle,SE,E,uE,EMAP,uE2E,sharp);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::sharp_edges<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
template void igl::sharp_edges<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, int, int>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > >&, std::vector<int, std::allocator<int> >&);
#endif


#include <Eigen/Eigenvalues>
#include "ndt_mcl/particle_filter.hpp"
#include "ros/ros.h"

bool myfunction (double i,double j) { return (i>j); }

perception_oru::particle_filter::particle_filter(perception_oru::NDTMap *ndtMap_, int particleCount_ /*, init_type initializationType_*/, bool be2D_, bool forceSIR_, double varLimit_, int sirCount_){
	be2D = be2D_;
	ndtMap = ndtMap_;
	// initializationType=initializationType_;
	particleCount = particleCount_;
	forceSIR = forceSIR_;
	varLimit = varLimit_;
	sirCount = sirCount_;
	weights.resize(particleCount_,0);
}

void perception_oru::particle_filter::Reset(){
	tmp.clear();
	delete ndt_ISSMap;
	particleCloud.clear();
}

void perception_oru::particle_filter::InitializeNormal(double x, double y, double th, double var){

	if(particleCloud.size() > 0){
		particleCloud.clear();
		tmp.clear();
	}
	m_pose<<x,y,th;
	tmp.resize(particleCount);
	std::default_random_engine generator;

	for(int parNo = 0; parNo < particleCount; parNo++){
		std::normal_distribution<double> distribution_x(x, var);
		std::normal_distribution<double> distribution_y(y, var);
		std::normal_distribution<double> distribution_t(th, th * var);
		particleCloud.emplace_back(0.0, 0.0, distribution_t(generator), distribution_x(generator), distribution_y(generator), 0.0);
	}
}

void perception_oru::particle_filter::InitializeNormal(double x, double y, double var){

	if(particleCloud.size() > 0){
		particleCloud.clear();
		tmp.clear();
	}
		m_pose<<x,y,0;
	tmp.resize(particleCount);
	std::default_random_engine generator;
	for(int parNo = 0; parNo < particleCount; parNo++){
		std::normal_distribution<double> distribution_x(x, var);
		std::normal_distribution<double> distribution_y(y, var);
		std::uniform_real_distribution<double> distribution_t(0, 2 * 3.1415);
		particleCloud.emplace_back(0.0, 0.0, distribution_t(generator), distribution_x(generator), distribution_y(generator), 0.0);
	}
}

Eigen::Vector3d perception_oru::particle_filter::GetMeanPose2D(){
	double sumX = 0, sumY = 0;
	Eigen::Vector3d pos;
	double sumW = 0;
	double ax = 0, ay = 0;

	for(int i = 0; i < particleCloud.size(); i++){
		double x, y, z, r, p, t;
		particleCloud[i].GetXYZ(x, y, z);
		particleCloud[i].GetRPY(r, p, t);
		sumX += particleCloud[i].GetProbability() * x;
		sumY += particleCloud[i].GetProbability() * y;
		ax += particleCloud[i].GetProbability() * cos(t);
		ay += particleCloud[i].GetProbability() * sin(t);
		sumW += particleCloud[i].GetProbability();
	}
	pos << sumX, sumY, atan2(ay, ax);
	return pos;
}

void perception_oru::particle_filter::GetPoseMeanAndVariance2D(Eigen::Vector3d &mean, Eigen::Matrix3d &cov){
	double sumX = 0, sumY = 0;
	double sumW = 0;
	double ax = 0, ay = 0;

	for(int i = 0; i < particleCloud.size(); i++){
		double x, y, z, r, p, t;
		particleCloud[i].GetXYZ(x, y, z);
		particleCloud[i].GetRPY(r, p, t);
		sumX += particleCloud[i].GetProbability() * x;
		sumY += particleCloud[i].GetProbability() * y;
		ax += particleCloud[i].GetProbability() * cos(t);
		ay += particleCloud[i].GetProbability() * sin(t);
		sumW += particleCloud[i].GetProbability();
	}
	mean << sumX, sumY, atan2(ay, ax);

	double xx = 0, yy = 0, xy = 0, aax = 0, aay = 0;
	double cax = cos(atan2(ay, ax));
	double say = sin(atan2(ay, ax));
	double w2 = 0;

	for(int i = 0; i < particleCloud.size(); i++){
		double x, y, z, r, p, t;
		particleCloud[i].GetXYZ(x, y, z);
		particleCloud[i].GetRPY(r, p, t);
		xx += particleCloud[i].GetProbability() * (x - sumX) * (x - sumX);
		yy += particleCloud[i].GetProbability() * (y - sumY) * (y - sumY);
		xy += particleCloud[i].GetProbability() * (x - sumX) * (y - sumY);
		aax += particleCloud[i].GetProbability() * (cos(t) - cax) * (cos(t) - cax);
		aay += particleCloud[i].GetProbability() * (sin(t) - say) * (sin(t) - say);
		w2 += particleCloud[i].GetProbability() * particleCloud[i].GetProbability();
	}

	if(w2 == 1.0){
		fprintf(stderr, "CParticleFilter::getDistributionVariances -- w2=%lf Should not happen!\n", w2);
		w2 = 0.99;
	}
	double wc = 1.0 / (1.0 - w2);
	cov << wc * xx, wc * xy, 0,
	wc * xy, wc * yy, 0,
	0, 0, atan2(wc * aay, wc * aax);
}

void perception_oru::particle_filter::InitializeUniformMap(){
	int pCount_ = particleCount;

	std::vector<perception_oru::NDTCell*> allCells = ndtMap->getAllInitializedCells();
	std::vector<perception_oru::NDTCell*> cells;
	for(int cInd = 0; cInd < allCells.size(); cInd++)
		if(allCells[cInd]->getOccupancy() < 0.0)
			cells.push_back(allCells[cInd]);

	std::default_random_engine generator;
	std::uniform_int_distribution<int> distribution_c(0, cells.size() - 1);
	tmp.resize(pCount_);
	for(int parNo = 0; parNo < pCount_; parNo++){
		int cellId = distribution_c(generator);
		double cx, cy, cz, sx, sy, sz;
		if(be2D){
			cells[cellId]->getCenter(cx, cy, cz);
			cells[cellId]->getDimensions(sx, sy, sz);
			std::uniform_real_distribution<double> distribution_x(cx - sx / 2.0, cx + sx / 2.0);
			std::uniform_real_distribution<double> distribution_y(cy - sy / 2.0, cy + sy / 2.0);
			std::uniform_real_distribution<double> distribution_t(0.0, 2 * M_PI);
			particleCloud.emplace_back(0.0, 0.0, distribution_t(generator), distribution_x(generator), distribution_y(generator), 0.0);
		}
		//here will go 3d distibution
	}
}

void perception_oru::particle_filter::InitializeFilter(){
	// switch(initializationType){
	//case uniform_map:
	InitializeUniformMap();
	// break;
	//  default:3
	//}
}

void perception_oru::particle_filter::UpdateAndPredict(Eigen::Affine3d tMotion, perception_oru::NDTMap ndtLocalMap_){
	Eigen::Vector3d tr = tMotion.translation();
	Eigen::Vector3d rot = tMotion.rotation().eulerAngles(0, 1, 2);

	if(tr[0] != 0.0 && tr[1] != 0.0 && rot[2] != 0){
		Predict2D(tr[0], tr[1], rot[2], tr[0] * 0.03 + 0.005, tr[1] * 0.03 + 0.005, rot[2] * 0.08 + 0.01);
				#pragma omp parallel for
		for(int i = 0; i < particleCloud.size(); i++){
			Eigen::Affine3d T = particleCloud[i].GetAsAffine();
			std::vector<perception_oru::NDTCell*> ndts;
			ndts = ndtLocalMap_.pseudoTransformNDT(T);
			double score = 1;
			if(ndts.size() == 0) fprintf(stderr, "ERROR no gaussians in measurement!!!\n");
			for(int n = 0; n < ndts.size(); n++){
				Eigen::Vector3d m = ndts[n]->getMean();
				perception_oru::NDTCell *cell;
				pcl::PointXYZ p;
				p.x = m[0]; p.y = m[1]; p.z = m[2];
				if(ndtMap->getCellForPoint(p, cell)){
					if(cell == NULL) continue;
					if(cell->hasGaussian_){
						Eigen::Matrix3d covCombined = cell->getCov() + ndts[n]->getCov();
						Eigen::Matrix3d icov;
						bool exists;
						double det = 0;
						covCombined.computeInverseAndDetWithCheck(icov, det, exists);
						if(!exists) continue;
						double l = (cell->getMean() - m).dot(icov * (cell->getMean() - m));
						if(l * 0 != 0) continue;
						score += 0.1 + 0.9 * exp(-0.05 * l / 2.0);
					}else {
					}
				}
			}
			particleCloud[i].SetLikelihood(score);
			for(unsigned int j = 0; j < ndts.size(); j++)
				delete ndts[j];
		}
		Normalize();
		if(forceSIR)
			SIRUpdate();
		else{
			double varP = 0;
			for(int i = 0; i < particleCloud.size(); i++)
				varP += (particleCloud[i].GetProbability() - 1.0 / double(particleCloud.size()))
				        * (particleCloud[i].GetProbability() - 1.0 / double(particleCloud.size()));
			varP /= double(particleCloud.size());
			varP = sqrt(varP);
			if(varP > varLimit || sinceSIR > sirCount){
				//fprintf(stderr,"-SIR- ");
				sinceSIR = 0;
				SIRUpdate();
			}else
				sinceSIR++;
		}
	}
}
void perception_oru::particle_filter::UpdateAndPredict(Eigen::Affine3d tMotion, perception_oru::NDTMap* ndtLocalMap_){
	Eigen::Vector3d tr = tMotion.translation();
	Eigen::Vector3d rot = tMotion.rotation().eulerAngles(0, 1, 2);

	if(tr[0] != 0.0 && tr[1] != 0.0 && rot[2] != 0){
		Predict2D(tr[0], tr[1], rot[2], tr[0] * 0.1 + 0.005, tr[1] * 0.1 + 0.005, rot[2] * 0.1 + 0.001);
				#pragma omp parallel for
		for(int i = 0; i < particleCloud.size(); i++){
			Eigen::Affine3d T = particleCloud[i].GetAsAffine();
			std::vector<perception_oru::NDTCell*> ndts;
			ndts = ndtLocalMap_->pseudoTransformNDT(T);
			double score = 1;
			if(ndts.size() == 0) fprintf(stderr, "ERROR no gaussians in measurement!!!\n");
			for(int n = 0; n < ndts.size(); n++){
				Eigen::Vector3d m = ndts[n]->getMean();
				perception_oru::NDTCell *cell;
				//pcl::PointXYZ p;
				//p.x = m[0];p.y=m[1];p.z=m[2];
				pcl::PointXYZ p(m[0], m[1], m[2]);
				if(ndtMap->getCellForPoint(p, cell)){
					if(cell == NULL) continue;
					if(cell->hasGaussian_){
						Eigen::Matrix3d covCombined = cell->getCov() + ndts[n]->getCov();
						Eigen::Matrix3d icov;
						bool exists;
						double det = 0;
						covCombined.computeInverseAndDetWithCheck(icov, det, exists);
						if(!exists) continue;
						double l = (cell->getMean() - m).dot(icov * (cell->getMean() - m));
						if(l * 0 != 0) continue;
						score += 0.1 + 0.9 * exp(-0.05 * l / 2.0);
					}else {
					}
				}
			}
			particleCloud[i].SetLikelihood(score);
			for(unsigned int j = 0; j < ndts.size(); j++)
				delete ndts[j];
		}
		Normalize();
		if(forceSIR)
			SIRUpdate();
		else{
			double varP = 0;
			for(int i = 0; i < particleCloud.size(); i++)
				varP += (particleCloud[i].GetProbability() - 1.0 / double(particleCloud.size()))
				        * (particleCloud[i].GetProbability() - 1.0 / double(particleCloud.size()));
			varP /= double(particleCloud.size());
			varP = sqrt(varP);
			if(varP > varLimit || sinceSIR > sirCount){
				//fprintf(stderr,"-SIR- ");
				sinceSIR = 0;
				SIRUpdate();
			}else
				sinceSIR++;
		}
	}
}

void perception_oru::particle_filter::UpdateAndPredictEff(Eigen::Affine3d tMotion, perception_oru::NDTMap* ndtLocalMap_, double subsample_level, double z_cut){ //you may add z cut here if necessarry
	if(subsample_level < 0 || subsample_level > 1) subsample_level = 1;

	Eigen::Vector3d tr = tMotion.translation();
	Eigen::Vector3d rot = tMotion.rotation().eulerAngles(0, 1, 2);
	if(tr[0] != 0.0 && tr[1] != 0.0 && rot[2] != 0){
	  int all_cells;
	  int subsample_cells;
	  int used_cells=0;
	  Predict2D(tr[0], tr[1], rot[2], fabs(tr[0]) * 0.50 + 0.001, fabs(tr[1]) * 0.70 + 0.001, fabs(rot[2]) * 0.15 + 0.001); //this line!!!!!!!!!!!!!!!!!!!!!!!!!
	  //Predict2D(tr[0], tr[1], rot[2], fabs(tr[0]) * 0.25 + 0.001, fabs(tr[1]) * 0.25 + 0.001, fabs(rot[2]) * 0.15 + 0.001); //this line!!!!!!!!!!!!!!!!!!!!!!!!!
		std::vector<perception_oru::NDTCell*> ndts0 = ndtLocalMap_->getAllCells();
		std::vector<perception_oru::NDTCell*> ndts;
		if(subsample_level != 1){
			srand(time(NULL));
			for(int i = 0; i < ndts0.size(); ++i){
				double p = ((double)rand()) / RAND_MAX;

				if(p < subsample_level && ndts0[i]->getMean()[2]>z_cut && ndts0[i]->getClass()!=perception_oru::NDTCell::HORIZONTAL)
					ndts.push_back(ndts0[i]);
				else
					delete ndts0[i];
			}
		} else
			ndts = ndts0;
//#pragma omp parallel for
		for(int i = 0; i < particleCloud.size(); i++){
			Eigen::Affine3d T = particleCloud[i].GetAsAffine();
			// std::vector<lslgeneric::NDTCell*> ndts;
			// ndts = ndtLocalMap_->pseudoTransformNDT(T);
			double score = 1;
			if(ndts.size() == 0) fprintf(stderr, "ERROR no gaussians in measurement!!!\n");
			for(int n = 0; n < ndts.size(); n++){
				Eigen::Vector3d m = T * ndts[n]->getMean();
				//if(m[2] < zfilt_min) continue;
				perception_oru::NDTCell *cell;
				pcl::PointXYZ p;
				p.x = m[0]; p.y = m[1]; p.z = m[2];
				if(ndtMap->getCellAtPoint(p, cell)){
					if(cell == NULL) continue;
					if(cell->getClass()==perception_oru::NDTCell::HORIZONTAL) continue;
					if(cell->hasGaussian_){
					  	Eigen::Matrix3d map_cov =
						  cell->getCov();
						//cell->blurCov( 10 );

						//Eigen::Matrix3d covCombined = cell->getCov() + T.rotation() * ndts[n]->getCov() * T.rotation().transpose();
						Eigen::Matrix3d covCombined = map_cov + T.rotation() * ndts[n]->getCov() * T.rotation().transpose();
						Eigen::Matrix3d icov;
						bool exists;
						double det = 0;
						covCombined.computeInverseAndDetWithCheck(icov, det, exists);
						if(!exists) continue;
						double l = (cell->getMean() - m).dot(icov * (cell->getMean() - m));
						if(l * 0 != 0) continue;
						score += 0.1 + 0.9 * exp(-0.005 * l / 2.0);
					}else{

					}
				}
			}
			particleCloud[i].SetLikelihood(score);
		}
		for(unsigned int j = 0; j < ndts.size(); j++)
			delete ndts[j];

		Normalize();
		if(forceSIR)
			SIRUpdate();
		else{
			double varP = 0;
			for(int i = 0; i < particleCloud.size(); i++)
				varP += (particleCloud[i].GetProbability() - 1.0 / double(particleCloud.size()))
				        * (particleCloud[i].GetProbability() - 1.0 / double(particleCloud.size()));
			varP /= double(particleCloud.size());
			varP = sqrt(varP);
			if(varP > varLimit || sinceSIR > sirCount){
				//fprintf(stderr,"-SIR- ");
				sinceSIR = 0;
				SIRUpdate();
			}else
				sinceSIR++;
		}
	}
}

void perception_oru::particle_filter::UpdateAndPredictEffRe(Eigen::Affine3d tMotion, perception_oru::NDTMap* ndtLocalMap_, double subsample_level, double z_cut, double x_var, double y_var, double th_var, double r_x_var, double r_y_var, double r_th_var, int tres){
	if(subsample_level < 0 || subsample_level > 1) subsample_level = 1;
	double tre=weights[tres];
	Eigen::Vector3d tr = tMotion.translation();
	Eigen::Vector3d rot = tMotion.rotation().eulerAngles(0, 1, 2);
	if(tr[0] != 0.0 || tr[1] != 0.0 || rot[2] != 0){
	  int all_cells;
	  int subsample_cells;
	  int used_cells=0;
	  Predict2D(tr[0], tr[1], rot[2], fabs(tr[0]) * x_var, fabs(tr[1]) * y_var, fabs(rot[2]) * th_var); //this line!!!!!!!!!!!!!!!!!!!!!!!!!
		std::vector<perception_oru::NDTCell*> ndts0 = ndtLocalMap_->getAllCells();
		std::vector<perception_oru::NDTCell*> ndts;
		if(subsample_level != 1){
			srand(time(NULL));
			for(int i = 0; i < ndts0.size(); ++i){
				double p = ((double)rand()) / RAND_MAX;

				if(p < subsample_level && ndts0[i]->getMean()[2]>z_cut && ndts0[i]->getClass()!=perception_oru::NDTCell::HORIZONTAL)
					ndts.push_back(ndts0[i]);
				else
					delete ndts0[i];
			}
		} else
			ndts = ndts0;

//#pragma omp parallel for
		for(int i = 0; i < particleCloud.size(); i++){
			Eigen::Affine3d T;
			if(particleCloud[i].GetProbability()>tre){
				T = particleCloud[i].GetAsAffine();
			}
			else{
				std::default_random_engine generator;
				std::normal_distribution<double> distribution_x(m_pose[0], r_x_var);
				std::normal_distribution<double> distribution_y(m_pose[1], r_y_var);
				std::normal_distribution<double> distribution_t(m_pose[2], r_th_var);
				particleCloud[i].Set(0.0, 0.0, distribution_t(generator), distribution_x(generator), distribution_y(generator), 0.0);

				T = particleCloud[i].GetAsAffine();
			}
				// std::vector<lslgeneric::NDTCell*> ndts;
				// ndts = ndtLocalMap_->pseudoTransformNDT(T);
				double score = 1;
				if(ndts.size() == 0) fprintf(stderr, "ERROR no gaussians in measurement!!!\n");
				for(int n = 0; n < ndts.size(); n++){
					Eigen::Vector3d m = T * ndts[n]->getMean();
					//if(m[2] < zfilt_min) continue;
					perception_oru::NDTCell *cell;
					pcl::PointXYZ p;
					p.x = m[0]; p.y = m[1]; p.z = m[2];
					if(ndtMap->getCellAtPoint(p, cell)){
						if(cell == NULL) continue;
						if(cell->hasGaussian_){
							Eigen::Matrix3d covCombined = cell->getCov() + T.rotation() * ndts[n]->getCov() * T.rotation().transpose();
							Eigen::Matrix3d icov;
							bool exists;
							double det = 0;
							covCombined.computeInverseAndDetWithCheck(icov, det, exists);
							if(!exists) continue;
							double l = (cell->getMean() - m).dot(icov * (cell->getMean() - m));
							if(l * 0 != 0) continue;
							score += 0.1 + 0.9 * exp(-0.5*l / 2.0);
						}else{

						}
					}
				}
			particleCloud[i].SetLikelihood(score);
		}
		for(unsigned int j = 0; j < ndts.size(); j++)
			delete ndts[j];

		Normalize();
		if(forceSIR)
			SIRUpdate();
		else{
			double varP = 0;
			for(int i = 0; i < particleCloud.size(); i++)
				varP += (particleCloud[i].GetProbability() - 1.0 / double(particleCloud.size()))
				        * (particleCloud[i].GetProbability() - 1.0 / double(particleCloud.size()));
			varP /= double(particleCloud.size());
			varP = sqrt(varP);
			if(varP > varLimit || sinceSIR > sirCount){
				//fprintf(stderr,"-SIR- ");
				sinceSIR = 0;
				SIRUpdate();
			}else
				sinceSIR++;
		}
	}
}

void perception_oru::particle_filter::Predict2D(double x, double y, double th, double sx, double sy, double sth){
	float dx = 0.0, dy = 0.0, dl = 0.0;
	float t = 0.0;
	float dxe, dye; ///<estimates
	std::default_random_engine generator;
	float eps=0.000001;
	for(int i = 0; i < particleCloud.size(); i++){
		double px, py, pz, pr, pp, pt;
		particleCloud[i].GetXYZ(px, py, pz);
		particleCloud[i].GetRPY(pr, pp, pt);

		std::normal_distribution<double> distribution_x(0, sx);
		std::normal_distribution<double> distribution_y(0, sy);
		std::normal_distribution<double> distribution_th(0, sth);
		///Generate noise from normal distribution
		dxe = x + distribution_x(generator);
		dye = y + distribution_y(generator);

		//if(fabs(x)<eps && fabs(y)<eps){
		//	dxe = x*0;
		//	dye = y*0;
		//
		//}

		dl = sqrt(dxe * dxe + dye * dye);

		//if(fabs(dxe)<eps && fabs(dye)<eps)
		//t = 0;
		//else
		t = atan2(dye, dxe);

		dx = dl * cos(pt + t);
		dy = dl * sin(pt + t);
		//ROS_INFO_STREAM("th"<<th);
		px += dx;
		py += dy;
		pt = pt + th + distribution_th(generator);
		toPI(pt);
		particleCloud[i].Set(pr, pp, pt, px, py, pz);
	}
	//    isAvgSet = false;
}
void perception_oru::particle_filter::to2PI(double &a){
	a = (double)fmod((double)(a), (double)( 2 * M_PI));
	if(a < 0) a += 2 * (double)M_PI;
}
void perception_oru::particle_filter::toPI(double &a){
	if(a > M_PI)
		while(a > M_PI) a -= 2.0 * M_PI;
	else
	if(a < -M_PI)
		while(a < -M_PI) a += 2.0 * M_PI;
}

void perception_oru::particle_filter::Normalize(){
	int i;
	double summ = 0;
	double sumX = 0, sumY = 0;
	double sumW = 0;
	double ax = 0, ay = 0;

	//isAvgSet = false;
	for(i = 0; i < particleCloud.size(); i++){
		particleCloud[i].SetProbability(particleCloud[i].GetProbability() * particleCloud[i].GetLikelihood());
		summ += particleCloud[i].GetProbability();
	}
	if(summ != 0){
		for(i = 0; i < particleCloud.size(); i++){
			particleCloud[i].SetProbability(particleCloud[i].GetProbability() / summ);
			weights[i]=particleCloud[i].GetProbability();
			double x, y, z, r, p, t;
			particleCloud[i].GetXYZ(x, y, z);
			particleCloud[i].GetRPY(r, p, t);
			sumX += particleCloud[i].GetProbability() * x;
			sumY += particleCloud[i].GetProbability() * y;
			ax += particleCloud[i].GetProbability() * cos(t);
			ay += particleCloud[i].GetProbability() * sin(t);
			sumW += particleCloud[i].GetProbability();
		}
	}
	else{
		for(i = 0; i < particleCloud.size(); i++){
			particleCloud[i].SetProbability(1.0 / particleCloud.size());
			weights[i]=particleCloud[i].GetProbability();
			double x, y, z, r, p, t;
			particleCloud[i].GetXYZ(x, y, z);
			particleCloud[i].GetRPY(r, p, t);
			sumX += particleCloud[i].GetProbability() * x;
			sumY += particleCloud[i].GetProbability() * y;
			ax += particleCloud[i].GetProbability() * cos(t);
			ay += particleCloud[i].GetProbability() * sin(t);
			sumW += particleCloud[i].GetProbability();
		}
	}
		m_pose << sumX, sumY, atan2(ay, ax);
		std::sort (weights.begin(), weights.end(),myfunction);
		//ROS_INFO_STREAM(weights.front()<<" "<< weights.back());
}
void perception_oru::particle_filter::SIRUpdate(){
	std::vector<particle> tmp2;
	std::default_random_engine generator;
	std::uniform_real_distribution<double> dist(0, 1);
	double U = 0, Q = 0;
	int i = 0, j = 0, k = 0;
	U = dist(generator) / (double)particleCloud.size();
	//fprintf(stderr,"SIRUpdate()::U=%.6f\n",U);
	while(U < 1.0){
		if(Q > U){
			U += 1.0 / (double)particleCloud.size();
			if(k >= particleCloud.size() || i >= particleCloud.size()){
				while(i < particleCloud.size()){
					tmp[i] = particleCloud[particleCloud.size() - 1];
					tmp[i].probability = 1.0 / (double)particleCloud.size();
					i++;
				}
				//fprintf(stderr,"ERROR: SIRupdate:: Invalid index k='%d' or i='%d'\n",k,i);
				tmp2 = particleCloud;
				particleCloud = tmp;
				tmp = tmp2;
				return;
			}
			tmp[i] = particleCloud[k];
			tmp[i].probability = 1.0 / (double)particleCloud.size();
			i++;
		}else {
			j++;
			k = j;
			if(j >= particleCloud.size()){
				while(i < particleCloud.size()){
					tmp[i] = particleCloud[particleCloud.size() - 1];
					tmp[i].probability = 1.0 / (double)particleCloud.size();
					i++;
				}
				//fprintf(stderr,"ERROR: SIRupdate:: Invalid index j='%d' \n",j);
				tmp2 = particleCloud;
				particleCloud = tmp;
				tmp = tmp2;
				return;
			}
			Q += particleCloud[j].probability;
			///j++; ///WAS HERE until 30.7.2008

			if(j == particleCloud.size()){
				while(i < particleCloud.size()){
					tmp[i] = particleCloud[k - 1];
					tmp[i].probability = 1.0 / (double)particleCloud.size();
					i++;
				}
				tmp2 = particleCloud;
				particleCloud = tmp;
				tmp = tmp2;
				return;
			}
		}
	} //While
	while(i < particleCloud.size()){
		if(k >= particleCloud.size()) k = particleCloud.size() - 1;
		tmp[i] = particleCloud[k];
		tmp[i].probability = 1.0 / (double)particleCloud.size();
		i++;
	}
	//  isAvgSet = false;
	tmp2 = particleCloud;
	particleCloud = tmp;
	tmp = tmp2;
}

void perception_oru::particle_filter::GetRandomPoint(perception_oru::NDTCell* cell, double &x, double &y, double &th){
	Eigen::Vector3d mean = cell->getMean();
	Eigen::Matrix3d cov = cell->getCov();
	double mX = mean[0], mY = mean[1], mTh = mean[2];
	double varX = cov(0, 0), varY = cov(1, 1), varTh = cov(2, 2);
	std::default_random_engine generator;

	std::normal_distribution<double> distribution_x(mX, varX);
	std::normal_distribution<double> distribution_y(mY, varY);
	std::normal_distribution<double> distribution_th(mTh, varTh);
	x = distribution_x(generator);
	y = distribution_y(generator);
	th = distribution_th(generator);
}

void perception_oru::particle_filter::EigenSort( Eigen::Vector3d &eigenvalues, Eigen::Matrix3d &eigenvectors ){
	int k, j, i;
	double p;

	for(i = 0; i < 2; i++){
		p = eigenvalues(k = i);
		for(j = i + 1; j < 3; j++)
			if(fabs(eigenvalues(j)) >= fabs(p))
				p = eigenvalues(k = j);
		if(k != i){
			eigenvalues.row(k).swap(eigenvalues.row(i));
			eigenvectors.col(k).swap(eigenvectors.col(i));
		}
	}
}

Eigen::Affine3d perception_oru::particle_filter::getAsAffine(float x, float y, float yaw ){
	Eigen::Matrix3d m;

	m = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
	    * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
	    * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
	Eigen::Translation3d v(x, y, 0);
	Eigen::Affine3d T = Eigen::Affine3d::Identity();
	T.rotate(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
	T.translation()<<x,y,0;
	return T;
}


/*
 *            Copyright 2009-2018 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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 <votca/xtp/bfgs-trm.h>
#include <boost/format.hpp>

namespace votca {
    namespace xtp {

        void BFGSTRM::Optimize(const Eigen::VectorXd& initialparameters) {
           _parameters=initialparameters;

            Eigen::VectorXd gradient=Eigen::VectorXd::Zero(_parameters.size());
            Eigen::VectorXd delta_p_trial=Eigen::VectorXd::Zero(_parameters.size());

            double lastcost=_costfunction.EvaluateCost(_parameters);
            Eigen::VectorXd last_gradient=Eigen::VectorXd::Zero(_parameters.size());
            double delta_cost=0;

            for(_iteration=0;_iteration<_max_iteration;_iteration++){
               gradient=_costfunction.EvaluateGradient(_parameters);
                bool step_accepted = false;
                for (int i=0;i<100;i++){
                    delta_p_trial=CalculateInitialStep(gradient);
                    if (delta_p_trial.norm()>_trust_radius){
                       delta_p_trial=CalculateRegularizedStep(delta_p_trial,gradient);
                    }
                    double trialcost=_costfunction.EvaluateCost(_parameters+delta_p_trial);
                    delta_cost=trialcost-lastcost;
                    step_accepted=AcceptRejectStep(delta_p_trial,gradient,delta_cost);
                    if(step_accepted){
                      _cost=trialcost;
                      _parameters+=delta_p_trial;
                      break;
                    }

                }
                if(_iteration>0){
                  UpdateHessian(delta_p_trial,gradient-last_gradient);
                }
                lastcost=_cost;
                last_gradient=gradient;
                for(auto& func:_callbacks){
                 func();
                }
                if(_costfunction.Converged(delta_p_trial,delta_cost,gradient)){
                  break;
                } else if(_iteration == _max_iteration-1) {
                  _success=false;
                  if(_logging){
                    CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("BFGS-TRM @iteration %1$d: not converged after %2$d iterations ")
                            % _iteration % _max_iteration).str() << std::flush;
                  }
                }

            }
            return;
        }

        /* Accept/reject the new geometry and adjust trust radius, if required */
        bool BFGSTRM::AcceptRejectStep(const Eigen::VectorXd& delta_p,const Eigen::VectorXd& gradient,double cost_delta) {
            bool step_accepted = false;
            if (cost_delta > 0.0) {
                // total energy has unexpectedly increased, half the trust radius
                _trust_radius = 0.25 * _trust_radius;
                if(_logging){
                  CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("BFGS-TRM @iteration %1$d: step rejected ")
                          % _iteration).str() << std::flush;
                  CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("BFGS-TRM @iteration %1$d: new trust radius %2$8.6f")
                          % _iteration % _trust_radius).str() << std::flush;
                }
            } else {
                // total energy has decreased, we accept the step but might update the trust radius
                step_accepted = true;
                // adjust trust radius, if required
                double tr_check = cost_delta / QuadraticEnergy(gradient,delta_p);
                double norm_delta_p=delta_p.squaredNorm();
                if (tr_check > 0.75 && 1.25 * norm_delta_p > _trust_radius*_trust_radius) {
                    _trust_radius = 2.0 * _trust_radius;
                } else if (tr_check < 0.25) {
                    _trust_radius = 0.25 * _trust_radius;
                }
                if(_logging){
                  CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("BFGS-TRM @iteration %1$d: step accepted ")
                          % _iteration).str() << std::flush;
                  CTP_LOG(ctp::logINFO, *_pLog) << (boost::format("BFGS-TRM @iteration %1$d: new trust radius %2$8.6f")
                          % _iteration % _trust_radius).str() << std::flush;
                }
            }
            return step_accepted;
        }

        void BFGSTRM::UpdateHessian(const Eigen::VectorXd& delta_pos,const Eigen::VectorXd& delta_gradient) {
                // second term in BFGS update (needs current Hessian)
               _hessian -= _hessian*delta_pos*delta_pos.transpose()*_hessian.transpose() / (delta_pos.transpose()*_hessian*delta_pos).value();
                // first term in BFGS update
                _hessian += (delta_gradient* delta_gradient.transpose()) / (delta_gradient.transpose()*delta_pos);
                // symmetrize Hessian (since d2E/dxidxj should be symmetric)
               _hessian=0.5*(_hessian+_hessian.transpose());
            return;
        }

        /* Predict displacement of atom coordinates */
        Eigen::VectorXd BFGSTRM::CalculateInitialStep(const Eigen::MatrixXd& gradient)const{
            return _hessian.colPivHouseholderQr().solve(-gradient);
        }

        /* Regularize step in case of prediction outside of Trust Region */
        Eigen::VectorXd BFGSTRM::CalculateRegularizedStep(const Eigen::VectorXd& delta_pos,const Eigen::VectorXd& gradient) const{
            Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(_hessian);
            // start value for lambda  a bit lower than lowest eigenvalue of Hessian
            double lambda= 1.05 * es.eigenvalues()(0);
            if (es.eigenvalues()(0) > 0.0) {
              lambda = -0.05 * std::abs(es.eigenvalues()(0));
            }
            // for constrained step, we expect
            double max_step_squared =delta_pos.squaredNorm();
            while (max_step_squared>(_trust_radius * _trust_radius)) {
              lambda -= 0.05 * std::abs(es.eigenvalues()(0));
              Eigen::VectorXd quotient=(es.eigenvalues().array()-lambda).cwiseAbs2();
              auto factor=(es.eigenvectors().transpose()*gradient).cwiseAbs2();
              max_step_squared = (factor.cwiseQuotient(quotient)).sum();
            }

            Eigen::VectorXd new_delta_pos = Eigen::VectorXd::Zero(delta_pos.size());
            for (unsigned i = 0; i < delta_pos.size(); i++) {
                new_delta_pos -= es.eigenvectors().col(i) * (es.eigenvectors().col(i).transpose()*gradient) / (es.eigenvalues()(i) - lambda);
            }
            return new_delta_pos;
        }

        /* Estimate energy change based on quadratic approximation */
        double BFGSTRM::QuadraticEnergy(const Eigen::VectorXd& gradient, const Eigen::VectorXd& delta_pos) const{
            return (gradient.transpose()* delta_pos).value() + 0.5*(delta_pos.transpose()*_hessian*delta_pos).value();
        }

    }
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2019 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-1
 */

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <cmath>
#include <fstream>
#include <iostream>
#include <tuple>

using namespace dealii;

std::tuple<unsigned int, unsigned int, unsigned int>
get_info_triangulation(const Triangulation<2> &tria)
{
  return std::make_tuple<unsigned int, unsigned int>(tria.n_active_cells(),
                                                     tria.n_cells(),
                                                     tria.n_levels());
}

void
first_grid()
{
  Triangulation<2> triangulation;

  GridGenerator::hyper_cube(triangulation);
  triangulation.refine_global(4);

  std::ofstream out("grid-1.vtk");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-1.vtk" << std::endl;

  auto info_tria = get_info_triangulation(triangulation);
  std::cout << std::get<0>(info_tria) << "\t" << std::get<1>(info_tria) << "\t"
            << std::get<2>(info_tria) << "\n";
}

void
second_grid()
{
  Triangulation<2> triangulation;

  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);
  for (unsigned int step = 0; step < 5; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }
      triangulation.reset_manifold(
        0); // Reset all parts of the triangulation, regardless of their
            // manifold_id, to use a FlatManifold object.
      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out("grid-2.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  auto info_tria = get_info_triangulation(triangulation);

  std::cout << "Grid written to grid-2.svg" << std::endl;
  std::cout << std::get<0>(info_tria) << "\t" << std::get<1>(info_tria) << "\t"
            << std::get<2>(info_tria) << "\n";
}

void
third_grid()
{
  Triangulation<2> triangulation;
  const double     left  = -1.0;
  const double     right = +1.0;
  GridGenerator::hyper_L(triangulation, left, right, true);
  std::ofstream out("grid-3.svg");
  GridOut       grid_out;

  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-3.svg" << std::endl;
  auto info_tria = get_info_triangulation(triangulation);
  std::cout << std::get<0>(info_tria) << "\t" << std::get<1>(info_tria) << "\t"
            << std::get<2>(info_tria) << "\n";

  std::cout << "Now refine the grid globally" << std::endl;
  const unsigned int initial_global_refinement = 2;

  triangulation.refine_global(initial_global_refinement);
  std::ofstream out_refined("grid-4.svg");

  grid_out.write_svg(triangulation, out_refined);
  std::cout << "Grid written to grid-4.svg" << std::endl;

  std::cout << "Refine adaptively around the re-entrant corner"
            << "\n";
  const Point<2>     corner(0.0, 0.0);
  const unsigned int no_refinements{4};
  for (unsigned int step = 0; step < no_refinements; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_corner =
                corner.distance(cell->center(v));
              // std::cout << cell->center(v) << cell->center() << "\n"; //check
              // about syntax
              if (std::fabs(distance_from_corner) <= (1.0) / (3.0))
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }
      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out_refined_locally("grid-5.svg");

  grid_out.write_svg(triangulation, out_refined_locally);
  std::cout << "Grid written to grid-5.svg" << std::endl;

  auto info_tria_L = get_info_triangulation(triangulation);
  std::cout << std::get<0>(info_tria_L) << "\t" << std::get<1>(info_tria_L)
            << "\t" << std::get<2>(info_tria_L) << "\n";
}

void
fourth_grid()
{
  Triangulation<2> triangulation;
  const double     radius = 1.0;
  const Point<2>   center(0.0, 0.0); // center of the ball

  const SphericalManifold<2> manifold(center);
  GridGenerator::hyper_ball(
    triangulation,
    center,
    radius,
    true); // spherical manifold set to true on the bdary
  std::ofstream out("grid-Spherical_Manifold_bdary.svg");
  GridOut       grid_out;

  std::cout << "Using Spherical Manifold on the boundary "
            << "\n";
  triangulation.reset_all_manifolds();
  triangulation.set_all_manifold_ids_on_boundary(0);
  triangulation.set_manifold(0, manifold);
  triangulation.refine_global(4);

  grid_out.write_svg(triangulation, out);

  //////////////EVERYWHERE
  Triangulation<2> triangulation_e;
  GridGenerator::hyper_ball(triangulation_e,
                            center,
                            radius,
                            true); // spherical manifold set to true

  std::ofstream out_e("grid-Spherical_Manifold_everywhere.svg");
  GridOut       grid_out_e;

  std::cout << "Using Spherical Manifold everywhere "
            << "\n";
  triangulation_e.reset_all_manifolds();
  // reenable the manifold:
  triangulation_e.set_all_manifold_ids(0);
  triangulation_e.set_manifold(0, manifold);
  triangulation_e.refine_global(2);

  grid_out_e.write_svg(triangulation_e, out_e);

  //////////EXCEPT THE CENTER
  Triangulation<2> triangulation_c;
  GridGenerator::hyper_ball(triangulation_c, center, radius, true);

  std::ofstream  out_c("grid-Spherical_Manifold_everywhere_but_center.svg");
  GridOut        grid_out_c;
  const Point<2> mesh_center;
  for (const auto &cell : triangulation_c.active_cell_iterators())
    {
      if (mesh_center.distance(cell->center()) > cell->diameter() / 10)
        {
          cell->set_all_manifold_ids(0);
        }
    }
  std::cout << "Using Spherical Manifold everywhere but the center"
            << "\n";
  triangulation_c.refine_global(4);

  grid_out_c.write_svg(triangulation_c, out_c);
}

int
main()
{
  first_grid();
  second_grid();
  third_grid();
  fourth_grid();
}