#pragma once

#include <vector>
#include <boost/numeric/odeint.hpp>
#include <cmath>
#include <ros/ros.h>
# define M_PI           3.14159265358979323846  /* pi */
#define toRadian( x )	( (x) / 180 * M_PI )
#define toDegree( x )	( (x) / M_PI * 180 )

namespace cardsflow_gazebo
{
    using namespace std;
	using namespace boost::numeric::odeint;
	//using namespace gazebo;
/*
    struct tendonType {
		vector<ignition::math::Vector3> MidPoint;
		vector<ignition::math::Vector3> Vector;
		//might need it to calculate length
		vector<ignition::math::Vector3> Orientation;
		vector<double> Pitch;
		vector<double> Roll;
	};
*/
	struct SEE {
		double stiffness = 30680.0; // N/m
		double length = 0.056; //m
		double expansion = 0.0; //m
		double force = 0.0;  //
		double length0 = 0; //m
	};

    class ISee {

		// c1-c3 are constand values, c4 is x0. documentation can be found at ____________________
		double c1 = 0.012, c2 = 0.008, c3 = 0.018, c4 = 0.039; //m
		// the angles alpha_* describe the angle the tendons attach to the see.element
		double alpha_1 = std::atan( c1 / c4 ), alpha_2 = std::atan( c2 /  (c3+c4) ); // radian
		// the angles beta_* describe the third angles of the corresponding triangles
		double beta_1  =  M_PI / 4 - alpha_1, beta_2 = M_PI / 4 - alpha_2; // radian
		// length_* is the tendonlength from the two triangles inside the motor
		double length_1 = sqrt( c1*c1 + c4*c4 ), length_2 = sqrt( c2*c2 + (c3+c4)*(c3+c4) ); //m
		// length_c* are constand tendonlengths inside the motor
		double length_c1 = 0.04, length_c2 = 0.013; //m
		// Tendon stiffness (this is a random high number. A real number still has to be set)
		double tendonStiffness = 1e6; // N/m
		double tendonForce = 0;

       public:
		//deltaX is the displacement of the spring inside the motor
		double deltaX = 0.0; //m
		// the Length of the tendon inside the motor. the internal length changes depending on the displacement of the spring.
		double internalLength = length_c1 + length_1 + length_2 + length_c2; //m
        SEE see;

        ISee();

		///////////////////////////////////////
		/// \brief Calculate elastic force of the series elastic element
		/// \param[in] The tandonLength represents the length of the entire tendon.from the motor to the last viapoint.
		/// \param[in] The muscleLength representes the length of the tendon forn the outside of the motor to the last viapoint.
		void ElasticElementModel(const double &tendonLength, const double &muscleLength);

		///////////////////////////////////////
		/// \brief apply the springForce onto the tendons going to motor and out the muscle. The force depends on the angle the tendons have towards the spring
		/// \parm[in] the force going out the muscle
		/// \parm[in] the force going toward the motor
		void applyTendonForce( double &_muscleForce , double &_actuatorForce );
	};
}


/*
 [auto_generated]
 libs/numeric/odeint/examples/bind_member_functions.hpp

 [begin_description]
 tba.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

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

#include <iostream>
#include <array>
#include <type_traits>

#include <boost/numeric/odeint.hpp>

namespace odeint = boost::numeric::odeint;

typedef std::array< double , 3 > state_type;

struct lorenz
{
    void ode( const state_type &x , state_type &dxdt , double t ) const
    {
        const double sigma = 10.0;
        const double R = 28.0;
        const double b = 8.0 / 3.0;

        dxdt[0] = sigma * ( x[1] - x[0] );
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = -b * x[2] + x[0] * x[1];
    }
};

int main( int argc , char *argv[] )
{
    using namespace boost::numeric::odeint;
    //[ bind_member_function_cpp11
    namespace pl = std::placeholders;

    state_type x = {{ 10.0 , 10.0 , 10.0 }};
    integrate_const( runge_kutta4< state_type >() ,
                     std::bind( &lorenz::ode , lorenz() , pl::_1 , pl::_2 , pl::_3 ) ,
                     x , 0.0 , 10.0 , 0.01  );
    //]
    return 0;
}


#pragma once

#include <Eigen/Dense>

#include "Point.hpp"

namespace Geometry2d {
// A 2x3 transformation matrix.
//
// This is the 2D equivalent of the usual 3D tranformation matrix with the
// bottom row omitted because the bottom (third) element of a 2D point is always
// 1.  The third row of the matrix is understood to be [0 0 1].
class TransformMatrix {
public:
    TransformMatrix() {
        _m[0] = 1;
        _m[1] = 0;
        _m[2] = 0;
        _m[3] = 0;
        _m[4] = 1;
        _m[5] = 0;
    }

    TransformMatrix(float a, float b, float c, float d, float e, float f) {
        _m[0] = a;
        _m[1] = b;
        _m[2] = c;
        _m[3] = d;
        _m[4] = e;
        _m[5] = f;
    }

    TransformMatrix(Geometry2d::Point origin, float rotation = 0,
                    bool mirror = false, float scale = 1);

    TransformMatrix(const Eigen::Matrix<double, 3, 3>& other) {
        _m[0] = other(0, 0);
        _m[1] = other(0, 1);
        _m[2] = other(0, 2);
        _m[3] = other(1, 0);
        _m[4] = other(1, 1);
        _m[5] = other(1, 2);
    }

    TransformMatrix operator*(const TransformMatrix& other) const {
        float a = _m[0] * other._m[0] + _m[1] * other._m[3];
        float b = _m[0] * other._m[1] + _m[1] * other._m[4];
        float c = _m[0] * other._m[2] + _m[1] * other._m[5] + _m[2];
        float d = _m[3] * other._m[0] + _m[4] * other._m[3];
        float e = _m[3] * other._m[1] + _m[4] * other._m[4];
        float f = _m[3] * other._m[2] + _m[4] * other._m[5] + _m[5];

        return TransformMatrix(a, b, c, d, e, f);
    }

    TransformMatrix& operator*=(const TransformMatrix& other) {
        float a = _m[0] * other._m[0] + _m[1] * other._m[3];
        float b = _m[0] * other._m[1] + _m[1] * other._m[4];
        float c = _m[0] * other._m[2] + _m[1] * other._m[5] + _m[2];
        float d = _m[3] * other._m[0] + _m[4] * other._m[3];
        float e = _m[3] * other._m[1] + _m[4] * other._m[4];
        float f = _m[3] * other._m[2] + _m[4] * other._m[5] + _m[5];

        _m[0] = a;
        _m[1] = b;
        _m[2] = c;
        _m[3] = d;
        _m[4] = e;
        _m[5] = f;

        return *this;
    }

    operator Eigen::Matrix<double, 3, 3>() const {
        Eigen::Matrix<double, 3, 3> result;
        result << _m[0], _m[1], _m[2], _m[3], _m[4], _m[5], 0, 0, 1;
        return result;
    }

    Point operator*(const Point& pt) const {
        return Point(pt.x() * _m[0] + pt.y() * _m[1] + _m[2],
                     pt.x() * _m[3] + pt.y() * _m[4] + _m[5]);
    }

    // Transforms a direction vector (3rd element is zero)
    Point transformDirection(const Point& dir) const {
        return Point(dir.x() * _m[0] + dir.y() * _m[1],
                     dir.x() * _m[3] + dir.y() * _m[4]);
    }

    // Transforms the given angle in radians
    float transformAngle(float angle) const;

    // Returns the vector that represents the direction of the transformed
    // X-axis.
    Point x() const { return Point(_m[0], _m[3]); }

    // Returns the vector that represents the direction of the transformed
    // Y-axis.
    Point y() const { return Point(_m[1], _m[4]); }

    // Returns the origin of the transformed coordinate system
    Point origin() const { return Point(_m[2], _m[5]); }

    // Returns the scaling along the transformed X-axis.
    float xScale() const { return x().mag(); }

    // Returns the scaling along the transformed Y-axis.
    float yScale() const { return y().mag(); }

    // Returns the clockwise angle from the transformed Y axis to the original Y
    // axis. This is not affected by horizontal reflection.
    float rotation() const;

    // Returns true if the coordinate system has been mirrored (i.e. is now
    // left-handed).
    bool mirrored() const;

    const float* m() const { return _m; }

    ////////////////
    // Functions to build common transformations:

    // Translation
    static TransformMatrix translate(const Point& delta) {
        return TransformMatrix(1, 0, delta.x(), 0, 1, delta.y());
    }

    static TransformMatrix translate(float x, float y) {
        return translate(Point(x, y));
    }

    // Rotation in radians around origin
    static TransformMatrix rotate(float angle) {
        float c = cos(angle);
        float s = sin(angle);

        return TransformMatrix(c, -s, 0, s, c, 0);
    }

    // Uniform scale
    static TransformMatrix scale(float s) {
        return TransformMatrix(s, 0, 0, 0, s, 0);
    }

    // Non-uniform scale
    static TransformMatrix scale(float x, float y) {
        return TransformMatrix(x, 0, 0, 0, y, 0);
    }

    // Returns a matrix to rotate <angle> radians CCW around <center>.
    static TransformMatrix rotateAroundPoint(const Point& center, float angle);

    // Returns a matrix to reflect along the line parallel to the Y-axis
    // containing <center>.
    static TransformMatrix mirrorAroundPoint(const Point& center);

    static const TransformMatrix identity;
    static const TransformMatrix mirrorX;

protected:
    // Matrix values in row-major order.
    //
    // Indices:
    //   [0 1 2
    //    3 4 5]
    float _m[6];
};
}  // namespace Geometry2d


/*
 * Shared copyright notice and LGPLv3 license statement.
 *
 * Copyright (C) 2010 The Board of Trustees of The Leland Stanford Junior University. All rights reserved.
 * Copyright (C) 2010 University of Texas at Austin. All rights reserved.
 *
 * Authors: Roland Philippsen (Stanford) and Luis Sentis (UT Austin)
 *          http://cs.stanford.edu/group/manips/
 *          http://www.me.utexas.edu/~hcrl/
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program.  If not, see
 * <http://www.gnu.org/licenses/>
 */

#include <utils/pseudo_inverse.hpp>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <stdio.h>

using namespace std;

namespace sejong {

    void pseudoInverse(Matrix const & matrix,
                       double sigmaThreshold,
                       Matrix & invMatrix,
                       Vector * opt_sigmaOut)    {

        if ((1 == matrix.rows()) && (1 == matrix.cols())) {
            // workaround for Eigen2
            invMatrix.resize(1, 1);
            if (matrix.coeff(0, 0) > sigmaThreshold) {
                invMatrix.coeffRef(0, 0) = 1.0 / matrix.coeff(0, 0);
            }
            else {
                invMatrix.coeffRef(0, 0) = 0.0;
            }
            if (opt_sigmaOut) {
                opt_sigmaOut->resize(1);
                opt_sigmaOut->coeffRef(0) = matrix.coeff(0, 0);
            }
            return;
        }

        Eigen::JacobiSVD<Matrix> svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
        // not sure if we need to svd.sort()... probably not
        int const nrows(svd.singularValues().rows());
        Matrix invS;
        invS = Matrix::Zero(nrows, nrows);
        for (int ii(0); ii < nrows; ++ii) {
            if (svd.singularValues().coeff(ii) > sigmaThreshold) {
                invS.coeffRef(ii, ii) = 1.0 / svd.singularValues().coeff(ii);
            }
            else{
                // invS.coeffRef(ii, ii) = 1.0/ sigmaThreshold;
                // printf("sigular value is too small: %f\n", svd.singularValues().coeff(ii));
            }
        }
        invMatrix = svd.matrixV() * invS * svd.matrixU().transpose();
        if (opt_sigmaOut) {
            *opt_sigmaOut = svd.singularValues();
        }
    }

}


#pragma once

#include <armadillo>

class Composition;

namespace utils
{
    double calculateHeatCapacityConstantVolumeJFH(
            const double pressure);

    double calculateHeatCapacityConstantVolumeTGNet(
            const double molarMass,
            const double pressure,
            const double temperature);

    double calculateHeatCapacityConstantPressureJFH(
            const double molarMass,
            const double pressure,
            const double temperature);

    double calculateHeatCapacityConstantPressureLangelandsvik(
            const double molarMass,
            const double pressure,
            const double temperature);

    double calculateHeatCapacityConstantPressureTGNet(
            const double molarMassOfMixture,
            const double pressure,
            const double temperature);

    double calculateHeatCapacityConstantPressureKIO(
            const Composition& comp,
            const double pressure,
            const double temperature);

    double calculateIsobaricHeatCapacityJKH(
            const Composition& comp,
            const double pressure,
            const double temperature,
            const double Z = 0);

    arma::vec calculateViscosity(
            const arma::vec& molarMass,
            const arma::vec& temperature,
            const arma::vec& density);

    arma::vec calculateReynoldsNumber(
            const arma::vec& massFlow,
            const arma::vec& diameter,
            const arma::vec& viscosity);

    arma::vec calculateColebrookWhiteFrictionFactor(
            const arma::vec& sandGrainEquivalentRoughness,
            const arma::vec& diameter,
            const arma::vec& reynoldsNumber);

    double calculateColebrookWhiteFrictionFactor(
            const double sandGrainEquivalentRoughness,
            const double diameter,
            const double reynoldsNumber);

    double calculateHaalandFrictionFactor(
            const double sandGrainEquivalentRoughness,
            const double diameter,
            const double reynoldsNumber);

    namespace details
    {
        double KIOidealGasCP(
                const double specificGravity,
                const double temperature);

        double KIOdimensionlessResidualCP(
                const double reducedPressure,
                const double reducedTemperature);

        double JKHdimensionlessCP(
                const double molarMass,
                const double H2S,
                const double CO2,
                const double N2,
                const double H2,
                const double H2O,
                const double pressure,
                const double temperature,
                const double compressibility);

        double JKHidealGasCP(
                const Composition& comp,
                const double specificGravity,
                const double temperature);

        double JKHidealGasCP(
                const double specificGravity,
                const double H2S,
                const double CO2,
                const double N2,
                const double H2,
                const double H2O,
                const double temperature);

        double calculateHeatCapacityConstantPressureKIO(
                const Composition& comp,
                const double H2S,
                const double pressure,
                const double temperature);

        double colebrookWhiteFrictionFactor(
                const double sandGrainEquivalentRoughness,
                const double diameter,
                const double reynoldsNumber);

        double colebrookWhite(
                const double f,
                const double sandGrainEquivalentRoughness,
                const double diameter,
                const double reynoldsNumber);

        double colebrookWhiteDerivative(
                const double f,
                const double sandGrainEquivalentRoughness,
                const double diameter,
                const double reynoldsNumber);
    }
}


#include <Eigen/Core>
#include <functional>

#include "update_ops_cpp.hpp"
#include "utility.hpp"

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list,
    UINT target_qubit_index_count, const CTYPE* matrix, CTYPE* state,
    ITYPE dim) {
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(
        target_qubit_index_list, target_qubit_index_count);
    Eigen::Map<const Eigen::Matrix<std::complex<double>, Eigen::Dynamic,
                   Eigen::Dynamic, Eigen::RowMajor>,
        Eigen::Aligned>
        eigen_matrix((std::complex<double>*)matrix, matrix_dim, matrix_dim);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state =
        reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(
        target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create base index
        ITYPE basis_0 = state_index;
        for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(
                basis_0, 1ULL << insert_index, insert_index);
        }

        // fetch vector
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list,
    UINT target_qubit_index_count,
    const Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic,
        Eigen::RowMajor>& eigen_matrix,
    CTYPE* state, ITYPE dim) {
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(
        target_qubit_index_list, target_qubit_index_count);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state =
        reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(
        target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create base index
        ITYPE basis_0 = state_index;
        for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(
                basis_0, 1ULL << insert_index, insert_index);
        }

        // fetch vector
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list,
    UINT target_qubit_index_count, const Eigen::MatrixXcd& eigen_matrix,
    CTYPE* state, ITYPE dim) {
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(
        target_qubit_index_list, target_qubit_index_count);
    std::complex<double>* cppstate =
        reinterpret_cast<std::complex<double>*>(state);
    Eigen::VectorXcd buffer(matrix_dim);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(
        target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create base index
        ITYPE basis_0 = state_index;
        for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(
                basis_0, 1ULL << insert_index, insert_index);
        }

        // fetch vector
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            buffer[y] = cppstate[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            cppstate[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_sparse_matrix_gate_eigen(const UINT* target_qubit_index_list,
    UINT target_qubit_index_count,
    const Eigen::SparseMatrix<std::complex<double>>& eigen_matrix, CTYPE* state,
    ITYPE dim) {
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(
        target_qubit_index_list, target_qubit_index_count);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state =
        reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(
        target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create base index
        ITYPE basis_0 = state_index;
        for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(
                basis_0, 1ULL << insert_index, insert_index);
        }

        // fetch vector
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for (ITYPE y = 0; y < matrix_dim; ++y) {
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/

/** Example partys_height_average.cpp \file partys_height_average.cpp
    \brief Using <i>aggregate on overlap</i> a history of height averages of
           party guests is computed.

    In partys_height_average.cpp we compute yet another aggregation:
    The average height of guests as it changes over time. This is done by
    defining a class counted_sum that sums up heights and counts the number
    of guests via an operator +=.

    Based on the operator += we can aggregate counted sums on addition
    of interval value pairs into an interval_map.

    \include partys_height_average_/partys_height_average.cpp
*/
//[example_partys_height_average
// The next line includes <boost/date_time/posix_time/posix_time.hpp>
// and a few lines of adapter code.
#include <boost/icl/ptime.hpp>
#include <iostream>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost::posix_time;
using namespace boost::icl;

class counted_sum
{
public:
    counted_sum():_sum(0),_count(0){}
    counted_sum(int sum):_sum(sum),_count(1){}

    int sum()const  {return _sum;}
    int count()const{return _count;}
    double average()const{ return _count==0 ? 0.0 : _sum/static_cast<double>(_count); }

    counted_sum& operator += (const counted_sum& right)
    { _sum += right.sum(); _count += right.count(); return *this; }

private:
    int _sum;
    int _count;
};

bool operator == (const counted_sum& left, const counted_sum& right)
{ return left.sum()==right.sum() && left.count()==right.count(); }

void partys_height_average()
{
    interval_map<ptime, counted_sum> height_sums;

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        counted_sum(165)); // Mary is 1,65 m tall.

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        counted_sum(180)); // Harry is 1,80 m tall.

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        counted_sum(170)); // Diana is 1,70 m tall.

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        counted_sum(165)); // Susan is 1,65 m tall.

    height_sums +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 22:15"),
          time_from_string("2008-05-21 00:30")),
        counted_sum(200)); // Peters height is 2,00 m

    interval_map<ptime, counted_sum>::iterator height_sum_ = height_sums.begin();
    cout << "-------------- History of average guest height -------------------\n";
    while(height_sum_ != height_sums.end())
    {
        discrete_interval<ptime> when = height_sum_->first;

        double height_average = (*height_sum_++).second.average();
        cout << setprecision(3)
             << "[" << first(when) << " - " << upper(when) << ")"
             << ": " << height_average <<" cm = " << height_average/30.48 << " ft" << endl;
    }
}

int main()
{
    cout << ">>Interval Container Library: Sample partys_height_average.cpp  <<\n";
    cout << "------------------------------------------------------------------\n";
    partys_height_average();
    return 0;
}

// Program output:
/*-----------------------------------------------------------------------------
>>Interval Container Library: Sample partys_height_average.cpp  <<
------------------------------------------------------------------
-------------- History of average guest height -------------------
[2008-May-20 19:30:00 - 2008-May-20 20:10:00): 173 cm = 5.66 ft
[2008-May-20 20:10:00 - 2008-May-20 22:15:00): 170 cm = 5.58 ft
[2008-May-20 22:15:00 - 2008-May-20 23:00:00): 176 cm = 5.77 ft
[2008-May-20 23:00:00 - 2008-May-21 00:00:00): 178 cm = 5.85 ft
[2008-May-21 00:00:00 - 2008-May-21 00:30:00): 200 cm = 6.56 ft
-----------------------------------------------------------------------------*/
//]


// Copyright © 2018 Thomas Nagler
//
// This file is part of the wdm library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory
// or https://github.com/tnagler/wdmcpp/blob/master/LICENSE.

#pragma once

#include <Eigen/Dense>
#include "../wdm.hpp"

namespace wdm {

namespace utils {

    inline std::vector<double> convert_vec(const Eigen::VectorXd& x)
    {
        std::vector<double> xx(x.size());
        if (x.size() > 0)
            Eigen::VectorXd::Map(&xx[0], x.size()) = x;
        return xx;
    }

}

//! calculates (weighted) dependence measures.
//! @param x, y input data.
//! @param method the dependence measure; see details for possible values.
//! @param weights an optional vector of weights for the data.
//! @param remove_missing if `true`, all observations containing a `nan` are
//!    removed; otherwise throws an error if `nan`s are present.
//! @details
//! Available methods:
//!   - `"pearson"`, `"prho"`, `"cor"`: Pearson correlation
//!   - `"spearman"`, `"srho"`, `"rho"`: Spearman's \f$ \rho \f$
//!   - `"kendall"`, `"ktau"`, `"tau"`: Kendall's \f$ \tau \f$
//!   - `"blomqvist"`, `"bbeta"`, `"beta"`: Blomqvist's \f$ \beta \f$
//!   - `"hoeffding"`, `"hoeffd"`, `"d"`: Hoeffding's \f$ D \f$
//!
//! @return the dependence measure
inline double wdm(const Eigen::VectorXd& x,
                  const Eigen::VectorXd& y,
                  std::string method,
                  Eigen::VectorXd weights = Eigen::VectorXd(),
                  bool remove_missing = true)
{
    return wdm(utils::convert_vec(x),
               utils::convert_vec(y),
               method,
               utils::convert_vec(weights),
               remove_missing);
}

//! calculates a matrix of (weighted) dependence measures.
//! @param x input data.
//! @param method the dependence measure; see details for possible values.
//! @param weights an optional vector of weights for the data.
//! @param remove_missing if `true`, all observations containing a `nan` are
//!    removed; otherwise throws an error if `nan`s are present.
//! @details
//! Available methods:
//!   - `"pearson"`, `"prho"`, `"cor"`: Pearson correlation
//!   - `"spearman"`, `"srho"`, `"rho"`: Spearman's \f$ \rho \f$
//!   - `"kendall"`, `"ktau"`, `"tau"`: Kendall's \f$ \tau \f$
//!   - `"blomqvist"`, `"bbeta"`, `"beta"`: Blomqvist's \f$ \beta \f$
//!   - `"hoeffding"`, `"hoeffd"`, `"d"`: Hoeffding's \f$ D \f$
//!
//! @return a matrix of pairwise dependence measures.
inline Eigen::MatrixXd wdm(const Eigen::MatrixXd& x,
                           std::string method,
                           Eigen::VectorXd weights = Eigen::VectorXd(),
                           bool remove_missing = true)
{
    size_t d = x.cols();
    if (d == 1)
        throw std::runtime_error("x must have at least 2 columns.");

    Eigen::MatrixXd ms = Eigen::MatrixXd::Identity(d, d);
    for (size_t i = 0; i < d; i++) {
        for (size_t j = i + 1; j < d; j++) {
            ms(i, j) = wdm(utils::convert_vec(x.col(i)),
                           utils::convert_vec(x.col(j)),
                           method,
                           utils::convert_vec(weights),
                           remove_missing);
            ms(j, i) = ms(i, j);
        }
    }

    return ms;
}

}


#ifndef HYPSYS1D_FVM_RATE_OF_CHANGE_HPP
#define HYPSYS1D_FVM_RATE_OF_CHANGE_HPP

#include <memory>
#include <Eigen/Dense>

#include <ancse/config.hpp>
#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/rate_of_change.hpp>
#include <ancse/simulation_time.hpp>

/// Compute the rate of change due to FVM.
/** The semidiscrete approximation of a PDE using FVM is
 *      du_i/dt = - (F_{i+0.5} - F_{i-0.5}) / dx.
 *  This computes the right hand side of the ODE.
 *
 * @tparam NumericalFlux see e.g. `CentralFlux`.
 * @tparam Reconstruction see e.g. `PWConstantReconstruction`.
 */
template <class NumericalFlux, class Reconstruction>
class FVMRateOfChange : public RateOfChange
{
    public:
        FVMRateOfChange(const Grid& grid,
                        const std::shared_ptr<Model>& model,
                        const NumericalFlux& numerical_flux,
                        const Reconstruction& reconstruction)
            : grid(grid),
              model(model),
              numerical_flux(numerical_flux),
              reconstruction(reconstruction) {}

        virtual void operator()(Eigen::MatrixXd& dudt,
                                const Eigen::MatrixXd& u0) const override
        {
            // implement the flux loop here.
            const int n_cells= grid.n_cells;
            const int n_ghost= grid.n_ghost;

            const int n_vars= model->get_nvars();

            const double dx= grid.dx;
            Eigen::VectorXd fL= Eigen::VectorXd::Zero(n_vars), fR= Eigen::VectorXd::Zero(n_vars);
            Eigen::VectorXd uL, uR;

            for (int i= n_ghost - 1; i < n_cells - n_ghost; ++i)
            {
                std::tie(uL, uR)= reconstruction(u0, i);

                fL= fR;
                fR= numerical_flux(uL, uR);

                dudt.col(i)= (fL - fR) / dx;
            }
        }

    private:
        Grid grid;
        std::shared_ptr<Model> model;
        NumericalFlux numerical_flux;
        Reconstruction reconstruction;
};

std::shared_ptr<RateOfChange>
make_fvm_rate_of_change(const nlohmann::json &config,
                        const Grid &grid,
                        const std::shared_ptr<Model> &model,
                        const std::shared_ptr<SimulationTime> &simulation_time);

#endif // HYPSYS1D_FVM_RATE_OF_CHANGE_HPP


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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 RW_MATH_VECTOR3D_HPP
#define RW_MATH_VECTOR3D_HPP

/**
 * @file Vector3D.hpp
 */

#if !defined(SWIG)
#include <rw/common/Serializable.hpp>

#include <Eigen/Eigen>
#endif
namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A 3D vector @f$ \mathbf{v}\in \mathbb{R}^3 @f$
     *
     * @f$ \robabx{i}{j}{\mathbf{v}} = \left[
     *  \begin{array}{c}
     *  v_x \\
     *  v_y \\
     *  v_z
     *  \end{array}
     *  \right]
     *  @f$
     *
     *  Usage example:
     *
     *  \code
     *  const Vector3D<> v1(1.0, 2.0, 3.0);
     *  const Vector3D<> v2(6.0, 7.0, 8.0);
     *  const Vector3D<> v3 = cross(v1, v2);
     *  const double d = dot(v1, v2);
     *  const Vector3D<> v4 = v2 - v1;
     *  \endcode
     */
    template< class T = double > class Vector3D
    {
      public:
        //! Eigen type equivalent to Vector3D
        typedef Eigen::Matrix< T, 3, 1 > EigenVector3D;

        //! Value type.
        typedef T value_type;

        /**
         * @brief Creates a 3D vector initialized with 0's
         */
        Vector3D ()
        {
            _vec[0] = 0;
            _vec[1] = 0;
            _vec[2] = 0;
        }

        /**
         * @brief Creates a 3D vector
         * @param x [in] @f$ x @f$
         * @param y [in] @f$ y @f$
         * @param z [in] @f$ z @f$
         */
        Vector3D (T x, T y, T z)
        {
            _vec[0] = x;
            _vec[1] = y;
            _vec[2] = z;
        }

        /**
         * @brief Copy constructor
         * @param vec [in] vector to copy
         */
        Vector3D (const Vector3D<T>& copy_vec): _vec(copy_vec._vec)
        {
        }

        /**
         * @brief Creates a 3D vector from vector_expression
         * @param r [in] an Eigen Vector
         */
        template< class R > explicit Vector3D (const Eigen::MatrixBase< R >& r)
        {
            _vec[0] = T( r.row (0) (0));
            _vec[1] = T( r.row (1) (0));
            _vec[2] = T( r.row (2) (0));
        }

        /**
         * @brief construct vector from std::vector
         * @param vec [in] the vector to construct from
         */
        Vector3D (const std::vector< T >& vec)
        {
            if (vec.size () != 3u) {
                RW_THROW ("Wrong Size vector matrix: N of size:" << 3 << " and vector of size: "
                                                                 << vec.size () << "given");
            }
            for (size_t i = 0; i < 3u; i++) {
                _vec[i] = vec[i];
            }
        }

        /**
         *  @brief The dimension of the vector (i.e. 3).
         * This method is provided to help support generic algorithms using
         * size() and operator[].
         * @return the size
         */
        size_t size () const { return 3u; }

        /**
         * @brief Get zero vector.
         * @return vector.
         */
        static Vector3D< T > zero () { return Vector3D< T > (0, 0, 0); }

        /**
         * @brief Get x vector (1,0,0)
         * @return vector.
         */
        static Vector3D< T > x () { return Vector3D< T > (1.0, 0, 0); }

        /**
         * @brief Get y vector (0,1,0)
         * @return vector.
         */
        static Vector3D< T > y () { return Vector3D< T > (0, 1.0, 0); }

        /**
         * @brief Get z vector (0,0,1)
         * @return vector.
         */
        static Vector3D< T > z () { return Vector3D< T > (0, 0, 1.0); }

        // ###################################################
        // #                 Math Operations                 #
        // ###################################################

        // ########## Eigen Operations

        /**
         * @brief element wise division.
         * @param rhs [in] the vector being devided with
         * @return the resulting Vector3D
         */
        template< class R > Vector3D< T > elemDivide (const Eigen::MatrixBase< R >& rhs) const
        {
            Vector3D< T > ret = *this;
            for (size_t i = 0; i < size (); i++) {
                ret._vec[i] /= rhs[i];
            }
            return ret;
        }

        /**
         * @brief Elementweise multiplication.
         * @param rhs [in] vector
         * @return the element wise product
         */
        template< class R > Vector3D< T > elemMultiply (const Eigen::MatrixBase< R >& rhs) const
        {
            Vector3D< T > ret = *this;
            for (size_t i = 0; i < size (); i++) {
                ret._vec[i] *= rhs[i];
            }
            return ret;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > Vector3D< T > operator- (const Eigen::MatrixBase< R >& rhs) const
        {
            return Vector3D< T > (_vec - rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend Vector3D< T > operator- (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs - rhs.e ());
        }

        /**
         * @brief Vector addition.
         */
        template< class R > Vector3D< T > operator+ (const Eigen::MatrixBase< R >& rhs) const
        {
            return Vector3D< T > (_vec + rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend Vector3D< T > operator+ (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs + rhs.e ());
        }

        // ########## Vector3D Operations

        /**
         * @brief element wise division.
         * @param rhs [in] the vector being devided with
         * @return the resulting Vector3D
         */
        Vector3D< T > elemDivide (const Vector3D< T >& rhs) const
        {
            return Vector3D< T > (
                _vec[0] / rhs._vec[0], _vec[1] / rhs._vec[1], _vec[2] / rhs._vec[2]);
        }

        /**
         * @brief Elementweise multiplication.
         * @param rhs [in] vector
         * @return the element wise product
         */
        Vector3D< T > elemMultiply (const Vector3D< T >& rhs) const
        {
            return Vector3D< T > (
                _vec[0] * rhs._vec[0], _vec[1] * rhs._vec[1], _vec[2] * rhs._vec[2]);
        }

        /**
         * @brief Vector subtraction.
         */
        Vector3D< T > operator- (const Vector3D< T >& b) const
        {
            return Vector3D< T > (_vec[0] - b[0], _vec[1] - b[1], _vec[2] - b[2]);
        }

        /**
         * @brief Vector addition.
         */
        Vector3D< T > operator+ (const Vector3D< T >& b) const
        {
            return Vector3D< T > (_vec[0] + b[0], _vec[1] + b[1], _vec[2] + b[2]);
        }

        /**
         * @brief Unary minus.
         * @brief negative version
         */
        Vector3D< T > operator- () const { return Vector3D< T > (-_vec[0], -_vec[1], -_vec[2]); }

        // ########## Scalar Operations

        /**
         * @brief Scalar division.
         * @param s [in] the scalar to devide with
         * @return result of devision
         */
        Vector3D< T > operator/ (T s) const
        {
            return Vector3D< T > (_vec[0] / s, _vec[1] / s, _vec[2] / s);
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar division.
         * @param lhs [in] the scalar to devide with
         * @param rhs [out] the vector beind devided
         * @return result of devision
         */
        friend Vector3D< T > operator/ (T lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs / rhs._vec[0], lhs / rhs._vec[1], lhs / rhs._vec[2]);
        }
#endif
        /**
         * @brief Scalar multiplication.
         * @param rhs [in] the scalar to multiply with
         * @return the product
         */
        Vector3D< T > operator* (T rhs) const
        {
            return Vector3D< T > (_vec[0] * rhs, _vec[1] * rhs, _vec[2] * rhs);
        }

        /**
         * @brief Scalar multiplication.
         * @param lhs [in] the scalar to multiply with
         * @param rhs [in] the Vector to be multiplied
         * @return the product
         */
        friend Vector3D< T > operator* (T lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs * rhs[0], lhs * rhs[1], lhs * rhs[2]);
        }

        /**
         * @brief Scalar multiplication.
         * @param rhs [in] the Eigen vector^T or matrix to multiply with
         * @return the product
         */
        template< class R > Vector3D< T > operator* (const Eigen::MatrixBase< R >& rhs) const
        {
            return Vector3D< T > (this->e()*rhs);
        }

        /**
         * @brief Scalar multiplication.
         * @param lhs [in] the Eigen vector^T or matrix to multiply with
         * @param rhs [in] the Vector to be multiplied
         * @return the product
         */
        template< class R > friend Vector3D< T > operator* (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return Vector3D< T > (lhs*rhs.e());
        }

        /**
         * @brief Scalar subtraction.
         */
        Vector3D< T > elemSubtract (const T rhs) const
        {
            return Vector3D< T > (_vec[0] - rhs, _vec[1] - rhs, _vec[2] - rhs);
        }

        /**
         * @brief Scalar addition.
         */
        Vector3D< T > elemAdd (const T rhs) const
        {
            return Vector3D< T > (_vec[0] + rhs, _vec[1] + rhs, _vec[2] + rhs);
        }

        // ########### Math Functions

        /**
         * @brief Returns the Euclidean norm (2-norm) of the vector
         * @return the norm
         */
        T norm2 () const
        {
            return sqrt (_vec[0] * _vec[0] + _vec[1] * _vec[1] + _vec[2] * _vec[2]);
        }

        /**
         * @brief Returns the Manhatten norm (1-norm) of the vector
         * @return the norm
         */
        T norm1 () const { return fabs (_vec[0]) + fabs (_vec[1]) + fabs (_vec[2]); }

        /**
         * @brief Returns the infinte norm (\f$\inf\f$-norm) of the vector
         * @return the norm
         */
        T normInf () const
        {
            T res      = fabs (_vec[0]);
            const T f1 = fabs (_vec[1]);
            if (f1 > res)
                res = f1;
            const T f2 = fabs (_vec[2]);
            if (f2 > res)
                res = f2;
            return res;
        }

        /**
         * @brief Calculate cross product
         * @param vec [in] the vector to cross with
         * @return the cross product
         */
        Vector3D< T > cross (const Vector3D& vec) const
        {
            return Vector3D< T > (_vec.cross (vec._vec));
        }

        /**
         * @brief calculate the dot product
         * @param vec [in] the vecor to be dotted
         * @return the dot product
         */
        T dot (const Vector3D& vec) const { return _vec.dot (vec._vec); }

        /**
         * @brief normalize vector to get length 1
         * @return the normalized Vector
         */
        Vector3D< T > normalize ()
        {
            T length = norm2 ();
            if (length != 0)
                return (*this) / length;
            else
                return Vector3D< T > (0, 0, 0);
        }

        // ###################################################
        // #                Acces Operators                  #
        // ###################################################

        /**
         * @brief Returns Reference to Eigen Vector
         * @return reference to underling eigen
         */
        EigenVector3D& e () { return _vec; }

        /**
         * @brief Returns Reference to Eigen Vector
         * @return copy of eigen vector
         */
        const EigenVector3D e () const { return _vec; }

#if !defined(SWIG)
        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator() (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator() (size_t i) { return _vec[i]; }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator[] (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to vector element
         * @param i [in] index in the vector \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator[] (size_t i) { return _vec[i]; }
#else
        ARRAYOPERATOR (T);
#endif
#if !defined(SWIG)
        /**
         * @brief Streaming operator.
         * @param out [in/out] the stream to continue
         * @param v [in] the vector to stream
         * @param reference to \b out
         */
        friend std::ostream& operator<< (std::ostream& out, const Vector3D< T >& v)
        {
            return out << "Vector3D(" << v[0] << ", " << v[1] << ", " << v[2] << ")";
        }
#else
        TOSTRING (rw::math::Vector3D< T >);
#endif
        // ###################################################
        // #             assignement Operators               #
        // ###################################################

        /**
         * @brief Scalar multiplication.
         */
        Vector3D< T >& operator*= (T s)
        {
            _vec[0] *= s;
            _vec[1] *= s;
            _vec[2] *= s;
            return *this;
        }

        /**
         * @brief Scalar division.
         */
        Vector3D< T >& operator/= (T s)
        {
            _vec[0] /= s;
            _vec[1] /= s;
            _vec[2] /= s;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        Vector3D< T >& operator+= (const Vector3D< T >& v)
        {
            _vec[0] += v._vec[0];
            _vec[1] += v._vec[1];
            _vec[2] += v._vec[2];
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        Vector3D< T >& operator-= (const Vector3D< T >& v)
        {
            _vec[0] -= v._vec[0];
            _vec[1] -= v._vec[1];
            _vec[2] -= v._vec[2];
            return *this;
        }

        /**
         * @brief copy a vector from eigen type
         * @param r [in] an Eigen Vector
         */
        template< class R > Vector3D< T >& operator= (const Eigen::MatrixBase< R >& r)
        {
            _vec = r;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        template< class R > Vector3D< T >& operator+= (const Eigen::MatrixBase< R >& r)
        {
            _vec += r;
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > Vector3D< T >& operator-= (const Eigen::MatrixBase< R >& r)
        {
            _vec -= r;
            return *this;
        }

        // ###################################################
        // #                    Comparetors                  #
        // ###################################################

        /**
         * @brief Compare with \b b for equality.
         * @param b [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const Vector3D< T >& b) const
        {
            return _vec[0] == b[0] && _vec[1] == b[1] && _vec[2] == b[2];
        }

        /**
           @brief Compare with \b b for inequality.
           @param b [in] other vector.
           @return True if a and b are different, false otherwise.
        */
        bool operator!= (const Vector3D< T >& b) const { return !(*this == b); }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R > bool operator== (const Eigen::MatrixBase< R >& rhs) const
        {
            return this->_vec == rhs;
        }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R >
        friend bool operator== (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return lhs == rhs._vec;
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R > bool operator!= (const Eigen::MatrixBase< R >& rhs) const
        {
            return !(*this == rhs);
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R >
        friend bool operator!= (const Eigen::MatrixBase< R >& lhs, const Vector3D< T >& rhs)
        {
            return !(lhs == rhs);
        }
#if !defined(SWIG)
        /**
         * @brief implicit conversion to EigenVector
         */
        operator EigenVector3D () const { return this->e (); }

        /**
         * @brief implicit conversion to EigenVector
         */
        operator EigenVector3D& () { return this->e (); }
#endif

      private:
        EigenVector3D _vec;
    };

    /**
     * @brief Calculates the 3D vector cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the 3D vector cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * The 3D vector cross product is defined as:
     * @f$
     * \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c}
     *  v1_y * v2_z - v1_z * v2_y \\
     *  v1_z * v2_x - v1_x * v2_z \\
     *  v1_x * v2_y - v1_y * v2_x
     * \end{array}\right]
     * @f$
     *
     * @relates Vector3D
     */
    template< class T > const Vector3D< T > cross (const Vector3D< T >& v1, const Vector3D< T >& v2)
    {
        return Vector3D< T > (v1[1] * v2[2] - v1[2] * v2[1],
                              v1[2] * v2[0] - v1[0] * v2[2],
                              v1[0] * v2[1] - v1[1] * v2[0]);
    }

    /**
     * @brief Calculates the 3D vector cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     * @param dst [out] the 3D vector cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * The 3D vector cross product is defined as:
     * @f$
     * \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c}
     *  v1_y * v2_z - v1_z * v2_y \\
     *  v1_z * v2_x - v1_x * v2_z \\
     *  v1_x * v2_y - v1_y * v2_x
     * \end{array}\right]
     * @f$
     *
     * @relates Vector3D
     */
    template< class T >
    void cross (const Vector3D< T >& v1, const Vector3D< T >& v2, Vector3D< T >& dst)
    {
        dst[0] = v1[1] * v2[2] - v1[2] * v2[1];
        dst[1] = v1[2] * v2[0] - v1[0] * v2[2];
        dst[2] = v1[0] * v2[1] - v1[1] * v2[0];
    }

    /**
     * @brief Calculates the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     *
     * @relates Vector3D
     */
    template< class T > T dot (const Vector3D< T >& v1, const Vector3D< T >& v2)
    {
        return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
        // return inner_prod(v1.m(), v2.m());
    }

    /**
     * @brief Returns the normalized vector \f$\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} \f$.
     * In case \f$ \|mathbf{v}\| = 0\f$ the zero vector is returned.
     * @param v [in] \f$ \mathbf{v} \f$ which should be normalized
     * @return the normalized vector \f$ \mathbf{n} \f$
     *
     * @relates Vector3D
     */
    template< class T > const Vector3D< T > normalize (const Vector3D< T >& v)
    {
        T length = v.norm2 ();
        if (length != 0)
            return Vector3D< T > (v (0) / length, v (1) / length, v (2) / length);
        else
            return Vector3D< T > (0, 0, 0);
    }

    /**
     * @brief Calculates the angle from @f$ \mathbf{v1}@f$ to @f$ \mathbf{v2} @f$
     * around the axis defined by @f$ \mathbf{v1} \times \mathbf{v2} @f$ with n
     * determining the sign.
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     * @param n [in] @f$ \mathbf{n} @f$
     *
     * @return the angle
     *
     * @relates Vector3D
     */
    template< class T >
    double angle (const Vector3D< T >& v1, const Vector3D< T >& v2, const Vector3D< T >& n)
    {
        const Vector3D< T > nv1 = normalize (v1);
        const Vector3D< T > nv2 = normalize (v2);
        const Vector3D< T > nn  = normalize (n);
        return atan2 (dot (nn, cross (nv1, nv2)), dot (nv1, nv2));
    }

    /**
     * @brief Calculates the angle from @f$ \mathbf{v1}@f$ to @f$ \mathbf{v2} @f$
     * around the axis defined by @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the angle
     *
     * @relates Vector3D
     */
    template< class T > double angle (const Vector3D< T >& v1, const Vector3D< T >& v2)
    {
        Vector3D< T > n = cross (v1, v2);
        return angle (v1, v2, n);
    }

    /**
     * @brief Casts Vector3D<T> to Vector3D<Q>
     * @param v [in] Vector3D with type T
     * @return Vector3D with type Q
     *
     * @relates Vector3D
     */
    template< class Q, class T > const Vector3D< Q > cast (const Vector3D< T >& v)
    {
        return Vector3D< Q > (
            static_cast< Q > (v (0)), static_cast< Q > (v (1)), static_cast< Q > (v (2)));
    }
#if !defined(SWIG)
    extern template class rw::math::Vector3D< double >;
    extern template class rw::math::Vector3D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Vector3Dd, rw::math::Vector3D< double >);
    SWIG_DECLARE_TEMPLATE (Vector3Df, rw::math::Vector3D< float >);
#endif

    using Vector3Dd = Vector3D< double >;
    using Vector3Df = Vector3D< float >;

    /**@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Vector3D
         */
        template<>
        void write (const rw::math::Vector3D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Vector3D
         */
        template<>
        void write (const rw::math::Vector3D< float >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Vector3D
         */
        template<>
        void read (rw::math::Vector3D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Vector3D
         */
        template<>
        void read (rw::math::Vector3D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

namespace boost { namespace serialization {
    /**
     * @brief Boost serialization.
     * @param archive [in] the boost archive to read from or write to.
     * @param vector [in/out] the vector to read/write.
     * @param version [in] class version (currently version 0).
     * @relatedalso rw::math::Vector3D
     */
    template< class Archive, class T >
    void serialize (Archive& archive, rw::math::Vector3D< T >& vector, const unsigned int version)
    {
        archive& vector[0];
        archive& vector[1];
        archive& vector[2];
    }
}}    // namespace boost::serialization

#endif    // end include guard


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

#include <iostream>
#include <complex>
#include <Eigen/Dense>

#include "tblis.h"

int main(int argc, char const *argv[]) {
	int s1=2, s2=3, s3=4;
	Eigen::MatrixXcd mA(s1,s2);
	mA.setRandom();
	Eigen::MatrixXcd mB(s2,s3);
	mB.setRandom();
	Eigen::MatrixXcd mC(s1,s3);
	mC.setRandom();
	std::cout << "/-------------------------------------------------/" << '\n';
	std::cout << "Matrix mA:" << '\n';
	std::cout << mA << '\n' << '\n';
	std::cout << "Matrix mB:" << '\n';
	std::cout << mB << '\n' << '\n';
	std::cout << "Matrix mA*mB+mC:" << '\n';
	std::cout << mA*mB+mC << '\n' << '\n';
	std::cout << "/-------------------------------------------------/" << '\n';

	tblis::tensor_view< std::complex<double> > A({s1,s2}, mA.data());
	tblis::tensor_view< std::complex<double> > B({s2,s3}, mB.data());
	tblis::tensor_view< std::complex<double> > C({s1,s3}, mC.data());

	tblis::mult(std::complex<double>(1.0),A,"ab",B,"bc",std::complex<double>(1.0),C,"ac");

	std::cout << "tblis results:" << '\n';
	std::cout << mC << '\n' << '\n';

	std::cout << "/-------------------------------------------------/" << '\n';
  return 0;
}


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
/**
 * @file omni.hpp
 * @author Boston Cleek
 * @date 30 Oct 2020
 * @brief Kinematic omni directional models control wheel velocities or body twist
 */
#ifndef OMNI_HPP
#define OMNI_HPP

#include <cmath>
#include <armadillo>

#include <ergodic_exploration/numerics.hpp>

namespace ergodic_exploration
{
namespace models
{
using arma::mat;
using arma::vec;

/**
 * @brief Kinematic model of 4 mecanum wheel robot
 * @details The state is [x, y, theta] and controls are the angular velocities of each
 * wheel [u0, u1, u2, u3] corresponding to (front left, front right, rear right, rear
 * left). Assumes the mecanum wheel rollers are at +/- 45 degrees
 */
struct Mecanum
{
  /**
   * @brief Constructor
   * @param wheel_radius - radius of wheel
   * @param wheel_base_x - distance from chassis center to wheel center along x-axis
   * @param wheel_base_y - distance from chassis center to wheel center along y-axis
   */
  Mecanum(double wheel_radius, double wheel_base_x, double wheel_base_y)
    : wheel_radius(wheel_radius)
    , wheel_base_x(wheel_base_x)
    , wheel_base_y(wheel_base_y)
    , state_space(3)
  {
  }

  /**
   * @brief Convert wheel velocities to a body frame twist
   * @param u - control [u0, u1, u2, u3]
   * @return twist in body frame Vb = [vx, vy, w]
   */
  vec wheels2Twist(const vec u) const
  {
    const auto l = 1.0 / (wheel_base_x + wheel_base_y);

    // pseudo inverse of jacobian matrix
    const mat Hp = { { 1.0, 1.0, 1.0, 1.0 }, { -1.0, 1.0, -1.0, 1.0 }, { -l, l, l, -l } };

    const vec vb = (wheel_radius / 4.0) * Hp * u;

    return { vb(0), vb(1), vb(2) };
  }

  /**
   * @brief Kinematic model of 4 mecanum wheel robot
   * @param x - state [x, y, theta]
   * @param u - control [u0, u1, u2, u3]
   * @return [xdot, ydot, thetadot] = f(x,u)
   */
  vec operator()(const vec x, const vec u) const
  {
    vec xdot(3);
    const auto s = (wheel_radius / 4.0) * std::sin(x(2));
    const auto c = (wheel_radius / 4.0) * std::cos(x(2));
    const auto l = wheel_radius / (4.0 * (wheel_base_x + wheel_base_y));

    xdot(0) = u(0) * (s + c) + u(1) * (-s + c) + u(2) * (s + c) + u(3) * (-s + c);
    xdot(1) = u(0) * (s - c) + u(1) * (s + c) + u(2) * (s - c) + u(3) * (s + c);
    xdot(2) = -u(0) * l + u(1) * l + u(2) * l - u(3) * l;

    return xdot;
  }

  /**
   * @brief Jacobian of the model with respect to the state
   * @param x - state [x, y, theta]
   * @param u - control [u0, u1, u2, u3]
   * @return A = D1(f(x,u)) of shape (3x3)
   */
  mat fdx(const vec x, const vec u) const
  {
    mat A(3, 3, arma::fill::zeros);

    const auto s = (wheel_radius / 4.0) * std::sin(x(2));
    const auto c = (wheel_radius / 4.0) * std::cos(x(2));

    const auto df0dth =
        u(0) * (-s + c) + u(1) * (-s - c) + u(2) * (-s + c) + u(3) * (-s - c);
    const auto df1dth =
        u(0) * (s + c) + u(1) * (-s + c) + u(2) * (s + c) + u(3) * (-s + c);

    A(0, 2) = df0dth;
    A(1, 2) = df1dth;

    return A;
  }

  /**
   * @brief Jacobian of the model with respect to the control
   * @param x - state [x, y, theta]
   * @return B = D2(f(x,u)) of shape (3x4)
   */
  mat fdu(const vec x) const
  {
    const auto s = (wheel_radius / 4.0) * std::sin(x(2));
    const auto c = (wheel_radius / 4.0) * std::cos(x(2));
    const auto l = wheel_radius / (4.0 * (wheel_base_x + wheel_base_y));

    const mat B = { { s + c, -s + c, s + c, -s + c },
                    { s - c, s + c, s - c, s + c },
                    { -l, l, l, -l } };
    return B;
  }

  double wheel_radius;       // radius of wheel
  double wheel_base_x;       // distance from chassis center to wheel center along x-axis
  double wheel_base_y;       // distance from chassis center to wheel center along y-axis
  unsigned int state_space;  // states space dimension
};

/**
 * @brief Kinematic model of omni directonal robot
 * @details The state is [x, y, theta] and controls are the linear and
 * angular velocities [vx, vy, w] (body twist)
 */
struct Omni
{
  /** @brief Constructor */
  Omni() : state_space(3)
  {
  }

  /**
   * @brief Kinematic model of 4 mecanum wheel robot
   * @param x - state [x, y, theta]
   * @param u - body twist control [vx, vy, w]
   * @return [xdot, ydot, thetadot] = f(x,u)
   */
  vec operator()(const vec x, const vec u) const
  {
    const auto xdot = u(0) * std::cos(x(2)) - u(1) * std::sin(x(2));
    const auto ydot = u(0) * std::sin(x(2)) + u(1) * std::cos(x(2));
    return { xdot, ydot, u(2) };
    // return { xdot, ydot, 0.0 };
    // return { u(0), u(1), u(2) };
  }

  /**
   * @brief Jacobian of the model with respect to the state
   * @param x - state [x, y, theta]
   * @param u - body twist control [vx, vy, w]
   * @return A = D1(f(x,u)) of shape (3x3)
   */
  mat fdx(const vec x, const vec u) const
  {
    mat A(3, 3, arma::fill::zeros);
    A(0, 2) = -u(0) * std::sin(x(2)) - u(1) * std::cos(x(2));
    A(1, 2) = u(0) * std::cos(x(2)) - u(1) * std::sin(x(2));
    return A;
  }

  /**
   * @brief Jacobian of the model with respect to the control
   * @param x - state [x, y, theta]
   * @return B = D2(f(x,u)) of shape (3x3)
   */
  mat fdu(const vec x) const
  {
    // mat B(3,3, arma::fill::eye);
    const mat B = { { std::cos(x(2)), -std::sin(x(2)), 0.0 },
                    { std::sin(x(2)), std::cos(x(2)), 0.0 },
                    { 0.0, 0.0, 1.0 } };
    return B;
  }

  unsigned int state_space;  // states space dimension
};
}  // namespace models
}  // namespace ergodic_exploration
#endif


#include <aslam/backend/MEstimatorPolicies.hpp>

#include <cmath>

#include <sstream>

#include <boost/math/distributions/chi_squared.hpp>

namespace aslam {
namespace backend {

MEstimator::~MEstimator() {}

NoMEstimator::~NoMEstimator() {}
double NoMEstimator::getWeight(double /* squaredError */) const { return 1.0; }
std::string NoMEstimator::name() const { return "none"; }

GemanMcClureMEstimator::GemanMcClureMEstimator(double sigma2) : _sigma2(sigma2) {}
GemanMcClureMEstimator::~GemanMcClureMEstimator() {}
double GemanMcClureMEstimator::getWeight(double error) const {
    double se = _sigma2 + error;
    return (_sigma2) / (se * se);
}
std::string GemanMcClureMEstimator::name() const {
    std::stringstream ss;
    ss << "Geman McClure (" << _sigma2 << ")";
    return ss.str();
}

CauchyMEstimator::CauchyMEstimator(double sigma2) : _sigma2(sigma2) {}
CauchyMEstimator::~CauchyMEstimator() {}
double CauchyMEstimator::getWeight(double error) const {
    double se = error / _sigma2;
    return 1.0 / (1.0 + se);
}
std::string CauchyMEstimator::name() const {
    std::stringstream ss;
    ss << "Cauchy (" << _sigma2 << ")";
    return ss.str();
}

HuberMEstimator::~HuberMEstimator() {}
HuberMEstimator::HuberMEstimator(double k) : _k(k), _k2(k * k) {}
double HuberMEstimator::getWeight(double error) const { return error < _k2 ? 1.0 : _k / sqrt(error); }
std::string HuberMEstimator::name() const {
    std::stringstream ss;
    ss << "Huber(" << _k << ")";
    return ss.str();
}

BlakeZissermanMEstimator::~BlakeZissermanMEstimator() {}
BlakeZissermanMEstimator::BlakeZissermanMEstimator(size_t df, double pCut, double wCut)
    : _df(df), _pCut(pCut), _wCut(wCut), _epsilon(computeEpsilon(df, pCut, wCut)) {}
BlakeZissermanMEstimator::BlakeZissermanMEstimator(const BlakeZissermanMEstimator& other)
    : MEstimator(other), _df(other._df), _pCut(other._pCut), _wCut(other._wCut), _epsilon(other._epsilon) {}
BlakeZissermanMEstimator& BlakeZissermanMEstimator::operator=(const BlakeZissermanMEstimator& other) {
    if (this != &other) {
        MEstimator::operator=(other);
        _df = other._df;
        _pCut = other._pCut;
        _wCut = other._wCut;
        _epsilon = other._epsilon;
    }
    return *this;
}
double BlakeZissermanMEstimator::getWeight(double mahalanobis2) const {
    return exp(-mahalanobis2) / (exp(-mahalanobis2) + _epsilon);
}
std::string BlakeZissermanMEstimator::name() const {
    std::stringstream ss;
    ss << "Blake-Zisserman(" << _epsilon << ")";
    return ss.str();
}
double BlakeZissermanMEstimator::chi2InvCDF(double p, size_t df) const {
    return boost::math::quantile(boost::math::chi_squared_distribution<>(df), p);
}
double BlakeZissermanMEstimator::computeEpsilon(size_t df, double pCut, double wCut) const {
    return (1 - wCut) / wCut * exp(-chi2InvCDF(pCut, df));
}

}  // namespace backend
}  // namespace aslam


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include math.hpp directly!"
#endif

#pragma once

#include <boost/math/constants/constants.hpp>

#include <cmath>

namespace matheval {

namespace math {

/// @brief Sign function
template <typename T>
T sgn(T x) {
    return (T(0) < x) - (x < T(0));
}

/// @brief isnan function with adjusted return type
template <typename T>
T isnan(T x) {
    return std::isnan(x);
}

/// @brief isinf function with adjusted return type
template <typename T>
T isinf(T x) {
    return std::isinf(x);
}

/// @brief Convert radians to degrees
template <typename T>
T deg(T x) {
    return x * boost::math::constants::radian<T>();
}

/// @brief Convert degrees to radians
template <typename T>
T rad(T x) {
    return x * boost::math::constants::degree<T>();
}

/// @brief unary plus
template <typename T>
T plus(T x) {
    return x;
}

/// @brief binary plus
template <typename T>
T plus(T x, T y) {
    return x + y;
}

/// @brief unary minus
template <typename T>
T minus(T x) {
    return -x;
}

/// @brief binary minus
template <typename T>
T minus(T x, T y) {
    return x - y;
}

/// @brief multiply
template <typename T>
T multiplies(T x, T y) {
    return x * y;
}

/// @brief divide
template <typename T>
T divides(T x, T y) {
    return x / y;
}

/// @brief unary not
template <typename T>
T unary_not(T x) {
    return !x;
}

/// @brief logical and
template <typename T>
T logical_and(T x, T y) {
    return x && y;
}

/// @brief logical or
template <typename T>
T logical_or(T x, T y) {
    return x || y;
}

/// @brief less
template <typename T>
T less(T x, T y) {
    return x < y;
}

/// @brief less equals
template <typename T>
T less_equals(T x, T y) {
    return x <= y;
}

/// @brief greater
template <typename T>
T greater(T x, T y) {
    return x > y;
}

/// @brief greater equals
template <typename T>
T greater_equals(T x, T y) {
    return x >= y;
}

/// @brief equals
template <typename T>
T equals(T x, T y) {
    return x == y;
}

/// @brief not equals
template <typename T>
T not_equals(T x, T y) {
    return x != y;
}

} // namespace math

} // namespace matheval


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <boost/math/special_functions/fpclassify.hpp> // isnan
#include <vinecopulib/bicop/family.hpp>
#include <vinecopulib/misc/tools_interpolation.hpp>
#include <vinecopulib/misc/tools_stats.hpp>
#include <wdm/eigen.hpp>

namespace vinecopulib {
inline TllBicop::TllBicop()
{
  family_ = BicopFamily::tll;
}

inline Eigen::VectorXd
TllBicop::gaussian_kernel_2d(const Eigen::MatrixXd& x)
{
  return tools_stats::dnorm(x).rowwise().prod();
}

//! selects the bandwidth matrix for local líkelihood estimator (covariance
//! times appropriate factor).
inline Eigen::Matrix2d
TllBicop::select_bandwidth(const Eigen::MatrixXd& x,
                           std::string method,
                           const Eigen::VectorXd& weights)
{
  size_t n = x.rows();
  double cor = wdm::wdm(x, "cor", weights)(0, 1);
  cor = std::min(std::max(cor, -0.95), 0.95);
  Eigen::Matrix2d cov = Eigen::MatrixXd::Identity(2, 2);
  cov(0, 1) = cor;
  cov(1, 0) = cor;

  double mult;
  if (method == "constant") {
    mult = std::pow(n, -1.0 / 3.0);
  } else {
    double degree;
    if (method == "linear") {
      degree = 1.0;
    } else {
      degree = 2.0;
    }
    mult = 1.5 * std::pow(n, -1.0 / (2.0 * degree + 1.0));
  }
  double mcor = tools_stats::pairwise_mcor(x, weights);
  double scale = std::pow(std::fabs(cor / mcor), 0.5 * mcor);

  return mult * cov * scale;
}

//! calculates the cholesky root of a 2x2 matrix.
inline Eigen::Matrix2d
chol22(const Eigen::Matrix2d& B)
{

  Eigen::Matrix2d rB;

  rB(0, 0) = std::sqrt(B(0, 0));
  rB(0, 1) = 0.0;
  rB(1, 0) = B(1, 0) / rB(0, 0);
  rB(1, 1) = std::sqrt(B(1, 1) - rB(1, 0) * rB(1, 0));

  return rB;
}

//! evaluates local likleihood density estimate.
//!
//! @param x Evaluation points.
//! @param x_data Observations.
//! @param B Bandwidth matrix.
//! @param method Order of local polynomial approximation; either `"constant"`,
//!   `"linear"`, or `"quadratic"`.
//! @param weights Vector of weights for the observations
//! @return a two-column matrix; first column is estimated density, second
//!    column is influence of evaluation point.
inline Eigen::MatrixXd
TllBicop::fit_local_likelihood(const Eigen::MatrixXd& x,
                               const Eigen::MatrixXd& x_data,
                               const Eigen::Matrix2d& B,
                               std::string method,
                               const Eigen::VectorXd& weights)
{
  size_t m = x.rows();      // number of evaluation points
  size_t n = x_data.rows(); // number of observations

  // pre-calculate inverse root of bandwidth matrix and determinant
  Eigen::Matrix2d irB = chol22(B).inverse();
  double det_irB = irB.determinant();

  // de-correlate data by applying B^{-1/2}
  Eigen::MatrixXd z = (irB * x.transpose()).transpose();
  Eigen::MatrixXd z_data = (irB * x_data.transpose()).transpose();

  Eigen::MatrixXd res(m, 2);
  res.col(0) = Eigen::VectorXd::Ones(m); // result will be a product
  Eigen::VectorXd kernels(n);
  Eigen::Vector2d f1;
  Eigen::Vector2d b;
  Eigen::Matrix2d S(B);
  Eigen::MatrixXd zz(n, 2), zz2(n, 2);
  for (size_t k = 0; k < m; ++k) {
    zz = z_data - z.row(k).replicate(n, 1);
    kernels = gaussian_kernel_2d(zz) * det_irB;
    if (weights.size() > 0)
      kernels = kernels.cwiseProduct(weights);
    double f0 = kernels.mean();
    if (method != "constant") {
      zz = (irB * zz.transpose()).transpose();
      f1 = zz.cwiseProduct(kernels.replicate(1, 2)).colwise().mean();
      b = f1 / f0;
      if (method == "quadratic") {
        zz2 = zz.cwiseProduct(kernels.replicate(1, 2)) /
              (f0 * static_cast<double>(n));
        b = B * b;
        S = (B * (zz.transpose() * zz2) * B - b * b.transpose()).inverse();
        res(k) *= std::sqrt(S.determinant()) / det_irB;
      }
      res(k) *= std::exp(-0.5 * double(b.transpose() * S * b));
      if ((boost::math::isnan)(res(k)) | (boost::math::isinf)(res(k))) {
        // inverse operation might go wrong due to rounding when
        // true value is equal or close to zero
        res(k) = 0.0;
      }
    }
    res(k, 0) *= f0;
    if (weights.size() > 0) {
      // average weight in neighborhood of evaluation point (essentially a
      // kernel regression estimate);
      // kernels have already been multiplied with weights above
      double w = kernels.sum() / kernels.cwiseQuotient(weights).sum();
      res(k, 1) = calculate_infl(n, f0, b, B, det_irB, S, method, w);
    } else {
      res(k, 1) = calculate_infl(n, f0, b, B, det_irB, S, method, 1.0);
    }
  }

  if (weights.size() > 0) {
    // estimate can be negative if negative weights are used
    res.col(0) = res.col(0).array().max(0.0);
  }

  return res;
}

//! calculate influence for data point for density estimate based on
//! quantities pre-computed in `fit_local_likelihood()`.
inline double
TllBicop::calculate_infl(const size_t& n,
                         const double& f0,
                         const Eigen::Vector2d& b,
                         const Eigen::Matrix2d& B,
                         const double& det_irB,
                         const Eigen::Matrix2d& S,
                         const std::string& method,
                         const double& weight)
{
  Eigen::MatrixXd M;
  if (method == "constant") {
    M = Eigen::MatrixXd::Constant(1, 1, f0);
  } else if (method == "linear") {
    M = Eigen::MatrixXd(3, 3);
    M(0, 0) = f0;
    M.col(0).tail(2) = B * b * f0;
    M.row(0).tail(2) = M.col(0).tail(2);
    M.block(1, 1, 2, 2) = f0 * B + f0 * B * b * b.transpose() * B;
  } else if (method == "quadratic") {
    M = Eigen::MatrixXd::Zero(6, 6);
    M(0, 0) = f0;
    M.col(0).segment(1, 2) = f0 * b;
    M.row(0).segment(1, 2) = M.col(0).segment(1, 2);
    M.block(1, 1, 2, 2) = f0 * B + f0 * b * b.transpose();
    M(3, 0) = 0.5 * M(1, 1);
    M(4, 0) = 0.5 * M(2, 2);
    M(5, 0) = M(1, 2);
    M.row(0).tail(3) = M.col(0).tail(3);
    Eigen::MatrixXd Si = S.inverse();
    M(3, 1) = 0.5 * f0 * (3.0 * Si(0, 0) * b(0) + std::pow(b(0), 3));
    M(4, 2) = 0.5 * f0 * (3.0 * Si(1, 1) * b(1) + std::pow(b(1), 3));
    M(4, 1) = 0.5 * f0;
    M(4, 1) *= 2.0 * Si(0, 1) * b(1) + Si(1, 1) * b(0) + b(0) * b(1) * b(1);
    M(3, 2) = 0.5 * f0;
    M(3, 2) *= 2.0 * Si(0, 1) * b(0) + Si(0, 0) * b(1) + b(1) * b(0) * b(0);
    M(5, 1) = 2.0 * M(3, 2);
    M(5, 2) = 2.0 * M(4, 1);
    M.block(1, 3, 2, 3) = M.block(3, 1, 3, 2).transpose();
    M(3, 3) = 0.25 * f0;
    M(3, 3) *= 3.0 * Si(0, 0) * Si(0, 0) + 6.0 * Si(0, 0) * b(0) * b(0) +
               std::pow(b(0), 4);
    M(4, 4) = 0.25 * f0;
    M(4, 4) *= 3.0 * Si(1, 1) * Si(1, 1) + 6.0 * Si(1, 1) * b(1) * b(1) +
               std::pow(b(1), 4);
    M(5, 5) = Si(0, 0) * Si(1, 1) + 2.0 * S(0, 1) + b(0) * b(0) * b(1) * b(1);
    M(5, 5) += 4.0 * Si(0, 1) * b(0) * b(1);
    M(5, 5) += Si(0, 0) * b(1) * b(1) + Si(1, 1) * b(0) * b(0);
    M(5, 5) *= f0;
    M(4, 3) = M(5, 5) * 0.25;
    M(3, 4) = M(4, 3);
    M(5, 3) = 3.0 * Si(0, 0) * Si(0, 1) + 3.0 * Si(0, 1) * b(0) * b(0);
    M(5, 3) += 3.0 * Si(0, 0) * b(0) * b(1) + b(1) * std::pow(b(0), 3);
    M(5, 3) *= 0.5 * f0;
    M(3, 5) = M(5, 3);
    M(5, 4) = 3.0 * Si(1, 1) * Si(0, 1) + 3.0 * Si(0, 1) * b(1) * b(1);
    M(5, 4) += 3.0 * Si(1, 1) * b(0) * b(1) + b(0) * std::pow(b(1), 3);
    M(5, 4) *= 0.5 * f0;
    M(4, 5) = M(5, 4);
  }

  double infl = gaussian_kernel_2d(Eigen::MatrixXd::Zero(1, 2))(0) * det_irB;
  infl *= M.inverse()(0, 0) * weight / static_cast<double>(n);
  return infl;
}

inline void
TllBicop::fit(const Eigen::MatrixXd& data,
              std::string method,
              double mult,
              const Eigen::VectorXd& weights)
{
  using namespace tools_interpolation;

  // construct default grid (equally spaced on Gaussian scale)
  size_t m = 30;
  auto grid_points = this->make_normal_grid(m);

  // expand the interpolation grid; a matrix with two columns where each row
  // contains one combination of the grid points
  auto grid_2d = tools_eigen::expand_grid(grid_points);

  // transform evaluation grid and data by inverse Gaussian cdf
  Eigen::MatrixXd z = tools_stats::qnorm(grid_2d);

  // use jittering in case observations are discrete
  auto psobs = tools_stats::to_pseudo_obs(data.leftCols(2), "random");
  Eigen::MatrixXd z_data = tools_stats::qnorm(psobs);

  // find bandwidth matrix
  Eigen::Matrix2d B = select_bandwidth(z_data, method, weights);
  B *= mult;

  // compute the density estimator (first column estimate, second influence)
  Eigen::MatrixXd ll_fit = fit_local_likelihood(z, z_data, B, method, weights);

  // transform density estimate to copula scale
  Eigen::VectorXd c =
    ll_fit.col(0).cwiseQuotient(tools_stats::dnorm(z).rowwise().prod());
  // store values in mxm grid
  Eigen::MatrixXd values(m, m);
  values = Eigen::Map<Eigen::MatrixXd>(c.data(), m, m).transpose();

  // for interpolation, we shift the limiting gridpoints to 0 and 1
  grid_points(0) = 0.0;
  grid_points(m - 1) = 1.0;
  interp_grid_ = std::make_shared<InterpolationGrid>(grid_points, values);

  // compute effective degrees of freedom via interpolation ---------
  // stabilize interpolation by restricting to plausible range
  Eigen::VectorXd infl_vec = ll_fit.col(1).cwiseMin(1.3).cwiseMax(-0.2);
  Eigen::MatrixXd infl(m, m);
  infl = Eigen::Map<Eigen::MatrixXd>(infl_vec.data(), m, m).transpose();
  // don't normalize margins of the EDF! (norm_times = 0)
  auto infl_grid = InterpolationGrid(grid_points, infl, 0);
  if ((var_types_[0] == "d") | (var_types_[1] == "d")) {
    // for discrete, use mid ranks to compute EDF and log-likelihood
    // (this is closer to "observations" than jittered or "upper" pseudo data)
    psobs = 0.5 * (data.leftCols(2) + data.rightCols(2)).array();
    npars_ = tools_eigen::unique(infl_grid.interpolate(psobs)).sum();
    npars_ = std::max(npars_, 1.0);
  } else {
    npars_ = std::max(infl_grid.interpolate(data).sum(), 1.0);
  }
  set_loglik(pdf(data).array().log().sum());
}
}


//============================================================================
// Name        : polynomfit.cpp
// Author      :
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C, Ansi-style
//============================================================================

//#include <stdio.h>
//#include <stdlib.h>
#include <Eigen/Dense>
#include "monopoly.h"
#include "include/tensor_serie.hh"
#include "full_correlation_tensor_serie.hh"
#include <random>

#include <iostream>

using namespace std;

int main(void) {
//	puts("Hello World!!!");
//	return EXIT_SUCCESS;

	cout << "Hello world" << endl;
	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic > m_eigen_matrix;
	m_eigen_matrix.resize(2,2);

	m_eigen_matrix << 1, 2,
					3,4;
	cout << m_eigen_matrix << endl;

	std::vector<std::vector<double> > data(10000, std::vector<double>(2,0.0) );
	std::random_device rd;
	std::mt19937 gen(rd());

	std::normal_distribution<> d(0,1);
	std::uniform_int_distribution<> distrib(1, 6);
	for(int i = 0; i!= data.size(); i++) {
		const double x =  distrib(gen) + d(gen);
		const double y = -0.1*x*x*x + 0.7*x*x + 0.01*x + 0.5 + d(gen);
		data[i][0] = x;
		data[i][1] = y;
	}

	full_correlation_tensor_serie<double> polynom(1,10);
	tensor_serie<double> m_x(1,10*4); //the internal tensors orders are 4 times larger

	for(int i=0; i!= data.size(); i++) {
		//cout<< i << data[i][0] << " " << data[i][1] << endl;
		m_x.create_diad_1dim(data[i][0]);
		polynom.fill_1dim(m_x, data[i][1], 1.0);
	}
	polynom.normalize();

	cout << "Solving for degree 4" << std::endl;
	tensor_serie_function<double> pol4 = polynom.solve(4);
	cout << "chi2\t expected chi2 \t the traditional bias \t full bias" << endl;
	cout << pol4.m_chi2 << "\t" << (pol4.m_chi2 + pol4.m_bias) << "\t" << pol4.m_biaso << "\t" << pol4.m_bias << endl;
	cout << "Solving for degree 10" << std::endl;
	tensor_serie_function<double> pol10 = polynom.solve(10);
	cout << "chi2\t expected chi2 \t the traditional bias \t full bias" << endl;
	cout << pol10.m_chi2 << "\t" << (pol10.m_chi2 + pol10.m_bias) << "\t" << pol10.m_biaso << "\t" << pol4.m_bias << endl;

	for(double x=0; x< 1.0; x+=0.1) {
		double y = (-0.1*x*x*x + 0.7*x*x + 0.01*x + 0.5);
		m_x.create_diad_1dim(x);
		cout << "p("<<x<<")=" << pol4.eval(m_x) << " vs " << y << endl;
	}

	//polynom.print("10 degree fit");

}


#include "delaunay_cpu_interpolator_base.h"
#include <CGAL/Delaunay_triangulation.h>
#include <CGAL/Epick_d.h>
#include <Eigen/Dense>
#include <bitset>

template<typename index_t, int N_DIMS, int N_OPS>
DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::DelaunayCPUInterpolatorBase(
        operator_set_evaluator_iface *supporting_point_evaluator,
        const std::array<int, N_DIMS> &axes_points,
        const std::array<double, N_DIMS> &axes_min,
        const std::array<double, N_DIMS> &axes_max)
        : InterpolatorBase<index_t, N_DIMS, N_OPS>(supporting_point_evaluator,
                    axes_points, axes_min, axes_max) {
    typedef CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<N_DIMS>>> T;
    std::array<typename T::Point, 1 << N_DIMS> points;
    for (int j = 0; j < points.size(); ++j) {
        std::bitset<N_DIMS> binary(j);
        double point[N_DIMS];
        for (int i = 0; i < N_DIMS; i++)
            point[i] = binary[i];
        typename T::Point p(&point[0], &point[N_DIMS]);
        points[j] = p;
    }
    delaunay_triangulation_.insert(points.begin(), points.end());
}

template<typename index_t, int N_DIMS, int N_OPS>
int DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::Interpolate(
        const std::vector<double> &point, std::vector<double> &values) {
    std::array<int, N_DIMS> hypercube;
    std::array<double, N_DIMS> scaled_point;
    this->FindHypercube(point, hypercube, scaled_point);

    Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> vertex_matrix;
    std::array<std::array<int, N_DIMS>, N_DIMS + 1> simplex;
    FindSimplex(hypercube, scaled_point, vertex_matrix, simplex);

    Eigen::Matrix<double, N_DIMS + 1, 1> weights;
    ComputeBarycentricCoordinates(vertex_matrix, scaled_point, weights);

    values.assign(N_OPS, 0.0);
    for (int dim_i = 0; dim_i <= N_DIMS; dim_i++) {
        std::array<double, N_OPS> supp_values;
        this->GetSupportingPoint(simplex[dim_i], supp_values);
        for (int op_i = 0; op_i < N_OPS; op_i++)
            values[op_i] += weights[dim_i] * supp_values[op_i];
    }

    return 0;
}

template<typename index_t, int N_DIMS, int N_OPS>
int
DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::InterpolateWithDerivatives(
        const std::vector<double> &points, const std::vector<int> &points_idxs,
        std::vector<double> &values, std::vector<double> &derivatives) {

    for (std::size_t point_i = 0; point_i < points_idxs.size(); point_i++) {
        int point_offset = points_idxs[point_i];

        std::array<int, N_DIMS> hypercube;
        std::array<double, N_DIMS> scaled_point;
        this->FindHypercube(points, hypercube, scaled_point,
                            point_offset * N_DIMS);

        Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> vertex_matrix;
        std::array<std::array<int, N_DIMS>, N_DIMS + 1> simplex;
        FindSimplex(hypercube, scaled_point, vertex_matrix, simplex);

        Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> vertex_matrix_inv;
        Eigen::Matrix<double, N_DIMS + 1, 1> weights;
        ComputeBarycentricCoordinatesAndVertexMatrixInv(vertex_matrix,
                scaled_point, weights, vertex_matrix_inv);

        std::fill(values.begin() + point_offset * N_OPS, values.begin() + (point_offset + 1) * N_OPS, 0);
        std::fill(derivatives.begin() + point_offset * N_OPS * N_DIMS, derivatives.begin() + (point_offset + 1) * N_OPS * N_DIMS, 0);
        for (int dim_i = 0; dim_i <= N_DIMS; dim_i++) {
            std::array<double, N_OPS> supp_values;
            this->GetSupportingPoint(simplex[dim_i], supp_values);
            for (int op_i = 0; op_i < N_OPS; op_i++) {
                values[point_offset * N_OPS + op_i] +=
                        weights[dim_i] * supp_values[op_i];
                for (int dim_j = 0; dim_j < N_DIMS; dim_j++)
                    derivatives[(point_offset * N_OPS + op_i) * N_DIMS + dim_j] +=
                            vertex_matrix_inv(dim_i, dim_j) * supp_values[op_i];
            }
        }
        for (int op_i = 0; op_i < N_OPS; op_i++)
            for (int dim_j = 0; dim_j < N_DIMS; dim_j++)
                derivatives[(point_offset * N_OPS + op_i) * N_DIMS + dim_j] *=
                        this->axes_step_inv_[dim_j];
    }
    return 0;
}

template<typename index_t, int N_DIMS, int N_OPS>
void DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::FindSimplex(
        const std::array<int, N_DIMS> &hypercube,
        const std::array<double, N_DIMS> &scaled_point,
        Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> &vertex_matrix,
        std::array<std::array<int, N_DIMS>, N_DIMS + 1> &simplex) {
    typedef CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<N_DIMS>>> T;
    typename T::Point t_scaled_point(scaled_point.begin(), scaled_point.end());
    typename T::Full_cell_handle handle = delaunay_triangulation_.locate(
            t_scaled_point);

    vertex_matrix.row(N_DIMS) = Eigen::Matrix<double, 1, N_DIMS + 1>::Constant(
            1.0);
    for (int vertex_i = 0; vertex_i <= N_DIMS; vertex_i++) {
        typename T::Point vertex = handle->vertex(vertex_i)->point();
        for (int dim_i = 0; dim_i < N_DIMS; dim_i++) {
            simplex[vertex_i][dim_i] = hypercube[dim_i] + vertex[dim_i];
            vertex_matrix(dim_i, vertex_i) = vertex[dim_i];
        }
    }
}

template<typename index_t, int N_DIMS, int N_OPS>
void
DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::ComputeBarycentricCoordinates(
        const Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> &vertex_matrix,
        const std::array<double, N_DIMS> &scaled_point,
        Eigen::Matrix<double, N_DIMS + 1, 1> &weights) {
    Eigen::Matrix<double, N_DIMS + 1, 1> scaled_point_vector;
    for (int dim_i = 0; dim_i < N_DIMS; dim_i++)
        scaled_point_vector[dim_i] = scaled_point[dim_i];
    scaled_point_vector[N_DIMS] = 1.0;

    weights = vertex_matrix.colPivHouseholderQr().solve(scaled_point_vector);
}

template<typename index_t, int N_DIMS, int N_OPS>
void
DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::
        ComputeBarycentricCoordinatesAndVertexMatrixInv(
        const Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> &vertex_matrix,
        const std::array<double, N_DIMS> &scaled_point,
        Eigen::Matrix<double, N_DIMS + 1, 1> &weights,
        Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> &vertex_matrix_inv) {
    Eigen::Matrix<double, N_DIMS + 1, 1> scaled_point_vector;
    for (int dim_i = 0; dim_i < N_DIMS; dim_i++)
        scaled_point_vector[dim_i] = scaled_point[dim_i];
    scaled_point_vector[N_DIMS] = 1.0;

    vertex_matrix_inv = vertex_matrix.inverse();
    weights = vertex_matrix_inv * scaled_point_vector;
}