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

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "mkl.h"
#include "mkl_lapacke.h"

namespace votca { namespace tools {

using namespace std;

void linalg_qrsolve(ub::vector<double> &x, ub::matrix<double> &A, ub::vector<double> &b, ub::vector<double> *residual){
    // check matrix for zero column
    int nonzero_found = 0;
    for(size_t j=0; j<A.size2(); j++) {
        nonzero_found = 0;
        for(size_t i=0; i<A.size1(); i++) {
            if(fabs(A(i,j))>0) {
                nonzero_found = 1;
            }
        }
        if(nonzero_found==0) {
            throw "qrsolve_zero_column_in_matrix";
        }
    }

    MKL_INT info;
    MKL_INT sizeM = A.size1();
    MKL_INT sizeN = A.size2();
    MKL_INT nrhs = 1;
    char trans = 'N';

    // pointer for LAPACK
    double * pA = const_cast<double*>(&A.data().begin()[0]);
    double * pb = const_cast<double*>(&b.data()[0]);

    info = LAPACKE_dgels( LAPACK_ROW_MAJOR , trans , sizeM, sizeN, nrhs , pA , sizeM , pb , sizeM );

    if ( info != 0 )
        throw std::runtime_error("QR least-squares solver failed");

    for (size_t i =0 ; i < x.size(); i++){
        x(i) = b(i);
    }

    for (size_t i = 0 ; i < b.size(); i++){
        (*residual)(i) = b(i + x.size() );
    }
}

void linalg_constrained_qrsolve(ub::vector<double> &x, ub::matrix<double> &A, ub::vector<double> &b, ub::matrix<double> &constr){
        // matrix inversion using MKL
    throw std::runtime_error("linalg_constrained_qrsolve is not compiled-in due to disabling of GSL - recompile Votca Tools with GSLsupport");
}

}}


#include "Algorithms/PeakFinding.h"

#include <boost/test/unit_test.hpp>
#include <vector>

using Algorithms::PeakFinding::OneDim::straightforward_search;
using std::vector;

BOOST_AUTO_TEST_SUITE(Algorithms)
BOOST_AUTO_TEST_SUITE(PeakFinding_tests)
BOOST_AUTO_TEST_SUITE(OneDim_tests)

// cf. https://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-006-introduction-to-algorithms-fall-2011/lecture-videos/MIT6_006F11_lec01.pdf

const vector<int> a {6, 7, 4, 3, 2, 1, 4, 5};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateStraightforwardAlgorithm)
{
  BOOST_TEST(straightforward_search(a.data(), a.size()) == a.size() - 1);
  BOOST_TEST(straightforward_search(a) == a.size() - 1);
}

BOOST_AUTO_TEST_SUITE_END() // OneDim_tests
BOOST_AUTO_TEST_SUITE_END() // PeakFinding_tests
BOOST_AUTO_TEST_SUITE_END() // Algorithms


//==================================================================================================
/*
  Copyright 2016 NumScale SAS

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

//! [threshold]
#include <algorithm>
#include <chrono>
#include <cstdint>
#include <iostream>
#include <limits>
#include <vector>

#include <boost/simd/pack.hpp>

#include <boost/simd/function/aligned_store.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/function/if_zero_else_one.hpp>
#include <boost/simd/function/deinterleave_first.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/memory/allocator.hpp>

int main()
{
  namespace bs = boost::simd;
  using namespace std::chrono;
  int image_size = 2560 * 2560;
  std::vector<std::int16_t, bs::allocator<std::int16_t>> image(image_size);
  std::vector<std::int8_t, bs::allocator<std::int8_t>> binary(image_size);
  std::generate(image.begin(), image.end(),
                []() { return std::rand() % std::numeric_limits<std::int16_t>::max(); });
  // select arbitrary threshold
  std::int16_t threshold = 5000;
  auto t0                = high_resolution_clock::now();
  //! [scalar-threshold]
  for (int i = 0; i < image.size(); ++i) {
    if (image[i] < threshold) {
      binary[i] = 0;
    } else {
      binary[i] = 1;
    }
  }
  auto t1 = high_resolution_clock::now();
  std::cout << "scalar       " << duration_cast<microseconds>(t1 - t0).count() << std::endl;
  //! [scalar-threshold]

  //! [simd-threshold]
  using pack_t    = bs::pack<std::int16_t>;

  static const std::size_t cardinal = pack_t::static_size;

  //! [simd-threshold-downgrade]
  using pack_8           = bs::pack<std::int8_t>;
  std::size_t cardinal_8 = pack_8::static_size;
  std::vector<std::int8_t, bs::allocator<std::int8_t>> binary_8(image_size);
  t0 = high_resolution_clock::now();
  for (int i = 0; i < image.size(); i += cardinal_8) {
    pack_t v_image0(&image[i]);
    pack_t v_image1(&image[i + cardinal]);
    pack_t v_binary0      = bs::if_zero_else_one(bs::is_less(v_image0, threshold));
    pack_t v_binary1      = bs::if_zero_else_one(bs::is_less(v_image1, threshold));
    pack_8 v_binary_group = bs::group(v_binary0, v_binary1);
    bs::aligned_store(v_binary_group, &binary_8[i]);
  }
  t1 = high_resolution_clock::now();
  std::cout << "downgrade " << duration_cast<microseconds>(t1 - t0).count() << std::endl;
  //! [simd-threshold-downgrade]
}
// This code can be compiled using (for instance for gcc)
// g++ thresholding.cpp -msse4.2 -std=c++11 -O3 -DNDEBUG -o thresholding
// -I/path_to/boost_simd/ -I/path_to/boost/

//! [threshold]


/**
 * @file lars_test.cpp
 * @author Nishant Mehta
 *
 * Test for LARS.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

// Note: We don't use BOOST_REQUIRE_CLOSE in the code below because we need
// to use FPC_WEAK, and it's not at all intuitive how to do that.
#include <mlpack/methods/lars/lars.hpp>
#include <mlpack/core/data/load.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::regression;

BOOST_AUTO_TEST_SUITE(LARSTest);

void GenerateProblem(
    arma::mat& X, arma::rowvec& y, size_t nPoints, size_t nDims)
{
  X = arma::randn(nDims, nPoints);
  arma::vec beta = arma::randn(nDims, 1);
  y = beta.t() * X;
}

void LARSVerifyCorrectness(arma::vec beta, arma::vec errCorr, double lambda)
{
  size_t nDims = beta.n_elem;
  const double tol = 1e-10;
  for (size_t j = 0; j < nDims; j++)
  {
    if (beta(j) == 0)
    {
      // Make sure that |errCorr(j)| <= lambda.
      BOOST_REQUIRE_SMALL(std::max(fabs(errCorr(j)) - lambda, 0.0), tol);
    }
    else if (beta(j) < 0)
    {
      // Make sure that errCorr(j) == lambda.
      BOOST_REQUIRE_SMALL(errCorr(j) - lambda, tol);
    }
    else // beta(j) > 0
    {
      // Make sure that errCorr(j) == -lambda.
      BOOST_REQUIRE_SMALL(errCorr(j) + lambda, tol);
    }
  }
}

void LassoTest(size_t nPoints, size_t nDims, bool elasticNet, bool useCholesky)
{
  arma::mat X;
  arma::rowvec y;

  for (size_t i = 0; i < 100; i++)
  {
    GenerateProblem(X, y, nPoints, nDims);

    // Armadillo's median is broken, so...
    arma::vec sortedAbsCorr = sort(abs(X * y.t()));
    double lambda1 = sortedAbsCorr(nDims / 2);
    double lambda2;
    if (elasticNet)
      lambda2 = lambda1 / 2;
    else
      lambda2 = 0;

    LARS lars(useCholesky, lambda1, lambda2);
    arma::vec betaOpt;
    lars.Train(X, y, betaOpt);

    arma::vec errCorr = (X * trans(X) + lambda2 *
        arma::eye(nDims, nDims)) * betaOpt - X * y.t();

    LARSVerifyCorrectness(betaOpt, errCorr, lambda1);
  }
}

BOOST_AUTO_TEST_CASE(LARSTestLassoCholesky)
{
  LassoTest(100, 10, false, true);
}

BOOST_AUTO_TEST_CASE(LARSTestLassoGram)
{
  LassoTest(100, 10, false, false);
}

BOOST_AUTO_TEST_CASE(LARSTestElasticNetCholesky)
{
  LassoTest(100, 10, true, true);
}

BOOST_AUTO_TEST_CASE(LARSTestElasticNetGram)
{
  LassoTest(100, 10, true, false);
}

// Ensure that LARS doesn't crash when the data has linearly dependent features
// (meaning that there is a singularity).  This test uses the Cholesky
// factorization.
BOOST_AUTO_TEST_CASE(CholeskySingularityTest)
{
  arma::mat X;
  arma::mat Y;

  data::Load("lars_dependent_x.csv", X);
  data::Load("lars_dependent_y.csv", Y);

  arma::rowvec y = Y.row(0);

  // Test for a couple values of lambda1.
  for (double lambda1 = 0.0; lambda1 < 1.0; lambda1 += 0.1)
  {
    LARS lars(true, lambda1, 0.0);
    arma::vec betaOpt;
    lars.Train(X, y, betaOpt);

    arma::vec errCorr = (X * X.t()) * betaOpt - X * y.t();

    LARSVerifyCorrectness(betaOpt, errCorr, lambda1);
  }
}

// Same as the above test but with no cholesky factorization.
BOOST_AUTO_TEST_CASE(NoCholeskySingularityTest)
{
  arma::mat X;
  arma::mat Y;

  data::Load("lars_dependent_x.csv", X);
  data::Load("lars_dependent_y.csv", Y);

  arma::rowvec y = Y.row(0);

  // Test for a couple values of lambda1.
  for (double lambda1 = 0.0; lambda1 < 1.0; lambda1 += 0.1)
  {
    LARS lars(false, lambda1, 0.0);
    arma::vec betaOpt;
    lars.Train(X, y, betaOpt);

    arma::vec errCorr = (X * X.t()) * betaOpt - X * y.t();

    // #373: this test fails on i386 only sometimes.
//    LARSVerifyCorrectness(betaOpt, errCorr, lambda1);
  }
}

// Make sure that Predict() provides reasonable enough solutions.
BOOST_AUTO_TEST_CASE(PredictTest)
{
  for (size_t i = 0; i < 2; ++i)
  {
    // Run with both true and false.
    bool useCholesky = bool(i);

    arma::mat X;
    arma::rowvec y;

    GenerateProblem(X, y, 1000, 100);

    for (double lambda1 = 0.0; lambda1 < 1.0; lambda1 += 0.2)
    {
      for (double lambda2 = 0.0; lambda2 < 1.0; lambda2 += 0.2)
      {
        LARS lars(useCholesky, lambda1, lambda2);
        arma::vec betaOpt;
        lars.Train(X, y, betaOpt);

        // Calculate what the actual error should be with these regression
        // parameters.
        arma::vec betaOptPred = (X * X.t()) * betaOpt;
        arma::rowvec predictions;
        lars.Predict(X, predictions);
        arma::vec adjPred = X * predictions.t();

        BOOST_REQUIRE_EQUAL(predictions.n_elem, 1000);
        for (size_t i = 0; i < betaOptPred.n_elem; ++i)
        {
          if (std::abs(betaOptPred[i]) < 1e-5)
            BOOST_REQUIRE_SMALL(adjPred[i], 1e-5);
          else
            BOOST_REQUIRE_CLOSE(adjPred[i], betaOptPred[i], 1e-5);
        }
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(PredictRowMajorTest)
{
  arma::mat X;
  arma::rowvec y;
  GenerateProblem(X, y, 1000, 100);

  // Set lambdas to 0.

  LARS lars(false, 0, 0);
  arma::vec betaOpt;
  lars.Train(X, y, betaOpt);

  // Get both row-major and column-major predictions.  Make sure they are the
  // same.
  arma::rowvec rowMajorPred, colMajorPred;

  lars.Predict(X, colMajorPred);
  lars.Predict(X.t(), rowMajorPred, true);

  BOOST_REQUIRE_EQUAL(colMajorPred.n_elem, rowMajorPred.n_elem);
  for (size_t i = 0; i < colMajorPred.n_elem; ++i)
  {
    if (std::abs(colMajorPred[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(rowMajorPred[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(colMajorPred[i], rowMajorPred[i], 1e-5);
  }
}

/**
 * Make sure that if we train twice, there is no issue.
 */
BOOST_AUTO_TEST_CASE(RetrainTest)
{
  arma::mat origX;
  arma::rowvec origY;
  GenerateProblem(origX, origY, 1000, 50);

  arma::mat newX;
  arma::rowvec newY;
  GenerateProblem(newX, newY, 750, 75);

  LARS lars(false, 0.1, 0.1);
  arma::vec betaOpt;
  lars.Train(origX, origY, betaOpt);

  // Now train on new data.
  lars.Train(newX, newY, betaOpt);

  arma::vec errCorr = (newX * trans(newX) + 0.1 *
        arma::eye(75, 75)) * betaOpt - newX * newY.t();

  LARSVerifyCorrectness(betaOpt, errCorr, 0.1);
}

/**
 * Make sure if we train twice using the Cholesky decomposition, there is no
 * issue.
 */
BOOST_AUTO_TEST_CASE(RetrainCholeskyTest)
{
  arma::mat origX;
  arma::rowvec origY;
  GenerateProblem(origX, origY, 1000, 50);

  arma::mat newX;
  arma::rowvec newY;
  GenerateProblem(newX, newY, 750, 75);

  LARS lars(true, 0.1, 0.1);
  arma::vec betaOpt;
  lars.Train(origX, origY, betaOpt);

  // Now train on new data.
  lars.Train(newX, newY, betaOpt);

  arma::vec errCorr = (newX * trans(newX) + 0.1 *
        arma::eye(75, 75)) * betaOpt - newX * newY.t();

  LARSVerifyCorrectness(betaOpt, errCorr, 0.1);
}

/**
 * Make sure that we get correct solution coefficients when running training
 * and accessing solution coefficients separately.
 */
BOOST_AUTO_TEST_CASE(TrainingAndAccessingBetaTest)
{
  arma::mat X;
  arma::rowvec y;

  GenerateProblem(X, y, 1000, 100);

  LARS lars1;
  arma::vec beta;
  lars1.Train(X, y, beta);

  LARS lars2;
  lars2.Train(X, y);

  BOOST_REQUIRE_EQUAL(beta.n_elem, lars2.Beta().n_elem);
  for (size_t i = 0; i < beta.n_elem; ++i)
    BOOST_REQUIRE_CLOSE(beta[i], lars2.Beta()[i], 1e-5);
}

/**
 * Make sure that we learn the same when running training separately and through
 * constructor. Test it with default parameters.
 */
BOOST_AUTO_TEST_CASE(TrainingConstructorWithDefaultsTest)
{
  arma::mat X;
  arma::rowvec y;

  GenerateProblem(X, y, 1000, 100);

  LARS lars1;
  arma::vec beta;
  lars1.Train(X, y, beta);

  LARS lars2(X, y);

  BOOST_REQUIRE_EQUAL(beta.n_elem, lars2.Beta().n_elem);
  for (size_t i = 0; i < beta.n_elem; ++i)
    BOOST_REQUIRE_CLOSE(beta[i], lars2.Beta()[i], 1e-5);
}

/**
 * Make sure that we learn the same when running training separately and through
 * constructor. Test it with non default parameters.
 */
BOOST_AUTO_TEST_CASE(TrainingConstructorWithNonDefaultsTest)
{
  arma::mat X;
  arma::rowvec y;

  GenerateProblem(X, y, 1000, 100);

  bool transposeData = true;
  bool useCholesky = true;
  double lambda1 = 0.2;
  double lambda2 = 0.4;

  LARS lars1(useCholesky, lambda1, lambda2);
  arma::vec beta;
  lars1.Train(X, y, beta);

  LARS lars2(X, y, transposeData, useCholesky, lambda1, lambda2);

  BOOST_REQUIRE_EQUAL(beta.n_elem, lars2.Beta().n_elem);
  for (size_t i = 0; i < beta.n_elem; ++i)
    BOOST_REQUIRE_CLOSE(beta[i], lars2.Beta()[i], 1e-5);
}

/**
 * Test that LARS::Train() returns finite correlation value.
 */
BOOST_AUTO_TEST_CASE(LARSTrainReturnCorrelation)
{
  arma::mat X;
  arma::mat Y;

  data::Load("lars_dependent_x.csv", X);
  data::Load("lars_dependent_y.csv", Y);

  arma::rowvec y = Y.row(0);

  double lambda1 = 0.1;
  double lambda2 = 0.1;

  // Test with Cholesky decomposition and with lasso.
  LARS lars1(true, lambda1, 0.0);
  arma::vec betaOpt1;
  double maxCorr = lars1.Train(X, y, betaOpt1);

  BOOST_REQUIRE_EQUAL(std::isfinite(maxCorr), true);

  // Test without Cholesky decomposition and with lasso.
  LARS lars2(false, lambda1, 0.0);
  arma::vec betaOpt2;
  maxCorr = lars2.Train(X, y, betaOpt2);

  BOOST_REQUIRE_EQUAL(std::isfinite(maxCorr), true);

  // Test with Cholesky decomposition and with elasticnet.
  LARS lars3(true, lambda1, lambda2);
  arma::vec betaOpt3;
  maxCorr = lars3.Train(X, y, betaOpt3);

  BOOST_REQUIRE_EQUAL(std::isfinite(maxCorr), true);

  // Test without Cholesky decomposition and with elasticnet.
  LARS lars4(false, lambda1, lambda2);
  arma::vec betaOpt4;
  maxCorr = lars4.Train(X, y, betaOpt4);

  BOOST_REQUIRE_EQUAL(std::isfinite(maxCorr), true);
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef SKYLARK_MMT_HPP
#define SKYLARK_MMT_HPP

#ifndef SKYLARK_SKETCH_HPP
#error "Include top-level sketch.hpp instead of including individuals headers"
#endif

#include <boost/random.hpp>

namespace skylark { namespace sketch {

/**
 * Meng-Mahoney Transform
 *
 * Meng-Mahoney Transform is very similar to the Clarkson-Woodruff Transform:
 * it replaces the +1/-1 diagonal with Cauchy random enteries. Thus, it
 * provides a low-distortion of l1-norm subspace embedding.
 *
 * See Meng and Mahoney's STOC'13 paper.
 */

template < typename InputMatrixType,
           typename OutputMatrixType = InputMatrixType >
struct MMT_t :
        public MMT_data_t,
        virtual public sketch_transform_t<InputMatrixType, OutputMatrixType > {

public:

    // We use composition to defer calls to hash_transform_t
    typedef hash_transform_t< InputMatrixType, OutputMatrixType,
                              boost::random::uniform_int_distribution,
                              boost::random::cauchy_distribution> transform_t;

    typedef MMT_data_t data_type;
    typedef data_type::params_t params_t;

    MMT_t(int N, int S, base::context_t& context)
        : data_type(N, S, context), _transform(*this) {

    }

    MMT_t(int N, int S, const params_t& params, base::context_t& context)
        : data_type(N, S, params, context),
          _transform(*this) {

    }

    MMT_t(const boost::property_tree::ptree& pt)
        : data_type(pt), _transform(*this) {

    }

    template< typename OtherInputMatrixType,
              typename OtherOutputMatrixType >
    MMT_t(const MMT_t<OtherInputMatrixType,OtherOutputMatrixType>& other)
        : data_type(other), _transform(*this) {

    }

    MMT_t(const data_type& other)
        : data_type(other), _transform(*this) {

    }

    /**
     * Apply columnwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply (const typename transform_t::matrix_type& A,
                typename transform_t::output_matrix_type& sketch_of_A,
                columnwise_tag dimension) const {
        _transform.apply(A, sketch_of_A, dimension);
    }

    /**
     * Apply rowwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply (const typename transform_t::matrix_type& A,
                typename transform_t::output_matrix_type& sketch_of_A,
                rowwise_tag dimension) const {
        _transform.apply(A, sketch_of_A, dimension);
    }

    int get_N() const { return this->_N; } /**< Get input dimesion. */
    int get_S() const { return this->_S; } /**< Get output dimesion. */

    const sketch_transform_data_t* get_data() const { return this; }

private:
    transform_t _transform;
};

template<>
class MMT_t<boost::any, boost::any> :
  public MMT_data_t,
  virtual public sketch_transform_t<boost::any, boost::any > {

public:

    typedef MMT_data_t data_type;
    typedef data_type::params_t params_t;

    MMT_t(int N, int S, base::context_t& context)
        : data_type(N, S, context) {

    }

    MMT_t(int N, int S, const params_t& params, base::context_t& context)
        : data_type(N, S, params, context) {

    }

    MMT_t(const boost::property_tree::ptree &pt)
        : data_type(pt) {

    }

    /**
     * Copy constructor
     */
    template <typename OtherInputMatrixType,
              typename OtherOutputMatrixType>
    MMT_t (const MMT_t<OtherInputMatrixType, OtherOutputMatrixType>& other)
        : data_type(other) {

    }

    /**
     * Constructor from data
     */
    MMT_t (const data_type& other)
        : data_type(other) {

    }

    /**
     * Apply columnwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply(const boost::any &A, const boost::any &sketch_of_A,
                columnwise_tag dimension) const {

#if     !(defined SKYLARK_NO_ANY) || (defined SKYLARK_WITH_MMT_ANY)

        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::matrix_t, mdtypes::matrix_t,
            MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
            mdtypes::matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
            mdtypes::sparse_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::shared_matrix_t,
            mdtypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::root_matrix_t,
            mdtypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::dist_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::dist_matrix_vc_star_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::dist_matrix_vr_star_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_star_vc_t,
            mdtypes::dist_matrix_star_vc_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_star_vr_t,
            mdtypes::dist_matrix_star_vr_t, MMT_t);

        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::matrix_t, mftypes::matrix_t,
            MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::sparse_matrix_t,
            mftypes::matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::sparse_matrix_t,
            mftypes::sparse_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::shared_matrix_t,
            mftypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::root_matrix_t,
            mftypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::dist_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::dist_matrix_vc_star_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::dist_matrix_vr_star_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_star_vc_t,
            mftypes::dist_matrix_star_vc_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_star_vr_t,
            mftypes::dist_matrix_star_vr_t, MMT_t);

#endif

        SKYLARK_THROW_EXCEPTION (
          base::sketch_exception()
              << base::error_msg(
                 "This combination has not yet been implemented for MMT"));

    }

    /**
     * Apply rowwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply (const boost::any &A, const boost::any &sketch_of_A,
        rowwise_tag dimension) const {

#if     !(defined SKYLARK_NO_ANY) || (defined SKYLARK_WITH_MMT_ANY)

        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::matrix_t, mdtypes::matrix_t,
            MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
            mdtypes::matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
            mdtypes::sparse_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::shared_matrix_t,
            mdtypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::root_matrix_t,
            mdtypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::dist_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::dist_matrix_vc_star_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::dist_matrix_vr_star_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_star_vc_t,
            mdtypes::dist_matrix_star_vc_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_star_vr_t,
            mdtypes::dist_matrix_star_vr_t, MMT_t);

        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::matrix_t, mftypes::matrix_t,
            MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::sparse_matrix_t,
            mftypes::matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::sparse_matrix_t,
            mftypes::sparse_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::shared_matrix_t,
            mftypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::root_matrix_t,
            mftypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::root_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::shared_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::dist_matrix_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::dist_matrix_vc_star_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::dist_matrix_vr_star_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_star_vc_t,
            mftypes::dist_matrix_star_vc_t, MMT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_star_vr_t,
            mftypes::dist_matrix_star_vr_t, MMT_t);
#endif

        SKYLARK_THROW_EXCEPTION (
          base::sketch_exception()
              << base::error_msg(
                 "This combination has not yet been implemented for MMT"));
    }

    int get_N() const { return this->_N; } /**< Get input dimesion. */
    int get_S() const { return this->_S; } /**< Get output dimesion. */

    const sketch_transform_data_t* get_data() const { return this; }
};

#undef _SL_HTBASE

} } /** namespace skylark::sketch */

#endif // SKYLARK_MMT_HPP


/* ----------------------------------------------------------------------------
 * Copyright 2021, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#include <Eigen/StdVector>
#include <stdio.h>
#include <math.h>
#include <algorithm>
#include <vector>
#include <stdlib.h>

#include "panther.hpp"
#include "timer.hpp"
#include "termcolor.hpp"

using namespace termcolor;

// Uncomment the type of timer you want:
// typedef ROSTimer MyTimer;
// typedef ROSWallTimer MyTimer;
typedef PANTHER_timers::Timer MyTimer;

Panther::Panther(mt::parameters par) : par_(par)
{
  drone_status_ == DroneStatus::YAWING;
  G_.pos << 0, 0, 0;
  G_term_.pos << 0, 0, 0;

  mtx_initial_cond.lock();
  stateA_.setZero();
  mtx_initial_cond.unlock();

  changeDroneStatus(DroneStatus::GOAL_REACHED);
  resetInitialization();

  mt::basisConverter basis_converter;

  if (par.basis == "MINVO")
  {
    A_basis_deg1_rest_ = basis_converter.getArestMinvoDeg1();
    A_basis_deg2_rest_ = basis_converter.getArestMinvoDeg2();
    A_basis_deg3_rest_ = basis_converter.getArestMinvoDeg3();
  }
  else if (par.basis == "BEZIER")
  {
    A_basis_deg1_rest_ = basis_converter.getArestBezierDeg1();
    A_basis_deg2_rest_ = basis_converter.getArestBezierDeg2();
    A_basis_deg3_rest_ = basis_converter.getArestBezierDeg3();
  }
  else if (par.basis == "B_SPLINE")
  {
    A_basis_deg1_rest_ = basis_converter.getArestBSplineDeg1();
    A_basis_deg2_rest_ = basis_converter.getArestBSplineDeg2();
    A_basis_deg3_rest_ = basis_converter.getArestBSplineDeg3();
  }
  else
  {
    std::cout << red << "Basis " << par.basis << " not implemented yet" << reset << std::endl;
    std::cout << red << "============================================" << reset << std::endl;
    abort();
  }

  A_basis_deg1_rest_inverse_ = A_basis_deg1_rest_.inverse();
  A_basis_deg2_rest_inverse_ = A_basis_deg2_rest_.inverse();
  A_basis_deg3_rest_inverse_ = A_basis_deg3_rest_.inverse();

  log_ptr_ = std::shared_ptr<mt::log>(new mt::log);

  solver_ = new SolverIpopt(par_, log_ptr_);

  separator_solver_ = new separator::Separator();
}

void Panther::dynTraj2dynTrajCompiled(const mt::dynTraj& traj, mt::dynTrajCompiled& traj_compiled)
{
  if (traj.use_pwp_field == true)
  {
    traj_compiled.pwp_mean = traj.pwp_mean;
    traj_compiled.pwp_var = traj.pwp_var;
    traj_compiled.is_static =
        ((traj.pwp_mean.eval(0.0) - traj.pwp_mean.eval(1e30)).norm() < 1e-5);  // TODO: Improve this
  }
  else
  {
    mtx_t_.lock();

    typedef exprtk::symbol_table<double> symbol_table_t;
    typedef exprtk::expression<double> expression_t;
    typedef exprtk::parser<double> parser_t;

    // Compile the mean
    for (auto function_i : traj.s_mean)
    {
      symbol_table_t symbol_table;
      symbol_table.add_variable("t", t_);
      symbol_table.add_constants();
      expression_t expression;
      expression.register_symbol_table(symbol_table);
      parser_t parser;
      parser.compile(function_i, expression);
      traj_compiled.s_mean.push_back(expression);
    }

    // Compile the variance
    for (auto function_i : traj.s_var)
    {
      symbol_table_t symbol_table;
      symbol_table.add_variable("t", t_);
      symbol_table.add_constants();
      expression_t expression;
      expression.register_symbol_table(symbol_table);
      parser_t parser;
      parser.compile(function_i, expression);
      traj_compiled.s_var.push_back(expression);
    }

    mtx_t_.unlock();

    traj_compiled.is_static =
        (traj.s_mean[0].find("t") == std::string::npos) &&  // there is no dependence on t in the coordinate x
        (traj.s_mean[1].find("t") == std::string::npos) &&  // there is no dependence on t in the coordinate y
        (traj.s_mean[2].find("t") == std::string::npos);    // there is no dependence on t in the coordinate z
  }

  traj_compiled.use_pwp_field = traj.use_pwp_field;
  traj_compiled.is_agent = traj.is_agent;
  traj_compiled.bbox = traj.bbox;
  traj_compiled.id = traj.id;
  traj_compiled.time_received = traj.time_received;  // ros::Time::now().toSec();
}

// Note that this function is here because I need t_ for this evaluation
Eigen::Vector3d Panther::evalMeanDynTrajCompiled(const mt::dynTrajCompiled& traj, double t)
{
  Eigen::Vector3d tmp;

  if (traj.use_pwp_field == true)
  {
    tmp = traj.pwp_mean.eval(t);
  }
  else
  {
    mtx_t_.lock();
    t_ = t;
    tmp << traj.s_mean[0].value(),  ////////////////
        traj.s_mean[1].value(),     ////////////////
        traj.s_mean[2].value();     ////////////////

    mtx_t_.unlock();
  }
  return tmp;
}

// Note that this function is here because it needs t_ for this evaluation
Eigen::Vector3d Panther::evalVarDynTrajCompiled(const mt::dynTrajCompiled& traj, double t)
{
  Eigen::Vector3d tmp;

  if (traj.use_pwp_field == true)
  {
    tmp = traj.pwp_var.eval(t);
  }
  else
  {
    mtx_t_.lock();
    t_ = t;
    tmp << traj.s_var[0].value(),  ////////////////
        traj.s_var[1].value(),     ////////////////
        traj.s_var[2].value();     ////////////////

    mtx_t_.unlock();
  }
  return tmp;
}

void Panther::removeOldTrajectories()
{
  double time_now = ros::Time::now().toSec();
  std::vector<int> ids_to_remove;

  mtx_trajs_.lock();

  for (int index_traj = 0; index_traj < trajs_.size(); index_traj++)
  {
    if ((time_now - trajs_[index_traj].time_received) > par_.max_seconds_keeping_traj)
    {
      ids_to_remove.push_back(trajs_[index_traj].id);
    }
  }

  for (auto id : ids_to_remove)
  {
    // ROS_WARN_STREAM("Removing " << id);
    trajs_.erase(
        std::remove_if(trajs_.begin(), trajs_.end(), [&](mt::dynTrajCompiled const& traj) { return traj.id == id; }),
        trajs_.end());
  }

  mtx_trajs_.unlock();
}

// Note that we need to compile the trajectories inside panther.cpp because t_ is in panther.hpp
void Panther::updateTrajObstacles(mt::dynTraj traj)
{
  MyTimer tmp_t(true);

  if (started_check_ == true && traj.is_agent == true)
  {
    have_received_trajectories_while_checking_ = true;
  }

  // std::cout << on_blue << bold << "in  updateTrajObstacles(), waiting to lock mtx_trajs_" << reset << std::endl;
  mtx_trajs_.lock();

  std::vector<mt::dynTrajCompiled>::iterator obs_ptr =
      std::find_if(trajs_.begin(), trajs_.end(),
                   [=](const mt::dynTrajCompiled& traj_compiled) { return traj_compiled.id == traj.id; });

  bool exists_in_local_map = (obs_ptr != std::end(trajs_));

  mt::dynTrajCompiled traj_compiled;
  dynTraj2dynTrajCompiled(traj, traj_compiled);

  if (exists_in_local_map)
  {  // if that object already exists, substitute its trajectory
    *obs_ptr = traj_compiled;
  }
  else
  {  // if it doesn't exist, add it to the local map
    trajs_.push_back(traj_compiled);
    // ROS_WARN_STREAM("Adding " << traj_compiled.id);
  }

  // and now let's delete those trajectories of the obs/agents whose current positions are outside the local map
  // Note that these positions are obtained with the trajectory stored in the past in the local map
  std::vector<int> ids_to_remove;

  double time_now = ros::Time::now().toSec();

  for (int index_traj = 0; index_traj < trajs_.size(); index_traj++)
  {
    bool traj_affects_me = false;

    Eigen::Vector3d center_obs = evalMeanDynTrajCompiled(trajs_[index_traj], time_now);

    // mtx_t_.unlock();
    if (((traj_compiled.is_static == true) && (center_obs - state_.pos).norm() > 2 * par_.Ra) ||  ////
        ((traj_compiled.is_static == false) && (center_obs - state_.pos).norm() > 4 * par_.Ra))
    // #### Static Obstacle: 2*Ra because: traj_{k-1} is inside a sphere of Ra.
    // Then, in iteration k the point A (which I don't
    // know yet)  is taken along that trajectory, and
    // another trajectory of radius Ra will be obtained.
    // Therefore, I need to take 2*Ra to make sure the
    // extreme case (A taken at the end of traj_{k-1} is
    // covered).

    // #### Dynamic Agent: 4*Ra. Same reasoning as above, but with two agets
    // #### Dynamic Obstacle: 4*Ra, it's a heuristics.

    // ######REMEMBER######
    // Note that removeTrajsThatWillNotAffectMe will later
    // on take care of deleting the ones I don't need once
    // I know A
    {
      ids_to_remove.push_back(trajs_[index_traj].id);
    }
  }

  for (auto id : ids_to_remove)
  {
    // ROS_WARN_STREAM("Removing " << id);
    trajs_.erase(
        std::remove_if(trajs_.begin(), trajs_.end(), [&](mt::dynTrajCompiled const& traj) { return traj.id == id; }),
        trajs_.end());
  }

  mtx_trajs_.unlock();
  // std::cout << red << bold << "in updateTrajObstacles(), mtx_trajs_ unlocked" << reset << std::endl;

  have_received_trajectories_while_checking_ = false;
  // std::cout << bold << blue << "updateTrajObstacles took " << tmp_t << reset << std::endl;
}

std::vector<Eigen::Vector3d> Panther::vertexesOfInterval(mt::PieceWisePol& pwp, double t_start, double t_end,
                                                         const Eigen::Vector3d& delta)
{
  std::vector<Eigen::Vector3d> points;

  std::vector<double>::iterator low = std::lower_bound(pwp.times.begin(), pwp.times.end(), t_start);
  std::vector<double>::iterator up = std::upper_bound(pwp.times.begin(), pwp.times.end(), t_end);

  // Example: times=[1 2 3 4 5 6 7]
  // t_start=1.5;
  // t_end=5.5
  // then low points to "2" (low - pwp.times.begin() is 1)
  // and up points to "6" (up - pwp.times.begin() is 5)

  int index_first_interval = low - pwp.times.begin() - 1;  // index of the interval [1,2]
  int index_last_interval = up - pwp.times.begin() - 1;    // index of the interval [5,6]

  saturate(index_first_interval, 0, (int)(pwp.all_coeff_x.size() - 1));
  saturate(index_last_interval, 0, (int)(pwp.all_coeff_x.size() - 1));

  // push all the complete intervals
  for (int i = index_first_interval; i <= index_last_interval; i++)
  {
    Eigen::VectorXd coeff_x_scaled;
    Eigen::VectorXd coeff_y_scaled;
    Eigen::VectorXd coeff_z_scaled;

    if (i == index_first_interval && i != index_last_interval)
    {
      double u_t_start = pwp.t2u(t_start);

      changeDomPoly(pwp.all_coeff_x[i], u_t_start, 1.0, coeff_x_scaled, 0.0, 1.0);
      changeDomPoly(pwp.all_coeff_y[i], u_t_start, 1.0, coeff_y_scaled, 0.0, 1.0);
      changeDomPoly(pwp.all_coeff_z[i], u_t_start, 1.0, coeff_z_scaled, 0.0, 1.0);
      std::cout << "=====================================================" << std::endl;
      pwp.print();

      // std::cout << red << bold << "pwp.all_coeff_x[i]= " << pwp.all_coeff_x[i].transpose() << reset << std::endl;
      std::cout << red << bold << "t_start= " << t_start << reset << std::endl;
      std::cout << red << bold << "coeff_x_scaled= " << coeff_x_scaled.transpose() << reset << std::endl;
      std::cout << red << bold << "u_t_start= " << u_t_start << reset << std::endl;
    }
    else if (i == index_last_interval && i != index_first_interval)
    {
      double u_t_end = pwp.t2u(t_end);
      changeDomPoly(pwp.all_coeff_x[i], 0.0, u_t_end, coeff_x_scaled, 0.0, 1.0);
      changeDomPoly(pwp.all_coeff_y[i], 0.0, u_t_end, coeff_y_scaled, 0.0, 1.0);
      changeDomPoly(pwp.all_coeff_z[i], 0.0, u_t_end, coeff_z_scaled, 0.0, 1.0);
    }
    else if (i == index_first_interval && i == index_last_interval)  // happens where there is only one interval
    {
      double u_t_start = pwp.t2u(t_start);
      double u_t_end = pwp.t2u(t_end);
      changeDomPoly(pwp.all_coeff_x[i], u_t_start, u_t_end, coeff_x_scaled, 0.0, 1.0);
      changeDomPoly(pwp.all_coeff_y[i], u_t_start, u_t_end, coeff_y_scaled, 0.0, 1.0);
      changeDomPoly(pwp.all_coeff_z[i], u_t_start, u_t_end, coeff_z_scaled, 0.0, 1.0);
    }
    else
    {
      coeff_x_scaled = pwp.all_coeff_x[i];
      coeff_y_scaled = pwp.all_coeff_y[i];
      coeff_z_scaled = pwp.all_coeff_z[i];
    }

    int deg = pwp.getDeg();
    /// TODO
    if (deg > 3)
    {
      std::cout << bold << red << "The part below is assumming that degree<=3. You have deg=" << pwp.getDeg()
                << " Aborting" << reset << std::endl;
      abort();
    }
    ////////

    int tmp = coeff_x_scaled.size();

    Eigen::Matrix<double, 3, -1> P(3, deg + 1);
    Eigen::Matrix<double, 3, -1> V(3, deg + 1);  // 3 x number of vertexes of the simplex

    P.row(0) = coeff_x_scaled;
    P.row(1) = coeff_y_scaled;
    P.row(2) = coeff_z_scaled;

    // P = Eigen::Matrix<double, 3, 4>::Zero();
    // (P.row(0)).tail(tmp) = coeff_x_scaled;
    // (P.row(1)).tail(tmp) = coeff_y_scaled;
    // (P.row(2)).tail(tmp) = coeff_z_scaled;

    if (deg == 3)
    {
      V = P * A_basis_deg3_rest_inverse_;
    }
    else if (deg == 2)
    {
      V = P * A_basis_deg2_rest_inverse_;
    }
    else if (deg == 1)
    {
      V = P * A_basis_deg1_rest_inverse_;
    }
    else
    {
      // TODO
      std::cout << "Not implemented yet. Aborting" << std::endl;
      abort();
    }

    // std::cout << "P= \n" << P << std::endl;
    // std::cout << "V= \n" << V << std::endl;

    for (int j = 0; j < V.cols(); j++)
    {
      double x = V(0, j);
      double y = V(1, j);
      double z = V(2, j);  //[x,y,z] is the point

      if (delta.norm() < 1e-6)
      {  // no inflation
        std::cout << "No inflation" << std::endl;
        points.push_back(Eigen::Vector3d(x, y, z));
      }
      else
      {
        // points.push_back(Eigen::Vector3d(V(1, j), V(2, j), V(3, j)));  // x,y,z
        points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z + delta.z()));
        points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z - delta.z()));
        points.push_back(Eigen::Vector3d(x + delta.x(), y + delta.y(), z - delta.z()));
        points.push_back(Eigen::Vector3d(x + delta.x(), y - delta.y(), z + delta.z()));
        points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z - delta.z()));
        points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z + delta.z()));
        points.push_back(Eigen::Vector3d(x - delta.x(), y + delta.y(), z - delta.z()));
        points.push_back(Eigen::Vector3d(x - delta.x(), y - delta.y(), z + delta.z()));
      }
    }
  }

  return points;
}

// // return a vector that contains all the vertexes of the polyhedral approx of an interval.
std::vector<Eigen::Vector3d> Panther::vertexesOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end)
{
  // every side of the box will be increased by 2*delta (+delta on one end, -delta on the other)
  // note that we use the variance at t_end (which is going to be higher that the one at t_start)
  Eigen::Vector3d delta = traj.bbox / 2.0 + (par_.drone_radius) * Eigen::Vector3d::Ones() +  //////
                          par_.norminv_prob * (evalVarDynTrajCompiled(traj, t_end)).cwiseSqrt();

  if (traj.use_pwp_field == false)
  {
    std::vector<Eigen::Vector3d> points;

    // Will always have a sample at the beginning of the interval, and another at the end.
    for (double t = t_start;                           /////////////
         (t < t_end) ||                                /////////////
         ((t > t_end) && ((t - t_end) < par_.gamma));  /////// This is to ensure we have a sample a the end
         t = t + par_.gamma)
    {
      Eigen::Vector3d tmp =
          evalMeanDynTrajCompiled(traj, std::min(t, t_end));  // this min only has effect on the last sample

      //"Minkowski sum along the trajectory: box centered on the trajectory"
      points.push_back(Eigen::Vector3d(tmp.x() + delta.x(), tmp.y() + delta.y(), tmp.z() + delta.z()));
      points.push_back(Eigen::Vector3d(tmp.x() + delta.x(), tmp.y() - delta.y(), tmp.z() - delta.z()));
      points.push_back(Eigen::Vector3d(tmp.x() + delta.x(), tmp.y() + delta.y(), tmp.z() - delta.z()));
      points.push_back(Eigen::Vector3d(tmp.x() + delta.x(), tmp.y() - delta.y(), tmp.z() + delta.z()));
      points.push_back(Eigen::Vector3d(tmp.x() - delta.x(), tmp.y() - delta.y(), tmp.z() - delta.z()));
      points.push_back(Eigen::Vector3d(tmp.x() - delta.x(), tmp.y() + delta.y(), tmp.z() + delta.z()));
      points.push_back(Eigen::Vector3d(tmp.x() - delta.x(), tmp.y() + delta.y(), tmp.z() - delta.z()));
      points.push_back(Eigen::Vector3d(tmp.x() - delta.x(), tmp.y() - delta.y(), tmp.z() + delta.z()));
    }

    return points;
  }
  else
  {
    return vertexesOfInterval(traj.pwp_mean, t_start, t_end, delta);
  }
}

// See https://doc.cgal.org/Manual/3.7/examples/Convex_hull_3/quickhull_3.cpp
CGAL_Polyhedron_3 Panther::convexHullOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end)
{
  std::vector<Eigen::Vector3d> points = vertexesOfInterval(traj, t_start, t_end);

  std::vector<Point_3> points_cgal;
  for (auto point_i : points)
  {
    points_cgal.push_back(Point_3(point_i.x(), point_i.y(), point_i.z()));
  }

  return convexHullOfPoints(points_cgal);
}

// trajs_ is already locked when calling this function
void Panther::removeTrajsThatWillNotAffectMe(const mt::state& A, double t_start, double t_end)
{
  std::vector<int> ids_to_remove;

  for (auto traj : trajs_)
  {
    bool traj_affects_me = false;

    // STATIC OBSTACLES/AGENTS
    if (traj.is_static == true)
    {
      Eigen::Vector3d center_obs =
          evalMeanDynTrajCompiled(traj, t_start);  // Note that t_start is constant along the trajectory

      Eigen::Vector3d positive_half_diagonal;
      positive_half_diagonal << traj.bbox[0] / 2.0, traj.bbox[1] / 2.0, traj.bbox[2] / 2.0;

      Eigen::Vector3d c1 = center_obs - positive_half_diagonal;
      Eigen::Vector3d c2 = center_obs + positive_half_diagonal;
      traj_affects_me = boxIntersectsSphere(A.pos, par_.Ra, c1, c2);
    }
    else
    {                                                            // DYNAMIC OBSTACLES/AGENTS
      double deltaT = (t_end - t_start) / (1.0 * par_.num_seg);  // num_seg is the number of intervals
      for (int i = 0; i < par_.num_seg; i++)                     // for each interval
      {
        std::vector<Eigen::Vector3d> points =
            vertexesOfInterval(traj, t_start + i * deltaT, t_start + (i + 1) * deltaT);

        for (auto point_i : points)  // for every vertex of each interval
        {
          if ((point_i - A.pos).norm() <= par_.Ra)
          {
            traj_affects_me = true;
            goto exit;
          }
        }
      }
    }

  exit:
    if (traj_affects_me == false)
    {
      // std::cout << red << bold << "Going to  delete traj " << trajs_[index_traj].id << reset << std::endl;
      ids_to_remove.push_back(traj.id);
    }
  }

  for (auto id : ids_to_remove)
  {
    // ROS_INFO_STREAM("traj " << id << " doesn't affect me");
    trajs_.erase(
        std::remove_if(trajs_.begin(), trajs_.end(), [&](mt::dynTrajCompiled const& traj) { return traj.id == id; }),
        trajs_.end());
  }

  /*  std::cout << "After deleting the trajectory, we have these ids= " << std::endl;

    for (auto traj : trajs_)
    {
      std::cout << traj.id << std::endl;
    }*/
}

bool Panther::IsTranslating()
{
  return (drone_status_ == DroneStatus::GOAL_SEEN || drone_status_ == DroneStatus::TRAVELING);
}

ConvexHullsOfCurve Panther::convexHullsOfCurve(mt::dynTrajCompiled& traj, double t_start, double t_end)
{
  ConvexHullsOfCurve convexHulls;
  double deltaT = (t_end - t_start) / (1.0 * par_.num_seg);  // num_seg is the number of intervals

  for (int i = 0; i < par_.num_seg; i++)
  {
    convexHulls.push_back(convexHullOfInterval(traj, t_start + i * deltaT, t_start + (i + 1) * deltaT));
  }

  return convexHulls;
}

ConvexHullsOfCurves Panther::convexHullsOfCurves(double t_start, double t_end)
{
  ConvexHullsOfCurves result;

  for (auto traj : trajs_)
  {
    result.push_back(convexHullsOfCurve(traj, t_start, t_end));
  }

  return result;
}

// argmax_prob_collision is the index of trajectory I should focus on
// a negative value means that there are no trajectories to track
void Panther::sampleFeaturePosVel(int argmax_prob_collision, double t_start, double t_end,
                                  std::vector<Eigen::Vector3d>& pos, std::vector<Eigen::Vector3d>& vel)
{
  pos.clear();
  vel.clear();

  double delta = (t_end - t_start) / par_.num_samples_simpson;

  for (int i = 0; i < par_.num_samples_simpson; i++)
  {
    if (argmax_prob_collision >= 0)
    {
      double ti = t_start + i * delta;  // which is constant along the trajectory
      Eigen::Vector3d pos_i = evalMeanDynTrajCompiled(trajs_[argmax_prob_collision], ti);

      pos.push_back(pos_i);

      // MyTimer timer(true);
      // This commented part always returns 0.0. TODO: find out why. For now, let's use finite differences
      // See also
      // https://github.com/ArashPartow/exprtk/blob/66bed77369557fe1872df4c999c9d9ccb3adc3f6/readme.txt#LC4010:~:text=This%20free%20function%20will%20attempt%20to%20perform%20a%20numerical%20differentiation
      // Eigen::Vector3d vel_i = Eigen::Vector3d(exprtk::derivative(trajs_[wt].function[0], "t"),  ////////////
      //                                         exprtk::derivative(trajs_[wt].function[1], "t"),  ////////////
      //                                         exprtk::derivative(trajs_[wt].function[2], t_));
      // std::cout << "time to take derivatives= " << timer << std::endl;
      // std::cout << on_green << bold << "vel= " << vel_i.transpose() << reset << std::endl;
      // std::cout << on_green << bold << "pos= " << pos[i].transpose() << reset << std::endl;

      // Use finite differences to obtain the derivative
      double epsilon = 1e-6;

      Eigen::Vector3d pos_i_epsilon = evalMeanDynTrajCompiled(trajs_[argmax_prob_collision], ti + epsilon);

      vel.push_back((pos_i_epsilon - pos_i) / epsilon);

      // std::cout << bold << "Velocity= " << vel[i].transpose() << reset << std::endl;
      //////////////////////////////
    }
    else
    {
      pos.push_back(G_term_.pos);              // last_state_tracked_.pos
      vel.push_back(Eigen::Vector3d::Zero());  // last_state_tracked_.vel
    }
  }

  if (argmax_prob_collision < 0)
  {
    std::cout << bold << "There is no dynamic obstacle to track" << reset << std::endl;
  }

  last_state_tracked_.pos = pos.front();  // pos.back();
  last_state_tracked_.vel = vel.front();  // vel.back();
}

void Panther::setTerminalGoal(mt::state& term_goal)
{
  mtx_G_term.lock();
  G_term_ = term_goal;
  mtx_G_term.unlock();

  if (state_initialized_ == true)  // because I need plan_size()>=1
  {
    doStuffTermGoal();
  }
  else
  {
    std::cout << "need_to_do_stuff_term_goal_= " << need_to_do_stuff_term_goal_ << std::endl;
    need_to_do_stuff_term_goal_ = true;  // will be done in updateState();
  }
}

void Panther::getG(mt::state& G)
{
  G = G_;
}

void Panther::getState(mt::state& data)
{
  mtx_state.lock();
  data = state_;
  mtx_state.unlock();
}

void Panther::updateState(mt::state data)
{
  state_ = data;

  if (state_initialized_ == false)
  {
    plan_.clear();  // (actually not needed because done in resetInitialization()
    mt::state tmp;
    tmp.pos = data.pos;
    tmp.yaw = data.yaw;
    plan_.push_back(tmp);
  }

  state_initialized_ = true;

  if (need_to_do_stuff_term_goal_)
  {
    // std::cout << "DOING STUFF TERM GOAL -----------" << std::endl;
    doStuffTermGoal();
    need_to_do_stuff_term_goal_ = false;
  }
}

// This function needs to be called once the state has been initialized
void Panther::doStuffTermGoal()
{
  // if (state_initialized_ == false)  // because I need plan_size()>=1
  // {
  //   std::cout << "[Panther::setTerminalGoal] State not initialized yet, doing nothing" << std::endl;
  //   return;
  // }

  // std::cout << "[doStuffTermGoal]" << std::endl;
  mtx_G_term.lock();
  mtx_state.lock();
  mtx_planner_status_.lock();

  G_.pos = G_term_.pos;
  if (drone_status_ == DroneStatus::GOAL_REACHED)
  {
    /////////////////////////////////
    mtx_plan_.lock();  // must be before changeDroneStatus

    changeDroneStatus(DroneStatus::YAWING);
    mt::state last_state = plan_.back();

    double desired_yaw = atan2(G_term_.pos[1] - last_state.pos[1], G_term_.pos[0] - last_state.pos[0]);
    double diff = desired_yaw - last_state.yaw;
    angle_wrap(diff);

    double dyaw =
        copysign(1, diff) *
        std::min(2.0, par_.ydot_max);  // par_.ydot_max; Changed to 0.5 (in HW the drone stops the motors when
                                       // status==YAWING and ydot_max is too high, due to saturation + calibration of
                                       // the ESCs) see https://gitlab.com/mit-acl/fsw/snap-stack/snap/-/issues/3

    int num_of_el = (int)fabs(diff / (par_.dc * dyaw));

    verify((plan_.size() >= 1), "plan_.size() must be >=1");

    for (int i = 1; i < (num_of_el + 1); i++)
    {
      mt::state state_i = plan_.get(i - 1);
      state_i.yaw = state_i.yaw + dyaw * par_.dc;
      if (i == num_of_el)
      {
        state_i.dyaw = 0;  // 0 final yaw velocity
      }
      else
      {
        state_i.dyaw = dyaw;
      }
      plan_.push_back(state_i);
    }
    mtx_plan_.unlock();
    /////////////////////////////////
  }
  if (drone_status_ == DroneStatus::GOAL_SEEN)
  {
    changeDroneStatus(DroneStatus::TRAVELING);
  }
  terminal_goal_initialized_ = true;

  // std::cout << bold << red << "[FA] Received Term Goal=" << G_term_.pos.transpose() << reset << std::endl;
  // std::cout << bold << red << "[FA] Received Proj Goal=" << G_.pos.transpose() << reset << std::endl;

  mtx_state.unlock();
  mtx_G_term.unlock();
  mtx_planner_status_.unlock();
}

bool Panther::initializedAllExceptPlanner()
{
  if (!state_initialized_ || !terminal_goal_initialized_)
  {
    /*    std::cout << "state_initialized_= " << state_initialized_ << std::endl;
        std::cout << "terminal_goal_initialized_= " << terminal_goal_initialized_ << std::endl;*/
    return false;
  }
  return true;
}

bool Panther::initializedStateAndTermGoal()
{
  if (!state_initialized_ || !terminal_goal_initialized_)
  {
    return false;
  }
  return true;
}

bool Panther::initialized()
{
  if (!state_initialized_ || !terminal_goal_initialized_ || !planner_initialized_)
  {
    /*    std::cout << "state_initialized_= " << state_initialized_ << std::endl;
        std::cout << "terminal_goal_initialized_= " << terminal_goal_initialized_ << std::endl;
        std::cout << "planner_initialized_= " << planner_initialized_ << std::endl;*/
    return false;
  }
  return true;
}

// check wheter a mt::dynTrajCompiled and a pwp_optimized are in collision in the interval [t_start, t_end]
bool Panther::trajsAndPwpAreInCollision(mt::dynTrajCompiled traj, mt::PieceWisePol pwp_optimized, double t_start,
                                        double t_end)
{
  Eigen::Vector3d n_i;
  double d_i;

  double deltaT = (t_end - t_start) / (1.0 * par_.num_seg);  // num_seg is the number of intervals
  for (int i = 0; i < par_.num_seg; i++)                     // for each interval
  {
    // This is my trajectory (no inflation)
    std::vector<Eigen::Vector3d> pointsA =
        vertexesOfInterval(pwp_optimized, t_start + i * deltaT, t_start + (i + 1) * deltaT, Eigen::Vector3d::Zero());

    // This is the trajectory of the other agent/obstacle
    std::vector<Eigen::Vector3d> pointsB = vertexesOfInterval(traj, t_start + i * deltaT, t_start + (i + 1) * deltaT);

    // std::cout << "Going to solve model with pointsA.size()= " << pointsA.size() << std::endl;
    // for (auto point_i : pointsA)
    // {
    //   std::cout << point_i.transpose() << std::endl;
    // }

    // std::cout << "Going to solve model with pointsB.size()= " << pointsB.size() << std::endl;
    // for (auto point_i : pointsB)
    // {
    //   std::cout << point_i.transpose() << std::endl;
    // }

    if (separator_solver_->solveModel(n_i, d_i, pointsA, pointsB) == false)
    {
      return true;  // There is not a solution --> they collide
    }
  }

  // if reached this point, they don't collide
  return false;
}
// Checks that I have not received new trajectories that affect me while doing the optimization
bool Panther::safetyCheckAfterOpt(mt::PieceWisePol pwp_optimized)
{
  started_check_ = true;

  bool result = true;
  for (auto traj : trajs_)
  {
    if (traj.time_received > time_init_opt_ && traj.is_agent == true)
    {
      if (trajsAndPwpAreInCollision(traj, pwp_optimized, pwp_optimized.times.front(), pwp_optimized.times.back()))
      {
        ROS_ERROR_STREAM("Traj collides with " << traj.id);
        result = false;  // will have to redo the optimization
        break;
      }
    }
  }

  // and now do another check in case I've received anything while I was checking. Note that mtx_trajs_ is locked!
  if (have_received_trajectories_while_checking_ == true)
  {
    ROS_ERROR_STREAM("Recvd traj while checking ");
    result = false;
  }
  started_check_ = false;

  return result;
}

bool Panther::isReplanningNeeded()
{
  if (initializedStateAndTermGoal() == false)
  {
    return false;  // Note that log is not modified --> will keep its default values
  }

  //////////////////////////////////////////////////////////////////////////
  mtx_G_term.lock();

  mt::state G_term = G_term_;  // Local copy of the terminal terminal goal

  mtx_G_term.unlock();

  // Check if we have reached the goal
  double dist_to_goal = (G_term.pos - plan_.front().pos).norm();
  // std::cout << "dist_to_goal= " << dist_to_goal << std::endl;
  if (dist_to_goal < par_.goal_radius)
  {
    changeDroneStatus(DroneStatus::GOAL_REACHED);
    exists_previous_pwp_ = false;
  }

  // Check if we have seen the goal in the last replan
  mtx_plan_.lock();
  double dist_last_plan_to_goal = (G_term.pos - plan_.back().pos).norm();
  // std::cout << "dist_last_plan_to_goal= " << dist_last_plan_to_goal << std::endl;
  mtx_plan_.unlock();
  if (dist_last_plan_to_goal < par_.goal_radius && drone_status_ == DroneStatus::TRAVELING)
  {
    changeDroneStatus(DroneStatus::GOAL_SEEN);
    std::cout << "Status changed to GOAL_SEEN!" << std::endl;
    exists_previous_pwp_ = false;
  }

  // Don't plan if drone is not traveling
  if (drone_status_ == DroneStatus::GOAL_REACHED || (drone_status_ == DroneStatus::YAWING) ||
      (drone_status_ == DroneStatus::GOAL_SEEN))
  {
    // std::cout << "No replanning needed because" << std::endl;
    // printDroneStatus();
    return false;
  }
  return true;
}

bool Panther::replan(mt::Edges& edges_obstacles_out, std::vector<mt::state>& X_safe_out,
                     std::vector<Hyperplane3D>& planes, int& num_of_LPs_run, int& num_of_QCQPs_run,
                     mt::PieceWisePol& pwp_out, mt::log& log)
{
  (*log_ptr_) = {};  // Reset the struct with the default values

  mtx_G_term.lock();
  mt::state G_term = G_term_;  // Local copy of the terminal terminal goal
  mtx_G_term.unlock();

  log_ptr_->pos = state_.pos;
  log_ptr_->G_term_pos = G_term.pos;
  log_ptr_->drone_status = drone_status_;

  if (isReplanningNeeded() == false)
  {
    log_ptr_->replanning_was_needed = false;
    log = (*log_ptr_);
    return false;
  }

  std::cout << bold << on_white << "**********************IN REPLAN CB*******************" << reset << std::endl;

  log_ptr_->replanning_was_needed = true;
  log_ptr_->tim_total_replan.tic();

  log_ptr_->tim_initial_setup.tic();

  removeOldTrajectories();

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Select mt::state A /////////////////////////////
  //////////////////////////////////////////////////////////////////////////

  mt::state A;
  int k_index_end, k_index;

  // If k_index_end=0, then A = plan_.back() = plan_[plan_.size() - 1]

  mtx_plan_.lock();

  saturate(deltaT_, par_.lower_bound_runtime_snlopt / par_.dc, par_.upper_bound_runtime_snlopt / par_.dc);

  k_index_end = std::max((int)(plan_.size() - deltaT_), 0);

  if (plan_.size() < 5)
  {
    k_index_end = 0;
  }

  k_index = plan_.size() - 1 - k_index_end;
  A = plan_.get(k_index);

  mtx_plan_.unlock();

  // std::cout << blue << "k_index:" << k_index << reset << std::endl;
  // std::cout << blue << "k_index_end:" << k_index_end << reset << std::endl;
  // std::cout << blue << "plan_.size():" << plan_.size() << reset << std::endl;

  double runtime_snlopt;

  if (k_index_end != 0)
  {
    runtime_snlopt = k_index * par_.dc;  // std::min(, par_.upper_bound_runtime_snlopt);
  }
  else
  {
    runtime_snlopt = par_.upper_bound_runtime_snlopt;  // I'm stopped at the end of the trajectory
  }
  saturate(runtime_snlopt, par_.lower_bound_runtime_snlopt, par_.upper_bound_runtime_snlopt);

  // std::cout << green << "Runtime snlopt= " << runtime_snlopt << reset << std::endl;

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Get point G ////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////
  double distA2TermGoal = (G_term.pos - A.pos).norm();
  double ra = std::min((distA2TermGoal - 0.001), par_.Ra);  // radius of the sphere S
  mt::state G;
  G.pos = A.pos + ra * (G_term.pos - A.pos).normalized();

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Set Times in optimization //////////////////////
  //////////////////////////////////////////////////////////////////////////

  solver_->setMaxRuntimeKappaAndMu(runtime_snlopt, par_.kappa, par_.mu);

  //////////////////////
  double time_now = ros::Time::now().toSec();

  double t_start = k_index * par_.dc + time_now;

  // double factor_alloc_tmp = par_.factor_alloc;

  // std::cout << "distA2TermGoal= " << distA2TermGoal << std::endl;

  //// when it's near the terminal goal --> force the final condition (if not it may oscillate)
  // if (distA2TermGoal < par_.distance_to_force_final_pos)
  // {
  //   std::cout << "distA2TermGoal= " << distA2TermGoal << std::endl;
  //   std::cout << bold << blue << "Forcing final Pos" << reset << std::endl;

  //   factor_alloc_tmp = par_.factor_alloc_when_forcing_final_pos;
  //   solver_->par_.c_final_pos = 0.0;  // Is a constraint --> don't care about this cost
  //   solver_->par_.c_fov = 5.0;
  //   solver_->par_.force_final_pos = true;
  // }
  // else
  // {
  // solver_->par_.c_final_pos = par_.c_final_pos;
  // solver_->par_.c_fov = par_.c_fov;
  // solver_->par_.force_final_pos = false;  // par_.force_final_pos;
  // }

  double time_allocated = getMinTimeDoubleIntegrator3D(A.pos, A.vel, G.pos, G.vel, par_.v_max, par_.a_max);

  // std::cout << green << bold << "Time allocated= " << time_allocated << reset << std::endl;

  double t_final = t_start + par_.factor_alloc * time_allocated;

  /////////////////////////////////////////////////////////////////////////
  ////////////////////////Compute trajectory to focus on //////////////////
  /////////////////////////////////////////////////////////////////////////

  double max_prob_collision = -std::numeric_limits<double>::max();  // it's actually a heuristics of the probability (we
                                                                    // are summing below --> can be >1)
  int argmax_prob_collision = -1;  // will contain the index of the trajectory to focus on

  int num_samplesp1 = 20;
  double delta = 1.0 / num_samplesp1;
  Eigen::Vector3d R = par_.drone_radius * Eigen::Vector3d::Ones();

  std::vector<double> all_probs;

  mtx_trajs_.lock();
  std::cout << green << bold << "trajs_.size()= " << trajs_.size() << reset << std::endl;
  for (int i = 0; i < trajs_.size(); i++)
  {
    double prob_i = 0.0;
    for (int j = 0; j <= num_samplesp1; j++)
    {
      double t = t_start + j * delta * (t_final - t_start);

      Eigen::Vector3d pos_drone = A.pos + j * delta * (G_term_.pos - A.pos);  // not a random variable
      Eigen::Vector3d pos_obs_mean = evalMeanDynTrajCompiled(trajs_[i], t);
      Eigen::Vector3d pos_obs_std = (evalVarDynTrajCompiled(trajs_[i], t)).cwiseSqrt();
      // std::cout << "pos_obs_std= " << pos_obs_std << std::endl;
      prob_i += probMultivariateNormalDist(-R, R, pos_obs_mean - pos_drone, pos_obs_std);
    }

    all_probs.push_back(prob_i);
    // std::cout << "[Selection] Trajectory " << i << " has P(collision)= " << prob_i * pow(10, 15) << "e-15" <<
    // std::endl;

    if (prob_i > max_prob_collision)
    {
      max_prob_collision = prob_i;
      argmax_prob_collision = i;
    }
  }

  std::cout << "[Selection] Probs of coll --> ";
  for (int i = 0; i < all_probs.size(); i++)
  {
    std::cout << all_probs[i] * pow(10, 15) << "e-15,   ";
  }
  std::cout << std::endl;

  // std::cout.precision(30);
  std::cout << bold << "[Selection] Chosen Trajectory " << argmax_prob_collision
            << ", P(collision)= " << max_prob_collision * pow(10, 5) << "e-5" << std::endl;

  ////

  double angle = 3.14;
  if (argmax_prob_collision >= 0)
  {
    Eigen::Vector3d A2G = G_term.pos - A.pos;
    Eigen::Vector3d A2Obstacle = evalMeanDynTrajCompiled(trajs_[argmax_prob_collision], t_start) - A.pos;
    angle = angleBetVectors(A2G, A2Obstacle);
  }

  // bool focus_on_obstacle = true;

  double angle_deg = angle * 180 / 3.14;

  if (fabs(angle_deg) > par_.angle_deg_focus_front)
  {  //
    std::cout << bold << yellow << "[Selection] Focusing on front of me, angle=" << angle_deg << " deg" << reset
              << std::endl;
    // focus_on_obstacle = false;
    solver_->par_.c_final_yaw = 0.0;
    solver_->par_.c_fov = 0.0;
    solver_->par_.c_yaw_smooth = 0.0;
    solver_->setFocusOnObstacle(false);
    G.yaw = atan2(G_term_.pos[1] - A.pos[1], G_term_.pos[0] - A.pos[0]);
    // solver_->use_straight_yaw_guess_ = true;
  }
  else
  {
    std::cout << bold << yellow << "[Selection] Focusing on obstacle, angle=" << angle_deg << " deg" << reset
              << std::endl;
    // focus_on_obstacle = true;
    solver_->setFocusOnObstacle(true);
    solver_->par_.c_fov = par_.c_fov;
    solver_->par_.c_final_yaw = par_.c_final_yaw;
    solver_->par_.c_yaw_smooth = par_.c_yaw_smooth;
    // solver_->use_straight_yaw_guess_ = false;
  }
  ////

  std::vector<Eigen::Vector3d> w_posfeature;      // velocity of the feature expressed in w
  std::vector<Eigen::Vector3d> w_velfeaturewrtw;  // velocity of the feature wrt w, expressed in w
  sampleFeaturePosVel(argmax_prob_collision, t_start, t_final, w_posfeature,
                      w_velfeaturewrtw);  // need to do it here so that argmax_prob_collision does not become invalid
                                          // with new updates

  log_ptr_->tracking_now_pos = w_posfeature.front();
  log_ptr_->tracking_now_vel = w_velfeaturewrtw.front();

  mtx_trajs_.unlock();

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Set init and final states //////////////////////
  //////////////////////////////////////////////////////////////////////////
  bool correctInitialCond =
      solver_->setInitStateFinalStateInitTFinalT(A, G, t_start,
                                                 t_final);  // note that here t_final may have been updated

  if (correctInitialCond == false)
  {
    logAndTimeReplan("Solver cannot guarantee feasibility for v1", false, log);
    return false;
  }

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Solve optimization! ////////////////////////////
  //////////////////////////////////////////////////////////////////////////

  mtx_trajs_.lock();

  time_init_opt_ = ros::Time::now().toSec();
  // removeTrajsThatWillNotAffectMe(A, t_start, t_final);  // TODO: Commented (4-Feb-2021)
  log_ptr_->tim_convex_hulls.tic();
  ConvexHullsOfCurves hulls = convexHullsOfCurves(t_start, t_final);
  log_ptr_->tim_convex_hulls.toc();

  mtx_trajs_.unlock();

  ConvexHullsOfCurves_Std hulls_std = vectorGCALPol2vectorStdEigen(hulls);
  // poly_safe_out = vectorGCALPol2vectorJPSPol(hulls);
  edges_obstacles_out = vectorGCALPol2edges(hulls);

  solver_->setHulls(hulls_std);

  solver_->setSimpsonFeatureSamples(w_posfeature, w_velfeaturewrtw);

  //////////////////////
  std::cout << on_cyan << bold << "Solved so far" << solutions_found_ << "/" << total_replannings_ << reset
            << std::endl;

  log_ptr_->tim_initial_setup.toc();
  bool result = solver_->optimize();

  num_of_LPs_run = solver_->getNumOfLPsRun();
  num_of_QCQPs_run = solver_->getNumOfQCQPsRun();

  total_replannings_++;
  if (result == false)
  {
    logAndTimeReplan("Solver failed", false, log);
    return false;
  }

  solver_->getPlanes(planes);

  solutions_found_++;

  mt::PieceWisePol pwp_now;
  solver_->getSolution(pwp_now);

  MyTimer check_t(true);

  mtx_trajs_.lock();
  bool is_safe_after_opt = safetyCheckAfterOpt(pwp_now);
  mtx_trajs_.unlock();

  if (is_safe_after_opt == false)
  {
    logAndTimeReplan("SafetyCheckAfterOpt not satisfied", false, log);
    return false;
  }

  M_ = G_term;

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Append to plan /////////////////////////////////
  //////////////////////////////////////////////////////////////////////////
  mtx_plan_.lock();

  int plan_size = plan_.size();

  if ((plan_size - 1 - k_index_end) < 0)
  {
    // std::cout << "plan_size= " << plan_size << std::endl;
    // std::cout << "k_index_end= " << k_index_end << std::endl;
    mtx_plan_.unlock();
    logAndTimeReplan("Point A already published", false, log);
    return false;
  }
  else
  {
    plan_.erase(plan_.end() - k_index_end - 1, plan_.end());    // this deletes also the initial condition...
    for (int i = 0; i < (solver_->traj_solution_).size(); i++)  //... which is included in traj_solution_[0]
    {
      plan_.push_back(solver_->traj_solution_[i]);
    }
  }

  mtx_plan_.unlock();

  ////////////////////
  ////////////////////

  if (exists_previous_pwp_ == true)
  {
    pwp_out = composePieceWisePol(time_now, par_.dc, pwp_prev_, pwp_now);
    pwp_prev_ = pwp_out;
  }
  else
  {  //
    pwp_out = pwp_now;
    pwp_prev_ = pwp_now;
    exists_previous_pwp_ = true;
  }

  X_safe_out = plan_.toStdVector();

  ///////////////////////////////////////////////////////////
  ///////////////       OTHER STUFF    //////////////////////
  //////////////////////////////////////////////////////////

  // Check if we have planned until G_term
  // mt::state F = plan_.back();  // Final point of the safe path (\equiv final point of the comitted path)
  double dist = (G_term_.pos - plan_.back().pos).norm();

  if (dist < par_.goal_radius)
  {
    changeDroneStatus(DroneStatus::GOAL_SEEN);
  }

  planner_initialized_ = true;

  logAndTimeReplan("Success", true, log);
  return true;
}

void Panther::logAndTimeReplan(const std::string& info, const bool& success, mt::log& log)
{
  log_ptr_->info_replan = info;
  log_ptr_->tim_total_replan.toc();
  log_ptr_->success_replanning = success;

  double total_time_ms = log_ptr_->tim_total_replan.getMsSaved();

  mtx_offsets.lock();
  if (success == false)
  {
    std::cout << bold << red << log_ptr_->info_replan << reset << std::endl;
    int states_last_replan = ceil(total_time_ms / (par_.dc * 1000));  // Number of states that
                                                                      // would have been needed for
                                                                      // the last replan
    deltaT_ = std::max(par_.factor_alpha * states_last_replan, 1.0);
    deltaT_ = std::min(1.0 * deltaT_, 2.0 / par_.dc);
  }
  else
  {
    int states_last_replan = ceil(total_time_ms / (par_.dc * 1000));  // Number of states that
                                                                      // would have been needed for
                                                                      // the last replan
    deltaT_ = std::max(par_.factor_alpha * states_last_replan, 1.0);
  }
  mtx_offsets.unlock();

  log = (*log_ptr_);
}

void Panther::resetInitialization()
{
  planner_initialized_ = false;
  state_initialized_ = false;

  terminal_goal_initialized_ = false;
  plan_.clear();
}

bool Panther::getNextGoal(mt::state& next_goal)
{
  if (initializedStateAndTermGoal() == false)  // || (drone_status_ == DroneStatus::GOAL_REACHED && plan_.size() == 1))
                                               // TODO: if included this part commented out, the last state (which is
                                               // the one that has zero accel) will never get published
  {
    // std::cout << "Not publishing new goal" << std::endl;
    // std::cout << "plan_.size() ==" << plan_.size() << std::endl;
    // std::cout << "plan_.content[0] ==" << std::endl;
    // plan_.content[0].print();
    return false;
  }

  mtx_goals.lock();
  mtx_plan_.lock();

  next_goal.setZero();
  next_goal = plan_.front();

  if (plan_.size() > 1)
  {
    plan_.pop_front();
  }

  if (plan_.size() == 1 && drone_status_ == DroneStatus::YAWING)
  {
    changeDroneStatus(DroneStatus::TRAVELING);
  }

  if (par_.mode == "ysweep")
  {
    double t = ros::Time::now().toSec();
    // double T = 1.0;
    double amplitude_deg = 90;
    double amplitude_rd = (amplitude_deg * M_PI / 180);
    next_goal.yaw = amplitude_rd * sin(t * (par_.ydot_max / amplitude_rd));
    next_goal.dyaw = par_.ydot_max * cos(t * (par_.ydot_max / amplitude_rd));
  }

  if (fabs(next_goal.dyaw) > (par_.ydot_max + 1e-4))
  {
    std::cout << red << "par_.ydot_max not satisfied!!" << reset << std::endl;
    std::cout << red << "next_goal.dyaw= " << next_goal.dyaw << reset << std::endl;
    std::cout << red << "par_.ydot_max= " << par_.ydot_max << reset << std::endl;
    abort();
  }

  // verify(fabs(next_goal.dyaw) <= par_.ydot_max, "par_.ydot_max not satisfied!!");

  mtx_goals.unlock();
  mtx_plan_.unlock();
  return true;
}

// Debugging functions
void Panther::changeDroneStatus(int new_status)
{
  if (new_status == drone_status_)
  {
    return;
  }

  std::cout << "Changing DroneStatus from ";
  switch (drone_status_)
  {
    case DroneStatus::YAWING:
      std::cout << bold << "YAWING" << reset;
      break;
    case DroneStatus::TRAVELING:
      std::cout << bold << "TRAVELING" << reset;
      break;
    case DroneStatus::GOAL_SEEN:
      std::cout << bold << "GOAL_SEEN" << reset;
      break;
    case DroneStatus::GOAL_REACHED:
      std::cout << bold << "GOAL_REACHED" << reset;
      break;
  }
  std::cout << " to ";

  switch (new_status)
  {
    case DroneStatus::YAWING:
      std::cout << bold << "YAWING" << reset;
      break;
    case DroneStatus::TRAVELING:
      std::cout << bold << "TRAVELING" << reset;
      break;
    case DroneStatus::GOAL_SEEN:
      std::cout << bold << "GOAL_SEEN" << reset;
      break;
    case DroneStatus::GOAL_REACHED:
      std::cout << bold << "GOAL_REACHED" << reset;
      break;
  }

  std::cout << std::endl;

  drone_status_ = new_status;
}

void Panther::printDroneStatus()
{
  switch (drone_status_)
  {
    case DroneStatus::YAWING:
      std::cout << bold << "status_=YAWING" << reset << std::endl;
      break;
    case DroneStatus::TRAVELING:
      std::cout << bold << "status_=TRAVELING" << reset << std::endl;
      break;
    case DroneStatus::GOAL_SEEN:
      std::cout << bold << "status_=GOAL_SEEN" << reset << std::endl;
      break;
    case DroneStatus::GOAL_REACHED:
      std::cout << bold << "status_=GOAL_REACHED" << reset << std::endl;
      break;
  }
}


#include "AdaptiveLaw.hpp"
#include <Eigen/Dense>
#include <stdexcept>

using namespace Eigen;
using namespace std;

AdaptiveLaw::AdaptiveLaw(void)
{
    this->dt = 0.0;
}
AdaptiveLaw::~AdaptiveLaw(void)
{

}

void AdaptiveLaw::initialize(const AdaptiveLaw_Config_t _config)
{
    this->Gamma = _config.Gamma;
    this->P = _config.P;
    this->B = _config.B;
    this->dt = _config.dt;

    reset();
}

void AdaptiveLaw::reset()
{
	this->BigTheta = MatrixXd::Zero(this->Gamma.rows(), this->B.cols());
}

void AdaptiveLaw::update(const VectorXd error, const VectorXd  phi)
{
    MatrixXd BigTheta_dot = this->Gamma * phi * error.transpose() * this->P * this->B;
    this->BigTheta += BigTheta_dot * this->dt;
}

void AdaptiveLaw::get_gains(MatrixXd &outputs)
{
    outputs = this->BigTheta;
}


//=================================================================================================
//                    Copyright (C) 2016 Olivier Mallet - All Rights Reserved
//=================================================================================================

#include "../include/URT.hpp"

#ifndef USE_ARMA
  #include <armadillo>
#endif

// define USE_FLOAT when compiling to switch to single precision
#ifdef USE_FLOAT
  using T = float;
#else
  using T = double;
#endif

int main()
{
   int niter = 0;

   arma::wall_clock timer;

   std::vector<int> sizes = {100,150,200,250,300,350,400,450,500,1000,1500,2000,2500,3000,3500,4000,4500,5000};

   std::cout << std::fixed << std::setprecision(1);

   for (int i = 0; i < sizes.size(); ++i) {

      urt::Vector<T> data = urt::wiener_process<T>(sizes[i]);

      (sizes[i] < 1000) ? niter = 10000 : niter = 1000;

      timer.tic();
      for (int k = 0; k < niter; ++k) {
         urt::ADF<T> test(data, "AIC");
         test.statistic();
      }

      auto duration = timer.toc();

      std::cout << std::setw(8) << sizes[i];
      std::cout << std::setw(8) << duration << "\n";
   }
}


//  Copyright Matt Borland 2021.
//  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)
//
// A sanity check that this file
// #includes all the files that it needs to.
//
#include <boost/math/interpolators/bezier_polynomial.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"
//
// This test includes <vector> becasue many of the interpolators are not compatible with pointers/c-style arrays
//
#include <vector>

void compile_and_link_test()
{
   std::vector<std::vector<double>> control_points {{0.0, 0.0}, {1.0, 1.0}};
   auto bp = boost::math::interpolators::bezier_polynomial(std::move(control_points));

   check_result<double>(bp(0)[0]);
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file joint_distribution_iid.hpp
 * \date Febuary 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/traits.hpp>

#include <fl/distribution/interface/moments.hpp>

namespace fl
{

// Forward declaration
template <typename...Distributions> class JointDistribution;

/**
 * \internal
 * Traits of JointDistribution<Distribution, Count>
 */
template <typename Distribution, int Count>
struct Traits<JointDistribution<MultipleOf<Distribution, Count>>>
{
    typedef typename Distribution::Variate MarginalVariate;

    enum : signed int
    {
        MarginalCount = Count,
        JointSize = ExpandSizes<SizeOf<MarginalVariate>::Value, Count>::Size
    };

    typedef typename MarginalVariate::Scalar Scalar;

    typedef Eigen::Matrix<Scalar, JointSize, 1> Variate;
};

/**
 * \ingroup distributions
 */
template <typename MarginalDistribution, int Count>
class JointDistribution<MultipleOf<MarginalDistribution, Count>>
    : public Moments<
                typename Traits<
                    JointDistribution<MultipleOf<MarginalDistribution, Count>>
                >::Variate>
{
public:
    /** Typdef of \c This for #from_traits(TypeName) helper */
    typedef JointDistribution This;

    typedef typename Traits<This>::Variate Variate;
    typedef typename Moments<Variate>::SecondMoment SecondMoment;
    typedef Eigen::Array<MarginalDistribution, Count, 1> MarginalDistributions;

public:
    explicit
    JointDistribution(MarginalDistribution marginal,
                      int count = ToDimension<Count>::Value)
        : distributions_(MarginalDistributions(count, 1))
    {
        assert(count > 0);

        for (int i = 0; i < distributions_.rows(); ++i)
        {
            distributions_(i) = marginal;
        }

        dimension_ = marginal.dimension() * count;
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~JointDistribution() noexcept { }

    virtual Variate mean() const
    {
        Variate mu = Variate(dimension(), 1);

        int offset = 0;
        for (int i = 0; i < distributions_.size(); ++i)
        {
            const MarginalDistribution& marginal = distributions_(i);
            int dim =  marginal.dimension();

            mu.middleRows(offset, dim) = marginal.mean();

            offset += dim;
        }

        return mu;
    }

    virtual SecondMoment covariance() const
    {
        SecondMoment cov = SecondMoment::Zero(dimension(), dimension());

        int offset = 0;
        for (int i = 0; i < distributions_.size(); ++i)
        {
            const MarginalDistribution& marginal = distributions_(i);
            int dim =  marginal.dimension();

            cov.block(offset, offset, dim, dim) = marginal.covariance();

            offset += dim;
        }

        return cov;
    }

    virtual int dimension() const
    {
        return dimension_;
    }

    MarginalDistributions& distributions()
    {
        return distributions_;
    }

    const MarginalDistributions& distributions() const
    {
        return distributions_;
    }

    MarginalDistribution& distribution(int index)
    {
        assert(index < distributions_.size());
        return distributions_(index);
    }

    const MarginalDistribution& distribution(int index) const
    {
        assert(index < distributions_.size());
        return distributions_(index);
    }

protected:
    MarginalDistributions distributions_;
    int dimension_;
};

}


/*
 * BSD 3-Clause License
 * Copyright (c) 2020, Roxána Provender
 * All rights reserved.
 *
 * You may obtain a copy of the License at
 * https://opensource.org/licenses/BSD-3-Clause
 */

#define _USE_MATH_DEFINES // for Windows to have M_PI
#include <cmath>

#include <boost/lexical_cast.hpp>
#include "Eigen/Geometry"

#include "GPSHelper.h"
#include "KalmanHelper.h"
#include "CsvFileWriter.h"
#include "TinyEKFHelper.h"

// No strptime for Windows :(
// source: https://stackoverflow.com/questions/321849/strptime-equivalent-on-windows/321940
#ifdef _WIN32
#include <time.h>
#include <iomanip>
#include <sstream>

extern "C" char* strptime(const char* s, const char* f, struct tm* tm)
{
  std::istringstream input(s);
  input.imbue(std::locale(setlocale(LC_ALL, nullptr)));
  input >> std::get_time(tm, f);
  if (input.fail()) {
    return nullptr;
  }
  return (char*)(s + input.tellg());
}
#endif

namespace olp
{
namespace helper
{
namespace gps
{

static const double DEG_TO_RAD = M_PI / 180;
static const double EARTH_RADIUS_IN_METERS = 6372797.560856;

double convert(const std::string& input)
{
    if (input == "")
        return 0;

    return boost::lexical_cast<double>(input);
}

double convertDegree(const std::string& input)
{
    std::vector<std::string> vec;
    boost::algorithm::split(vec, input, boost::is_any_of(" "));

    double degree = convert(vec[0]);
    double minutes = convert(vec[1]) / 60;
    double second = convert(vec[2]) / 3600;

    return (degree + minutes + second);
}

double calculateAccuracy(double value)
{
    return (100 / 11) * (11.5 - value);
}

GPSStonex createStonex(std::vector<std::string>& data)
{
    GPSStonex gpsData = GPSStonex();

    gpsData.id = ::atoi(data[0].c_str());
    gpsData.latitude = convertDegree(data[1]);
    gpsData.longitude = convertDegree(data[2]);
    gpsData.elevation = convert(data[3]);
    gpsData.cartesianX = convert(data[4]);
    gpsData.cartesianY = convert(data[5]);
    gpsData.cartesianZ = convert(data[6]);
    gpsData.localN = convert(data[7]);
    gpsData.localE = convert(data[8]);
    gpsData.localZ = convert(data[9]);
    gpsData.antennaHeight = data[10];
    gpsData.pdop = convert(data[16]);
    gpsData.hdop = convert(data[17]);
    gpsData.vdop = convert(data[18]);

    std::string date = data[22];
    strptime(date.data(), "%a. %d %b %Y %H:%M:%S", &gpsData.datetime);
    gpsData.secondsSinceReference = mktime(&gpsData.datetime) - 2 *  (60 * 60);
    gpsData.accuracy = calculateAccuracy(gpsData.pdop * 0.5);

    return gpsData;
}

GPS create(std::vector<std::string>& data)
{
    GPS gpsData = GPS();

    gpsData.latitude = convert(data[2]);
    gpsData.longitude = convert(data[3]);
    gpsData.accuracy = calculateAccuracy(convert(data[4]));
    gpsData.elevation = convert(data[5]);

    std::string date = data[1];
    strptime(date.data(), "%Y-%m-%d %H:%M:%S", &gpsData.datetime);
    gpsData.secondsSinceReference = mktime(&gpsData.datetime) + (60 * 60);

    return gpsData;
}

int getStartIndex(std::vector<GPS> gpsData, uint64_t time)
{
    int j = 0;
    bool found = false;
    while (!found && j < gpsData.size() - 1)
    {
        if (gpsData[j].secondsSinceReference < time)
            j++;
        else
        {
            found = calculateDistance(Point(gpsData[j].latitude, gpsData[j].longitude),
                                      Point(gpsData[j + 1].latitude, gpsData[j + 1].longitude)) > 0.09;
            j++;
        }
    }

    return j;
}

std::vector<GPS> kalmanFilter(std::vector<GPS>& gpsData, int startIndex, GPSSource source)
{
    std::vector<GPS> newData;

    if (gpsData.size() == 0)
        return newData;

    TinyEKFHelper f = TinyEKFHelper();

    Point newCoord;
    GPS data = GPS();
    int secondsSinceRefrence = gpsData[startIndex].secondsSinceReference;
    int i = startIndex;
    double prevAzimuth = 0, currentAzimuth, deltaAzimuth;
    GPS prevData = gpsData[startIndex];
    bool hasMatch;

    while (i < gpsData.size())
    {
        if (i > startIndex)
            secondsSinceRefrence++;

        data.secondsSinceReference = secondsSinceRefrence;
        if (secondsSinceRefrence == gpsData[i].secondsSinceReference)
        {
            //if(source == GPSSource::mobile)
                f.setRMatrix((1000 - gpsData[i].accuracy) * 10);
            //else
               // f.setRMatrix(1000);

            if(i == startIndex)
                f.update(gpsData[i], gpsData[i]);
            else
                f.update(gpsData[i], newData[newData.size() - 1]);
            data.accuracy = gpsData[i].accuracy;
            i++;
            hasMatch = true;
        }
        else
        {
            hasMatch = false;
            f.update(gpsData[i], newData[newData.size() - 1]);
            //data.accuracy = newData[newData.size() - 1].accuracy - 1;
        }

        newCoord = f.getNewCoord();
        data.latitude = newCoord.x;
        data.longitude = newCoord.y;
        data.elevation = newCoord.z;

        currentAzimuth = calculateAzimuth({data.latitude, data.longitude, data.elevation},
                                          {prevData.latitude, prevData.longitude, prevData.elevation});
        deltaAzimuth = abs(prevAzimuth - currentAzimuth);
        prevAzimuth = currentAzimuth;
        // penalty for points created by the filter making sharp turns
        if(!hasMatch)
            data.accuracy -= deltaAzimuth * 180;

        prevData = data;
        newData.push_back(data);
    }

    return newData;
}

std::vector<GPS> read(const std::string& fileName, GPSSource source)
{

    std::ifstream file(fileName);
    std::vector<GPS> gpsData;

    std::string line;
    std::vector<std::string> vec;

    if (file.is_open())
    {
        getline(file, line);
        while (getline(file, line))
        {
            boost::erase_all(line, "\"");
            std::replace(line.begin(), line.end(), ',', '.');

            boost::algorithm::split(vec, line, boost::is_any_of(";"));

            switch (source)
            {
                case mobile:
                    gpsData.push_back(create(vec));
                    break;
                case stonex:
                    gpsData.push_back(createStonex(vec));
                    break;
            }
        }
        file.close();
    }
    return gpsData;
}

void write(const std::string& fileName, const std::vector<GPS>& gpsData)
{
    helper::csvfile csv(fileName);
    csv << "latitude" << "longitude" << "elevation" << helper::endrow;
    for (int i = 0; i < gpsData.size(); i++)
    {
        csv << gpsData[i].latitude << gpsData[i].longitude << gpsData[i].elevation << helper::endrow;
    }
}

Point calculateDistanceFromEOV(const Point& from, const Point& to)
{
    double diffY = to.y - from.y;
    double diffX = to.x - from.x;

    return Point(diffY, diffX);
}

double haversine(const Point& from, const Point& to)
{
    double dlong = (to.y - from.y) * DEG_TO_RAD;
    double dlat = (to.x - from.x) * DEG_TO_RAD;
    double a = pow(sin(dlat / 2.0), 2) + cos(from.x * DEG_TO_RAD) * cos(to.x * DEG_TO_RAD) * pow(sin(dlong / 2.0), 2);
    double c = 2 * atan2(sqrt(a), sqrt(1 - a));

    return c;
}

double calculateDistance(const Point& from, const Point& to)
{
    return EARTH_RADIUS_IN_METERS * haversine(from, to);
}

double calculateAngleOfElevation(double deltaXy, double deltaZ) {
    return  atan2(deltaZ,deltaXy);
    double d2 = (deltaZ) / deltaXy - deltaXy / (2*EARTH_RADIUS_IN_METERS);
    double d3 = asin(deltaZ/deltaXy);
}

TransformData calculateTransformData(GPS& data1, GPS& data2, TransformData& actTransformation, GPSCoordType coordType)
{
    double deltaX, deltaY, deltaZ, angle, angleOfElevation = 0;
    double actAngle = getRotZ(actTransformation.transform);
    double actAngleOfElevation = getRotX(actTransformation.transform);
    switch (coordType)
    {
        case latlong:
            deltaY = calculateDistance(Point(data1.latitude, data1.longitude),
                                       Point(data2.latitude, data1.longitude));
            deltaX = calculateDistance(Point(data1.latitude, data1.longitude),
                                       Point(data1.latitude, data2.longitude));

            if (data2.longitude < data1.longitude) deltaX *= -1;
            if (data2.latitude < data1.latitude) deltaY *= -1;
            break;

        case EOV:
            Point EOVdistance = calculateDistanceFromEOV(Point(data1.localN, data1.localE),
                                                         Point(data2.localN, data2.localE));
            deltaX = EOVdistance.x;
            deltaY = EOVdistance.y;
            break;
    }

    if (deltaY != 0 || deltaX != 0)
    {
        angle = calculateAngle(deltaX, deltaY);
        angle -= actAngle;
    }

    deltaZ = data2.elevation - data1.elevation;

    if(deltaZ != 0)
    {
        double deltaXY = calculateDistance(Point(data1.latitude, data1.longitude),
                                           Point(data2.latitude, data2.longitude));

        angleOfElevation = calculateAngleOfElevation(deltaXY, deltaZ);
        angleOfElevation -= actAngleOfElevation;
    }

    TransformData result = TransformData();
    result.transform = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()) *
                       Eigen::AngleAxisd(angleOfElevation, Eigen::Vector3d::UnitX());
    result.transform.translation() << deltaX, deltaY, deltaZ;
    result.transform.translation() = actTransformation.transform.rotation().inverse() * result.transform.translation();
    return result;
}

double calculateAzimuth(const Point& from, const Point& to)
{

    double fromXRad = from.x * DEG_TO_RAD;
    double toXRad = to.x * DEG_TO_RAD;
    double toYRad = to.y * DEG_TO_RAD;
    double fromYRad = from.y * DEG_TO_RAD;
    double longDiffRad = (toYRad - fromYRad);

    double theta = atan2(sin(longDiffRad) * cos(toXRad),
                         cos(fromXRad) * sin(toXRad) - sin(fromXRad) * cos(toXRad) * cos(longDiffRad));
    return theta;
}

double calculateAngle(const Point& from, const Point& to)
{

    double dot = from.x * to.x + from.y * to.y;
    double det = from.x * to.y - from.y * to.x;

    double delta = atan2(det, dot);
    return delta;
}

double calculateAngle(double dx, double dy)
{
    if (dx != 0 && dy != 0)
        return (M_PI) - atan(dx / dy);
    if (dx > 0)
        return M_PI / 2;
    if (dx < 0)
        return M_PI / 2 * -1;
    if (dy > 0)
        return 0;
    if (dy < 0)
        return M_PI;

    return -1;
}

void displayDistances(const MapPointVector& mp, int v1, int v2)
{
    MapPointVector::const_iterator it1 = mp.find(v1);
    MapPointVector::const_iterator it2 = mp.find(v2);
    if (it1 != mp.end() && it2 != mp.end())
    {
        const PointVector& vec1 = it1->second;
        const PointVector& vec2 = it2->second;
        for (size_t i = 0; i < vec1.size(); ++i)
        {
            for (size_t j = 0; j < vec2.size(); ++j)
                std::cout << calculateDistance(vec1[i], vec2[j]) << "\n";
        }
    }
}

} // gps
} // helper
} // olp


// Copyright 2020 Norwegian University of Science and Technology.
//
// 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.

#pragma once

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Core>

#include <glog/logging.h>
#include <iostream>

namespace eris::hand_eye_calibration
{
class CostFunctor
{
public:
  CostFunctor(const Eigen::Vector4d& qi, const Eigen::Vector3d& ti, const Eigen::Vector3d& pi, const Eigen::Vector4d& qj, const Eigen::Vector3d& tj,
              const Eigen::Vector3d& pj)
    : qi_(qi), ti_(ti), pi_(pi), qj_(qj), tj_(tj), pj_(pj)
  {
  }

  template <typename T>
  auto operator()(const T* const qx, const T* const tx, T* residual) const -> bool
  {
    Eigen::Matrix<T, 4, 1> qi = qi_.cast<T>();
    Eigen::Matrix<T, 3, 1> ti = ti_.cast<T>();
    Eigen::Matrix<T, 3, 1> pi = pi_.cast<T>();
    Eigen::Matrix<T, 4, 1> qj = qj_.cast<T>();
    Eigen::Matrix<T, 3, 1> tj = tj_.cast<T>();
    Eigen::Matrix<T, 3, 1> pj = pj_.cast<T>();

    Eigen::Matrix<T, 3, 1> ppi;
    Eigen::Matrix<T, 3, 1> pppi;
    ceres::QuaternionRotatePoint(qx, pi.data(), ppi.data());
    ppi(0) += tx[0];
    ppi(1) += tx[1];
    ppi(2) += tx[2];
    ceres::QuaternionRotatePoint(qi.data(), ppi.data(), pppi.data());
    pppi(0) += ti[0];
    pppi(1) += ti[1];
    pppi(2) += ti[2];

    Eigen::Matrix<T, 3, 1> ppj;
    Eigen::Matrix<T, 3, 1> pppj;
    ceres::QuaternionRotatePoint(qx, pj.data(), ppj.data());
    ppj(0) += tx[0];
    ppj(1) += tx[1];
    ppj(2) += tx[2];
    ceres::QuaternionRotatePoint(qj.data(), ppj.data(), pppj.data());
    pppj(0) += tj[0];
    pppj(1) += tj[1];
    pppj(2) += tj[2];

    residual[0] = pppj(0) - pppi(0);
    residual[1] = pppj(1) - pppi(1);
    residual[2] = pppj(2) - pppi(2);
    return true;
  }

private:
  const Eigen::Vector4d qi_;
  const Eigen::Vector3d ti_;
  const Eigen::Vector3d pi_;
  const Eigen::Vector4d qj_;
  const Eigen::Vector3d tj_;
  const Eigen::Vector3d pj_;
};

class Solver
{
public:
  Solver(const Eigen::Vector4d& q_init, const Eigen::Vector3d t_init) : q_opt_(q_init), t_opt_(t_init)
  {
  }

  auto AddResidualBlock(const Eigen::Vector4d&, const Eigen::Vector3d&, const Eigen::Vector3d&, const Eigen::Vector4d&, const Eigen::Vector3d&,
                        const Eigen::Vector3d&) -> bool;

  auto Solve() -> std::tuple<Eigen::Vector4d, Eigen::Vector3d>;

  auto Summary() -> ceres::Solver::Summary;

  auto Options() -> ceres::Solver::Options;

private:
  ceres::Problem problem_;
  ceres::Solver::Options options_;
  ceres::Solver::Summary summary_;

  bool local_parameterization_is_set_ = false;

  Eigen::Vector4d q_opt_;
  Eigen::Vector3d t_opt_;
};
}  // namespace eris::hand_eye_calibration