/*
        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;
*/


/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
   Copyright 2021 Kenneth W. Chapman

   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 "log.h"
#include "calibbowtie.h"
#include <vector>
#include <limits>
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/exception/exception.hpp>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/format.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>
#include "findline.h"
#include "searchlines.h"

#ifdef __DEBUG_CALIB
#undef __DEBUG_CALIB
#endif

#ifdef LOG_CALIB_VALUES
#undef LOG_CALIB_VALUES
#endif

using namespace cv;
using namespace std;
using namespace boost;
namespace fs = boost::filesystem;

namespace gc
{

enum PIX_POS_INDEX
{
    PIX_POS_X = 0,
    PIX_POS_Y = 1
};

void CalibBowtie::clear()
{
    m_model.clear();
    m_matHomogPixToWorld = Mat();
    m_matHomogWorldToPix = Mat();
    // m_worldToPixParams.clear();
}
GC_STATUS CalibBowtie::Calibrate( const std::vector< cv::Point2d > pixelPts, const std::vector< cv::Point2d > worldPts,
                                  const double moveSearchROIMultiplier, const std::string &controlJson, const cv::Size gridSize,
                                  const cv::Size imgSize, std::vector< cv::Point > &searchLineCorners )
{
    GC_STATUS retVal = GC_OK;
    if ( pixelPts.size() != worldPts.size() || pixelPts.empty() || worldPts.empty() ||
         gridSize.width * gridSize.height != static_cast< int >( pixelPts.size() ) || 0 >= gridSize.width )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::Calibrate] Calibration world/pixel coordinate point counts do not match or are empty";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            m_model.clear();
            m_model.imgSize = imgSize;
            m_model.gridSize = gridSize;
            m_model.pixelPoints.clear();
            m_model.worldPoints.clear();
            for ( size_t i = 0; i < pixelPts.size(); ++i )
            {
                m_model.pixelPoints.push_back( pixelPts[ i ] );
                m_model.worldPoints.push_back( worldPts[ i ] );
            }

            m_matHomogPixToWorld = findHomography( m_model.pixelPoints, m_model.worldPoints );
            invert( m_matHomogPixToWorld, m_matHomogWorldToPix );

            if ( !searchLineCorners.empty() )
            {
                SearchLines searchLines;
                retVal = searchLines.CalcSearchLines( searchLineCorners, m_model.searchLineSet );
            }
            if ( GC_OK == retVal )
            {
                m_model.moveSearchROIMultiplier = moveSearchROIMultiplier;
                m_model.moveSearchRegionLft = Rect( std::max( 0, cvRound( m_model.pixelPoints[ 0 ].x ) - GC_BOWTIE_TEMPLATE_DIM ),
                                                    std::max( 0, cvRound( m_model.pixelPoints[ 0 ].y )- GC_BOWTIE_TEMPLATE_DIM ),
                                                    std::min( imgSize.width - cvRound( m_model.pixelPoints[ 0 ].x ), GC_BOWTIE_TEMPLATE_DIM * 2 ),
                                                    std::min( imgSize.height - cvRound( m_model.pixelPoints[ 0 ].y ), GC_BOWTIE_TEMPLATE_DIM * 2 ) );
                if ( 0.0 < moveSearchROIMultiplier )
                {
                    int lftWidth = cvRound( moveSearchROIMultiplier * static_cast< double >( m_model.moveSearchRegionLft.width ) );
                    int lftHeight = cvRound( moveSearchROIMultiplier * static_cast< double >( m_model.moveSearchRegionLft.height ) );
                    int lftX = std::max( 0, m_model.moveSearchRegionLft.x - ( lftWidth - m_model.moveSearchRegionLft.width ) / 2 );
                    int lftY = std::max( 0, m_model.moveSearchRegionLft.y - ( lftHeight - m_model.moveSearchRegionLft.height ) / 2 );
                    m_model.moveSearchRegionLft = Rect( lftX, lftY, std::min( imgSize.width - lftX, lftWidth ), std::min( imgSize.height - lftY, lftHeight ) );
                }

                size_t idx = static_cast< size_t >( m_model.gridSize.width - 1 );
                m_model.moveSearchRegionRgt = Rect( std::max( 0, cvRound( m_model.pixelPoints[ idx ].x ) - GC_BOWTIE_TEMPLATE_DIM ),
                                                    std::max( 0, cvRound( m_model.pixelPoints[ idx ].y ) - GC_BOWTIE_TEMPLATE_DIM ),
                                                    std::min( imgSize.width - cvRound( m_model.pixelPoints[ idx ].x ), GC_BOWTIE_TEMPLATE_DIM * 2 ),
                                                    std::min( imgSize.height - cvRound( m_model.pixelPoints[ idx ].y ), GC_BOWTIE_TEMPLATE_DIM * 2 ) );
                if ( 0.0 < moveSearchROIMultiplier )
                {
                    int rgtWidth = cvRound( moveSearchROIMultiplier * static_cast< double >( m_model.moveSearchRegionRgt.width ) );
                    int rgtHeight = cvRound( moveSearchROIMultiplier * static_cast< double >( m_model.moveSearchRegionRgt.height ) );
                    int rgtX = std::max( 0, m_model.moveSearchRegionRgt.x - ( rgtWidth - m_model.moveSearchRegionRgt.width ) / 2 );
                    int rgtY = std::max( 0, m_model.moveSearchRegionRgt.y - ( rgtHeight - m_model.moveSearchRegionRgt.height ) / 2 );
                    m_model.moveSearchRegionRgt = Rect( rgtX, rgtY, std::min( imgSize.width - rgtX, rgtWidth ), std::min( imgSize.height - rgtY, rgtHeight ) );
                }

                int width = std::max( cvRound( m_model.pixelPoints[ idx ].x - m_model.pixelPoints[ 0 ].x ),
                                      cvRound( m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].x - m_model.pixelPoints[ m_model.pixelPoints.size() - 2 ].x ) );
                int height = std::max( cvRound( m_model.pixelPoints[ m_model.pixelPoints.size() - 2 ].y - m_model.pixelPoints[ 0 ].y ),
                                       cvRound( m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].y - m_model.pixelPoints[ idx ].y ) );
                int left = std::max( 0, cvRound( m_model.pixelPoints[ 0 ].x ) - width / 2 );
                int top = std::max( 0, cvRound( m_model.pixelPoints[ 0 ].y ) - height / 2 );

                width *= 2;
                height *= 2;
                if ( left + width > imgSize.width - 1 )
                {
                    width = imgSize.width - left - 1;
                }
                if ( top + height > imgSize.height - 1 )
                {
                    height = imgSize.height - top - 1;
                }

                m_model.wholeTargetRegion = Rect( left, top, width, height );
            }
            if ( m_model.pixelPoints.empty() || m_model.worldPoints.empty() || m_model.searchLineSet.empty() )
            {
                FILE_LOG( logERROR ) << "[CalibBowtie::Calibrate] No valid calibration for drawing";
                retVal = GC_ERR;
            }
            else if ( m_matHomogPixToWorld.empty() || m_matHomogWorldToPix.empty() )
            {
                FILE_LOG( logERROR ) << "[CalibBowtie::Calibrate] System not calibrated";
                retVal = GC_ERR;
            }
            else
            {
                m_model.controlJson = controlJson;
            }
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::Calibrate] " << e.what();
            return GC_EXCEPT;
        }
    }
    return retVal;
}
GC_STATUS CalibBowtie::GetSearchRegionBoundingRect( cv::Rect &rect )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        if ( m_model.searchLineSet.empty() )
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::GetSearchRegionBoundingRect] System not calibrated";
            retVal = GC_ERR;
        }
        else
        {
            int left = std::min( m_model.searchLineSet[ 0 ].top.x, m_model.searchLineSet[ 0 ].bot.x );
            int top = std::min( m_model.searchLineSet[ 0 ].top.y, m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].top.y );
            int right = std::max( m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].top.x, m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].bot.x );
            int bottom = std::max( m_model.searchLineSet[ 0 ].bot.y, m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].bot.y );
            rect = Rect( left, top, right - left, bottom - top );
        }
    }
    catch( Exception &e )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::GetSearchRegionBoundingRect] " << e.what();
        return GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS CalibBowtie::DrawOverlay( const cv::Mat img, cv::Mat &imgOut, const bool drawCalib,
                              const bool drawMoveROIs, const bool drawSearchROI )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        if ( CV_8UC1 == img.type() )
        {
             cvtColor( img, imgOut, COLOR_GRAY2BGR );
        }
        else if ( CV_8UC3 == img.type() )
        {
            imgOut = img.clone();
        }
        else
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::DrawOverlay] Invalid image format for calibration";
            retVal = GC_ERR;
        }

        if ( m_model.pixelPoints.empty() || m_model.worldPoints.empty() || m_model.searchLineSet.empty() )
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::DrawOverlay] No valid calibration for drawing";
            retVal = GC_ERR;
        }
        else if ( m_matHomogPixToWorld.empty() || m_matHomogWorldToPix.empty() )
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::DrawOverlay] System not calibrated";
            retVal = GC_ERR;
        }

        if ( GC_OK == retVal )
        {
            int textOffset = cvRound( static_cast< double >( imgOut.rows ) / 6.6666667 );
            int circleSize =  std::max( 5, cvRound( static_cast< double >( imgOut.rows ) / 120.0 ) );
            int textStroke = std::max( 1, cvRound( static_cast< double >( imgOut.rows ) / 300.0 ) );
            double fontScale = 1.0 + static_cast< double >( imgOut.rows ) / 1200.0;

            if ( drawMoveROIs )
            {
                rectangle( imgOut, m_model.moveSearchRegionLft, Scalar( 0, 0, 255 ), textStroke );
                rectangle( imgOut, m_model.moveSearchRegionRgt, Scalar( 0, 0, 255 ), textStroke );
            }

            if ( drawSearchROI )
            {
                if ( m_model.searchLineSet.empty() )
                {
                    FILE_LOG( logWARNING ) << "[CalibBowtie::Calibrate] Search lines not calculated properly so they cannot be drawn";
                    retVal = GC_WARN;
                }
                else
                {
                    line( imgOut, m_model.searchLineSet[ 0 ].top, m_model.searchLineSet[ 0 ].bot, Scalar( 255, 0, 0 ), textStroke );
                    line( imgOut, m_model.searchLineSet[ 0 ].top, m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].top, Scalar( 255, 0, 0 ), textStroke );
                    line( imgOut, m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].top, m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].bot, Scalar( 255, 0, 0 ), textStroke );
                    line( imgOut, m_model.searchLineSet[ 0 ].bot, m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].bot, Scalar( 255, 0, 0 ), textStroke );
                }
            }

            if ( drawCalib )
            {
                Point2d topLft, botRgt;
                retVal = PixelToWorld( m_model.pixelPoints[ 0 ], topLft );
                if ( GC_OK == retVal )
                {
                    retVal = PixelToWorld( m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ], botRgt );
                    if ( GC_OK == retVal )
                    {
                        Point2d pt1, pt2;
                        double minCol = std::min( topLft.x, botRgt.x );
                        double maxCol = std::max( topLft.x, botRgt.x );
                        double minRow = std::min( topLft.y, botRgt.y );
                        double maxRow = std::max( topLft.y, botRgt.y );
                        double rowInc = ( maxRow - minRow ) / static_cast< double >( m_model.gridSize.height + 2 );
                        double colInc = ( maxCol - minCol ) / static_cast< double >( m_model.gridSize.width );
                        minRow -= rowInc;
                        maxRow += rowInc;
                        stringstream buf;

                        bool first;
                        double row, col;
                        int rowInt, colInt;
                        for ( rowInt = 0, row = maxRow; row > minRow; row -= rowInc, ++rowInt )
                        {
                            first = true;
                            for ( colInt = 0, col = minCol; col < maxCol; col += colInc, ++colInt )
                            {
                                retVal = WorldToPixel( Point2d( col, row ), pt1 );
                                if ( GC_OK == retVal )
                                {
                                    retVal = WorldToPixel( Point2d( col + colInc, row ), pt2 );
                                    if ( GC_OK == retVal )
                                    {
                                        line( imgOut, pt1, pt2, Scalar( 0, 255, 255 ), textStroke );
                                        retVal = WorldToPixel( Point2d( col, row - rowInc ), pt2 );
                                        if ( GC_OK == retVal && pt1.y < imgOut.rows )
                                        {
                                            line( imgOut, pt1, pt2, Scalar( 0, 255, 255 ), textStroke );
                                            if ( ( ( rowInt % 2 ) == 1 ) && ( ( colInt % 2 ) == 0 ) )
                                                circle( imgOut, pt1, circleSize, Scalar( 0, 255, 0 ), textStroke );
                                        }
                                    }
                                }
                                if ( first )
                                {
                                    first = false;
                                    buf.str( string() ); buf << boost::format( "%.1f" ) % row;
                                    putText( imgOut, buf.str(), Point( cvRound( pt1.x ) - textOffset, cvRound( pt1.y ) + 5 ),
                                             FONT_HERSHEY_COMPLEX, fontScale * 0.5, Scalar( 0, 255, 255 ), textStroke );
                                }
                            }
                            retVal = WorldToPixel( Point2d( maxCol, row ), pt1 );
                            if ( GC_OK == retVal && pt1.y < imgOut.rows )
                            {
                                retVal = WorldToPixel( Point2d( maxCol, row - rowInc ), pt2 );
                                if ( GC_OK == retVal )
                                {
                                    line( imgOut, pt1, pt2, Scalar( 0, 255, 255 ), textStroke );
                                    if ( ( rowInt % 2 ) == 1 )
                                        circle( imgOut, pt1, circleSize, Scalar( 0, 255, 0 ), textStroke );
                                }
                            }
                        }
                        first = true;
                        for ( double col = minCol; col < maxCol; col += colInc )
                        {
                            retVal = WorldToPixel( Point2d( col, minRow ), pt1 );
                            if ( GC_OK == retVal )
                            {
                                retVal = WorldToPixel( Point2d( col + colInc, minRow ), pt2 );
                                if ( GC_OK == retVal )
                                {
                                    line( imgOut, pt1, pt2, Scalar( 0, 255, 255 ), textStroke );
                                }
                            }
                            if ( first )
                            {
                                first = false;
                                buf.str( string() ); buf << boost::format( "%.1f" ) % minRow;
                                putText( imgOut, buf.str(), Point( cvRound( pt1.x ) - textOffset, cvRound( pt1.y ) + 5 ),
                                         FONT_HERSHEY_COMPLEX, fontScale * 0.5, Scalar( 0, 255, 255 ), textStroke );
                            }
                        }
                    }
                }
            }
        }
    }
    catch( Exception &e )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::DrawOverlay] " << e.what();
        return GC_EXCEPT;
    }
    return retVal;
}
GC_STATUS CalibBowtie::PixelToWorld( const Point2d ptPixel, Point2d &ptWorld )
{
    GC_STATUS retVal = GC_OK;

    if ( m_matHomogPixToWorld.empty() )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::PixelToWorld] No calibration for pixel to world conversion";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            vector< Point2d > vecIn, vecOut;
            vecIn.push_back( ptPixel );
            perspectiveTransform( vecIn, vecOut, m_matHomogPixToWorld );
            ptWorld = vecOut[ 0 ];
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::PixelToWorld] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
GC_STATUS CalibBowtie::WorldToPixel( const Point2d ptWorld, Point2d &ptPixel )
{
    GC_STATUS retVal = GC_OK;
    // if ( 9 != m_worldToPixParams.size() )
    if ( m_matHomogWorldToPix.empty() )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::WorldToPixel] No calibration for world to pixel conversion";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
#if 1
            vector< Point2d > vecIn, vecOut;
            vecIn.push_back( ptWorld );
            perspectiveTransform( vecIn, vecOut, m_matHomogWorldToPix );
            ptPixel = vecOut[ 0 ];
#else
            double tx = m_worldToPixParams[ 0 ] * ptWorld.x + m_worldToPixParams[ 1 ] * ptWorld.y + m_worldToPixParams[ 2 ];
            double ty = m_worldToPixParams[ 3 ] * ptWorld.x + m_worldToPixParams[ 4 ] * ptWorld.y + m_worldToPixParams[ 5 ];
            double tz = m_worldToPixParams[ 6 ] * ptWorld.x + m_worldToPixParams[ 7 ] * ptWorld.y + m_worldToPixParams[ 8 ];

            ptPixel = Point2d( tx / tz, ty / tz );
#endif

            // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
            // https://stackoverflow.com/questions/32366786/opencv-homography-matrix-h-and-inverse-h-to-transform-a-point-is-not-getting-exp
            // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
            //def transformPoints(self, x, y, reverse=False, integer=True):
            //
            //    if reverse == False:
            //        H = self.transform_matrix
            //    else:
            //        val, H = cv2.invert(self.transform_matrix)
            //
            //    # get the elements in the transform matrix
            //    h0 = H[0,0]
            //    h1 = H[0,1]
            //    h2 = H[0,2]
            //    h3 = H[1,0]
            //    h4 = H[1,1]
            //    h5 = H[1,2]
            //    h6 = H[2,0]
            //    h7 = H[2,1]
            //    h8 = H[2,2]
            //
            //    tx = (h0*x + h1*y + h2)
            //    ty = (h3*x + h4*x + h5)
            //    tz = (h6*x + h7*y + h8)
            //
            //    if integer==True:
            //        px = int(tx/tz)
            //        py = int(ty/tz)
            //        Z = int(1/tz)
            //    else:
            //        px = tx/tz
            //        py = ty/tz
            //        Z = 1/tz
            //
            //    return (px, py)
            // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::WorldToPixel] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
cv::Rect CalibBowtie::MoveSearchROI( const bool isLeft )
{
    return isLeft ? m_model.moveSearchRegionLft :
                    m_model.moveSearchRegionRgt;
}
GC_STATUS CalibBowtie::MoveRefPoint( cv::Point2d &lftRefPt, cv::Point2d &rgtRefPt )
{
    GC_STATUS retVal = GC_OK;
    Point2d pt( numeric_limits< double >::min(), numeric_limits< double >::min() );
    if ( m_model.pixelPoints.empty() )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::MoveRefPoint] Cannot retrieve move reference point from an uncalibrated system";
        retVal = GC_ERR;
    }
    else if ( static_cast< size_t >( m_model.gridSize.width * m_model.gridSize.height ) != m_model.pixelPoints.size() )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::MoveRefPoint] Cannot retrieve move reference point with invalid calibration";
        retVal = GC_ERR;
    }
    else
    {
        lftRefPt = m_model.pixelPoints[ 0 ];
        rgtRefPt = m_model.pixelPoints[ static_cast< size_t >( m_model.gridSize.width - 1 ) ];
    }
    return retVal;
}
GC_STATUS CalibBowtie::Load( const string &jsonCalibString )
{
    GC_STATUS retVal = GC_OK;

    try
    {
        if ( jsonCalibString.empty() )
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::Load] Bow tie calibration string is empty";
            retVal = GC_ERR;
        }
        else
        {
            stringstream ss;
            ss << jsonCalibString;
            property_tree::ptree ptreeTop;
            property_tree::json_parser::read_json( ss, ptreeTop );

            m_model.imgSize.width = ptreeTop.get< int >( "imageWidth", 0 );
            m_model.imgSize.height = ptreeTop.get< int >( "imageHeight", 0 );
            m_model.moveSearchROIMultiplier = ptreeTop.get< double >( "moveSearchROIMultiplier", 0.0 );
            property_tree::ptree ptreeCalib = ptreeTop.get_child( "PixelToWorld" );

            Point2d ptTemp;
            size_t cols = ptreeCalib.get< size_t >( "columns", 2 );
            size_t rows = ptreeCalib.get< size_t >( "rows", 4 );
            m_model.pixelPoints.clear();
            m_model.worldPoints.clear();

            BOOST_FOREACH( property_tree::ptree::value_type &node, ptreeCalib.get_child( "points" ) )
            {
                ptTemp.x = node.second.get< double >( "pixelX", 0.0 );
                ptTemp.y = node.second.get< double >( "pixelY", 0.0 );
                m_model.pixelPoints.push_back( ptTemp );
                ptTemp.x = node.second.get< double >( "worldX", 0.0 );
                ptTemp.y = node.second.get< double >( "worldY", 0.0 );
                m_model.worldPoints.push_back( ptTemp );
            }

            const property_tree::ptree &ptreeMoveSearch = ptreeTop.get_child( "MoveSearchRegions" );
            property_tree::ptree::const_iterator end = ptreeMoveSearch.end();
            for ( property_tree::ptree::const_iterator iter = ptreeMoveSearch.begin(); iter != end; ++iter )
            {
                if ( iter->first == "Left" )
                {
                    m_model.moveSearchRegionLft.x =      iter->second.get< int >( "x", 0 );
                    m_model.moveSearchRegionLft.y =      iter->second.get< int >( "y", 0 );
                    m_model.moveSearchRegionLft.width =  iter->second.get< int >( "width", 0 );
                    m_model.moveSearchRegionLft.height = iter->second.get< int >( "height", 0 );
                }
                else if ( iter->first == "Right" )
                {
                    m_model.moveSearchRegionRgt.x =      iter->second.get< int >( "x", 0 );
                    m_model.moveSearchRegionRgt.y =      iter->second.get< int >( "y", 0 );
                    m_model.moveSearchRegionRgt.width =  iter->second.get< int >( "width", 0 );
                    m_model.moveSearchRegionRgt.height = iter->second.get< int >( "height", 0 );
                }
                else if ( iter->first == "Target" )
                {
                    m_model.wholeTargetRegion.x =      iter->second.get< int >( "x", 0 );
                    m_model.wholeTargetRegion.y =      iter->second.get< int >( "y", 0 );
                    m_model.wholeTargetRegion.width =  iter->second.get< int >( "width", 0 );
                    m_model.wholeTargetRegion.height = iter->second.get< int >( "height", 0 );
                }
            }

            Point ptTop, ptBot;
            m_model.searchLineSet.clear();
            BOOST_FOREACH( property_tree::ptree::value_type &node, ptreeTop.get_child( "SearchLines" ) )
            {
                ptTop.x = node.second.get< int >( "topX", std::numeric_limits< int >::min() );
                ptTop.y = node.second.get< int >( "topY", std::numeric_limits< int >::min() );
                ptBot.x = node.second.get< int >( "botX", std::numeric_limits< int >::min() );
                ptBot.y = node.second.get< int >( "botY", std::numeric_limits< int >::min() );
                m_model.searchLineSet.push_back( LineEnds( ptTop, ptBot ) );
            }

#ifdef LOG_CALIB_VALUES
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~";
            FILE_LOG( logINFO ) << "Camera calibration association points";
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~";
            FILE_LOG( logINFO ) << "Columns=" << cols << " Rows=" << rows;
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~";
#endif
            if ( cols * rows != m_model.pixelPoints.size() )
            {
                FILE_LOG( logERROR ) << "[CalibBowtie::Load] Invalid association point count";
                retVal = GC_ERR;
            }
            else
            {
#ifdef LOG_CALIB_VALUES
                for ( size_t i = 0; i < m_settings.pixelPoints.size(); ++i )
                {
                    FILE_LOG( logINFO ) << "[r=" << i / cols << " c=" << i % cols << "] " << \
                                           " pixelX=" << m_settings.pixelPoints[ i ].x << " pixelY=" << m_settings.pixelPoints[ i ].y << \
                                           " worldX=" << m_settings.worldPoints[ i ].x << " worldY=" << m_settings.worldPoints[ i ].y;
                }
#endif
                m_model.gridSize = Size( static_cast< int >( cols ), static_cast< int >( rows ) );

                m_model.controlJson = ptreeTop.get< string >( "control_json", "{}" );

                Mat matIn, matOut;
                vector< Point > searchLineCorners = { m_model.searchLineSet[ 0 ].top, m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].top,
                                                      m_model.searchLineSet[ 0 ].bot, m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].bot };
                retVal = Calibrate( m_model.pixelPoints, m_model.worldPoints, m_model.moveSearchROIMultiplier,
                                    m_model.controlJson, m_model.gridSize, m_model.imgSize, searchLineCorners );
            }
#ifdef LOG_CALIB_VALUES
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << endl;
            FILE_LOG( logINFO ) << "Search lines";
            FILE_LOG( logINFO ) << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~";
            for ( size_t i = 0; i < m_settings.searchLines.size(); ++i )
            {
                FILE_LOG( logINFO ) << "[index=" << i << "] " << \
                                       " topX=" << m_settings.searchLines[ i ].top.x << " topY=" << m_settings.searchLines[ i ].top.y << \
                                       " botX=" << m_settings.searchLines[ i ].bot.x << " botY=" << m_settings.searchLines[ i ].bot.y;
            }
#endif
        }
    }
    catch( boost::exception &e )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::Load] " << diagnostic_information( e );
        retVal = GC_EXCEPT;
    }

    return retVal;
}
GC_STATUS CalibBowtie::Save( const string jsonCalFilepath )
{
    GC_STATUS retVal = GC_OK;

    if ( m_model.pixelPoints.empty() || m_model.worldPoints.empty() ||
         m_model.pixelPoints.size() != m_model.worldPoints.size() ||
         2 > m_model.gridSize.width || 4 > m_model.gridSize.height || m_model.searchLineSet.empty() )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::Save] Invalid calib grid dimension(s) or empty cal point vector(s)";
        retVal = GC_ERR;
    }
    else
    {
        try
        {
            ofstream fileStream( jsonCalFilepath, ios::out );
            if ( fileStream.is_open() )
            {
                fileStream << "{" << endl;
                fileStream << "  \"calibType\":\"BowTie\"" << "," << endl;
                fileStream << "  \"imageWidth\":" << m_model.imgSize.width << "," << endl;
                fileStream << "  \"imageHeight\":" << m_model.imgSize.height << "," << endl;
                fileStream << "  \"moveSearchROIMultiplier\":" << m_model.moveSearchROIMultiplier << "," << endl;
                fileStream << "  \"PixelToWorld\": " << endl;
                fileStream << "  {" << endl;
                fileStream << "    \"columns\": " << m_model.gridSize.width << "," << endl;
                fileStream << "    \"rows\": " <<    m_model.gridSize.height << "," << endl;
                fileStream << "    \"points\": [" << endl;
                fileStream << fixed << setprecision( 3 );
                for ( size_t i = 0; i < m_model.pixelPoints.size() - 1; ++i )
                {
                    fileStream << "      { \"pixelX\": " << m_model.pixelPoints[ i ].x << ", " << \
                                          "\"pixelY\": " << m_model.pixelPoints[ i ].y << ", " << \
                                          "\"worldX\": " << m_model.worldPoints[ i ].x << ", " << \
                                          "\"worldY\": " << m_model.worldPoints[ i ].y << " }," << endl;
                }
                fileStream << "      { \"pixelX\": " << m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].x << ", " << \
                                      "\"pixelY\": " << m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].y << ", " << \
                                      "\"worldX\": " << m_model.worldPoints[ m_model.pixelPoints.size() - 1 ].x << ", " << \
                                      "\"worldY\": " << m_model.worldPoints[ m_model.pixelPoints.size() - 1 ].y << " }" << endl;
                fileStream << "    ]" << endl;
                fileStream << "  }," << endl;
                fileStream << "  \"MoveSearchRegions\": " << endl;
                fileStream << "  {" << endl;
                fileStream << fixed << setprecision( 0 );
                fileStream << "    \"Left\":  { " << \
                                    "\"x\": " <<      m_model.moveSearchRegionLft.x << ", " << \
                                    "\"y\": " <<      m_model.moveSearchRegionLft.y << ", " << \
                                    "\"width\": " <<  m_model.moveSearchRegionLft.width << ", " << \
                                    "\"height\": " << m_model.moveSearchRegionLft.height << " }, " << endl;
                fileStream << "    \"Right\": { " << \
                                    "\"x\": " <<      m_model.moveSearchRegionRgt.x << ", " << \
                                    "\"y\": " <<      m_model.moveSearchRegionRgt.y << ", " << \
                                    "\"width\": " <<  m_model.moveSearchRegionRgt.width << ", " << \
                                    "\"height\": " << m_model.moveSearchRegionRgt.height << " }," << endl;
                fileStream << "    \"Target\": { " << \
                                    "\"x\": " <<      m_model.wholeTargetRegion.x << ", " << \
                                    "\"y\": " <<      m_model.wholeTargetRegion.y << ", " << \
                                    "\"width\": " <<  m_model.wholeTargetRegion.width << ", " << \
                                    "\"height\": " << m_model.wholeTargetRegion.height << " }" << endl;
                fileStream << "  }," << endl;
                fileStream << "  \"SearchLines\": [" << endl;
                for ( size_t i = 0; i < m_model.searchLineSet.size() - 1; ++i )
                {
                    fileStream << "      { \"topX\": " << m_model.searchLineSet[ i ].top.x << ", " << \
                                          "\"topY\": " << m_model.searchLineSet[ i ].top.y << ", " << \
                                          "\"botX\": " << m_model.searchLineSet[ i ].bot.x << ", " << \
                                          "\"botY\": " << m_model.searchLineSet[ i ].bot.y << " }," << endl;
                }
                fileStream << "      { \"topX\": " << m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].top.x << ", " << \
                                      "\"topY\": " << m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].top.y << ", " << \
                                      "\"botX\": " << m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].bot.x << ", " << \
                                      "\"botY\": " << m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].bot.y << " }" << endl;
                fileStream << "  ]," << endl;

                string escaped;
                for ( size_t i = 0; i < m_model.controlJson.size(); ++i )
                {
                    if ( m_model.controlJson[ i ] == '\"' )
                        escaped += "\\\"";
                    else
                        escaped += m_model.controlJson[ i ];
                }

                fileStream << "  \"control_json\": \"" << escaped << "\"" << endl;
                fileStream << "}" << endl;
                fileStream.close();
            }
            else
            {
                FILE_LOG( logERROR ) << "[CalibBowtie::Save] Could not open calibration save file " << jsonCalFilepath;
                retVal = GC_ERR;
            }
        }
        catch( boost::exception &e )
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::Save] " << diagnostic_information( e );
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
string CalibBowtie::ModelJsonString()
{
    stringstream ss;

    if ( m_model.pixelPoints.empty() || m_model.worldPoints.empty() ||
         m_model.pixelPoints.size() != m_model.worldPoints.size() ||
         2 > m_model.gridSize.width || 4 > m_model.gridSize.height || m_model.searchLineSet.empty() )
    {
        FILE_LOG( logERROR ) << "[CalibBowtie::SettingsJsonString] Invalid calib grid dimension(s) or empty cal point vector(s)";
    }
    else
    {
        try
        {
            ss << "{" << endl;
            ss << "  \"PixelToWorld\": " << endl;
            ss << "  {" << endl;
            ss << "    \"columns\": " << m_model.gridSize.width << "," << endl;
            ss << "    \"rows\": " <<    m_model.gridSize.height << "," << endl;
            ss << "    \"points\": [" << endl;
            for ( size_t i = 0; i < m_model.pixelPoints.size() - 1; ++i )
            {
                ss << "      { \"pixelX\": " << m_model.pixelPoints[ i ].x << ", " << \
                                      "\"pixelY\": " << m_model.pixelPoints[ i ].y << ", " << \
                                      "\"worldX\": " << m_model.worldPoints[ i ].x << ", " << \
                                      "\"worldY\": " << m_model.worldPoints[ i ].y << " }," << endl;
            }
            ss << "      { \"pixelX\": " << m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].x << ", " << \
                                  "\"pixelY\": " << m_model.pixelPoints[ m_model.pixelPoints.size() - 1 ].y << ", " << \
                                  "\"worldX\": " << m_model.worldPoints[ m_model.pixelPoints.size() - 1 ].x << ", " << \
                                  "\"worldY\": " << m_model.worldPoints[ m_model.pixelPoints.size() - 1 ].y << " }" << endl;
            ss << "    ]" << endl;
            ss << "  }," << endl;
            ss << "  \"MoveSearchRegions\": " << endl;
            ss << "  {" << endl;
            ss << "    \"Left\":  { " << \
                                "\"x\": " <<      m_model.moveSearchRegionLft.x << ", " << \
                                "\"y\": " <<      m_model.moveSearchRegionLft.y << ", " << \
                                "\"width\": " <<  m_model.moveSearchRegionLft.width << ", " << \
                                "\"height\": " << m_model.moveSearchRegionLft.height << " }, " << endl;
            ss << "    \"Right\": { " << \
                                "\"x\": " <<      m_model.moveSearchRegionRgt.x << ", " << \
                                "\"y\": " <<      m_model.moveSearchRegionRgt.y << ", " << \
                                "\"width\": " <<  m_model.moveSearchRegionRgt.width << ", " << \
                                "\"height\": " << m_model.moveSearchRegionRgt.height << " }" << endl;
            ss << "  }," << endl;
            ss << "  \"SearchLines\": [" << endl;
            for ( size_t i = 0; i < m_model.searchLineSet.size() - 1; ++i )
            {
                ss << "      { \"topX\": " << m_model.searchLineSet[ i ].top.x << ", " << \
                                      "\"topY\": " << m_model.searchLineSet[ i ].top.y << ", " << \
                                      "\"botX\": " << m_model.searchLineSet[ i ].bot.x << ", " << \
                                      "\"botY\": " << m_model.searchLineSet[ i ].bot.y << " }," << endl;
            }
            ss << "      { \"topX\": " << m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].top.x << ", " << \
                                  "\"topY\": " << m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].top.y << ", " << \
                                  "\"botX\": " << m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].bot.x << ", " << \
                                  "\"botY\": " << m_model.searchLineSet[ m_model.searchLineSet.size() - 1 ].bot.y << " }" << endl;
            ss << "  ]" << endl;
            ss << "}" << endl;
        }
        catch( boost::exception &e )
        {
            FILE_LOG( logERROR ) << "[CalibBowtie::SettingsJsonString] " << diagnostic_information( e );
        }
    }

    return ss.str();
}

} // namespace gc


/*
 *  (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


//  (C) Copyright Eric Niebler 2005.
//  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)

#include <boost/test/unit_test.hpp>
#include <boost/time_series/constant_series.hpp>
#include <boost/time_series/dense_series.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/ordered_inserter.hpp>
#include <boost/time_series/numeric/partial_sum.hpp>

using namespace boost;
using namespace unit_test;

namespace seq = boost::sequence;
namespace rrs = boost::range_run_storage;

///////////////////////////////////////////////////////////////////////////////
// test_partial_sum
//
void test_partial_sum()
{
    using namespace time_series;

    // expected result for test 1:
    piecewise_constant_series<int> result;
    make_ordered_inserter(result)
        (2, 0)(4)(6)(8)(10)(12)(14)(16)(18)(20)
        (24)(28)(32)(36)(40)(44)(48)(52)(56)(60)
        (60, 20, 90)
        (62)(64)(66)(68)(70)(72)(74)(76)
        (78, 98, inf)
    .commit();

    piecewise_constant_series<int> p;
    make_ordered_inserter(p)
        (2, 0, 10)
        (4, 10, 20)
        (2, 90, 99)
    .commit();
    piecewise_constant_series<int> s3 = partial_sum(p);
    BOOST_CHECK_EQUAL(s3, result);

    // expected result for test 2:
    piecewise_constant_series<int, monthly> result2;
    make_ordered_inserter(result2)
        (1, 1)(3)(6)(10)(15)(21)(28)(36)(45, 9, inf)
    .commit();

    typedef rrs::unit_run<std::ptrdiff_t> dense_run;
    dense_series<int, monthly> d(stop = 10);
    for(int i=0; i<10; ++i)
        range_run_storage::set_at(d, dense_run(i), i);

    piecewise_constant_series<int, monthly> s4 = partial_sum(d);
    BOOST_CHECK_EQUAL(s4, result2);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("partial_sum test");

    test->add(BOOST_TEST_CASE(&test_partial_sum));

    return test;
}


// 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_


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

#include <Eigen/Dense>
#include <iostream>
#include <limits>
#include "helpers.h"

using namespace std;

void Helpers::printVector( const Eigen::VectorXd& vector, const string& name )
{
    Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
    cout << name << ":" << endl << vector.format(CleanFmt) << endl;
}

void Helpers::printMatrix( const Eigen::MatrixXd& mat, const string& name )
{
    Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
    cout << name << ":" << endl << mat.format(CleanFmt) << endl;
}

void Helpers::maxElement( const Eigen::MatrixXd& mat, unsigned long& m_idx, unsigned long& n_idx, double& maxVal)
{
    maxVal = numeric_limits<double>::min();
    for( unsigned int m = 0; m < mat.rows(); m++ )
    {
        for( unsigned int n = 0; n < mat.cols(); n++ )
        {
            double val = mat(m,n);
            if( val > maxVal )
            {
                maxVal = val;
                m_idx = m; n_idx = n;
            }
        }
    }
}

Eigen::MatrixXd Helpers::mean( const std::vector<Eigen::MatrixXd>& input )
{
    size_t n = input.size();
    if( n == 0 )
    {
        std::cerr << "Helpers::mean: empty.";
        return Eigen::MatrixXd(0,0);
    }

    Eigen::MatrixXd accum = Eigen::MatrixXd::Constant(input.at(0).rows(),1, 0.0);
    for(size_t i = 0; i < n; i++)
        accum = accum + input.at(i);

    accum = accum / static_cast<double>(n);

    return accum;
}


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

#ifndef SPHEROCYLINDER_H
#define SPHEROCYLINDER_H

using namespace std;
// using namespace boost::numeric::ublas;
namespace ublas = boost::numeric::ublas;

class Crosslink {
public:
	//Crosslink(int id, ublas::vector<double> r);
	Crosslink(int id, ublas::vector<double> r, ublas::matrix<int> conTabSelf, ublas::matrix<int> conTabOther);

    void updatePosition(ublas::vector<double> r);

	ublas::vector<double> incrPosition(ublas::vector<double> r);

	void updateForce(ublas::vector<double> f);

	void incrForce(ublas::vector<double> f, int filId);

	ublas::vector<double> getPosition();

	ublas::vector<double> getForce();
	ublas::vector<double> getFilForce(int filId);
	int getId();

	ublas::matrix<int> getConTabSelf();
	ublas::matrix<int> getConTabOther();

	void updateFilForce(ublas::vector<double> f, int filId);

private:

	ublas::vector<double> r_;

	ublas::vector<double> f_;

	ublas::matrix<int> conTabSelf_; // gives indices for WBX,WBY,WBZ tables that stand for the connections of the crosslink and its neighbors
	ublas::matrix<int> conTabOther_;

	int id_;

	ublas::vector<double> f1_; //force from filament 1
	ublas::vector<double> f2_; //force from filament 2

};

#endif


#pragma once

#include <Eigen/Dense>

#include "GNAObject.hh"
#include "OscillationVariables.hh"

class OscProb2nu: public GNAObject,
                  public TransformationBind<OscProb2nu> {
public:
  OscProb2nu();

  template <typename DerivedA, typename DerivedB>
  void probability(const Eigen::ArrayBase<DerivedA> &Enu,
                   Eigen::ArrayBase<DerivedB> &ret);
protected:
  variable<double> m_L;
  std::unique_ptr<OscillationVariables> m_param;
};