#include <iostream>
#include <Eigen/Dense>
#include <random>
#include <vector>
#include <iomanip>
#include <fstream>
#include <bitset>
#include <string>
#include <map>

#ifndef QST_TOMOGRAPHY_HPP
#define QST_TOMOGRAPHY_HPP

namespace qst{

//Quantum State Tomography class
template<class NNState,class Observer,class Optimizer> class Tomography {

    NNState & NNstate_;                         // Neural network representation of the state
    Optimizer & opt_;                           // Optimizer
    Observer &obs_;                             // Observer

    int N_;                                     // Number of physical degrees of freedom
    int npar_;                                  // Number of variational parameters
    int nparLambda_;                            // Number of amplitude variational parameters
    int nparMu_;                                // Number of phase variational parameters
    int bs_;                                    // Batch size
    int cd_;                                    // Number of Gibbs stepos in contrastive divergence
    int epochs_;                                // Number of training iterations
    double lr_;                                 // Learning rate
    double l2_;                                 // L2 regularization constant

    Eigen::VectorXd grad_;                      // Gradients
    Eigen::VectorXcd rotated_grad_;             // Rotated gradients
    std::mt19937 rgen_;                         // Random number generator
    std::map<std::string,Eigen::MatrixXcd> U_;  // Structure containin the single unitary rotations
public:

    Tomography(Optimizer & opt,NNState & NNstate,Observer &obs,Parameters &par):opt_(opt),NNstate_(NNstate),obs_(obs),N_(NNstate.N()),bs_(par.bs_),cd_(par.cd_) {

        npar_=NNstate_.Npar();
        nparLambda_ = NNstate_.NparLambda();
        nparMu_ = NNstate_.NparMu();
        opt_.SetNpar(npar_);
        bs_ = par.bs_;
        cd_ = par.cd_;
        lr_ = par.lr_;
        l2_ = par.l2_;
        epochs_ = par.ep_;
        grad_.resize(npar_);
        rotated_grad_.resize(npar_);
    }

    //Compute gradient of KL divergence
    void ComputeGradient(const Eigen::MatrixXd & batchSamples,const std::vector<std::vector<std::string> >& batchBases){
        grad_.setZero();

        int bID = 0;
        //Positive Phase
        for(int k=0;k<bs_;k++){
            bID = 0;
            for(int j=0;j<N_;j++){ // Check if the basis is the reference one
                if (batchBases[k][j]!="Z"){
                    bID = 1;
                    break;
                }
            }
            if (bID==0){ // Positive phase - Lambda gradient in the reference basis
                grad_.head(nparLambda_) += NNstate_.LambdaGrad(batchSamples.row(k))/double(bs_);
            }
            else { // Positive phase - Lambda and Mu gradients for non-trivial bases
                NNstate_.rotatedGrad(batchBases[k],batchSamples.row(k),U_,rotated_grad_);
                //getRotatedGradient(batchBases[k],batchSamples.row(k),rotated_grad_);
                grad_.head(nparLambda_) += rotated_grad_.head(nparLambda_).real()/double(bs_);
                grad_.tail(nparMu_) -= rotated_grad_.tail(nparMu_).imag()/double(bs_);
            }
        }

        //Negative Phase
        NNstate_.Sample(cd_);
        for(int k=0;k<NNstate_.Nchains();k++){
            grad_.head(nparLambda_) -= NNstate_.LambdaGrad(NNstate_.VisibleStateRow(k))/double(NNstate_.Nchains());
        }
        opt_.getUpdates(grad_);
    }

    // Update rbm parameters
    void UpdateParameters(){
        auto pars=NNstate_.GetParameters();
        opt_.Update(pars);
        NNstate_.SetParameters(pars);
    }

    ////Run the tomography
    void Run(Eigen::MatrixXd & trainData,std::vector<std::vector<std::string> >& trainBases){
        //opt_.Reset();
        int index;
        int counter = 0;
        int trainSize = trainData.rows();
        int saveFrequency =  int(trainSize / bs_);
        Eigen::MatrixXd batch_samples;
        std::vector<std::vector<std::string> > batch_bases;
        std::uniform_int_distribution<int> distribution(0,trainSize-1);

        int epoch = 0;
        for(int i=0;i<epochs_;i++){
            // Randomize a batch and set the visible layer to a data point
            SetUpTrainingStep(trainData,batch_samples,trainBases,batch_bases,distribution);
            // Perform one step of optimization
            ComputeGradient(batch_samples,batch_bases);
            UpdateParameters();
            //Compute stuff and print
            if (counter == saveFrequency){
                epoch += 1;
                obs_.Scan(epoch);
                counter = 0;
            }
            counter++;
        }
    }

    //Set the value of the target wavefunction
    void setBasisRotations(std::map<std::string,Eigen::MatrixXcd> & U){
        U_ = U;
    }
    // Setup the training batch and visible layer initial configuration
    void SetUpTrainingStep(Eigen::MatrixXd & trainData,
                           Eigen::MatrixXd &batch_samples,
                           std::vector<std::vector<std::string> >& trainBases,
                           std::vector<std::vector<std::string> > &batch_bases,
                           std::uniform_int_distribution<int> & distribution){
            int index;
            // Initialize the visible layer to random data samples
            batch_samples.resize(NNstate_.Nchains(),N_);
            for(int k=0;k<NNstate_.Nchains();k++){
                index = distribution(rgen_);
                batch_samples.row(k) = trainData.row(index);
            }
            NNstate_.SetVisibleLayer(batch_samples);

            // Build the batch of data
            batch_samples.resize(bs_,N_);
            batch_bases.resize(bs_,std::vector<std::string>(N_));
            for(int k=0;k<bs_;k++){
                index = distribution(rgen_);
                batch_samples.row(k) = trainData.row(index);
                batch_bases[k] = trainBases[index];
            }
    }
};
}

#endif


/*! file io_test.cpp
 *
 * @copyright Licensed under MIT license by hyperQ – Ewa Hendzel
 *
 */
#include "model/qubo.hpp"
#include "model/solution.hpp"
#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>
#include <sstream>

const std::string
    qubo_file_contents("c qubo Target MaxNodes NumNodes NumLinks\n"
                       "p qubo 0 100 2 2\n"
                       "c comment\n"
                       "0 0 -0.5\n"
                       "0 1 2.0\n"
                       "1 2 4\n"
                       "2 2 -0.7\n");

const std::string grammatically_incorrect_file_contents[]{
    std::string("p qubo 0 1 100 3 492\n"
                "0 0 -0.5\n"
                "0 1 2.0\n"),
    std::string("p qubo 0 100 3 492\n"
                "0 0 1 -0.5\n"
                "0 1 2.0\n"),
    std::string("p qubo 0 100 3 492\n"
                "0 0 -0.5\n"
                "0 2.0\n"),
    std::string("p qubo 0 100 492\n"
                "0 0 -0.5\n"
                "0 1 2.0\n"),
    std::string("p qubo 0 100 3 492\n"
                "0 0 -0.5\n"
                "unexpected string\n"
                "0 1 2.0\n")};

BOOST_AUTO_TEST_SUITE(io_test)

BOOST_AUTO_TEST_CASE(number_of_variable_agrees_with_given_in_input_stream) {
  std::stringstream stream(qubo_file_contents);
  stream.unsetf(std::ios::skipws);
  qubo::QUBOModel<int, double> problem =
      qubo::QUBOModel<int, double>::load(stream);

  BOOST_TEST_REQUIRE(problem.get_nodes() == 3);
}

BOOST_AUTO_TEST_CASE(all_linear_coefficients_are_present_in_the_model) {
  std::stringstream stream(qubo_file_contents);
  stream.unsetf(std::ios::skipws);
  qubo::QUBOModel<int, double> problem =
      qubo::QUBOModel<int, double>::load(stream);

  BOOST_TEST_REQUIRE(problem.get_variable(0) == -0.5);
  BOOST_TEST_REQUIRE(problem.get_variable(2) == -0.7);
}

BOOST_AUTO_TEST_CASE(all_quadratic_coefficients_are_present_in_the_model) {
  std::stringstream stream(qubo_file_contents);
  stream.unsetf(std::ios::skipws);
  qubo::QUBOModel<int, double> problem =
      qubo::QUBOModel<int, double>::load(stream);

  BOOST_TEST_REQUIRE(problem.get_connection(std::pair(0, 1)) == 2.0);
  BOOST_TEST_REQUIRE(problem.get_connection(std::pair(1, 2)) == 4.0);
}

BOOST_AUTO_TEST_CASE(
    read_qubo_throws_if_interaction_is_placed_in_reversed_order) {
  std::string invalid_qubo_file_contents("p qubo 0 100 2 2\n"
                                         "0 0 -0.5\n"
                                         "1 0 2.0\n"
                                         "1 2 4\n"
                                         "2 2 -0.7\n");

  std::stringstream stream(invalid_qubo_file_contents);
  stream.unsetf(std::ios::skipws);

  BOOST_CHECK_THROW((qubo::QUBOModel<int, double>::load(stream)),
                    std::invalid_argument);
}

BOOST_DATA_TEST_CASE(read_qubo_throws_if_stream_contents_dont_adhere_to_grammar,
                     grammatically_incorrect_file_contents) {
  std::stringstream stream(sample);
  stream.unsetf(std::ios::skipws);

  BOOST_CHECK_THROW((qubo::QUBOModel<int, double>::load(stream)),
                    std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(number_of_quadratic_terms_is_equal_to_the_declared_one) {
  std::string invalid_qubo_file_contents("p qubo 0 100 3 3\n"
                                         "0 0 -0.5\n"
                                         "0 1 2.0\n"
                                         "1 2 4\n"
                                         "2 2 -0.7\n");

  std::stringstream stream(invalid_qubo_file_contents);
  stream.unsetf(std::ios::skipws);

  BOOST_CHECK_THROW((qubo::QUBOModel<int, double>::load(stream)),
                    std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(saved_solution_has_correct_structure) {
  char state[] = {0, 1, 1, 0, 1};
  qubo::Solution solution(state, state + 5, -12.5);

  std::string expected_output = "0,1,2,3,4,energy\n0,1,1,0,1,-12.5\n";

  std::stringstream stream("");
  solution.save(stream);

  BOOST_TEST_REQUIRE(stream.str() == expected_output);
}

BOOST_AUTO_TEST_SUITE_END()


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

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

 * See LICENSE for the license information

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

/*
 * testDecisionTreeFactor.cpp
 *
 *  @date Feb 5, 2012
 *  @author Frank Dellaert
 *  @author Duy-Nguyen Ta
 */

#include <gtsam/discrete/Signature.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
using namespace boost::assign;

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST( DecisionTreeFactor, constructors)
{
	DiscreteKey X(0,2), Y(1,3), Z(2,2);

	DecisionTreeFactor f1(X, "2 8");
	DecisionTreeFactor f2(X & Y, "2 5 3 6 4 7");
	DecisionTreeFactor f3(X & Y & Z, "2 5 3 6 4 7 25 55 35 65 45 75");
	EXPECT_LONGS_EQUAL(1,f1.size());
	EXPECT_LONGS_EQUAL(2,f2.size());
	EXPECT_LONGS_EQUAL(3,f3.size());

	//		f1.print("f1:");
	//		f2.print("f2:");
	//		f3.print("f3:");

	DecisionTreeFactor::Values values;
	values[0] = 1; // x
	values[1] = 2; // y
	values[2] = 1; // z
	EXPECT_DOUBLES_EQUAL(8, f1(values), 1e-9);
	EXPECT_DOUBLES_EQUAL(7, f2(values), 1e-9);
	EXPECT_DOUBLES_EQUAL(75, f3(values), 1e-9);
}

/* ************************************************************************* */
TEST_UNSAFE( DecisionTreeFactor, multiplication)
{
	// Declare a bunch of keys
	DiscreteKey v0(0,2), v1(1,2), v2(2,2);

	// Create a factor
	DecisionTreeFactor f1(v0 & v1, "1 2 3 4");
	DecisionTreeFactor f2(v1 & v2, "5 6 7 8");
//	f1.print("f1:");
//	f2.print("f2:");

	DecisionTreeFactor expected(v0 & v1 & v2, "5 6 14 16 15 18 28 32");

	DecisionTreeFactor actual = f1 * f2;
//	actual.print("actual: ");
	CHECK(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( DecisionTreeFactor, sum_max)
{
	// Declare a bunch of keys
	DiscreteKey v0(0,3), v1(1,2);

	// Create a factor
	DecisionTreeFactor f1(v0 & v1, "1 2  3 4  5 6");

	DecisionTreeFactor expected(v1, "9 12");
	DecisionTreeFactor::shared_ptr actual = f1.sum(1);
	CHECK(assert_equal(expected, *actual, 1e-5));
//	f1.print("f1:");
//	actual->print("actual: ");
//	actual->printCache("actual cache: ");

	DecisionTreeFactor expected2(v1, "5 6");
	DecisionTreeFactor::shared_ptr actual2 = f1.max(1);
	CHECK(assert_equal(expected2, *actual2));

	DecisionTreeFactor f2(v1 & v0, "1 2  3 4  5 6");
	DecisionTreeFactor::shared_ptr actual22 = f2.sum(1);
//	f2.print("f2: ");
//	actual22->print("actual22: ");

}

/* ************************************************************************* */
int main() {
	TestResult tr;
	return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#pragma once

#include <boost/foreach.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <iostream>

#include "modprop/optim/SumCost.hpp"
#include "modprop/utils/SubsetSamplers.hpp"

namespace percepto
{

/*! \brief Randomly samples subsets of the population to return
 * estimates of the objective and gradient.
 *
 * Resampling is executed on calls to Evaluate() only, since adding an
 * explicit Resample() method would break the cost function interface.
 *
 * The container holding the costs can be altered to change the
 * population. Make sure to call Evaluate() after alteration to set
 * the active indices.
 */
template <typename DataType>
class StochasticSumCost
: public SumCost<DataType>
{
public:

	typedef DataType OutputType;
	typedef SumCost<DataType> ParentCost;

	StochasticSumCost()
	: _subsetSize( 0 ), _ssRatio( 0 )
	{
		boost::random::random_device rng;
		_generator.seed( rng );
	}

	void SetBatchSize( unsigned int ss )
	{
		_ssRatio = 0;
		_subsetSize = ss;
	}
	void SetSubsample( double ss )
	{
		_ssRatio = std::min( ss, 1.0 );
		_subsetSize = 0;
	}

	virtual void BackpropImplementation( const MatrixType& nextDodx )
	{
		if( _activeInds.size() == 0 ) { return; }
		for( unsigned int i = 0; i < _activeInds.size(); i++ )
		{
			ParentCost::_sinks[ _activeInds[i] ].Backprop( nextDodx );
		}
	}

	void Resample()
	{
		RandomSample();
	}

	const std::vector<unsigned int>& GetActiveInds() const
	{
		return _activeInds;
	}

	/*! \brief Calculate the objective function by averaging the
	 * underlying cost function over the population. Resamples the
	 * active population at the beginning of the call. When making sequential
	 * calls to Evaluate() and Derivative(), make sure to call Evaluate()
	 * first to get an equivalent Derivative(). */
	virtual void Foreprop()
	{
		if( _activeInds.size() == 0 )
		{
			 // TODO Generalize to other datatypes
			ParentCost::SourceType::SetOutput( 0 );
			ParentCost::SourceType::Foreprop();
			return;
		}
		for( unsigned int i = 0; i < _activeInds.size(); i++ )
		{
			if( !ParentCost::_sinks[ _activeInds[i] ].IsValid() ) { return; }
		}

		OutputType acc = ParentCost::_sinks[ _activeInds[0] ].GetInput();
		for( unsigned int i = 1; i < _activeInds.size(); i++ )
		{
			acc += ParentCost::_sinks[ _activeInds[i] ].GetInput();
		}

		ParentCost::SourceType::SetOutput( acc );
		ParentCost::SourceType::Foreprop();
	}

	virtual void Invalidate()
	{
		ParentCost::SourceType::Invalidate();
	}

private:

	// Used for random selection
	mutable boost::random::mt19937 _generator;

	unsigned int _subsetSize;
	double _ssRatio;
	mutable std::vector<unsigned int> _activeInds;

	void RandomSample() const
	{
		unsigned int ss;
		if( _subsetSize > 0 )
		{
			ss = _subsetSize;
		}
		else
		{
			ss = (unsigned int) std::ceil( _ssRatio * ParentCost::_sinks.size() );
		}
		if( ss < 1 ) { ss = 1; }

		if( ParentCost::_sinks.size() > ss )
		{
			BitmapSampling( ParentCost::_sinks.size(), ss,
			                   _activeInds, _generator );
			return;
		}

		// If there isn't enough data, just use all of it
		_activeInds.clear();
		_activeInds.reserve( ParentCost::_sinks.size() );
		for( unsigned int i = 0; i < ParentCost::_sinks.size(); ++i )
		{
			_activeInds.push_back( i );
		}
	}

};

}


/**
 * @file stereoFrame.cpp
 * @brief shows VERTIGO left camera frame wrt SPH fixed body frame
 * @date November 16, 2018
 * @author Antonio Terán, teran@mit.edu
 */

#include <iostream>

#include <boost/thread/thread.hpp>
#include <Eigen/Dense>
#include <pcl/visualization/pcl_visualizer.h>

bool g_nextIteration = false;

void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,
    void* nothing){
  if (event.getKeySym () == "space" && event.keyDown ())
    g_nextIteration = true;
}// keyboardEventOccurred

int main() {

  std::cout << "\n Visualizing stereo camera frame \n";

  /* create the viewer */
  boost::shared_ptr<pcl::visualization::PCLVisualizer>
      viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
  viewer->setBackgroundColor(0,0,0); /*dark*/
  viewer->addCoordinateSystem(0.103); /*sph radius apprx*/
  viewer->initCameraParameters();

  /* SPH body-fixed frame B to left stereo cam frame C pose */
  Eigen::Transform<float, 3, Eigen::Affine> PBtoC =
            Eigen::Transform<float, 3, Eigen::Affine>::Identity();
  PBtoC.translate( Eigen::Vector3f(0.234-0.025, 0.045, 0.030) );
  PBtoC.rotate( (Eigen::Matrix3f() <<  0,  0,  1,
                                      -1,  0,  0,
                                       0, -1,  0).finished() );
  viewer->addCoordinateSystem(0.05, PBtoC, "PBtoC", 0);
  /* add CCD plane */
  pcl::ModelCoefficients stereoLeftCCDcoeffs; /* ax + by + cz + d = 0 */
  stereoLeftCCDcoeffs.values.push_back(1.0); /* a */
  stereoLeftCCDcoeffs.values.push_back(0.0); /* b */
  stereoLeftCCDcoeffs.values.push_back(0.0); /* c */
  stereoLeftCCDcoeffs.values.push_back(0.0); /* d */
  viewer->addPlane(stereoLeftCCDcoeffs, 0.234-0.025, 0.045, 0.03, "leftStereoCCD");

  /* register keyboard callback */
  viewer->registerKeyboardCallback(&keyboardEventOccurred, (void*) NULL);

  while (!viewer->wasStopped()) {

    viewer->spinOnce(100);
    boost::this_thread::sleep(boost::posix_time::microseconds(100000));

    if (g_nextIteration) {
      g_nextIteration = false;
    }

  }/*while*/

}/*main*/


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//                 polar v1.0 - August the 15th 2019                         //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//  Copyright 2019 Matteo Tommasini                                          //
//                                                                           //
//  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.                                           //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#ifndef STATES_H
#define STATES_H

#include <Eigen/Core>
#include <iostream>
#include <fstream>
#include <vector>

class Polarizability;

class States
{

friend class Polarizability;

private:
   // attributes
   Eigen::VectorXd energy;
   std::vector<Eigen::Vector3d>  el_dipole,
                                mag_dipole;

public:
   States();
   explicit States(const std::string&);
   void Print() const;
   Eigen::VectorXd OscillatorStrengths() const;
   Eigen::VectorXd RotatoryStrengths() const;
   double SumRule() const;
   void DestroyLastNstates(const int);

   // getters
   unsigned int GetNumberOfStates() const;
   std::vector<Eigen::Vector3d> GetElectricDipole() const;
   std::vector<Eigen::Vector3d> GetMagneticDipole() const;
   Eigen::VectorXd GetElectricTransitionDipoleNorm() const;
   Eigen::VectorXd GetMagneticTransitionDipoleRobustNorm() const;
   Eigen::VectorXd GetEnergy() const;

};

#endif // STATES_H


/*******************************************************************************
 *
 * Implementation of bignums based on GMP, the Gnu Multiple Precision Arithmetic
 * Library (http://gmplib.org).
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 *
 * Notices:
 *
 * Copyright (c) 2011-2014 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <climits>
#include <gmpxx.h>
#include <boost/functional/hash.hpp>
#include <crab/common/types.hpp>

namespace ikos {

class z_number {
  friend class q_number;

private:
  mpz_class _n;

public:

  z_number(mpz_class n) : _n(n) {}

  static z_number from_ulong(unsigned long n) {
    mpz_class b(n);
    return z_number(b);
  }

  static z_number from_slong(signed long n) {
    mpz_class b(n);
    return z_number(b);
  }

  // overloaded typecast operators
  explicit operator long() const {
    if (_n.fits_slong_p ()) {
      return _n.get_si ();
    }
    else {
      CRAB_ERROR("mpz_class ", _n.get_str(), " does not fit into a signed long integer");
    }
  }

  explicit operator int() const {
    if (_n.fits_sint_p ()) {
      // get_si returns a signed long so we cast it to int
      return (int) _n.get_si ();
    }
    else {
      CRAB_ERROR("mpz_class ", _n.get_str(), " does not fit into a signed integer");
    }
  }

  explicit operator mpz_class() const {
    return _n;
  }

public:

  z_number() : _n(0) {}

  z_number(std::string s) {
    try {
      this->_n = s;
    } catch (std::invalid_argument& e) {
      CRAB_ERROR ("z_number: invalid string in constructor", s);
    }
  }

  z_number(signed long long int n) : _n((signed long int) n) {
    if (n > LONG_MAX) {
      CRAB_ERROR(n, " cannot fit into a signed long int: use another mpz_class constructor");
    }
  }

  std::string get_str () const {
    return _n.get_str();
  }

  bool fits_sint() const {
    return _n.fits_sint_p();
  }

  bool fits_slong() const {
    return _n.fits_slong_p();
  }

  z_number operator+(z_number x) const {
    mpz_class r = this->_n + x._n;
    return z_number(r);
  }

  z_number operator*(z_number x) const {
    mpz_class r = this->_n * x._n;
    return z_number(r);
  }

  z_number operator-(z_number x) const {
    mpz_class r = this->_n - x._n;
    return z_number(r);
  }

  z_number operator-() const {
    mpz_class r = -this->_n;
    return z_number(r);
  }

  z_number operator/(z_number x) const {
    if (x._n == 0) {
      CRAB_ERROR("z_number: division by zero [1]");
    } else {
      mpz_class r = this->_n / x._n;
      return z_number(r);
    }
  }

  z_number operator%(z_number x) const {
    if (x._n == 0) {
      CRAB_ERROR("z_number: division by zero [2]");
    } else {
      mpz_class r = this->_n % x._n;
      return z_number(r);
    }
  }

  z_number& operator+=(z_number x) {
    this->_n += x._n;
    return *this;
  }

  z_number& operator*=(z_number x) {
    this->_n *= x._n;
    return *this;
  }

  z_number& operator-=(z_number x) {
    this->_n -= x._n;
    return *this;
  }

  z_number& operator/=(z_number x) {
    if (x._n == 0) {
      CRAB_ERROR("z_number: division by zero [3]");
    } else {
      this->_n /= x._n;
      return *this;
    }
  }

  z_number& operator%=(z_number x) {
    if (x._n == 0) {
      CRAB_ERROR("z_number: division by zero [4]");
    } else {
      this->_n %= x._n;
      return *this;
    }
  }

  z_number& operator--() {
    --(this->_n);
    return *this;
  }

  z_number& operator++() {
    ++(this->_n);
    return *this;
  }

  z_number operator++(int) {
    z_number r(*this);
    ++(*this);
    return r;
  }

  z_number operator--(int) {
    z_number r(*this);
    --(*this);
    return r;
  }

  bool operator==(z_number x) const { return this->_n == x._n; }

  bool operator!=(z_number x) const { return this->_n != x._n; }

  bool operator<(z_number x) const { return this->_n < x._n; }

  bool operator<=(z_number x) const { return this->_n <= x._n; }

  bool operator>(z_number x) const { return this->_n > x._n; }

  bool operator>=(z_number x) const { return this->_n >= x._n; }

  z_number operator&(z_number x) const { return z_number(this->_n & x._n); }

  z_number operator|(z_number x) const { return z_number(this->_n | x._n); }

  z_number operator^(z_number x) const { return z_number(this->_n ^ x._n); }

  z_number operator<<(z_number x) const {
    mpz_t tmp;
    mpz_init(tmp);
    mpz_mul_2exp(tmp, this->_n.get_mpz_t(), mpz_get_ui(x._n.get_mpz_t()));
    mpz_class result(tmp);
    return z_number(result);
  }

  z_number operator>>(z_number x) const {
    mpz_class tmp(this->_n);
    return z_number(tmp.operator>>=(mpz_get_ui(x._n.get_mpz_t())));
  }

  z_number fill_ones() const {
    assert(this->_n >= 0);
    if (this->_n == 0) {
      return z_number(0);
    }

    mpz_class result;
    for (result = 1; result < this->_n; result = 2 * result + 1)
      ;
    return z_number(result);
  }

  void write(crab::crab_os& o) { o << this->_n.get_str(); }

}; // class z_number

inline crab::crab_os& operator<<(crab::crab_os& o, z_number z) {
  z.write(o);
  return o;
}

inline std::size_t hash_value(const z_number& n) {
  boost::hash<std::string> hasher;
  return hasher(n.get_str());
}

class q_number {
private:
  mpq_class _n;

public:

  q_number() : _n(0) {}

  q_number(mpq_class n) : _n(n) {}

  q_number(std::string s) {
    try {
      this->_n = s;
      this->_n.canonicalize();
    } catch (std::invalid_argument& e) {
      CRAB_ERROR("q_number: invalid string in constructor ",s);
    }
  }

  q_number(double n): _n(n) { this->_n.canonicalize(); }

  q_number(z_number n) : _n(n._n) { this->_n.canonicalize(); }

  q_number(z_number n, z_number d) : _n(n._n, d._n) { this->_n.canonicalize(); }

  explicit operator mpq_class() const {
    return _n;
  }

  std::string get_str () const {
    return _n.get_str();
  }

  q_number operator+(q_number x) const {
    mpq_class r = this->_n + x._n;
    return q_number(r);
  }

  q_number operator*(q_number x) const {
    mpq_class r = this->_n * x._n;
    return q_number(r);
  }

  q_number operator-(q_number x) const {
    mpq_class r = this->_n - x._n;
    return q_number(r);
  }

  q_number operator-() const {
    mpq_class r = -this->_n;
    return q_number(r);
  }

  q_number operator/(q_number x) const {
    if (x._n == 0) {
      CRAB_ERROR("q_number: division by zero [1]");
    } else {
      mpq_class r = this->_n / x._n;
      return q_number(r);
    }
  }

  q_number& operator+=(q_number x) {
    this->_n += x._n;
    return *this;
  }

  q_number& operator*=(q_number x) {
    this->_n *= x._n;
    return *this;
  }

  q_number& operator-=(q_number x) {
    this->_n -= x._n;
    return *this;
  }

  q_number& operator/=(q_number x) {
    if (x._n == 0) {
      CRAB_ERROR("q_number: division by zero [2]");
    } else {
      this->_n /= x._n;
      return *this;
    }
  }

  q_number& operator--() {
    --(this->_n);
    return *this;
  }

  q_number& operator++() {
    ++(this->_n);
    return *this;
  }

  q_number operator--(int) {
    q_number r(*this);
    --(*this);
    return r;
  }

  q_number operator++(int) {
    q_number r(*this);
    ++(*this);
    return r;
  }

  bool operator==(q_number x) const { return this->_n == x._n; }

  bool operator!=(q_number x) const { return this->_n != x._n; }

  bool operator<(q_number x) const { return this->_n < x._n; }

  bool operator<=(q_number x) const { return this->_n <= x._n; }

  bool operator>(q_number x) const { return this->_n > x._n; }

  bool operator>=(q_number x) const { return this->_n >= x._n; }

  z_number numerator() const { return z_number(this->_n.get_num()); }

  z_number denominator() const { return z_number(this->_n.get_den()); }

  z_number round_to_upper() const {
    z_number num = numerator();
    z_number den = denominator();
    z_number q = num / den;
    z_number r = num % den;
    if (r == 0 || *this < 0) {
      return q;
    } else {
      return q + 1;
    }
  }

  z_number round_to_lower() const {
    z_number num = numerator();
    z_number den = denominator();
    z_number q = num / den;
    z_number r = num % den;
    if (r == 0 || *this > 0) {
      return q;
    } else {
      return q - 1;
    }
  }

  void write(crab::crab_os& o) { o << this->_n.get_str(); }

}; // class q_number

inline crab::crab_os& operator<<(crab::crab_os& o, q_number q) {
  q.write(o);
  return o;
}

inline std::size_t hash_value(const q_number& n) {
  boost::hash<std::string> hasher;
  return hasher(n.get_str());
}
}


#ifndef STAN_MATH_PRIM_ARR_FUNCTOR_INTEGRATE_ODE_RK45_HPP
#define STAN_MATH_PRIM_ARR_FUNCTOR_INTEGRATE_ODE_RK45_HPP

#include <stan/math/prim/arr/err/check_nonzero_size.hpp>
#include <stan/math/prim/arr/err/check_ordered.hpp>
#include <stan/math/prim/arr/functor/coupled_ode_system.hpp>
#include <stan/math/prim/arr/functor/coupled_ode_observer.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/err/check_less.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/invalid_argument.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <boost/version.hpp>
#if BOOST_VERSION == 106400
#include <boost/serialization/array_wrapper.hpp>
#endif
#include <boost/numeric/odeint.hpp>
#include <ostream>
#include <vector>

namespace stan {
namespace math {

/**
 * Return the solutions for the specified system of ordinary
 * differential equations given the specified initial state,
 * initial times, times of desired solution, and parameters and
 * data, writing error and warning messages to the specified
 * stream.
 *
 * Warning: If the system of equations is stiff, roughly
 * defined by having varying time scales across dimensions, then
 * this solver is likely to be slow.
 *
 * This function is templated to allow the initial times to be
 * either data or autodiff variables and the parameters to be data
 * or autodiff variables.  The autodiff-based implementation for
 * reverse-mode are defined in namespace stan::math
 * and may be invoked via argument-dependent lookup by including
 * their headers.
 *
 * This function uses the Dormand-Prince
 * method as implemented in Boost's
 * boost::numeric::odeint::runge_kutta_dopri5 integrator.
 *
 * @tparam F type of ODE system function.
 * @tparam T1 type of scalars for initial values.
 * @tparam T2 type of scalars for parameters.
 * @param[in] f functor for the base ordinary differential equation.
 * @param[in] y0 initial state.
 * @param[in] t0 initial time.
 * @param[in] ts times of the desired solutions, in strictly
 * increasing order, all greater than the initial time.
 * @param[in] theta parameter vector for the ODE.
 * @param[in] x continuous data vector for the ODE.
 * @param[in] x_int integer data vector for the ODE.
 * @param[out] msgs the print stream for warning messages.
 * @param[in] relative_tolerance relative tolerance parameter
 *   for Boost's ode solver. Defaults to 1e-6.
 * @param[in] absolute_tolerance absolute tolerance parameter
 *   for Boost's ode solver. Defaults to 1e-6.
 * @param[in] max_num_steps maximum number of steps to take within
 *   the Boost ode solver.
 * @return a vector of states, each state being a vector of the
 * same size as the state variable, corresponding to a time in ts.
 */
template <typename F, typename T1, typename T2>
std::vector<std::vector<typename stan::return_type<T1, T2>::type> >
integrate_ode_rk45(const F& f, const std::vector<T1>& y0, double t0,
                   const std::vector<double>& ts, const std::vector<T2>& theta,
                   const std::vector<double>& x, const std::vector<int>& x_int,
                   std::ostream* msgs = nullptr,
                   double relative_tolerance = 1e-6,
                   double absolute_tolerance = 1e-6, int max_num_steps = 1E6) {
  using boost::numeric::odeint::integrate_times;
  using boost::numeric::odeint::make_dense_output;
  using boost::numeric::odeint::max_step_checker;
  using boost::numeric::odeint::runge_kutta_dopri5;

  check_finite("integrate_ode_rk45", "initial state", y0);
  check_finite("integrate_ode_rk45", "initial time", t0);
  check_finite("integrate_ode_rk45", "times", ts);
  check_finite("integrate_ode_rk45", "parameter vector", theta);
  check_finite("integrate_ode_rk45", "continuous data", x);

  check_nonzero_size("integrate_ode_rk45", "times", ts);
  check_nonzero_size("integrate_ode_rk45", "initial state", y0);
  check_ordered("integrate_ode_rk45", "times", ts);
  check_less("integrate_ode_rk45", "initial time", t0, ts[0]);

  if (relative_tolerance <= 0)
    invalid_argument("integrate_ode_rk45", "relative_tolerance,",
                     relative_tolerance, "", ", must be greater than 0");
  if (absolute_tolerance <= 0)
    invalid_argument("integrate_ode_rk45", "absolute_tolerance,",
                     absolute_tolerance, "", ", must be greater than 0");
  if (max_num_steps <= 0)
    invalid_argument("integrate_ode_rk45", "max_num_steps,", max_num_steps, "",
                     ", must be greater than 0");

  // creates basic or coupled system by template specializations
  coupled_ode_system<F, T1, T2> coupled_system(f, y0, theta, x, x_int, msgs);

  // first time in the vector must be time of initial state
  std::vector<double> ts_vec(ts.size() + 1);
  ts_vec[0] = t0;
  for (size_t n = 0; n < ts.size(); n++)
    ts_vec[n + 1] = ts[n];

  std::vector<std::vector<double> > y_coupled(ts_vec.size());
  coupled_ode_observer observer(y_coupled);

  // the coupled system creates the coupled initial state
  std::vector<double> initial_coupled_state = coupled_system.initial_state();

  const double step_size = 0.1;
  integrate_times(
      make_dense_output(absolute_tolerance, relative_tolerance,
                        runge_kutta_dopri5<std::vector<double>, double,
                                           std::vector<double>, double>()),
      boost::ref(coupled_system), initial_coupled_state, boost::begin(ts_vec),
      boost::end(ts_vec), step_size, observer, max_step_checker(max_num_steps));

  // remove the first state corresponding to the initial value
  y_coupled.erase(y_coupled.begin());

  // the coupled system also encapsulates the decoupling operation
  return coupled_system.decouple_states(y_coupled);
}

}  // namespace math

}  // namespace stan

#endif


/*
 * ValidVariablesConstraint.cpp
 * @brief ValidVariablesConstraint constraint
 * @date Mar 20, 2022
 * @author Yoonwoo Kim
 */

#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <cpp/planning/MultiValueConstraint.h>
#include <cpp/planning/BinarySameConstraint.h>
#include <cpp/planning/ValidVariablesConstraint.h>

#include <boost/make_shared.hpp>
using namespace gtsam;
using namespace std;

namespace gtsam_planner {

/* ************************************************************************* */
ValidVariablesConstraint::ValidVariablesConstraint(const BinarySameConstraint& b_factor,
    const vector<MultiValueConstraint>& m_factors, const DiscreteKeys& dkeys)
: DiscreteFactor(dkeys.indices()) {
  b_factor_ = b_factor;
  for (const MultiValueConstraint& factor: m_factors) m_factors_.push_back(factor);
  dkeys_ = dkeys;

}

/* ************************************************************************* */
void ValidVariablesConstraint::print(const string& s, const KeyFormatter& formatter) const {
  cout << s << "ValidVariablesConstraint on ";
  for (DiscreteKey dkey : dkeys_) std::cout << formatter(dkey.first) << " ";
  cout << endl;
}
/* ************************************************************************* */
double ValidVariablesConstraint::operator()(const DiscreteValues& values) const {
  for (const MultiValueConstraint factor: m_factors_) {
    if (factor(values) == 1.0) return 1.0;
  }
  if (b_factor_(values) == 1.0) return 1.0;
  return 0.0;
}

/* ************************************************************************* */
DecisionTreeFactor ValidVariablesConstraint::toDecisionTreeFactor() const {
  vector<double> table;
  const auto assignments = DiscreteValues::CartesianProduct(dkeys_);
  for (const DiscreteValues assignment : assignments) {
    size_t is_true = 0;
    for (MultiValueConstraint factor : m_factors_) {
      if (factor(assignment) == 1.0) is_true += factor(assignment);
    }
    if (b_factor_(assignment) == 1.0) is_true += b_factor_(assignment);
    table.push_back(is_true > 0);
  }
  DiscreteKeys rev_dkeys = {dkeys_.rbegin(), dkeys_.rend()};
  DecisionTreeFactor converted(rev_dkeys, table);
  return converted;
}

/* ************************************************************************* */
DecisionTreeFactor ValidVariablesConstraint::operator*(const DecisionTreeFactor& f) const {
  // TODO: can we do this more efficiently?
  return toDecisionTreeFactor() * f;
}

}  // namespace gtsam_planner


#pragma once

#include <Eigen/Core>
#include <polyvec/curve-tracer/spline.hpp>
#include <polyvec/curve-tracer/curve.hpp>
#include <polyvec/polygon-tracer/boundary-graph.hpp>
#include <polyvec/utils/matrix.hpp>

namespace draw_paper_figures {
    struct Data {
        const Eigen::Matrix2Xd* B;
        const Eigen::Matrix2Xd* PP;
        const Eigen::VectorXi*  P;
        const std::vector<polyfit::BoundaryGraph::Edge>* E;
        const std::vector<polyvec::CurvePrimitive>* curves;
        const std::vector<polyvec::TangentFitType>* tangent_fits;
        const std::vector<polyvec::AttemptInfo>* curves_attempts;
		Eigen::Vector4d color = Eigen::Vector4d(0, 0, 0, 1);
    };

    inline void grid(const Data& data) {
        const auto& raster = *data.B;
        const auto& style = Style::outline(colors::gray * 1.75, .05);

        Eigen::Vector2d min, max;
        min.setConstant(std::numeric_limits<double>::infinity());
        max = -min;

        for (int i = 0; i < raster.cols(); ++i) {
            for (int j = 0; j < 2; ++j) {
                if (raster.coeff(j, i) < min(j)) {
                    min(j) = raster.coeff(j, i);
                }

                if (raster.coeff(j, i) > max(j)) {
                    max(j) = raster.coeff(j, i);
                }
            }
        }

        min.x() = std::floor(min.x() + 0.5 - 1);
        min.y() = std::floor(min.y() + 0.5 - 1);
        max.x() = std::ceil(max.x() + 0.5 + 1);
        max.y() = std::ceil(max.y() + 0.5 + 1);

        for (int i = (int)min(0); i <= (int)max(0); ++i) {
            draw::line(Eigen::Vector2d((double)i - 0.5, min(1) - 0.5), Eigen::Vector2d((double)i - 0.5, max(1) - 0.5), style);
        }

        for (int i = (int)min(1); i <= (int)max(1); ++i) {
            draw::line(Eigen::Vector2d(min(0) - 0.5, (double)i - 0.5), Eigen::Vector2d(max(0) - 0.5, (double)i - 0.5), style);
        }
    }

    inline void raster_boundary(const Data& data) {
        const auto& raster = *data.B;
        std::vector<real2> points;

        for (int i = 0; i < raster.cols(); ++i) {
            points.emplace_back(raster.col(i));
        }

        draw::polygon(points, Style::fill(real3(61.f/255, 104.f/255, 174.f/255)));
    }

    inline void graph(const Data& data) {
        const auto& B = *data.B;
        const auto& E = *data.E;

        for (size_t i = 0; i < E.size(); ++i) {
            const auto p0 = B.col(E[i].v0);
            const auto p1 = B.col(E[i].v1);
            draw::line(p0, p1, Style::outline(colors::forest_green, .03));
        }
    }

    inline void polygon(const Data& data) {
        draw::polyline(*data.PP, Style::outline(colors::black, 0.075));
    }

    inline void polygon_curves(const Data& data) {
        const auto& PP = *data.PP;
        for (Eigen::Index i = 0; i < PP.cols(); ++i) {
            const auto pp = PP.col(i);
            const auto pn = polyfit::CircularAt(PP, i + 1);
            const auto t = pn - pp;
            Eigen::Vector2d n(-t.y(), t.x());
            n.normalize();
            const auto mid = .5 * (pp + pn);
            draw::line(mid, mid + n * .2, Style::outline(colors::black, .1));
            draw::line(mid, mid - n * .2, Style::outline(colors::black, .1));
        }

        polygon(data);
    }

    inline void curve_primitives_classification(const Data& data) {
        using namespace polyfit;
        const auto& PP = *data.PP;
        const auto& B = *data.B;

        for (Eigen::Index i = 0; i < PP.cols(); ++i) {
            const int tangent_fit = data.tangent_fits->at(i);

            //if (tangent_fit == TANGENT_FIT_CONSTANT) {
            //    const auto pp = .5 * (CircularAt(PP, i - 1) + CircularAt(PP, i));
            //    const auto pn = .5 * (CircularAt(PP, i) + CircularAt(PP, i + 1));
            //    draw::line(CircularAt(PP, i), pp, Style::outline(colors::red, .1));
            //    draw::line(CircularAt(PP, i), pn, Style::outline(colors::red, .1));
            //    draw::point(pp, .1, Style::fill(colors::black));
            //    draw::point(pn, .1, Style::fill(colors::black));
            //    draw::point(CircularAt(PP, i), .1, Style::fill(colors::black));
            //}else {
                const auto& curves = data.curves_attempts->at(tangent_fit).attempt[0].primitive_seq.primitives;
                for (size_t j = 0; j < curves.size(); ++j) {
                    if (curves[j].corner == i) {
                        switch (curves[j].curve->get_curve()->get_type()) {
                        case GLOBFIT_CURVE_BEZIER:
                            draw::curve(curves[j].curve->get_curve().get(), .1, colors::calm_blue);
                            break;
                        case GLOBFIT_CURVE_LINE:
                            draw::curve(curves[j].curve->get_curve().get(), .1, colors::red);
                            break;
                        }

                        draw::point(curves[j].curve->get_curve()->pos(0.), .1, Style::fill(colors::black));
                        draw::point(curves[j].curve->get_curve()->pos(1.), .1, Style::fill(colors::black));
                    }
                }
            //}
        }

        //draw_curve_primitives(curves, PrimitiveTypeColorFunctor());

        //for (size_t i = 0; i < curves.size(); ++i) {
            //draw::point(curves[i].curve->get_curve()->pos(1.), .1, Style::fill(colors::black));
        //}
    }

    inline void curve_primitives(const Data& data) {
        draw_curve_primitives(*data.curves, PrimitiveTypeColorFunctor());

        for (size_t i = 0; i < data.curves->size(); ++i) {
            draw::point(data.curves->at(i).curve->get_curve()->pos(1.), .1, Style::fill(colors::black));
        }
    }

    inline void curve_fill_classification(const Data& data) {
        draw_curve_primitives_closed(data.curves_attempts->at(0).attempt[0].primitive_seq.primitives, colors::black);
    }

    inline void curve_fill(const Data& data) {
        draw_curve_primitives_closed(*data.curves, data.color);
    }
}


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_mean

#include <iostream>

#include <boost/test/unit_test.hpp>

#include <limbo/mean/constant.hpp>
#include <limbo/mean/function_ard.hpp>
#include <limbo/mean/null_function.hpp>
#include <limbo/tools/macros.hpp>
#include <limbo/tools/random_generator.hpp>

using namespace limbo;
struct Params {
    struct mean_constant {
        BO_PARAM(double, constant, 1);
    };
};

// Check gradient via finite differences method
template <typename Mean>
std::tuple<double, Eigen::MatrixXd, Eigen::MatrixXd> check_grad(const Mean& mean, const Eigen::VectorXd& x, const Eigen::VectorXd& v, double e = 1e-4)
{
    Eigen::MatrixXd analytic_result, finite_diff_result;
    Mean me = mean;
    me.set_h_params(x);

    analytic_result = me.grad(v, v);

    finite_diff_result = Eigen::MatrixXd::Zero(me(v, v).size(), x.size());
    for (int j = 0; j < x.size(); j++) {
        Eigen::VectorXd test1 = x, test2 = x;
        test1[j] -= e;
        test2[j] += e;
        Mean m1 = mean;
        m1.set_h_params(test1);
        Mean m2 = mean;
        m2.set_h_params(test2);
        Eigen::VectorXd res1 = m1(v, v);
        Eigen::VectorXd res2 = m2(v, v);
        finite_diff_result.col(j) = (res2 - res1).array() / (2.0 * e);
    }

    return std::make_tuple((analytic_result - finite_diff_result).norm(), analytic_result, finite_diff_result);
}

template <typename Mean>
void check_mean(size_t dim_in, size_t dim_out, size_t K)
{
    Mean mean(dim_out);

    for (size_t i = 0; i < K; i++) {
        Eigen::VectorXd hp = tools::random_vector(mean.h_params_size()).array() * 10. - 5.;

        double error;
        Eigen::MatrixXd analytic, finite_diff;

        Eigen::VectorXd v = tools::random_vector(dim_in).array() * 10. - 5.;

        std::tie(error, analytic, finite_diff) = check_grad(mean, hp, v);
        // std::cout << error << ": " << analytic << " vs " << finite_diff << std::endl;
        BOOST_CHECK(error < 1e-6);
    }
}

BOOST_AUTO_TEST_CASE(test_mean_constant)
{
    for (int k = 1; k <= 10; k++) {
        for (int i = 1; i <= 10; i++) {
            check_mean<mean::Constant<Params>>(k, i, 100);
        }
    }
}

BOOST_AUTO_TEST_CASE(test_mean_function_ard)
{
    // This test checks the gradients computation of FunctionARD when the base mean function
    // also has tunable parameters
    for (int k = 1; k <= 10; k++) {
        for (int i = 1; i <= 10; i++) {
            check_mean<mean::FunctionARD<Params, mean::Constant<Params>>>(k, i, 100);
        }
    }
}

BOOST_AUTO_TEST_CASE(test_mean_function_ard_dummy)
{
    // This test checks the gradients computation of FunctionARD when the base mean function
    // has no tunable parameters
    for (int k = 1; k <= 10; k++) {
        for (int i = 1; i <= 10; i++) {
            check_mean<mean::FunctionARD<Params, mean::NullFunction<Params>>>(k, i, 100);
        }
    }
}


//
// Created by sehny on 2/25/16.
//

#include "gk_functions.h"
#include "gk_utils.h"
#include "gk_types.h"
#include "gk_operators.h"

#include <opencv2/core/utility.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>

#include <boost/container/map.hpp>
#include <thread>

using cv::Mat; using cv::Mat_;

namespace gekon {

    population_t normalize(population_t values);

    // I thought it'd be needed. I was wrong.
    template<typename T>
    T sum_vec(std::vector <T> vec) {
        T ret = 0;
        std::for_each(vec.begin(), vec.end(), [&](auto it) { ret += it; });
        return ret;
    }

    population_t first_generation(const size_t size, int ksize) {
        population_t new_population;
        new_population.resize(size);
        for (unsigned int j = 0; j < size; ++j) {
            auto new_kernel = Mat(ksize, ksize, KERNEL_TYPE);
            // generate random matrix with lower bound 0, upper bound 255 and uniform random distribution
            cv::randu(new_kernel, cv::Scalar::all(-20), cv::Scalar::all(20));
            new_population[j] = std::make_pair(0, new_kernel);
        }
        return new_population;
    }

    double fitness_mse(const tr_sample_t sample, const candidate_t candidate) {

        Mat_ <ker_num_t> conv_result;
        filter2D(sample.original, conv_result, -1, candidate, cv::Point(-1, -1), 0, cv::BORDER_CONSTANT);

        double sum_square = 0;
        cv::MatConstIterator_ <ker_num_t> it = conv_result.begin(),
                it_end = conv_result.end(),
                it2 = sample.modified.begin<ker_num_t>(),
                it2_end = sample.modified.end<ker_num_t>();
        for (; it != it_end && it2 != it2_end; ++it, ++it2) {
            sum_square += pow(double(*it) - double(*it2), 2);
        }

        double mse = sqrt(double(conv_result.rows * conv_result.cols) / (sum_square + 0.0001));
        return exp(5 * mse);
    }

    population_t normalize(population_t values) {
        population_t norm;
        double sum = 0;

        for (auto const &val: values) {
            sum += val.first;
        }

        for (auto const &val: values) {
            norm.push_back(std::make_pair(val.first / sum, val.second));
        }

        return norm;
    }

    population_t s_roulette(const population_t prev_population) {

        population_t normPop = normalize(prev_population);
        population_t selPop;

        double sfitt = 0;
        int left = 0;
        if (normPop.size() / 2 % 2 == 0)
            left = normPop.size() / 2;
        else
            left = normPop.size() / 2 + 1;

        for (int i = 0; i < (int) normPop.size(); i++) {
            sfitt += normPop[i].first;
            normPop[i].first = sfitt;
        }

        std::cout << "Selected positions: ";
        for (int i = 0; i < left; i++) {
            double random = gekon::random(0, 1);
            for (unsigned int j = 0; j < normPop.size(); ++j) {
                if (normPop[j].first > random) {
                    selPop.push_back(std::make_pair(normPop[j].first, normPop[j].second));
                    std::cout << j << ", ";
                    break;
                }
            }
        }
        std::cout << std::endl;

        return selPop;
    }

    population_t s_tournament(const population_t prev_population) {
        unsigned int new_gen_size;
        unsigned int prev_gen_size = (unsigned int) prev_population.size();

        if (prev_gen_size / 2 % 2 == 0)
            new_gen_size = prev_gen_size / 2;
        else
            new_gen_size = prev_gen_size / 2 + 1;

        population_t new_gen;
        new_gen.resize(new_gen_size);

        for (unsigned int j = 0; j < new_gen_size; ++j) {
            unsigned int fst = (unsigned int) floor(random(0, prev_gen_size)),
                    snd = (unsigned int) floor(random(0, prev_gen_size));
            new_gen[j] = prev_population[(fst < snd) ? fst : snd];
        }
        return new_gen;
    }

    population_t s_rank_selection(const population_t prev_population) {
        population_t mod_pop; // modified population - replace fitness with reversed index
        size_t pop_size = prev_population.size();
        mod_pop.resize(prev_population.size());
        for (size_t j = 0; j < pop_size; ++j) {
            // now perform the replacement
            mod_pop[j].first = double(pop_size - j);
            mod_pop[j].second = prev_population[j].second;
        }
        population_t ret_pop = s_roulette(mod_pop);
        // use already implemented roulette selection
        for (size_t j = 0; j < ret_pop.size(); ++j) {
            // pick fitness values back from previous population
            size_t prev_index = pop_size - size_t(ret_pop[j].first);
            ret_pop[j].first = prev_population[prev_index].first;
        }
        return ret_pop;
    }

    bool cmp_candidates(const std::pair<double, candidate_t> a, const std::pair<double, candidate_t> b) {
        double fit_a, fit_b;
        fit_a = a.first;
        fit_b = b.first;
        return fit_a > fit_b; //reverse the order because bigger fitness means better cadidate
    }

    static inline double fitness_vec(std::vector <tr_sample_t> samples,
                                     candidate_t candidate,
                                     fitness_fcn_t fit_fcn) {
        double ret_value = 0;
        std::for_each(samples.begin(), samples.end(), [&](auto &iter) {
            ret_value += fit_fcn(iter, candidate);
        });
        ret_value /= double(samples.size());
        return ret_value;
    }

    void fitness(fitness_fcn_t fit_fcn,
                 std::vector <tr_sample_t> samples,
                 population_t &generation,
                 const unsigned int threads_num) {
        size_t vec_size = generation.size();
        unsigned int num_of_threads = threads_num;
        std::vector <size_t> boundaries;

        unsigned int step = (unsigned int) vec_size / num_of_threads;
        for (unsigned int i = 0; i < num_of_threads; ++i) {
            //fixme: last thread has biggest portion
            boundaries.push_back(i * step);
        }
        boundaries.push_back(vec_size);
        std::thread *tt = new std::thread[num_of_threads];

        for (unsigned int i = 0; i < num_of_threads; ++i) {
            tt[i] = std::thread([&](size_t low, size_t high) {
                for (size_t j = low; j < high; ++j) {
                    //generation[j].first = fit_fcn(sample, generation[j].second);
                    generation[j].first = fitness_vec(samples, generation[j].second, fit_fcn);
                }
            }, boundaries[i], boundaries[i + 1]);
        }
        //fixme: main thread is idle
        for (unsigned int i = 0; i < num_of_threads; ++i)
            tt[i].join();

        delete[] tt;
    }

    std::vector <tr_sample_t> load_samples(std::string orig, std::string mod) {
        Mat_ <ker_num_t> mod_img;
        Mat_ <ker_num_t> orig_img;

        Mat imod = cv::imread(mod, cv::IMREAD_UNCHANGED);
        Mat iorig = cv::imread(orig, cv::IMREAD_UNCHANGED);

        if (!iorig.data || !imod.data) {
            std::cout << "Error while reading training samples." << std::endl;
            exit(-1);
        }

        if (imod.channels() != iorig.channels()) {
            std::cerr << "Original and modified images need to have the same number of channels!" << std::endl;
            exit(-1);
        }

        if (imod.channels() == 3) {
            Mat bgr_mod_type[3];
            split(imod, bgr_mod_type);
            Mat bgr_orig_type[3];
            split(iorig, bgr_orig_type);
            Mat bgr_orig[3], bgr_mod[3];
            for (int i = 0; i < 3; ++i) {
                bgr_orig_type[i].convertTo(bgr_orig[i], KERNEL_TYPE, 1 / 255.0);
                bgr_mod_type[i].convertTo(bgr_mod[i], KERNEL_TYPE, 1 / 255.0);
            }
            return {{bgr_orig[0], bgr_mod[0]},
                    {bgr_orig[1], bgr_mod[1]},
                    {bgr_orig[2], bgr_mod[2]}};
        } else if (imod.channels() == 1) {
            imod.convertTo(mod_img, KERNEL_TYPE, 1 / 255.0);
            iorig.convertTo(orig_img, KERNEL_TYPE, 1 / 255.0);
            return {{orig_img, mod_img}};
        } else {
            std::cerr << "Unknown image format with " << imod.channels() << " channels!" << std::endl;
            exit(-1);
        }

    }

    cv::Mat vec2image(std::vector <tr_sample_t> samples, candidate_t kernel) {
        if (samples.size() == 1) {
            Mat conv_result;
            cv::filter2D(samples[0].original, conv_result, -1, kernel, cv::Point(-1, -1), 0, cv::BORDER_CONSTANT);
            return conv_result;
        } else if (samples.size() == 3) {
            std::vector <Mat> img_vec = {
                    samples[0].original,
                    samples[1].original,
                    samples[2].original
            };
            Mat image;
            Mat conv_result;
            cv::merge(img_vec, image);
            cv::filter2D(image, conv_result, -1, kernel, cv::Point(-1, -1), 0, cv::BORDER_CONSTANT);
            return conv_result;
        } else {
            std::cerr << "Cannot construct result image. Wrong number of channels!" << std::endl;
            exit(-1);
        }

    }

    double fitness_ssim(const tr_sample_t sample, const candidate_t candidate) {

        double C1 = 6.5025, C2 = 58.5225;

        IplImage
                *img1 = NULL, *img2 = NULL, *img1_img2 = NULL,
                *img1_temp = NULL, *img2_temp = NULL,
                *img1_sq = NULL, *img2_sq = NULL,
                *mu1 = NULL, *mu2 = NULL,
                *mu1_sq = NULL, *mu2_sq = NULL, *mu1_mu2 = NULL,
                *sigma1_sq = NULL, *sigma2_sq = NULL, *sigma12 = NULL,
                *ssim_map = NULL, *temp1 = NULL, *temp2 = NULL, *temp3 = NULL;

        /***************************** INITS **********************************/

        Mat conv_result;
        filter2D(sample.original, conv_result, -1, candidate, cv::Point(-1, -1), 0, cv::BORDER_CONSTANT);

        CvMat img1_mat = CvMat(conv_result);
        CvMat img2_mat = CvMat(sample.modified);

        img1_temp = cvCreateImage(cvSize(img1_mat.width, img1_mat.height), IPL_DEPTH_8U, 1);
        img2_temp = cvCreateImage(cvSize(img2_mat.width, img2_mat.height), IPL_DEPTH_8U, 1);

        cvConvertImage(&img1_mat, img1_temp);
        cvConvertImage(&img2_mat, img2_temp);

        if (img1_temp == NULL || img2_temp == NULL)
            return -1;

        int x = img1_temp->width, y = img1_temp->height;
        int nChan = img1_temp->nChannels, d = IPL_DEPTH_32F;
        CvSize size = cvSize(x, y);

        img1 = cvCreateImage(size, d, nChan);
        img2 = cvCreateImage(size, d, nChan);

        cvConvert(img1_temp, img1);
        cvConvert(img2_temp, img2);

        img1_sq = cvCreateImage(size, d, nChan);
        img2_sq = cvCreateImage(size, d, nChan);
        img1_img2 = cvCreateImage(size, d, nChan);

        cvPow(img1, img1_sq, 2);
        cvPow(img2, img2_sq, 2);
        cvMul(img1, img2, img1_img2, 1);

        mu1 = cvCreateImage(size, d, nChan);
        mu2 = cvCreateImage(size, d, nChan);

        mu1_sq = cvCreateImage(size, d, nChan);
        mu2_sq = cvCreateImage(size, d, nChan);
        mu1_mu2 = cvCreateImage(size, d, nChan);

        sigma1_sq = cvCreateImage(size, d, nChan);
        sigma2_sq = cvCreateImage(size, d, nChan);
        sigma12 = cvCreateImage(size, d, nChan);

        temp1 = cvCreateImage(size, d, nChan);
        temp2 = cvCreateImage(size, d, nChan);
        temp3 = cvCreateImage(size, d, nChan);

        ssim_map = cvCreateImage(size, d, nChan);
        /*************************** END INITS **********************************/

        //////////////////////////////////////////////////////////////////////////
        // PRELIMINARY COMPUTING
        cvSmooth(img1, mu1, CV_GAUSSIAN, 11, 11, 1.5);
        cvSmooth(img2, mu2, CV_GAUSSIAN, 11, 11, 1.5);

        cvPow(mu1, mu1_sq, 2);
        cvPow(mu2, mu2_sq, 2);
        cvMul(mu1, mu2, mu1_mu2, 1);

        cvSmooth(img1_sq, sigma1_sq, CV_GAUSSIAN, 11, 11, 1.5);
        cvAddWeighted(sigma1_sq, 1, mu1_sq, -1, 0, sigma1_sq);

        cvSmooth(img2_sq, sigma2_sq, CV_GAUSSIAN, 11, 11, 1.5);
        cvAddWeighted(sigma2_sq, 1, mu2_sq, -1, 0, sigma2_sq);

        cvSmooth(img1_img2, sigma12, CV_GAUSSIAN, 11, 11, 1.5);
        cvAddWeighted(sigma12, 1, mu1_mu2, -1, 0, sigma12);

        //////////////////////////////////////////////////////////////////////////
        // FORMULA

        // (2*mu1_mu2 + C1)
        cvScale(mu1_mu2, temp1, 2);
        cvAddS(temp1, cvScalarAll(C1), temp1);

        // (2*sigma12 + C2)
        cvScale(sigma12, temp2, 2);
        cvAddS(temp2, cvScalarAll(C2), temp2);

        // ((2*mu1_mu2 + C1).*(2*sigma12 + C2))
        cvMul(temp1, temp2, temp3, 1);

        // (mu1_sq + mu2_sq + C1)
        cvAdd(mu1_sq, mu2_sq, temp1);
        cvAddS(temp1, cvScalarAll(C1), temp1);

        // (sigma1_sq + sigma2_sq + C2)
        cvAdd(sigma1_sq, sigma2_sq, temp2);
        cvAddS(temp2, cvScalarAll(C2), temp2);

        // ((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2))
        cvMul(temp1, temp2, temp1, 1);

        // ((2*mu1_mu2 + C1).*(2*sigma12 + C2))./((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2))
        cvDiv(temp3, temp1, ssim_map, 1);

        CvScalar index_scalar = cvAvg(ssim_map);

        // through observation, there is approximately
        // 1% error max with the original matlab program

        // if you use this code within a program
        // don't forget to release the IplImages
        cvReleaseImage(&img1);
        cvReleaseImage(&img2);
        cvReleaseImage(&img1_img2);
        cvReleaseImage(&img1_temp);
        cvReleaseImage(&img2_temp);
        cvReleaseImage(&img1_sq);
        cvReleaseImage(&img2_sq);
        cvReleaseImage(&mu1);
        cvReleaseImage(&mu2);
        cvReleaseImage(&mu1_sq);
        cvReleaseImage(&mu2_sq);
        cvReleaseImage(&mu1_mu2);
        cvReleaseImage(&sigma1_sq);
        cvReleaseImage(&sigma2_sq);
        cvReleaseImage(&sigma12);
        cvReleaseImage(&ssim_map);
        cvReleaseImage(&temp1);
        cvReleaseImage(&temp2);
        cvReleaseImage(&temp3);
        return index_scalar.val[0] * 100;
    }
}


#define BOOST_TEST_MODULE test_neg
#include <boost/test/included/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <mave/mave.hpp>
#include <tests/generate_random_matrices.hpp>
#include <tests/tolerance.hpp>

typedef boost::mpl::list<
    mave::vector<double, 3>,    mave::vector<float, 3>,
    mave::matrix<double, 3, 3>, mave::matrix<float, 3, 3>
    > test_targets;

constexpr std::size_t N = 12000;

BOOST_AUTO_TEST_CASE_TEMPLATE(negation_1arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto vectors1 = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; ++i)
    {
        const auto& v1 = vectors1.at(i);
        const auto v2 = -v1;
        for(std::size_t j=0; j<v1.size(); ++j)
        {
            BOOST_TEST(v2[j] == -v1[j],
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(negation_2arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto vectors1 = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=2)
    {
        const auto& v1 = vectors1.at(i);
        const auto& v2 = vectors1.at(i+1);

        const auto neg = -std::tie(v1, v2);
        const auto& n1 = std::get<0>(neg);
        const auto& n2 = std::get<1>(neg);

        for(std::size_t j=0; j<v1.size(); ++j)
        {
            BOOST_TEST(n1[j] == -v1[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(n2[j] == -v2[j],
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
        BOOST_TEST(n1.diagnosis());
        BOOST_TEST(n2.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(negation_3arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto vectors1 = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=3)
    {
        const auto& v1 = vectors1.at(i);
        const auto& v2 = vectors1.at(i+1);
        const auto& v3 = vectors1.at(i+2);

        const auto neg = -std::tie(v1, v2, v3);
        const auto& n1 = std::get<0>(neg);
        const auto& n2 = std::get<1>(neg);
        const auto& n3 = std::get<2>(neg);

        for(std::size_t j=0; j<v1.size(); ++j)
        {
            BOOST_TEST(n1[j] == -v1[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(n2[j] == -v2[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(n3[j] == -v3[j],
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
        BOOST_TEST(v3.diagnosis());
        BOOST_TEST(n1.diagnosis());
        BOOST_TEST(n2.diagnosis());
        BOOST_TEST(n3.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(negation_4arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto vectors1 = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=4)
    {
        const auto& v1 = vectors1.at(i);
        const auto& v2 = vectors1.at(i+1);
        const auto& v3 = vectors1.at(i+2);
        const auto& v4 = vectors1.at(i+3);

        const auto neg = -std::tie(v1, v2, v3, v4);
        const auto& n1 = std::get<0>(neg);
        const auto& n2 = std::get<1>(neg);
        const auto& n3 = std::get<2>(neg);
        const auto& n4 = std::get<3>(neg);

        for(std::size_t j=0; j<v1.size(); ++j)
        {
            BOOST_TEST(n1[j] == -v1[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(n2[j] == -v2[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(n3[j] == -v3[j],
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(n4[j] == -v4[j],
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
        BOOST_TEST(v3.diagnosis());
        BOOST_TEST(v4.diagnosis());
        BOOST_TEST(n1.diagnosis());
        BOOST_TEST(n2.diagnosis());
        BOOST_TEST(n3.diagnosis());
        BOOST_TEST(n4.diagnosis());
    }
}


#ifndef STAN_MATH_PRIM_PROB_BINOMIAL_RNG_HPP
#define STAN_MATH_PRIM_PROB_BINOMIAL_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a pseudorandom binomial random variable for the given population
 * size and chance of success parameters using the specified random number
 * generator.
 *
 * beta can be a scalar or a one-dimensional container.
 *
 * @tparam T_N Type of population size parameter
 * @tparam T_theta Type of change of success parameter
 * @tparam RNG class of rng
 * @param N (Sequence of) population size parameter(s)
 * @param theta (Sequence of) chance of success parameter(s)
 * @param rng random number generator
 * @return (Sequence of) binomial random variate(s)
 * @throw std::domain_error if N is negative
 * @throw std::domain_error if theta is not a valid probability
 */
template <typename T_N, typename T_theta, class RNG>
inline typename VectorBuilder<true, int, T_N, T_theta>::type binomial_rng(
    const T_N& N, const T_theta& theta, RNG& rng) {
  using boost::binomial_distribution;
  using boost::variate_generator;
  using T_N_ref = ref_type_t<T_N>;
  using T_theta_ref = ref_type_t<T_theta>;
  static const char* function = "binomial_rng";
  check_consistent_sizes(function, "Population size parameter", N,
                         "Probability Parameter", theta);
  T_N_ref N_ref = N;
  T_theta_ref theta_ref = theta;
  check_nonnegative(function, "Population size parameter", N_ref);
  check_bounded(function, "Probability parameter", theta_ref, 0.0, 1.0);

  scalar_seq_view<T_N_ref> N_vec(N_ref);
  scalar_seq_view<T_theta_ref> theta_vec(theta_ref);
  size_t M = max_size(N, theta);
  VectorBuilder<true, int, T_N, T_theta> output(M);

  for (size_t m = 0; m < M; ++m) {
    variate_generator<RNG&, binomial_distribution<> > binomial_rng(
        rng, binomial_distribution<>(N_vec[m], theta_vec[m]));

    output[m] = binomial_rng();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


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

#pragma once
#include <boost/filesystem.hpp>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <boost/foreach.hpp>
#include <pcl/io/impl/synchronized_queue.hpp>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>

#include <pcl/search/search.h>
#include <pcl/common/common.h>

#define hypotenuse(x, y) (sqrt((x * x) + (y * y)))
#define slope(x1, y1, x2, y2) ((y2 - y1)/(x2 - x1))

enum class Side: unsigned char{
    LENGTH = 0,
    WIDTH = 1,
};

namespace nimbus
{
    template <class PointType>
    class BoxDetector
    {
        private:
            visualization_msgs::Marker _marker;
            uint32_t _shape = visualization_msgs::Marker::CUBE;
            EIGEN_ALIGN32 Eigen::Matrix<float, 4, 2> cornerBuffer;
            int cornerBufferCounter;
        protected:
            ros::NodeHandle _nh;
            ros::Publisher _pub_marker;
            EIGEN_ALIGN16 Eigen::Matrix3f _covariance_matrix;
            Eigen::Vector4f _centroid;
            Side sideSelect;
            std::vector<std::pair<float, int> > _meanYaw;
        public:
            BoxDetector(ros::NodeHandle nh);
            ~BoxDetector();
            /**
             * @brief Remove the points on the basis of Z Axis distace
             * @param blob Input cloud
             * @param max Maximum distance
             * @param min Minimum distance
             * @param res Resulting Cloud
             */
            void zAxisLimiter(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &blob,
                              double max, double min,
                              pcl::PointCloud<pcl::PointXYZ> &res);

            void outlineRemover(const boost::shared_ptr< const pcl::PointCloud<PointType>> blob,
                                int width, int height, float perW, float perH,
                                pcl::PointCloud<PointType> &res);

            bool getBaseModel(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &blob,
                          const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &raw,
                          double tolerence,
                          const boost::filesystem::path &path,
                          pcl::PointCloud<pcl::PointXYZ> &res);

            void meanFilter(pcl::SynchronizedQueue<pcl::PointCloud<pcl::PointXYZ>> &queue, pcl::PointCloud<pcl::PointXYZ> &res);

            /**
             * @brief Get the Mean Corners object for give number of frames and stores
             * @param queue
             * @param frameSize
             * @brief cornerBuffer: Eigen Matrix of size 4x2 \
             *                \--                 --/
             *                \|Xmin        Y_xMin|/
             *                \|Xmax        Y_xMax|/
             *                \|X_yMin      Ymin  |/
             *                \|X_yMax      Ymax  |/
             *                \--                --
             */
            bool getMeanCorners(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &blob, int frameSize);
            /**
             * @brief Compute the Least-Squares plane fit for a given set of points, using their indices,
             * and return the estimated plane parameters together with the surface curvature.
             * @param blob the input point cloud
             * @param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0)
             * @param curvature the estimated surface curvature as a measure of f[ lambda_0 / (lambda_0 + lambda_1 + lambda_2)]
             */
            bool computePointNormal(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &blob,
                                    Eigen::Vector4f &plane_parameters, float &curvature);

            void box3DCentroid(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &blob,
                               Eigen::Matrix<float, 4, 1> &centroid);

            unsigned int boxCovarianceMatrix (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &blob,
                                          const Eigen::Matrix<float, 4, 1> &centroid,
                                          Eigen::Matrix<float, 3, 3> &covariance_matrix);

            void boxCovarianceMatrixNormalized(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &blob,
                                               const Eigen::Matrix<float, 4, 1> &centroid,
                                               Eigen::Matrix<float, 3, 3> &covariance_matrix);

            unsigned int boxMeanAndCovarianceMatrix(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &blob,
                                                    Eigen::Matrix<float, 3, 3> &covariance_matrix,
                                                    Eigen::Matrix<float, 4, 1> &centroid);

            void solveBoxParameters (const Eigen::Matrix3f &covariance_matrix,
                                       const Eigen::Vector4f &centroid,
                                       Eigen::Vector4f &plane_param, float &curvature);

            void solveBoxParameters (const Eigen::Matrix3f &covariance_matrix,
                                       float &nx, float &ny, float &nz, float &curvature);

            bool boxYaw(const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> &blob,
                        const float width, const float length,
                        const Eigen::Vector4f &centroid,
                        float &yaw);
            void slopeWRTCoordinate(const float x1, const float y1, const float x2, const float y2, float &angle);
            void selectBestCorner(const float diagonal, const Eigen::Matrix<float, 4, 2> corners,
                                  const Eigen::Vector4f &centroid, unsigned int &best);
            void selectSide(const float x1, const float y1, const float x2, const float y2,
                            const float width, const float length, Side &sides);
    };

} // namespace nimbus


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

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include "log.h"

#include "entropymap.h"
#include <algorithm>
#include <opencv2/imgproc.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;
using namespace boost;

#ifdef DEBUG_ENTROPY
#undef DEBUG_ENTROPY
#include <opencv2/imgcodecs.hpp>
#ifdef WIN32
#define DEBUG_FOLDER (std::string("C:/junk/water/"))
#else
#define DEBUG_FOLDER (std::string("/var/tmp/water/"))
#endif
#endif

#ifndef DO_TIME_TEST
#define DO_TIME_TEST
#include <chrono>
#endif

using namespace cv;
using namespace std;
using namespace boost;

static const int ENTROPY_MAP_KERNEL_SIZE_MIN = 3;
static const int ENTROPY_MAP_KERNEL_SIZE_MAX = 1024;

#ifdef DO_TIME_TEST
string GetNowString()
{
    chrono::system_clock::time_point now = chrono::system_clock::now();
    time_t now_c = chrono::system_clock::to_time_t( now );
    tm now_tm = *std::localtime( &now_c );
    char timeStamp[ 64 ];
    strftime( timeStamp, 64, "%Y-%m-%dT%H:%M:%S", &now_tm );
    return string( timeStamp );
}
#endif

namespace gc
{

EntropyMap::EntropyMap()
{
}
EntropyMap::~EntropyMap()
{
}
GC_STATUS EntropyMap::CalcMap( const Mat &src, Mat &dst, const int kernelSize, const bool useEllipse )
{
    GC_STATUS retVal = src.empty() ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[" << __func__ << "][EntropyMap::CalcMap] Cannot calculate entropy map for empty image";
        retVal = GC_ERR;
    }
    else
    {
        if ( 3 > kernelSize )
        {
            FILE_LOG( logERROR ) << "[" << __func__ << "][EntropyMap::CalcMap] Cannot calculate entropy map with kernSize less than 3";
            retVal = GC_ERR;
        }
        else
        {
            try
            {
#ifdef DEBUT_ENTROPY
                imwrite( DEBUG_FOLDER + string("__gray_src.png"), src );
#endif
                Mat ellipseMask = Mat::zeros( kernelSize, kernelSize, CV_8UC1 );
                if ( useEllipse )
                {
                    int kernHalf = kernelSize >> 1;
                    ellipse( ellipseMask, Point( kernHalf, kernHalf ), Size( kernelSize, kernelSize ),
                             0.0, 0.0, 360.0, Scalar( 255 ), FILLED );
                }
                else
                {
                    ellipseMask.setTo( 255 );
                }
                Mat scratch = Mat::zeros( src.size(), CV_32FC1 );
                retVal = CalcTile( src, scratch, kernelSize, ellipseMask );
                if ( GC_OK == retVal )
#if 1
                    scratch.copyTo( dst );
#else
                    scratch( Rect( 0, 0, scratch.cols >> 2, scratch.rows >> 2 ) ).copyTo( dst );
#endif

#ifdef DEBUT_ENTROPY
                imwrite( DEBUG_FOLDER + string("__morph_8u.png"), dst );
#endif
            }
            catch( Exception &e )
            {
                FILE_LOG( logERROR ) << "[" << __func__ << "][EntropyMap::CalcMap] " << e.what();
                retVal = GC_EXCEPT;
            }
        }
    }
    return retVal;
}
GC_STATUS EntropyMap::CalcTile( const Mat &src, Mat &dst, const int kernelSize, const cv::Mat &mask )
{
    GC_STATUS retVal = src.empty() ? GC_ERR : GC_OK;
    if ( GC_OK != retVal )
    {
        FILE_LOG( logERROR ) << "[" << __func__ << "][EntropyMap::CalcTile] Cannot calculate entropy map for empty image";
        retVal = GC_ERR;
    }
    else
    {
        if ( ENTROPY_MAP_KERNEL_SIZE_MIN > kernelSize ||
             ENTROPY_MAP_KERNEL_SIZE_MAX < kernelSize )
        {
            FILE_LOG( logERROR ) << "[" << __func__ << "][EntropyMap::CalcTile] Invalid entropy map with kernSize=" << kernelSize \
                                 << ". Must be in range " << ENTROPY_MAP_KERNEL_SIZE_MIN << " to " << ENTROPY_MAP_KERNEL_SIZE_MAX << ".";
            retVal = GC_ERR;
        }
        try
        {
            int kernHalf = std::min( std::min( src.rows, src.cols ), kernelSize ) >> 1;

            if ( !mask.empty() )
            {
#ifdef DEBUT_ENTROPY
                imwrite(DEBUG_FOLDER + string("quarter_src.png"), src );
#endif
                float *pPix32f;
                for ( int row = kernHalf, centerRow = 0, dstRow = kernHalf >> 2;
                      row < src.rows - kernHalf; row += 4, centerRow += 4, ++dstRow )
                {
                    pPix32f = reinterpret_cast< float * >( dst.ptr( dstRow ) );
                    for ( int col = kernHalf, centerCol = 0, dstCol = kernHalf >> 2;
                          col < src.cols - kernHalf; col += 4, centerCol += 4, ++dstCol )
                    {
                        retVal = CalcEntropyValue( src( Rect( centerCol, centerRow, kernelSize, kernelSize ) ), pPix32f[ dstCol ], mask );
                    }
                }
#ifdef DEBUT_ENTROPY
                imwrite( DEBUG_FOLDER + "quarter_dst.png", dst );
#endif
            }
            else
            {
                FILE_LOG( logERROR ) << "[" << __func__ << "][EntropyMap::CalcTile] Has no mask";
            }
        }
        catch( Exception &e )
        {
            FILE_LOG( logERROR ) << "[" << __func__ << "][EntropyMap::CalcTile] " << e.what();
            retVal = GC_EXCEPT;
        }
    }

    return retVal;
}
GC_STATUS EntropyMap::CalcEntropyValue( const Mat &img, float &entropy, const Mat &mask )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        size_t histSize = 32;
        // apply the mask
        Mat imgLocal;
        img.copyTo( imgLocal, mask );

        // do simpler special hist
        std::vector< int > bins( histSize, 0 );// close to zero but finite
        unsigned char * pRow;
        for (int row = 0; row < imgLocal.rows; row++)
        {
            pRow = reinterpret_cast< unsigned char * >( imgLocal.ptr( row ) );
            for ( int col = 0; col < imgLocal.cols; col++ )
            {
                ++bins[ pRow[ col ] >> 3 ];    // [0...255] --> [0...63]
            }
        }
        int count = 0;
        for ( size_t i = 0; i < histSize; ++i )
        {
            if ( bins[ i ] )
                ++count;
        }

//        entropy = log( static_cast< float > ( count ) );
        entropy = static_cast< float >( count );
    }
    catch (Exception &e)
    {
        FILE_LOG(logERROR) << "[" << __func__ << "][EntropyMap::CalcEntropyValue] " << e.what();
        retVal = GC_EXCEPT;
    }

    return retVal;
}
GC_STATUS EntropyMap::BuildLogLUT( const Mat &mask, vector< float > &lut )
{
    GC_STATUS retVal = GC_OK;
    try
    {
        int count = cvRound( sum( mask ).val[ 0 ] / 255.0 );

        lut.clear();
        lut.push_back( 0.0f );
        for ( int i = 1; i < count; ++i )
            lut.push_back( static_cast< float >( log( static_cast< double >( i ) / static_cast< double >( count ) ) ) );
    }
    catch( Exception &e )
    {
        FILE_LOG( logERROR ) << "[" << __func__ << "][EntropyMap::BuildLogLUT] " << e.what();
        retVal = GC_EXCEPT;
    }

    return retVal;
}

}    // namespace gc


#include "parser.h"

#include <boost/spirit/home/x3.hpp>

namespace x3 = boost::spirit::x3;

namespace new_math {

x3::rule<class expression, uint64_t> const expression = "expression";
x3::rule<class term, uint64_t> const term = "term";

auto const set = [](auto &context) { x3::_val(context) = x3::_attr(context); };
auto const add = [](auto &context) { x3::_val(context) += x3::_attr(context); };
auto const mul = [](auto &context) { x3::_val(context) *= x3::_attr(context); };

auto const expression_def = term[set] >>
                            *(('+' >> term[add]) | ('*' >> term[mul]));
auto const term_def = x3::uint_ | ('(' >> expression >> ')');

BOOST_SPIRIT_DEFINE(expression, term);

} // namespace new_math

namespace advanced_math {

auto const set = [](auto &context) { x3::_val(context) = x3::_attr(context); };
auto const add = [](auto &context) { x3::_val(context) += x3::_attr(context); };
auto const mul = [](auto &context) { x3::_val(context) *= x3::_attr(context); };

x3::rule<class expression, uint64_t> const expression = "expression";
x3::rule<class term, uint64_t> const term = "term";
x3::rule<class factor, uint64_t> const factor = "factor";

auto const expression_def = factor[set] >> *('*' >> factor[mul]);
auto const factor_def = term[set] >> *('+' >> term[add]);
auto const term_def = x3::uint_ | ('(' >> expression >> ')');

BOOST_SPIRIT_DEFINE(expression, term, factor);

} // namespace advanced_math

uint64_t parse_expression(std::string input, Math math) {
  uint64_t result = 0;

  auto first = input.begin();
  auto last = input.end();

  auto passed = math == Math::New
                    ? x3::phrase_parse(first, last, new_math::expression,
                                       x3::space, result)
                    : x3::phrase_parse(first, last, advanced_math::expression,
                                       x3::space, result);

  if (not passed || first != last) {
    throw std::invalid_argument{"parse error"};
  }

  return result;
}


/**
 * @file   ProteinSegmentEnsemble.hpp
 * @author see AUTHORS
 * @brief  ProteinSegmentEnsemble header file.
 */

#ifndef PROTEINSEGMENTENSEMBLE_HPP
#define PROTEINSEGMENTENSEMBLE_HPP

#include <vector>

#include <Eigen/Dense>

#include "Averager.hpp"
#include "ProteinSegment.hpp"
#include "Frame.hpp"
#include "FrameSegment.hpp"

/**
 * @class ProteinSegmentEnsemble
 */
class ProteinSegmentEnsemble {
 public:
    static int add_frame_counter;

    /**
     * @param protein_segment
     * @sideeffect will change the protein segments upon creation.
     */
    explicit ProteinSegmentEnsemble(const std::shared_ptr<ProteinSegment> & protein_segment);

    ~ProteinSegmentEnsemble();

    /**
     * @param frame
     */
    void add_frame(const Frame & frame);

    /**
     * @param temparature
     */
    void compute_force_constant(double temperature);

 private:
    std::shared_ptr<ProteinSegment> protein_segment;
    Averager<Eigen::MatrixXd> covariance_averager;
    Averager<Eigen::VectorXd> displacement_vector_averager;
    Eigen::MatrixXd force_constant;
    FrameSegment frame_segment;
};

#endif

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4


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

#include <boost/hana/assert.hpp>
#include <boost/hana/ext/boost/mpl/vector.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/type.hpp>

#include <boost/mpl/vector.hpp>
#include <type_traits>
using namespace boost::hana;
namespace mpl = boost::mpl;

int main() {

{

//! [comparable]
BOOST_HANA_CONSTANT_CHECK(
    equal(mpl::vector2<int, char>{}, mpl::vector<int, char>{})
);
BOOST_HANA_CONSTANT_CHECK(
    not_equal(mpl::vector2<int, char>{}, mpl::vector<int, char, float>{})
);
//! [comparable]

}{

//! [foldable]
auto types = mpl::vector<long, float, short, float, long, long double>{};
auto number_of_floats = foldl(types, int_<0>, [](auto count, auto t) {
    return if_(trait<std::is_floating_point>(t),
        count + int_<1>,
        count
    );
});

BOOST_HANA_CONSTANT_CHECK(number_of_floats == int_<3>);
//! [foldable]

}{

//! [iterable]
BOOST_HANA_CONSTANT_CHECK(head(mpl::vector<int, char, void>{}) == type<int>);

BOOST_HANA_CONSTANT_CHECK(equal(
    tail(mpl::vector<int, char, void>{}),
    mpl::vector<char, void>{}
));

BOOST_HANA_CONSTANT_CHECK(equal(
    drop_while(mpl::vector<float, double const, int, float&>{},
               trait<std::is_floating_point>),
    mpl::vector<int, float&>{}
));
//! [iterable]

}{

//! [searchable]
BOOST_HANA_CONSTANT_CHECK(
    find_if(mpl::vector<int, float, char const*>{}, equal.to(type<float>))
    ==
    just(type<float>)
);

BOOST_HANA_CONSTANT_CHECK(
    find(mpl::vector<int, float, char const*>{}, type<void>)
    ==
    nothing
);
//! [searchable]

}

}