#pragma once

#include <Eigen/Sparse>

#include <crest/util/eigen_extensions.hpp>

namespace crest
{

    enum class Norm
    {
        L2,
        H1Semi,
        H1
    };

    template <typename Scalar>
    struct Assembly
    {
        Eigen::SparseMatrix<Scalar> stiffness;
        Eigen::SparseMatrix<Scalar> mass;
    };

    template <typename Scalar, typename Impl>
    class Basis
    {
    public:
        virtual ~Basis() {}

        virtual std::vector<int> interior_nodes() const = 0;
        virtual std::vector<int> boundary_nodes() const = 0;

        virtual Assembly<Scalar> assemble() const = 0;

        /**
         * The number of degrees of freedom associated with this basis.
         * @return
         */
        virtual int num_dof() const = 0;

        /**
         * Given a continuous function f, interpolate it in the space
         * represented by this basis, and return a vector of weights, such that
         * element i in the vector corresponds to the weight factor of basis function i.
         *
         * Naturally, there are multiple ways to interpolate a function in a finite
         * element space, so to have a single function for this is a simplifaction,
         * and for now we'll leave it up to the concrete implementation which
         * interpolation this corresponds to.
         *
         * @param f
         * @return
         */
        template <typename Function2d>
        VectorX<Scalar> interpolate(const Function2d &f) const
        {
            return static_cast<const Impl *>(this)->template interpolate<Function2d>(f);
        }

        /*
         * Given a continuous function g, evaluate the function at each boundary node
         * and return a vector of weights.
         */
        template <typename Function2d>
        VectorX<Scalar> interpolate_boundary(const Function2d &f) const
        {
            return static_cast<const Impl *>(this)->template interpolate_boundary<Function2d>(f);
        }

        /**
         * Computes the L2 inner product of a continuous function f
         * and every basis function b_i for every degree of freedom i.
         * More precisely, returns a vector whose ith element is
         * determined by the L2 inner product (f, b_i), where b_i
         * is the basis function associated with degree of freedom i.
         *
         * This is frequently used to compute the load vector in FEM
         * applications.
         * @param weights
         * @param f
         * @return
         */
        template <int QuadStrength, typename Function2d>
        VectorX<Scalar> load(const Function2d &f) const
        {
            return static_cast<const Impl *>(this)->template load<QuadStrength, Function2d>(f);
        };

        /**
         * Computes an approximation of the error between a continuous function f
         * and a function g in the space spanned by the basis given by
         * its basis weights in the L2 norm.
         *
         * More precisely, if g = sum w_i b_i for all degrees of freedom i,
         * where w_i is given by weights(i) and b_i denotes the basis function
         * associated with the degree of freedom i, then this function computes
         *
         * ||f - g||
         *
         * in the L2 norm.
         *
         * @param f
         * @param weights
         * @param norm
         * @return
         */
        template <int QuadStrength, typename Function2d>
        Scalar error_l2(const Function2d &f, const VectorX<Scalar> & weights) const;

        /**
         * Computes an approximation of the error between a continuous function f
         * and a function g in the space spanned by the basis given by
         * its basis weights in the H1 semi-norm.
         *
         * More precisely, if g = sum w_i b_i for all degrees of freedom i,
         * where w_i is given by weights(i) and b_i denotes the basis function
         * associated with the degree of freedom i, then this function computes
         *
         * ||f - g||
         *
         * in the H1 semi-norm, which is equivalent to
         *
         * || grad(f) - grad(g) ||
         *
         * in the L2 norm. Note that one specifies the derivatives f_x and f_y
         * for the computation of the H1 semi-norm.
         *
         * @param f
         * @param weights
         * @param norm
         * @return
         */
        template <int QuadStrength, typename Function2d_x, typename Function2d_y>
        Scalar error_h1_semi(const Function2d_x & f_x,
                             const Function2d_y & f_y,
                             const VectorX<Scalar> & weights) const;

        /**
         * Computes an approximation of the error between a continuous function f
         * and a function g in the space spanned by the basis given by
         * its basis weights in the H1 norm.
         *
         * More precisely, if g = sum w_i b_i for all degrees of freedom i,
         * where w_i is given by weights(i) and b_i denotes the basis function
         * associated with the degree of freedom i, then this function computes
         *
         * ||f - g||
         *
         * in the H1 norm. Note that the computation requires f, as well as its
         * spatial derivatives f_x and f_y.
         *
         * Also note that implementers of subclasses do not need to reimplement this
         * function, as it is implemented in terms of error_l2 and error_h1_semi.
         *
         * @param f
         * @param weights
         * @param norm
         * @return
         */
        template <int QuadStrength, typename Function2d, typename Function2d_x, typename Function2d_y>
        Scalar error_h1(const Function2d & f,
                        const Function2d_x & f_x,
                        const Function2d_y & f_y,
                        const VectorX<Scalar> & weights) const;
    };

    template <typename Scalar, typename Impl>
    template <int QuadStrength, typename Function2d>
    Scalar Basis<Scalar, Impl>::error_l2(const Function2d & f, const VectorX<Scalar> & weights) const
    {
        return static_cast<const Impl *>(this)->template error_l2<QuadStrength>(f, weights);
    };

    template <typename Scalar, typename Impl>
    template <int QuadStrength, typename Function2d_x, typename Function2d_y>
    Scalar Basis<Scalar, Impl>::error_h1_semi(const Function2d_x & f_x,
                                              const Function2d_y & f_y,
                                              const VectorX<Scalar> & weights) const
    {
        return static_cast<const Impl *>(this)->template error_h1_semi<QuadStrength>(f_x, f_y, weights);
    };

    template <typename Scalar, typename Impl>
    template <int QuadStrength, typename Function2d, typename Function2d_x, typename Function2d_y>
    Scalar Basis<Scalar, Impl>::error_h1(const Function2d & f,
                                         const Function2d_x & f_x,
                                         const Function2d_y & f_y,
                                         const VectorX<Scalar> & weights) const
    {
        const auto l2 = error_l2<QuadStrength>(f, weights);
        const auto h1_semi = error_h1_semi<QuadStrength>(f_x, f_y, weights);
        return std::sqrt(l2 * l2 + h1_semi * h1_semi);
    };
}


#include <iostream>
#include <fstream>
#include <iostream>
#include <sstream>
#include <istream>
#include <string>
#include <cstdio>
#include <cstdlib>
#include <vector>
#include <map>
#include <cmath>
#include <algorithm>
#include <boost/range/adaptor/indexed.hpp>

using namespace std;

vector<vector<int>> examples;
vector<vector<int>> centroids;
int iteration = 0;
bool finished = false;

int closest_centroid(vector<int> example)
{
    map<string, int> closest = {{"id", -1}, {"distance", INT_MAX}};
    for (auto centroid : centroids | boost::adaptors::indexed(1))
    {
        long long int distance = 0;
        for (auto axis : example | boost::adaptors::indexed(1))
        {
            if (axis.index() - 1 == centroid.value().size())
                break;
            int centroid_point = centroid.value().at(axis.index() - 1);
            int axis_value = axis.value();
            int sub = centroid_point - axis_value;
            distance += pow(sub, 2);
        }
        distance = sqrt(distance);
        if (distance < closest.at("distance"))
        {
            closest.at("id") = centroid.index() - 1;
            closest.at("distance") = distance;
        }
    }
    return closest.at("id");
}

vector<int> recalculate(int centroid_id, vector<int> centroid)
{
    int total = 0;
    vector<int> new_centroid(examples.at(0).size() - 1, 0);
    for (auto example : examples)
        if (example.at(example.size() - 1) == centroid_id)
        {
            for (auto example_axis : example | boost::adaptors::indexed(1))
            {
                if (example_axis.index() - 1 == new_centroid.size())
                    break;
                new_centroid.at(example_axis.index() - 1) += example_axis.value();
            }
            total++;
        }

    if (total)
        for (auto centroid_axis : new_centroid | boost::adaptors::indexed(1))
        {
            centroid_axis.value() /= total;
            if (centroid_axis.value() != centroid.at(centroid_axis.index() - 1))
                finished = false;
        }
    return total ? new_centroid : centroid;
}

void k_means()
{
    while (!finished)
    {
        for (auto &example : examples)
            example.at(example.size() - 1) = closest_centroid(example);
        finished = true;
        for (auto centroid : centroids | boost::adaptors::indexed(1))
            centroid.value() = recalculate(centroid.index() - 1, centroid.value());
        iteration++;
    }
}

int main(int argc, char *argv[])
{
    string extracted, comma, basename(argv[1]);
    ifstream base, centroid;
    for (int i = 0; i < 1; i++)
    {
        vector<int> temp;

        base.open("bases/int_base_" + basename + ".data");
        while (!base.eof())
        {
            getline(base, extracted);
            stringstream is(extracted);
            if (extracted.size() > 0)
            {
                while (getline(is, comma, ','))
                    temp.push_back(stoi(comma));
                temp.push_back(-1);
                examples.push_back(temp);
                temp.clear();
            }
        }
        base.close();
        centroid.open("bases/int_centroid_" + basename + "_20.data");
        while (!centroid.eof())
        {
            getline(centroid, extracted);
            istringstream is(extracted);
            if (extracted.size() > 0)
            {
                while (getline(is, comma, ','))
                    temp.push_back(stoi(comma));
                centroids.push_back(temp);
                temp.clear();
            }
        }
        centroid.close();
        k_means();
        ofstream myfile("results/my_saida_" + basename + "_seq");
        myfile << "numero de iteracoes:" << iteration;
        for (auto value : examples | boost::adaptors::indexed(1))
            myfile << "id=" << value.index() - 1 << ", classe=" << value.value().at(value.value().size() - 1) << endl;
    }
    return 0;
}


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

/*
mult           0.0163621 s
solveInverse   0.0201562 s
solveFullPivLu 0.202951 s
solveColPivHh  0.547893 s
solveFullPivHh 0.621699 s
solveComplete  0.844406 s

*/

Eigen::Vector3f mult(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m * v;
}

Eigen::Vector3f solveInverse(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.inverse() * v;
}

Eigen::Vector3f solveFullPivLu(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.fullPivLu().solve(v);
}

Eigen::Vector3f solveColPivHh(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.colPivHouseholderQr().solve(v);
}

Eigen::Vector3f solveFullPivHh(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.fullPivHouseholderQr().solve(v);
}

Eigen::Vector3f solveComplete(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.completeOrthogonalDecomposition().solve(v);
}

Eigen::Vector3f solveBdcsvd(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.bdcSvd().solve(v);
}

Eigen::Vector3f solveJacobiSvd(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.jacobiSvd().solve(v);
}

typedef Eigen::Vector3f func(Eigen::Matrix3f const &m, Eigen::Vector3f const &v);

int measure(char const *t, func &f) {
  Eigen::Matrix3f m;
  Eigen::Vector3f v,s;
  s << 0.0f, 0.0f, 0.0f;
  int n = 0;
  auto start = std::chrono::high_resolution_clock::now();
  for(int i = 0; i < 1000000; ++i) {
    m << ++n, ++n, ++n, ++n, ++n, ++n, ++n, ++n, ++n;
    v << ++n, ++n, ++n;
    n %= 71;
    s += f(m, v);
  }
  auto end = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> diff = end - start;
  std::cout << t << ' ' << diff.count() << " s\n";
  return s(0);
}

int main() {
  return measure("mult", mult) +
  measure("solveInverse", solveInverse) +
  measure("solveFullPivLu", solveFullPivLu) +
  measure("solveColPivHh", solveColPivHh) +
  measure("solveFullPivHh", solveFullPivHh) +
  measure("solveComplete", solveComplete)/* +
  measure("solveBdcSvd", solveBdcsvd) +
  measure("solveJacobiSvd", solveJacobiSvd)*/;
}


/*
 * fpu.cpp
 *
 * This example demonstrates how one can use odeint to solve the Fermi-Pasta-Ulam system.

 *  Created on: July 13, 2011
 *
 * Copyright 2011-2012 Karsten Ahnert
 * Copyright 2011 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 <numeric>
#include <cmath>
#include <vector>

#include <boost/numeric/odeint.hpp>

#ifndef M_PI //not there on windows
#define M_PI 3.1415927 //...
#endif

using namespace std;
using namespace boost::numeric::odeint;

//[ fpu_system_function
typedef vector< double > container_type;

struct fpu
{
    const double m_beta;

    fpu( const double beta = 1.0 ) : m_beta( beta ) { }

    // system function defining the ODE
    void operator()( const container_type &q , container_type &dpdt ) const
    {
        size_t n = q.size();
        double tmp = q[0] - 0.0;
        double tmp2 = tmp + m_beta * tmp * tmp * tmp;
        dpdt[0] = -tmp2;
        for( size_t i=0 ; i<n-1 ; ++i )
        {
            tmp = q[i+1] - q[i];
            tmp2 = tmp + m_beta * tmp * tmp * tmp;
            dpdt[i] += tmp2;
            dpdt[i+1] = -tmp2;
        }
        tmp = - q[n-1];
        tmp2 = tmp + m_beta * tmp * tmp * tmp;
        dpdt[n-1] += tmp2;
    }

    // calculates the energy of the system
    double energy( const container_type &q , const container_type &p ) const
    {
        // ...
        //<-
        double energy = 0.0;
        size_t n = q.size();

        double tmp = q[0];
        energy += 0.5 * tmp * tmp + 0.25 * m_beta * tmp * tmp * tmp * tmp;
        for( size_t i=0 ; i<n-1 ; ++i )
        {
            tmp = q[i+1] - q[i];
            energy += 0.5 * ( p[i] * p[i] + tmp * tmp ) + 0.25 * m_beta * tmp * tmp * tmp * tmp;
        }
        energy += 0.5 * p[n-1] * p[n-1];
        tmp = q[n-1];
        energy += 0.5 * tmp * tmp + 0.25 * m_beta * tmp * tmp * tmp * tmp;

        return energy;
        //->
    }

    // calculates the local energy of the system
    void local_energy( const container_type &q , const container_type &p , container_type &e ) const
    {
        // ...
        //<-
        size_t n = q.size();
        double tmp = q[0];
        double tmp2 = 0.5 * tmp * tmp + 0.25 * m_beta * tmp * tmp * tmp * tmp;
        e[0] = tmp2;
        for( size_t i=0 ; i<n-1 ; ++i )
        {
            tmp = q[i+1] - q[i];
            tmp2 = 0.25 * tmp * tmp + 0.125 * m_beta * tmp * tmp * tmp * tmp;
            e[i] += 0.5 * p[i] * p[i] + tmp2 ;
            e[i+1] = tmp2;
        }
        tmp = q[n-1];
        tmp2 = 0.5 * tmp * tmp + 0.25 * m_beta * tmp * tmp * tmp * tmp;
        e[n-1] += 0.5 * p[n-1] * p[n-1] + tmp2;
        //->
    }
};
//]

//[ fpu_observer
struct streaming_observer
{
    std::ostream& m_out;
    const fpu &m_fpu;
    size_t m_write_every;
    size_t m_count;

    streaming_observer( std::ostream &out , const fpu &f , size_t write_every = 100 )
    : m_out( out ) , m_fpu( f ) , m_write_every( write_every ) , m_count( 0 ) { }

    template< class State >
    void operator()( const State &x , double t )
    {
        if( ( m_count % m_write_every ) == 0 )
        {
            container_type &q = x.first;
            container_type &p = x.second;
            container_type energy( q.size() );
            m_fpu.local_energy( q , p , energy );
            for( size_t i=0 ; i<q.size() ; ++i )
            {
                m_out << t << "\t" << i << "\t" << q[i] << "\t" << p[i] << "\t" << energy[i] << "\n";
            }
            m_out << "\n";
            clog << t << "\t" << accumulate( energy.begin() , energy.end() , 0.0 ) << "\n";
        }
        ++m_count;
    }
};
//]

int main( int argc , char **argv )
{
    //[ fpu_integration
    const size_t n = 64;
    container_type q( n , 0.0 ) , p( n , 0.0 );

    for( size_t i=0 ; i<n ; ++i )
    {
        p[i] = 0.0;
        q[i] = 32.0 * sin( double( i + 1 ) / double( n + 1 ) * M_PI );
    }

    const double dt = 0.1;

    typedef symplectic_rkn_sb3a_mclachlan< container_type > stepper_type;
    fpu fpu_instance( 8.0 );

    integrate_const( stepper_type() , fpu_instance ,
            make_pair( boost::ref( q ) , boost::ref( p ) ) ,
            0.0 , 1000.0 , dt , streaming_observer( cout , fpu_instance , 10 ) );
    //]

    return 0;
}


#include "mtf/SSM/Affine.h"
#include "mtf/SSM/AffineEstimator.h"
#include "mtf/Utilities/warpUtils.h"
#include "mtf/Utilities/miscUtils.h"

#include <Eigen/SVD>

#define VALIDATE_AFFINE_WARP(warp)\
	assert(warp(2, 0) == 0.0 && warp(2, 1) == 0.0);\
	assert(warp(2, 2) == 1.0)

#define AFF_NORMALIZED_INIT 0
#define AFF_PT_BASED_SAMPLING 0
#define AFF_DEBUG_MODE 0

_MTF_BEGIN_NAMESPACE

AffineParams::AffineParams(const SSMParams *ssm_params,
bool _normalized_init, int _pt_based_sampling,
bool _debug_mode) :
SSMParams(ssm_params),
normalized_init(_normalized_init),
pt_based_sampling(_pt_based_sampling),
debug_mode(_debug_mode){}

AffineParams::AffineParams(const AffineParams *params) :
SSMParams(params),
normalized_init(AFF_NORMALIZED_INIT),
pt_based_sampling(AFF_PT_BASED_SAMPLING),
debug_mode(AFF_DEBUG_MODE){
	if(params){
		normalized_init = params->normalized_init;
		pt_based_sampling = params->pt_based_sampling;
		debug_mode = params->debug_mode;
	}
}
Affine::Affine(
	const ParamType *_params) : ProjectiveBase(_params),
	params(_params){

	printf("\n");
	printf("Using Affine SSM with:\n");
	printf("resx: %d\n", resx);
	printf("resy: %d\n", resy);
	printf("normalized_init: %d\n", params.normalized_init);
	printf("pt_based_sampling: %d\n", params.pt_based_sampling);
	printf("debug_mode: %d\n", params.debug_mode);

	name = "affine";
	state_size = 6;
	curr_state.resize(state_size);

	utils::getNormUnitSquarePts(norm_pts, norm_corners, resx, resy,
		1 - resx / 2.0, 1 - resy / 2.0, resx / 2.0, resy / 2.0);
	utils::homogenize(norm_pts, norm_pts_hm);
	utils::homogenize(norm_corners, norm_corners_hm);

	init_corners = getNormCorners();
	init_corners_hm = getHomNormCorners();
	init_pts = getNormPts();
	init_pts_hm = getHomNormPts();
}

void Affine::setCorners(const CornersT& corners){
	if(params.normalized_init){
		curr_warp = utils::computeAffineNDLT(init_corners, corners);
		getStateFromWarp(curr_state, curr_warp);

		curr_pts.noalias() = curr_warp.topRows<2>() * init_pts_hm;
		curr_corners.noalias() = curr_warp.topRows<2>() * init_corners_hm;

		utils::homogenize(curr_pts, curr_pts_hm);
		utils::homogenize(curr_corners, curr_corners_hm);
	} else {
		curr_corners = corners;
		utils::homogenize(curr_corners, curr_corners_hm);

		getPtsFromCorners(curr_warp, curr_pts, curr_pts_hm, curr_corners);

		init_corners = curr_corners;
		init_pts = curr_pts;
		utils::homogenize(init_corners, init_corners_hm);
		utils::homogenize(init_pts, init_pts_hm);

		curr_warp = Matrix3d::Identity();
		curr_state.fill(0);
	}
}

void Affine::compositionalUpdate(const VectorXd& state_update){
	validate_ssm_state(state_update);

	getWarpFromState(warp_update_mat, state_update);
	curr_warp = curr_warp * warp_update_mat;
	getStateFromWarp(curr_state, curr_warp);

	//curr_pts_hm.noalias() = curr_warp * init_pts_hm;
	//curr_corners_hm.noalias() = curr_warp * init_corners_hm;
	//utils::dehomogenize(curr_pts_hm, curr_pts);
	//utils::dehomogenize(curr_corners_hm, curr_corners);

	curr_pts.noalias() = curr_warp.topRows<2>() * init_pts_hm;
	curr_corners.noalias() = curr_warp.topRows<2>() * init_corners_hm;

	//utils::printMatrix(curr_warp, "curr_warp", "%15.9f");
	//utils::printMatrix(affine_warp_mat, "affine_warp_mat", "%15.9f");
}

void Affine::setState(const VectorXd &ssm_state){
	validate_ssm_state(ssm_state);
	curr_state = ssm_state;
	getWarpFromState(curr_warp, curr_state);
	curr_pts.noalias() = curr_warp.topRows<2>() * init_pts_hm;
	curr_corners.noalias() = curr_warp.topRows<2>() * init_corners_hm;
}

void Affine::getWarpFromState(Matrix3d &warp_mat,
	const VectorXd& ssm_state){
	validate_ssm_state(ssm_state);

	warp_mat(0, 0) = 1 + ssm_state(2);
	warp_mat(0, 1) = ssm_state(3);
	warp_mat(0, 2) = ssm_state(0);
	warp_mat(1, 0) = ssm_state(4);
	warp_mat(1, 1) = 1 + ssm_state(5);
	warp_mat(1, 2) = ssm_state(1);
	warp_mat(2, 0) = 0;
	warp_mat(2, 1) = 0;
	warp_mat(2, 2) = 1;
}

void Affine::getStateFromWarp(VectorXd &state_vec,
	const Matrix3d& warp_mat){
	validate_ssm_state(state_vec);
	VALIDATE_AFFINE_WARP(warp_mat);

	state_vec(0) = warp_mat(0, 2);
	state_vec(1) = warp_mat(1, 2);
	state_vec(2) = warp_mat(0, 0) - 1;
	state_vec(3) = warp_mat(0, 1);
	state_vec(4) = warp_mat(1, 0);
	state_vec(5) = warp_mat(1, 1) - 1;
}

void Affine::invertState(VectorXd& inv_state, const VectorXd& state){
	getWarpFromState(warp_mat, state);
	inv_warp_mat = warp_mat.inverse();
	inv_warp_mat /= inv_warp_mat(2, 2);
	getStateFromWarp(inv_state, inv_warp_mat);
}

void Affine::getInitPixGrad(Matrix2Xd &ssm_grad, int pix_id) {
	double x = init_pts(0, pix_id);
	double y = init_pts(1, pix_id);
	ssm_grad <<
		1, 0, x, y, 0, 0,
		0, 1, 0, 0, x, y;
}

void Affine::cmptInitPixJacobian(MatrixXd &dI_dp,
	const PixGradT &dI_dx){
	validate_ssm_jacobian(dI_dp, dI_dx);
	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);
		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			double Ix = dI_dx(ch_pt_id, 0);
			double Iy = dI_dx(ch_pt_id, 1);
			dI_dp(ch_pt_id, 0) = Ix;
			dI_dp(ch_pt_id, 1) = Iy;
			dI_dp(ch_pt_id, 2) = Ix * x;
			dI_dp(ch_pt_id, 3) = Ix * y;
			dI_dp(ch_pt_id, 4) = Iy * x;
			dI_dp(ch_pt_id, 5) = Iy * y;
			++ch_pt_id;
		}
	}
}

void Affine::cmptApproxPixJacobian(MatrixXd &dI_dp,
	const PixGradT &dI_dx) {
	validate_ssm_jacobian(dI_dp, dI_dx);
	double a = curr_state(2) + 1, b = curr_state(3);
	double c = curr_state(4), d = curr_state(5) + 1;
	double inv_det = 1.0 / (a*d - b*c);
	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);
		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			double Ix = dI_dx(ch_pt_id, 0);
			double Iy = dI_dx(ch_pt_id, 1);
			double Ixx = Ix * x;
			double Ixy = Ix * y;
			double Iyy = Iy * y;
			double Iyx = Iy * x;
			dI_dp(ch_pt_id, 0) = (Ix*d - Iy*c) * inv_det;
			dI_dp(ch_pt_id, 1) = (Iy*a - Ix*b) * inv_det;
			dI_dp(ch_pt_id, 2) = (Ixx*d - Iyx*c) * inv_det;
			dI_dp(ch_pt_id, 3) = (Ixy*d - Iyy*c) * inv_det;
			dI_dp(ch_pt_id, 4) = (Iyx*a - Ixx*b) * inv_det;
			dI_dp(ch_pt_id, 5) = (Iyy*a - Ixy*b) * inv_det;
			++ch_pt_id;
		}
	}
}

void Affine::cmptWarpedPixJacobian(MatrixXd &dI_dp,
	const PixGradT &dI_dx) {
	validate_ssm_jacobian(dI_dp, dI_dx);
	double a = curr_state(2) + 1, b = curr_state(3);
	double c = curr_state(4), d = curr_state(5) + 1;

	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);
		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			double Ix = dI_dx(ch_pt_id, 0);
			double Iy = dI_dx(ch_pt_id, 1);
			double Ixx = Ix * x;
			double Ixy = Ix * y;
			double Iyy = Iy * y;
			double Iyx = Iy * x;

			dI_dp(ch_pt_id, 0) = Ix*a + Iy*c;
			dI_dp(ch_pt_id, 1) = Ix*b + Iy*d;
			dI_dp(ch_pt_id, 2) = Ixx*a + Iyx*c;
			dI_dp(ch_pt_id, 3) = Ixy*a + Iyy*c;
			dI_dp(ch_pt_id, 4) = Ixx*b + Iyx*d;
			dI_dp(ch_pt_id, 5) = Ixy*b + Iyy*d;
			++ch_pt_id;
		}
	}
}
void Affine::cmptInitPixHessian(MatrixXd &d2I_dp2, const PixHessT &d2I_dw2,
	const PixGradT &dI_dw){
	validate_ssm_hessian(d2I_dp2, d2I_dw2, dI_dw);

	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);
		Matrix26d dw_dp;
		dw_dp <<
			1, 0, x, y, 0, 0,
			0, 1, 0, 0, x, y;
		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			Map<Matrix6d>(d2I_dp2.col(ch_pt_id).data()) = dw_dp.transpose()*
				Map<const Matrix2d>(d2I_dw2.col(ch_pt_id).data())*dw_dp;
			++ch_pt_id;
		}
	}
}
void Affine::cmptWarpedPixHessian(MatrixXd &d2I_dp2, const PixHessT &d2I_dw2,
	const PixGradT &dI_dw) {
	validate_ssm_hessian(d2I_dp2, d2I_dw2, dI_dw);
	double a2 = curr_state(2) + 1, a3 = curr_state(3);
	double a4 = curr_state(4), a5 = curr_state(5) + 1;
	Matrix2d dw_dx;
	dw_dx <<
		a2, a3,
		a4, a5;

	unsigned int ch_pt_id = 0;
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id) {
		spi_pt_check_mc(spi_mask, pt_id, ch_pt_id);

		double x = init_pts(0, pt_id);
		double y = init_pts(1, pt_id);

		Matrix26d dw_dp;
		dw_dp <<
			1, 0, x, y, 0, 0,
			0, 1, 0, 0, x, y;

		for(unsigned int ch_id = 0; ch_id < n_channels; ++ch_id){
			Map<Matrix6d>(d2I_dp2.col(ch_pt_id).data()) = dw_dp.transpose()*
				dw_dx.transpose()*Map<const Matrix2d>(d2I_dw2.col(ch_pt_id).data())*dw_dx*dw_dp;
			++ch_pt_id;
		}
	}
}
void Affine::updateGradPts(double grad_eps){
	Vector2d diff_vec_x_warped = curr_warp.topRows<2>().col(0) * grad_eps;
	Vector2d diff_vec_y_warped = curr_warp.topRows<2>().col(1) * grad_eps;

	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check(spi_mask, pt_id);

		grad_pts(0, pt_id) = curr_pts(0, pt_id) + diff_vec_x_warped(0);
		grad_pts(1, pt_id) = curr_pts(1, pt_id) + diff_vec_x_warped(1);

		grad_pts(2, pt_id) = curr_pts(0, pt_id) - diff_vec_x_warped(0);
		grad_pts(3, pt_id) = curr_pts(1, pt_id) - diff_vec_x_warped(1);

		grad_pts(4, pt_id) = curr_pts(0, pt_id) + diff_vec_y_warped(0);
		grad_pts(5, pt_id) = curr_pts(1, pt_id) + diff_vec_y_warped(1);

		grad_pts(6, pt_id) = curr_pts(0, pt_id) - diff_vec_y_warped(0);
		grad_pts(7, pt_id) = curr_pts(1, pt_id) - diff_vec_y_warped(1);
	}
}

void Affine::updateHessPts(double hess_eps){
	double hess_eps2 = 2 * hess_eps;

	Vector2d diff_vec_xx_warped = curr_warp.topRows<2>().col(0) * hess_eps2;
	Vector2d diff_vec_yy_warped = curr_warp.topRows<2>().col(1) * hess_eps2;
	Vector2d diff_vec_xy_warped = (curr_warp.topRows<2>().col(0) + curr_warp.topRows<2>().col(1)) * hess_eps;
	Vector2d diff_vec_yx_warped = (curr_warp.topRows<2>().col(0) - curr_warp.topRows<2>().col(1)) * hess_eps;

	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		spi_pt_check(spi_mask, pt_id);

		hess_pts(0, pt_id) = curr_pts(0, pt_id) + diff_vec_xx_warped(0);
		hess_pts(1, pt_id) = curr_pts(1, pt_id) + diff_vec_xx_warped(1);

		hess_pts(2, pt_id) = curr_pts(0, pt_id) - diff_vec_xx_warped(0);
		hess_pts(3, pt_id) = curr_pts(1, pt_id) - diff_vec_xx_warped(1);

		hess_pts(4, pt_id) = curr_pts(0, pt_id) + diff_vec_yy_warped(0);
		hess_pts(5, pt_id) = curr_pts(1, pt_id) + diff_vec_yy_warped(1);

		hess_pts(6, pt_id) = curr_pts(0, pt_id) - diff_vec_yy_warped(0);
		hess_pts(7, pt_id) = curr_pts(1, pt_id) - diff_vec_yy_warped(1);

		hess_pts(8, pt_id) = curr_pts(0, pt_id) + diff_vec_xy_warped(0);
		hess_pts(9, pt_id) = curr_pts(1, pt_id) + diff_vec_xy_warped(1);

		hess_pts(10, pt_id) = curr_pts(0, pt_id) - diff_vec_xy_warped(0);
		hess_pts(11, pt_id) = curr_pts(1, pt_id) - diff_vec_xy_warped(1);

		hess_pts(12, pt_id) = curr_pts(0, pt_id) + diff_vec_yx_warped(0);
		hess_pts(13, pt_id) = curr_pts(1, pt_id) + diff_vec_yx_warped(1);

		hess_pts(14, pt_id) = curr_pts(0, pt_id) - diff_vec_yx_warped(0);
		hess_pts(15, pt_id) = curr_pts(1, pt_id) - diff_vec_yx_warped(1);
	}
}

void Affine::estimateWarpFromCorners(VectorXd &state_update, const Matrix24d &in_corners,
	const Matrix24d &out_corners){
	validate_ssm_state(state_update);
	Matrix3d warp_update_mat = utils::computeAffineDLT(in_corners, out_corners);
	getStateFromWarp(state_update, warp_update_mat);
}

void Affine::estimateWarpFromPts(VectorXd &state_update, vector<uchar> &mask,
	const vector<cv::Point2f> &in_pts, const vector<cv::Point2f> &out_pts,
	const EstimatorParams &est_params){
	cv::Mat warp_mat_cv = estimateAffine(in_pts, out_pts, mask, est_params);
	state_update(0) = warp_mat_cv.at<double>(0, 2);
	state_update(1) = warp_mat_cv.at<double>(1, 2);
	state_update(2) = warp_mat_cv.at<double>(0, 0) - 1;
	state_update(3) = warp_mat_cv.at<double>(0, 1);
	state_update(4) = warp_mat_cv.at<double>(1, 0);
	state_update(5) = warp_mat_cv.at<double>(1, 1) - 1;
}

void Affine::applyWarpToCorners(Matrix24d &warped_corners, const Matrix24d &orig_corners,
	const VectorXd &ssm_state){
	getWarpFromState(warp_mat, ssm_state);
	for(unsigned int corner_id = 0; corner_id < 4; corner_id++){
		warped_corners(0, corner_id) = warp_mat(0, 0)*orig_corners(0, corner_id) + warp_mat(0, 1)*orig_corners(1, corner_id) +
			warp_mat(0, 2);
		warped_corners(1, corner_id) = warp_mat(1, 0)*orig_corners(0, corner_id) + warp_mat(1, 1)*orig_corners(1, corner_id) +
			warp_mat(1, 2);
	}
}

void Affine::applyWarpToPts(Matrix2Xd &warped_pts, const Matrix2Xd &orig_pts,
	const VectorXd &ssm_state){
	getWarpFromState(warp_mat, ssm_state);
	unsigned int n_pts = orig_pts.cols();
	for(unsigned int pt_id = 0; pt_id < n_pts; ++pt_id){
		warped_pts(0, pt_id) = warp_mat(0, 0)*orig_pts(0, pt_id) + warp_mat(0, 1)*orig_pts(1, pt_id) +
			warp_mat(0, 2);
		warped_pts(1, pt_id) = warp_mat(1, 0)*orig_pts(0, pt_id) + warp_mat(1, 1)*orig_pts(1, pt_id) +
			warp_mat(1, 2);
	}
}
Vector6d Affine::geomToState(const Vector6d &geom){
	double s = geom[2], r = geom[4];
	double theta = geom[3], phi = geom[5];
	double cos_theta = cos(theta), sin_theta = sin(theta);
	double cos_phi = cos(phi), sin_phi = sin(phi);
	double ccc = cos_theta*cos_phi*cos_phi;
	double ccs = cos_theta*cos_phi*sin_phi;
	double css = cos_theta*sin_phi*sin_phi;
	double scc = sin_theta*cos_phi*cos_phi;
	double scs = sin_theta*cos_phi*sin_phi;
	double sss = sin_theta*sin_phi*sin_phi;
	Vector6d state;
	state[0] = geom[0];
	state[1] = geom[1];
	state[2] = s*(ccc + scs + r*(css - scs)) - 1;
	state[3] = s*(r*(ccs - scc) - ccs - sss);
	state[4] = s*(scc - ccs + r*(ccs + sss));
	state[5] = s*(r*(ccc + scs) - scs + css) - 1;
	return state;
}
Vector6d Affine::stateToGeom(const Vector6d &est){
	Matrix2d A;
	A << est[2] + 1, est[3], est[4], est[5] + 1;
	JacobiSVD<Matrix2d> svd(A, ComputeFullU | ComputeFullV);
	Vector2d singular_vals = svd.singularValues();
	Matrix2d S = singular_vals.asDiagonal();
	Matrix2d V = svd.matrixV().transpose();
	Matrix2d U = svd.matrixU();
	if(U.determinant() < 0){
		Matrix2d U_temp;
		U_temp << U(0, 1), U(0, 0), U(1, 1), U(1, 0);
		U = U_temp;
		Matrix2d V_temp;
		V_temp << V(0, 1), V(0, 0), V(1, 1), V(1, 0);
		V = V_temp;
		Matrix2d S_temp;
		S_temp << S(1, 1), S(1, 0), S(0, 1), S(0, 0);
		S = S_temp;
	}
	Vector6d q;
	q[0] = est[0];
	q[1] = est[1];
	q[3] = atan2(U(1, 0) * V(0, 0) + U(1, 1) * V(0, 1),
		U(0, 0) * V(0, 0) + U(0, 1) * V(0, 1));

	double phi = atan2(V(0, 1), V(0, 0));
	const double pi = 3.14159265358979323846;
	if(phi <= -pi / 2){
		double cos_phi = cos(-pi / 2);
		double sin_phi = sin(-pi / 2);
		Matrix2d R;
		R << cos_phi, -sin_phi, sin_phi, cos_phi;
		V = V * R;
		S = R.transpose()*S*R;
	}

	if(phi >= pi / 2){
		double cos_phi = cos(pi / 2);
		double sin_phi = sin(pi / 2);
		Matrix2d R;
		R << cos_phi, -sin_phi, sin_phi, cos_phi;
		V = V * R;
		S = R.transpose()*S*R;
	}
	q[2] = S(0, 0);
	q[4] = S(1, 1) / S(0, 0);
	q[5] = atan2(V(0, 1), V(0, 0));
	return q;
}

void Affine::generatePerturbation(VectorXd &perturbation){
	assert(perturbation.size() == state_size);
	if(params.pt_based_sampling){
		//! perturb three canonical points and estimate affine transformation using DLT
		Matrix23d orig_pts, perturbed_pts;
		//! use the bottom left, bottom right and top center points
		//! as canaonical points to add the random perturbations to;
		orig_pts.col(0) = init_corners.col(2);
		orig_pts.col(1) = init_corners.col(3);
		orig_pts.col(2) = (init_corners.col(0) + init_corners.col(1)) / 2.0;

		if(params.pt_based_sampling == 1){
			perturbed_pts = orig_pts;
			perturbed_pts(0, 0) += rand_dist[0](rand_gen[0]);
			perturbed_pts(1, 0) += rand_dist[1](rand_gen[1]);
			perturbed_pts(0, 1) += rand_dist[2](rand_gen[2]);
			perturbed_pts(1, 1) += rand_dist[3](rand_gen[3]);
			perturbed_pts(0, 2) += rand_dist[4](rand_gen[4]);
			perturbed_pts(1, 2) += rand_dist[5](rand_gen[5]);
		} else {
			//! different perturbation for x,y coordinates of each point
			//! followed by consistent translational perturbation to all corners
			Matrix23d rand_d;
			for(unsigned int pt_id = 0; pt_id < 3; ++pt_id){
				rand_d(0, pt_id) = rand_dist[1](rand_gen[1]);
				rand_d(1, pt_id) = rand_dist[1](rand_gen[1]);
			}
			perturbed_pts = (orig_pts + rand_d).colwise() + Vector2d(rand_dist[0](rand_gen[0]), rand_dist[0](rand_gen[0]));
		}
		Matrix3d aff_warp = utils::computeAffineDLT(orig_pts, perturbed_pts);
		getStateFromWarp(perturbation, aff_warp);
	} else{
		//! perform geometric perturbation
		Vector6d geom_perturbation;
		for(unsigned int state_id = 0; state_id < 6; state_id++){
			geom_perturbation(state_id) = rand_dist[state_id](rand_gen[state_id]);
		}
		perturbation = geomToState(geom_perturbation);
	}

}

// use Random Walk model to generate perturbed sample
void Affine::additiveRandomWalk(VectorXd &perturbed_state,
	const VectorXd &base_state){
	if(params.pt_based_sampling){
		throw mtf::utils::FunctonNotImplemented("Affine::additiveRandomWalk :: point based sampling is not implemented yet");
	} else{
		Vector6d geom_perturbation;
		for(unsigned int state_id = 0; state_id < 6; ++state_id){
			geom_perturbation(state_id) = rand_dist[state_id](rand_gen[state_id]);
		}
		Vector6d base_geom = stateToGeom(base_state);
		Vector6d perturbed_geom = base_geom + geom_perturbation;
		perturbed_state = geomToState(perturbed_geom);
	}
}

// use first order Auto Regressive model to generate perturbed sample
void Affine::additiveAutoRegression1(VectorXd &perturbed_state, VectorXd &perturbed_ar,
	const VectorXd &base_state, const VectorXd &base_ar, double a){
	if(params.pt_based_sampling){
		throw mtf::utils::FunctonNotImplemented("Affine::additiveAutoRegression1 :: point based sampling is not implemented yet");
	} else{
		Vector6d geom_perturbation;
		for(unsigned int state_id = 0; state_id < 6; ++state_id){
			geom_perturbation(state_id) = rand_dist[state_id](rand_gen[state_id]);
		}
		Vector6d base_geom = stateToGeom(base_state);
		Vector6d base_ar_geom = stateToGeom(base_ar);
		Vector6d perturbed_geom = base_geom + base_ar_geom + geom_perturbation;
		Vector6d perturbed_ar_geom = a*(perturbed_geom - base_geom);
		perturbed_state = geomToState(perturbed_geom);
		perturbed_ar = geomToState(perturbed_ar_geom);
	}
}
void Affine::compositionalRandomWalk(VectorXd &perturbed_state,
	const VectorXd &base_state){
	if(params.pt_based_sampling){
		generatePerturbation(state_perturbation);
		ProjWarpT base_warp, warp_perturbation;
		getWarpFromState(base_warp, base_state);
		getWarpFromState(warp_perturbation, state_perturbation);
		ProjWarpT perturbed_warp = base_warp * warp_perturbation;
		getStateFromWarp(perturbed_state, perturbed_warp);
	} else{
		throw mtf::utils::FunctonNotImplemented("Affine::compositionalRandomWalk :: geometric sampling is not implemented yet");

	}
}
_MTF_END_NAMESPACE


#include "eigen-dense.h"
#include <Eigen/Core>
#include <Eigen/LU>

using namespace Eigen;

template <class T>
Map< Matrix<T,Dynamic,Dynamic> > matrix(void* p, int r, int c) {
    return Map< Matrix<T,Dynamic,Dynamic> >((T*)p, r, c);
}

template <class T>
Map< Matrix<T,Dynamic,Dynamic> > matrix(const void* p, int r, int c) {
    return Map< Matrix<T,Dynamic,Dynamic> >((T*)p, r, c);
}

#define RET const char*

#define API(name,args,call) \
extern "C" RET eigen_##name args {\
    GUARD_START\
    switch (code) {\
        case 0: return name<T0>call;\
        case 1: return name<T1>call;\
        case 2: return name<T2>call;\
        case 3: return name<T3>call;\
    }\
    GUARD_END\
}

#define BINOP(name,op) \
template <class T>\
RET name(void* p, int r, int c,\
    const void* p1, int r1, int c1,\
    const void* p2, int r2, int c2)\
{\
    matrix<T>(p,r,c) = matrix<T>(p1,r1,c1) op matrix<T>(p2,r2,c2);\
    return 0;\
}\
API(name, (int code,\
    void* p, int r, int c,\
    const void* p1, int r1, int c1,\
    const void* p2, int r2, int c2), (p,r,c,p1,r1,c1,p2,r2,c2));

BINOP(add,+);
BINOP(sub,-);
BINOP(mul,*);

#define PROP(name) \
extern "C" RET __attribute__ ((noinline)) eigen_##name(int code, void* q, const void* p, int r, int c) {\
        GUARD_START\
        switch (code) {\
            case 0: *(T0*)q = matrix<T0>(p,r,c).name(); break;\
            case 1: *(T1*)q = matrix<T1>(p,r,c).name(); break;\
            case 2: *(T2*)q = matrix<T2>(p,r,c).name(); break;\
            case 3: *(T3*)q = matrix<T3>(p,r,c).name(); break;\
        }\
        GUARD_END\
    }

PROP(norm);
PROP(squaredNorm);
PROP(blueNorm);
PROP(hypotNorm);
PROP(sum);
PROP(prod);
PROP(mean);
PROP(trace);
PROP(determinant);

#define UNOP(name) \
extern "C" RET __attribute__((noinline)) eigen_##name(int code, void* p, int r, int c, const void* p1, int r1, int c1) {\
        GUARD_START\
        switch (code) {\
            case 0: matrix<T0>(p,r,c) = matrix<T0>(p1,r1,c1).name(); break;\
            case 1: matrix<T1>(p,r,c) = matrix<T1>(p1,r1,c1).name(); break;\
            case 2: matrix<T2>(p,r,c) = matrix<T2>(p1,r1,c1).name(); break;\
            case 3: matrix<T3>(p,r,c) = matrix<T3>(p1,r1,c1).name(); break;\
        }\
        GUARD_END\
    }

UNOP(inverse);
UNOP(adjoint);
UNOP(conjugate);
UNOP(diagonal);
UNOP(transpose);

extern "C" RET eigen_normalize(int code, void* p, int r, int c)
{
    GUARD_START
    switch (code) {
        case 0: matrix<T0>(p,r,c).normalize(); break;
        case 1: matrix<T1>(p,r,c).normalize(); break;
        case 2: matrix<T2>(p,r,c).normalize(); break;
        case 3: matrix<T3>(p,r,c).normalize(); break;
    }
    GUARD_END
}

extern "C" RET eigen_random(int code, void* p, int r, int c)
{
    GUARD_START
    switch (code) {
        case 0: matrix<T0>(p,r,c) = MatrixXf::Random(r,c); break;
        case 1: matrix<T1>(p,r,c) = MatrixXd::Random(r,c); break;
        case 2: matrix<T2>(p,r,c) = MatrixXcf::Random(r,c); break;
        case 3: matrix<T3>(p,r,c) = MatrixXcd::Random(r,c); break;
    }
    GUARD_END
}

extern "C" RET eigen_identity(int code, void* p, int r, int c)
{
    GUARD_START
    switch (code) {
        case 0: matrix<T0>(p,r,c) = MatrixXf::Identity(r,c); break;
        case 1: matrix<T1>(p,r,c) = MatrixXd::Identity(r,c); break;
        case 2: matrix<T2>(p,r,c) = MatrixXcf::Identity(r,c); break;
        case 3: matrix<T3>(p,r,c) = MatrixXcd::Identity(r,c); break;
    }
    GUARD_END
}


#include "PhysicsTools/Utilities/interface/Likelihood.h"
#include "PhysicsTools/Utilities/interface/BreitWigner.h"
#include "PhysicsTools/Utilities/interface/RootMinuitCommands.h"
#include "PhysicsTools/Utilities/interface/RootMinuit.h"
#include "PhysicsTools/Utilities/interface/Parameter.h"
#include "PhysicsTools/Utilities/interface/rootTf1.h"
#include "PhysicsTools/Utilities/interface/rootPlot.h"
#include "TFile.h"
#include "TH1.h"
#include "TF1.h"
#include "TCanvas.h"
#include "TROOT.h"
#include <boost/shared_ptr.hpp>
#include <iostream>
#include "PhysicsTools/Utilities/interface/Operations.h"
//using namespace std;
//using namespace boost;

int main() {
  gROOT->SetStyle("Plain");
  typedef funct::BreitWigner PDF;
  typedef std::vector<double> Sample;
  typedef funct::Product<funct::Parameter, PDF>::type FitFunction;
  typedef fit::Likelihood<Sample, FitFunction, funct::Parameter> Likelihood;
  try {
    fit::RootMinuitCommands<Likelihood> commands("PhysicsTools/Utilities/test/testZMassFitLikelihood.txt");

    const char* kYield = "Yield";
    const char* kMass = "Mass";
    const char* kGamma = "Gamma";

    funct::Parameter yield(kYield, commands.par(kYield));
    funct::Parameter mass(kMass, commands.par(kMass));
    funct::Parameter gamma(kGamma, commands.par(kGamma));
    funct::BreitWigner bw(mass, gamma);

    PDF pdf = bw;
    FitFunction f = yield * pdf;
    TF1 startFun = root::tf1("startFun", f, 0, 200, yield, mass, gamma);
    TH1D histo("histo", "Z mass (GeV/c)", 200, 0, 200);
    Sample sample;
    sample.reserve(yield);
    for (unsigned int i = 0; i < yield; ++i) {
      double m = startFun.GetRandom();
      histo.Fill(m);
      sample.push_back(m);
    }
    TCanvas canvas;
    startFun.Draw();
    canvas.SaveAs("breitWigner.eps");
    histo.Draw();
    canvas.SaveAs("breitWignerHisto.eps");
    startFun.Draw("same");
    canvas.SaveAs("breitWignerHistoFun.eps");
    histo.Draw("e");
    startFun.Draw("same");

    Likelihood like(sample, f, yield);
    fit::RootMinuit<Likelihood> minuit(like, true);
    commands.add(minuit, yield);
    commands.add(minuit, mass);
    commands.add(minuit, gamma);
    commands.run(minuit);
    ROOT::Math::SMatrix<double, 3, 3, ROOT::Math::MatRepSym<double, 3> > err;
    minuit.getErrorMatrix(err);
    std::cout << "error matrix:" << std::endl;
    for (size_t i = 0; i < 3; ++i) {
      for (size_t j = 0; j < 3; ++j) {
        std::cout << err(i, j) << "\t";
      }
      std::cout << std::endl;
    }
    root::plot<FitFunction>("breitWignerHistoFunFit.eps", histo, f, 80, 120, yield, mass, gamma);
  } catch (std::exception& err) {
    std::cerr << "Exception caught:\n" << err.what() << std::endl;
    return 1;
  }

  return 0;
}


/*
	Satellite Attitude Dynamics Stepper

	@author		:	siddharth deore
	@licence	:	MIT
*/

#include "Satellite.h"
#include <iostream>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include "ode_wrapper.h"

#include <fstream>
#include <windows.h>
HANDLE hOut= GetStdHandle(STD_OUTPUT_HANDLE);;
void clearScreen()
{
	COORD Position;

	Position.X = 0;
	Position.Y = 0;
	SetConsoleCursorPosition(hOut, Position);
}

// set bool visible = 0 - invisible, bool visible = 1 - visible
void setcursor(bool visible, DWORD size)
{
	if (size == 0)
	{
		// default cursor size Changing to numbers from 1 to 20, decreases cursor width
		size = 20;
	}
	CONSOLE_CURSOR_INFO lpCursor;
	lpCursor.bVisible = visible;
	lpCursor.dwSize = size;
	SetConsoleCursorInfo(hOut, &lpCursor);
}
namespace odeint = boost::numeric::odeint;

// Static variables common to all instance
double Satellite::Ixx = 1.0;
double Satellite::Iyy = 1.0;
double Satellite::Izz = 1.0;

double Satellite::Kp = 0.0;
double Satellite::Kd = 1.0;

double Satellite::qd[4] = { 1.0,0.0,0.0,0.0 };

void Satellite::Satelliite() {
	state_type X = { { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  } }; // initial conditions
}

int Satellite::setQuaternion(double q0, double q1, double q2, double q3) {
	this->X[0] = q0;
	this->X[1] = q1;
	this->X[2] = q2;
	this->X[3] = q3;
	return 0;
}

int Satellite::setAngulerVeolcities(double wx, double wy, double wz) {
	this->X[4] = wx;
	this->X[5] = wy;
	this->X[6] = wz;
	return 0;
}

int Satellite::setState(double q0, double q1, double q2, double q3, double wx, double wy, double wz) {
	setQuaternion(q0, q1, q2, q3);
	setAngulerVeolcities(wx, wy, wz);
	return 0;
}

int Satellite::getState(double& q0, double& q1, double& q2, double& q3, double& wx, double& wy, double& wz) {
	q0 = this->X[0];
	q1 = this->X[1];
	q2 = this->X[2];
	q3 = this->X[3];
	wx = this->X[5];
	wy = this->X[6];
	wz = this->X[7];
	return 0;
}

void Satellite::dynamics(const state_type& x, state_type& dxdt, const double t) {
	double q0 = x[0];
	double q1 = x[1];
	double q2 = x[2];
	double q3 = x[3];
	double norm = normalizeQuaternions(q0, q1, q2, q3);
	dxdt[0] = 0.5 * (q1 * x[6] - q2 * x[5] + q3 * x[4]);
	dxdt[1] = 0.5 * (q2 * x[4] - q0 * x[6] + q3 * x[5]);
	dxdt[2] = 0.5 * (q0 * x[5] - q1 * x[4] + q3 * x[6]);
	dxdt[3] = 0.5 * (-q0 * x[4] - q1 * x[5] - q2 * x[6]);

	qe[0] = q0 * this->qd[3] + q1 * this->qd[2] - q2 * this->qd[1] - q3 * this->qd[0];
	qe[1] = q2 * this->qd[0] - q0 * this->qd[2] + q1 * this->qd[3] - q3 * this->qd[1];
	qe[2] = q0 * this->qd[1] - q1 * this->qd[0] + q2 * this->qd[3] - q3 * this->qd[2];
	qe[3] = q0 * this->qd[0] + q1 * this->qd[1] + q2 * this->qd[2] + q3 * this->qd[3];
	double PD[3] = {
					this.Kp * qe[0] * qe[3] + this.Kd * x[4],
					this.Kp * qe[1] * qe[3] + this.Kd * x[5],
					this.Kp * qe[2] * qe[3] + this.Kd * x[6]
				};

	dxdt[4] = ((this->Iyy - this->Izz) * x[5] * x[6] - PD[0]) / this->Ixx;
	dxdt[5] = ((this->Izz - this->Ixx) * x[6] * x[4] - PD[1]) / this->Iyy;
	dxdt[6] = ((this->Ixx - this->Iyy) * x[4] * x[5] - PD[2]) / this->Izz;
}

double Satellite::normalizeQuaternions(double& _q0, double& _q1, double& _q2, double& _q3)
{
	double norm = std::sqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3);
	_q0 /= norm;
	_q1 /= norm;
	_q2 /= norm;
	_q3 /= norm;
	return norm;
}

int Satellite::step(double final_time, double dt, state_type& new_state)
{
	odeint::integrate(
		make_ode_wrapper(Satellite(), &Satellite::dynamics), // ODE funtion
		X,				// Initial state
		0.0,			// initial time
		final_time,		// final time
		dt,				// timestep
		make_observer_wrapper(Satellite(), &Satellite::write_state)// observer function
	);

	// return value to caller
	new_state = X;
	return 0;
}

void Satellite::setTargetQuaternion(double q0, double q1, double q2, double q3)
{
	this->qd[0] = q0;
	this->qd[1] = q1;
	this->qd[2] = q2;
	this->qd[3] = q3;
}

void Satellite::write_state(const state_type& state, const double t) {

	clearScreen(); // clear screen
	setcursor(0, 1); // Hide cursor

	std::cout << char(218) << std::string(83, char(196))<< char(191) << std::endl;
	std::cout<<"|     time    |    q0   |    q1   |    q2   |    q3   |    wx   |    wy   |    wz   |" << std::endl;
	std::cout << char(195) << std::string(83, char(196)) << char(180) << std::endl;
	std::cout << "|" << std::setw(12) << std::fixed << std::setprecision(4) << t << " |";
	std::cout << std::setw(8) << state[0] << " |";
	std::cout << std::setw(8) << state[1] << " |";
	std::cout << std::setw(8) << state[2] << " |";
	std::cout << std::setw(8) << state[3] << " |";
	std::cout << std::setw(8) << state[4] << " |";
	std::cout << std::setw(8) << state[5] << " |";
	std::cout << std::setw(8) << state[6] << " |" << std::endl;
	std::cout << char(192) << std::string(83, char(196)) << char(217) << std::endl;

	//Progress bar
	std::cout << std::string(int(t / 10000 * 85), char(178)) << std::string(int(85 - t / 10000 * 85), char(176)) << std::endl;

	// write to file
	std::string s =
		std::to_string(t) +		   ", " +
		std::to_string(state[0]) + ", " +
		std::to_string(state[1]) + ", " +
		std::to_string(state[2]) + ", " +
		std::to_string(state[3]) + ", " +
		std::to_string(state[4]) + ", " +
		std::to_string(state[5]) + ", " +
		std::to_string(state[6]) + "\n";
	std::ofstream outfile;

	outfile.open("results.csv", std::ios_base::app); // append instead of overwrite
	outfile << s;
	outfile.close();
	//

}

void Satellite::setInnertia(double Ix, double Iy, double Iz) {
	this->Ixx = Ix;
	this->Iyy = Iy;
	this->Izz = Iz;
}

void Satellite::setControllerGains(double Kp, double Kd)
{
	this->Kp = Kp;
	this->Kd = Kd;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2012.
// 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 <mpllibs/metamonad/lazy.hpp>
#include <mpllibs/metamonad/metafunction.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/eval_if.hpp>

#include <iostream>

using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::less;
using boost::mpl::eval_if;
using boost::mpl::int_;

using mpllibs::metamonad::lazy;

// Without lazy
///////////////

template <class N>
struct fib_without_lazy;

MPLLIBS_METAFUNCTION(fib_without_lazy_helper, (N))
((
  plus<
    typename fib_without_lazy<typename minus<N, int_<1> >::type>::type,
    typename fib_without_lazy<typename minus<N, int_<2> >::type>::type
  >
));

MPLLIBS_METAFUNCTION(fib_without_lazy, (N))
((
  eval_if<
    typename less<N, int_<2> >::type,
    int_<1>,
    fib_without_lazy_helper<N>
  >
));

// With lazy
////////////

MPLLIBS_METAFUNCTION(fib, (N))
((
  eval_if<
    typename less<N, int_<2> >::type,
    int_<1>,
    lazy<plus<fib<minus<N, int_<1> > >, fib<minus<N, int_<2> > > > >
  >
));

///////////

int main()
{
  using std::endl;

  std::cout
    << "With lazy: " << endl
    << "fib(0) == " << fib_without_lazy<int_<0> >::type::value << endl
    << "fib(1) == " << fib_without_lazy<int_<1> >::type::value << endl
    << "fib(2) == " << fib_without_lazy<int_<2> >::type::value << endl
    << "fib(3) == " << fib_without_lazy<int_<3> >::type::value << endl
    << "fib(4) == " << fib_without_lazy<int_<4> >::type::value << endl
    << "fib(5) == " << fib_without_lazy<int_<5> >::type::value << endl
    << endl
    << "With lazy: " << endl
    << "fib(0) == " << fib<int_<0> >::type::value << endl
    << "fib(1) == " << fib<int_<1> >::type::value << endl
    << "fib(2) == " << fib<int_<2> >::type::value << endl
    << "fib(3) == " << fib<int_<3> >::type::value << endl
    << "fib(4) == " << fib<int_<4> >::type::value << endl
    << "fib(5) == " << fib<int_<5> >::type::value << endl;
}


//==================================================================================================
/*
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
*/
//==================================================================================================
#pragma once

#include <eve/detail/overload.hpp>
#include <eve/module/core.hpp>
#include <eve/module/math.hpp>
#include <eve/module/complex.hpp>
#include <eve/module/complex/regular/traits.hpp>
#include <boost/math/complex/atanh.hpp>
namespace eve
{

  namespace detail
  {
    template<typename Z>
    EVE_FORCEINLINE auto complex_unary_dispatch( eve::tag::atanh_, Z const& a0) noexcept
    {
      // This implementation is a simd (i.e. no branch) transcription and adaptation of the
      // boost_math code which itself is a transcription of the pseudo-code in:
      //
      // Eric W. Weisstein. "Inverse Hyperbolic Tangent."
      // From MathWorld--A Wolfram Web Resource.
      // http://mathworld.wolfram.com/InverseHyperbolicTangent.html
      //
      // Also: The Wolfram Functions Site,
      // http://functions.wolfram.com/ElementaryFunctions/ArcTanh/
      //
      // Also "Abramowitz and Stegun. Handbook of Mathematical Functions."
      // at : http://jove.prohosting.com/~skripty/toc.htm
      //
      auto [a0r, a0i] = a0;
      auto realinf = is_eqz(a0i) && is_infinite(a0r);
      using rtype = decltype(a0r);
      const rtype alpha_crossover(0.3);
      auto  ltzra0 = is_ltz(a0r);
      auto  ltzia0 = is_ltz(a0i);
      auto s_min = eve::sqrtsmallestposval(as(a0r))*2;
      auto s_max = eve::sqrtvalmax(as(a0r))/2;
      rtype const two = rtype(2);
      rtype inf =  eve::inf(as(a0r));
      rtype x = eve::abs(a0r);
      rtype y = eve::abs(a0i);
      rtype r = zero(as(a0r));
      rtype i = zero(as(a0r));
      auto gtxmax = (x > s_max);
      auto ltxmin = (x < s_min);
      auto gtymax = (y > s_max);
      auto ltymin = (y < s_min);
      rtype xx = eve::sqr(x);
      rtype yy = eve::sqr(y);
      rtype sqrabs = xx + yy;

      auto not_in_safe_zone = ((gtxmax || ltxmin) || (gtymax || ltymin));
      if(eve::any(not_in_safe_zone))
      {
        //treat underflow or overflow
        // one or both of x and y are small, calculate divisor carefully:
        rtype div = one(as(a0r));
        div += eve::if_else(ltxmin, xx, zero);
        div += eve::if_else(ltxmin, yy, zero);

        rtype alpha = x/div;
        alpha += alpha;

        auto test =  gtymax;
        // big y, medium x, divide through by y:
        rtype tmp_alpha = (two*x/y) / (y + xx/y);
        // small x and y, whatever alpha is, it's too small to calculate:
        tmp_alpha = eve::if_else(x > one(as(a0r)), tmp_alpha, zero);
        alpha = eve::if_else(test && (x > one(as(a0r))), tmp_alpha, alpha);

        test =  eve::logical_andnot(gtxmax, test);

        // big x small y, as above but neglect y^2/x:
        tmp_alpha =  two/x;
        // big x: divide through by x:
        tmp_alpha =  eve::if_else((y > one(as(a0r))),  two / (x + y*y/x), tmp_alpha);
        // big x and y: divide alpha through by x*y:
        tmp_alpha =  eve::if_else(gtymax, (two/y) / (x/y + y/x), tmp_alpha);
        // x or y are infinite: the result is 0
        tmp_alpha = eve::if_else((y == inf) || (x == inf), zero, tmp_alpha);

        alpha = eve::if_else(test, tmp_alpha, alpha);
        r = eve::if_else((alpha < alpha_crossover),
                        eve::log1p(alpha) - eve::log1p(-alpha),
                         eve::log(inc(two*x + xx)) - eve::log(sqr(dec(x)))
                       );
        test = (x == one(as(a0r))) && ltymin;
        r = eve::if_else(test, -(two*(eve::log(y) - eve::log_2(as(a0r)))), r);
        r *= rtype(0.25);
        //compute the imag part
        // y^2 is negligible:
        i =  eve::atan2(two*y, eve::oneminus(xx));
        i =  if_else(gtymax || gtxmax, pi(as(a0r)), i);
        rtype tmp_i = eve::if_else(ltymin, atan2(two*y, one(as(a0r))),
                                  eve::atan2(two*y, eve::oneminus(yy)));
        i =  if_else(ltxmin, tmp_i, i);
      }
      auto test0 = (inf == x) && (inf == y);
      if(eve::any(test0))
      {
        //inf x, inf y
        r = eve::if_else(test0, zero, r);
        i = eve::if_else(test0, pi(as(a0r)), r);
      }
      auto test = eve::is_nan(a0);

      if(eve::any(test))
      {
        //nan x, inf y
        r = eve::if_else(eve::is_nan(x) && (y == inf), zero, r);
        i = eve::if_else(eve::is_nan(x) && (y == inf), pi(as(a0r)), r);

        r = eve::if_else(is_nan(y) && (x == inf), zero, r);
        i = eve::if_else(is_nan(y) && (x == inf), y, i);

        r = eve::if_else(is_nan(y) && eve::is_eqz(x), zero, r);
        i = eve::if_else(is_nan(y) && is_eqz(x), allbits, i);
      }
      //compute for safe zone
      // the real part is given by:
      //
      // eve::real(atanh(z)) == log((1 + x^2 + y^2 + 2x) / (1 + x^2 + y^2 - 2x))
      //
      // however, when x is either large (x > 1/e) or very small
      // (x < e) then this effectively simplifies
      // to log(1), leading to wildly inaccurate results.
      // by dividing the above (top and bottom) by (1 + x^2 + y^2) we get:
      //
      // eve::real(atanh(z)) == log((1 + (2x / (1 + x^2 + y^2))) / (1 - (-2x / (1 + x^2 + y^2))))
      //
      // which is much more sensitive to the value of x, when x is not near 1
      // (remember we can compute log(1+x) for small x very accurately).
      //
      // the cross-over from one method to the other has to be determined
      // experimentally, the value used below appears correct to within a
      // factor of 2 (and there are larger errors from other parts
      // of the input domain anyway).
      //
      rtype alpha = x*two / (eve::inc(sqrabs));
      rtype sqrxm1 = eve::sqr(eve::dec(x));
      rtype tmp_r = eve::if_else((alpha < alpha_crossover),
                                eve::log1p(alpha) - log1p(-alpha),
                                eve::log1p(x+x + sqrabs) - eve::log(sqrxm1 + yy)
                                )*rtype(0.25);
      r = eve::if_else(not_in_safe_zone, r, tmp_r);

      // compute the imag part
      i = eve::if_else(not_in_safe_zone,
                      i,
                      eve::atan2(y+y, (oneminus(sqrabs)))
                     )*half(as(a0r));

      r = eve::if_else( ltzra0,-r, r);
      i = eve::if_else(is_infinite(y), pio_2(as(a0r))*sign(y), i);
      i = eve::if_else( ltzia0,-i, i);
      r = if_else(realinf, zero(as(a0r)), r);
      i = if_else(realinf, -sign(a0r)*pio_2(as(a0r)), i);
      return  Z{r, i};
    }
  }
}


/*
 * demo.cpp
 *
 *  Created on: 25.09.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/mpi.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/point.h>
#include <deal.II/base/quadrature.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria.h>

#include <base/ConstantMesh.h>
#include <base/DiscretizedFunction.h>
#include <base/Util.h>
#include <forward/WaveEquation.h>

#include <stddef.h>
#include <ctgmath>
#include <exception>
#include <fstream>
#include <iostream>
#include <memory>
#include <vector>

using namespace dealii;
using namespace wavepi;
using namespace wavepi::forward;
using namespace wavepi::base;

template <int dim>
class DemoF : public Function<dim> {
 public:
  static const Point<dim> center;

  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    if (p.distance(center) < 0.75)
      return std::sin(this->get_time() * 2 * numbers::PI);
    else
      return 0.0;
  }
};

template <int dim>
class DemoC : public Function<dim> {
 public:
  static const Point<dim> center;

  virtual ~DemoC() = default;

  virtual double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    if (p.distance(center) < 2)
      return 1.0 / (4.0 + (1.0 - std::pow(p.distance(center) / 2, 4)) * 12.0 *
                              std::pow(std::sin(this->get_time() / 2.5 * numbers::PI), 2));
    else
      return 1.0 / (4.0 + 0.0);
  }
};

template <int dim>
class DemoWaveSpeed : public Function<dim> {
 public:
  DemoC<dim> base;

  virtual ~DemoWaveSpeed() = default;

  virtual double value(const Point<dim> &p, const unsigned int component = 0) const {
    return 1.0 / std::sqrt(base.value(p, component));
  }

  virtual void set_time(double t) {
    Function<dim>::set_time(t);
    base.set_time(t);
  }
};

template <>
const Point<2> DemoF<2>::center = Point<2>(1.0, 0.0);
template <>
const Point<2> DemoC<2>::center = Point<2>(0.0, 2.5);

template <int dim>
void demo() {
  std::ofstream logout("wavepi_demo.log", std::ios_base::trunc);
  deallog.attach(logout);
  deallog.depth_console(3);
  deallog.depth_file(100);
  deallog.precision(3);
  deallog.pop();

  auto triangulation = std::make_shared<Triangulation<dim>>();
  GridGenerator::hyper_cube(*triangulation, -5.0, 5.0);
  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(6);

  double t_end = 10;
  int steps    = t_end * 64;

  double t_start = 0.0, dt = t_end / steps;
  std::vector<double> times;

  for (size_t i = 0; t_start + i * dt <= t_end; i++)
    times.push_back(t_start + i * dt);

  FE_Q<dim> fe(1);
  Quadrature<dim> quad = QGauss<dim>(3);

  auto mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);
  WaveEquation<dim> wave_eq(mesh);

  DemoC<dim> demo_c_cont;
  auto demo_c = std::make_shared<DiscretizedFunction<dim>>(mesh, demo_c_cont);
  wave_eq.set_param_c(demo_c);

  auto sol = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(std::make_shared<DemoF<dim>>()));
  sol.write_pvd("./", "demo_u", "u");
  demo_c->write_pvd("./", "demo_c", "c");

  DemoWaveSpeed<dim> demo_wave_speed_cont;
  auto demo_wave_speed = std::make_shared<DiscretizedFunction<dim>>(mesh, demo_wave_speed_cont);
  demo_wave_speed->write_pvd("./", "demo_wave_speed", "wave speed");
}

int main(int argc, char *argv[]) {
  Utilities::MPI::MPI_InitFinalize mpi_init(argc, argv);

  try {
    demo<2>();
  } catch (std::exception &exc) {
    std::cerr << "Exception on processing: " << exc.what() << std::endl;
    return 1;
  } catch (...) {
    std::cerr << "Unknown exception!" << std::endl;
    return 1;
  }

  return 0;
}


#include "panel.h"

#include <iostream>

#include <Eigen/Dense>

namespace poflow {

void PanelSolver::init_arrays() {
  num_points_ = num_elements_ + 1;
  FN = Eigen::MatrixXd::Zero(num_elements_, num_elements_);
  FT = Eigen::MatrixXd::Zero(num_elements_, num_elements_);
  R = Eigen::VectorXd::Zero(num_elements_);
  source_density_ = Eigen::VectorXd::Zero(num_elements_);
}

void PanelSolver::build() { init_arrays(); }

void PanelSolver::build_equation_system(double vx, double vy) {
  double pi = 3.14159;
  for (int k = 0; k < num_elements_; ++k) {
    R(k) = vx * panels_[k].si - vy * panels_[k].ci;
    auto kth_panel = panels_[k];
    for (int j = 0; j < num_elements_; ++j) {
      auto jth_panel = panels_[j];
      if (k == j) {
        FN(k, j) = 2.0 * pi;
        FT(k, j) = 0.0;
      } else {
        double dyj = jth_panel.si * jth_panel.ds;
        double dxj = jth_panel.ci * jth_panel.ds;
        double sph = jth_panel.ds / 2.0;

        double xd = kth_panel.xc - jth_panel.xc;
        double yd = kth_panel.yc - jth_panel.yc;
        double rkj = std::sqrt(xd * xd + yd * yd);

        double bkj = std::atan2(yd, xd);
        double alj = std::atan2(dyj, dxj);
        double gkj = alj - bkj;

        double zik = rkj * std::cos(gkj);
        double etk = -rkj * std::sin(gkj);

        double r1s = std::pow((zik + sph), 2.0) + std::pow(etk, 2.0);
        double r2s = std::pow((zik - sph), 2.0) + std::pow(etk, 2.0);
        double qt = std::log(r1s / r2s);

        double den = zik * zik + etk * etk - sph * sph;
        double gnm = etk * jth_panel.ds;
        double qn = 2.0 * std::atan2(gnm, den);

        double ukj = qt * jth_panel.ci - qn * jth_panel.si;
        double vkj = qt * jth_panel.si + qn * jth_panel.ci;

        FN(k, j) = -ukj * kth_panel.si + vkj * kth_panel.ci;
        FT(k, j) = ukj * kth_panel.ci + vkj * kth_panel.si;
      }
    }
  }
}

std::map<std::string, Eigen::VectorXd>
PanelSolver::compute_surface_results(double vx, double vy) {
  build_equation_system(vx, vy);
  source_density_ = FN.colPivHouseholderQr().solve(R);

  int n = num_elements_;
  Eigen::VectorXd R(n);
  Eigen::VectorXd qt(n);
  Eigen::VectorXd qn(n);
  Eigen::VectorXd u(n);
  Eigen::VectorXd v(n);
  Eigen::VectorXd p(n);
  Eigen::VectorXd xc(n);
  Eigen::VectorXd yc(n);
  Eigen::VectorXd theta(n);

  auto qts = FT * source_density_;
  auto qns = FN * source_density_;

  for (int i = 0; i < n; ++i) {
    auto &panel = panels_[i];
    xc[i] = panel.xc;
    yc[i] = panel.yc;
    theta[i] = std::atan2(yc[i], xc[i]);
    qt[i] = qts[i] + vy * panel.si + vx * panel.ci;
    qn[i] = qns[i] + vy * panel.ci - vx * panel.si;
    u[i] = vx - qns[i] * panel.si + qts[i] * panel.ci;
    v[i] = vy + qns[i] * panel.ci + qts[i] * panel.si;
    p[i] = 1.0 - std::pow(u[i], 2.0) - std::pow(v[i], 2.0);
  }
  std::map<std::string, Eigen::VectorXd> results;
  results["xc"] = xc;
  results["yc"] = yc;
  results["theta"] = theta;
  results["qt"] = qt;
  results["qn"] = qn;
  results["u"] = u;
  results["v"] = v;
  results["p"] = p;

  return results;
}

} // namespace poflow


/** \file
 * Support for Eigen linear algebra library in Chaiscript.
 *
 * \author Truong X. Nghiem (xuan.nghiem@epfl.ch)
 */

#ifndef _CHAISCRIPT_EXTRAS_EIGEN_H
#define _CHAISCRIPT_EXTRAS_EIGEN_H

#include <vector>
#include <Eigen/Dense>
#include <chaiscript/chaiscript.hpp>

namespace chaiscript {
    namespace extras {
        namespace eigenlinalg {
            typedef std::vector<chaiscript::Boxed_Value> TChaiVector;   ///< The C++ type of a Chaiscript's vector object

            Eigen::IOFormat EigenMatlabFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");

            // Register with Chaiscript a new Eigen vector type
            template<typename CLS, typename Scalar>
            ModulePtr eigen_vector_type(const char* CLSNAME, ModulePtr m = std::make_shared<Module>()) {
                //// 1.1. Type and Construction
                m->add(user_type<CLS>(), CLSNAME);
                m->add(constructor<CLS ()>(), CLSNAME);
                m->add(constructor<CLS (typename CLS::Index)>(), CLSNAME);
                m->add(bootstrap::copy_constructor<CLS>(CLSNAME));
                m->add(fun(static_cast<CLS& (CLS::*)(const CLS&)>(&CLS::operator=)), "=");

                // Construct vector from a Chaiscript vector of elements of EXACTLY the type Scalar
                m->add(fun([](const TChaiVector& cv) {
                    auto n = cv.size();
                    CLS v(n);
                    if (n > 0) {
                        int k = 0;
                        for (auto& elem: cv) {
                            v(k++) = chaiscript::boxed_cast<Scalar>(elem);
                        }
                    }
                    return v;
                }), CLSNAME);

                //// 1.2. Accessors
                m->add(fun(static_cast<Scalar& (CLS::*)(typename CLS::Index)>(&CLS::operator())), "[]");
                m->add(fun(static_cast<Scalar& (CLS::*)(typename CLS::Index)>(&CLS::operator())), "coeff");

                //// 1.3. Size and resize
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::size)), "size");
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::rows)), "rows");
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::cols)), "cols");
                m->add(fun(static_cast<void (CLS::*)(typename CLS::Index)>(&CLS::resize)), "resize"); // destructive resize

                //// 1.4. Misc

                // Convert to string to print
                m->add(fun([](const CLS& v) {
                    std::stringstream s;
                    s << v.format(EigenMatlabFmt);
                    return s.str();
                }), "to_string");

                return m;
            }

            // Register with Chaiscript a new Eigen matrix type
            template<typename CLS, typename Scalar>
            ModulePtr eigen_matrix_type(const char* CLSNAME, ModulePtr m = std::make_shared<Module>()) {
                //// 1.1. Type and Construction
                m->add(user_type<CLS>(), CLSNAME);
                m->add(constructor<CLS ()>(), CLSNAME);
                // Somehow Eigen does not work nicely with Chaiscript's constructor utility, so I must use a workaround
                m->add(fun([](typename CLS::Index r, typename CLS::Index c) {
                    return CLS(r, c);
                }), CLSNAME);
                bootstrap::copy_constructor<CLS>(CLSNAME, *m);
                m->add(fun(static_cast<CLS& (CLS::*)(const CLS&)>(&CLS::operator=)), "=");

                // Conversions from vector to matrix
                //m->add(fun([](const Eigen::Matrix<Scalar,Eigen::Dynamic,1>& v) { return CLS(v); }), CLSNAME);
                //m->add(fun([](const Eigen::Matrix<Scalar,1,Eigen::Dynamic>& v) { return CLS(v); }), CLSNAME);

                // Construct a matrix of given size from a Chaiscript vector of elements of EXACTLY the type Scalar.
                // This is done ROW-WISE.
                // Only r*c elements are copied from the vector to the matrix. If the vector contains fewer elements, only those will be copied, the remaining elements of the matrix are undefined.
                m->add(fun([](typename CLS::Index r, typename CLS::Index c, const TChaiVector& cv) {
                    auto n = cv.size();
                    CLS v(r, c);
                    if (n > 0 && r*c > 0) {
                        int k = 0;
                        for (auto ri = 0; ri < r; ++ri) {
                            for (auto ci = 0; ci < c; ++ci) {
                                v(ri, ci) = chaiscript::Boxed_Number(cv[k++]).get_as<Scalar>();
                                if (k == n) {
                                    break;
                                }
                            }
                            if (k == n) {
                                break;
                            }
                        }
                    }
                    return v;
                }), CLSNAME);

                //// 1.2. Accessors
                m->add(fun(static_cast<Scalar& (CLS::*)(typename CLS::Index, typename CLS::Index)>(&CLS::operator())), "coeff");
                m->add(fun(static_cast<Scalar& (CLS::*)(typename CLS::Index)>(&CLS::operator())), "[]");

                //// 1.3. Size and resize
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::size)), "size");
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::rows)), "rows");
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::cols)), "cols");
                m->add(fun(static_cast<void (CLS::*)(typename CLS::Index, typename CLS::Index)>(&CLS::resize)), "resize"); // destructive resize

                //// 1.4. Misc
                // Convert to string to print
                m->add(fun([](const CLS& v) {
                    std::stringstream s;
                    s << v.format(EigenMatlabFmt);
                    return s.str();
                }), "to_string");

                return m;
            }

            // Predefined matrices
            template<typename CLS>
            ModulePtr eigen_matrix_predefined(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](typename CLS::Index r, typename CLS::Index c) { return CLS(CLS::Zero(r,c)); }), "zeros");  // Zero matrix
                m->add(fun([](CLS &m) { m.setZero(); }), "setZero");  // set all coefficients to 0

                m->add(fun([](typename CLS::Index r, typename CLS::Index c) { return CLS(CLS::Ones(r,c)); }), "ones");  // matrix of 1's
                m->add(fun([](CLS &m) { m.setOnes(); }), "setOnes");  // set all coefficients to 1

                m->add(fun(&CLS::fill), "fill");  // set all coefficients to a given constant

                m->add(fun([](typename CLS::Index r, typename CLS::Index c) { return CLS(CLS::Identity(r,c));}), "eyes");  // Identity matrix
                m->add(fun([](CLS &m) { m.setIdentity(); }), "setIdentity");  // set the matrix to identity
                m->add(fun([](CLS &m, typename CLS::Index r, typename CLS::Index c) { m.setIdentity(r,c); }), "setIdentity");  // resize and set the matrix to identity

                return m;
            }

            // Binary Addition and subtraction
            // CLS1 and CLS2 should be different types, where CLS1 should be "larger" in the sense that CLS2 is a special case of CLS1 (e.g. Matrix and Vector)
            template<typename CLS1, typename CLS2>
            ModulePtr binary_addition_substraction(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS1& a, const CLS2& b) { return CLS1(a + b); }), "+");
                m->add(fun([](const CLS2& a, const CLS1& b) { return CLS1(a + b); }), "+");
                m->add(fun([](const CLS1& a, const CLS2& b) { return CLS1(a - b); }), "-");
                m->add(fun([](const CLS2& a, const CLS1& b) { return CLS1(a - b); }), "-");
                return m;
            }

            // Binary Addition and subtraction for objects of same class
            template<typename CLS1>
            ModulePtr binary_addition_substraction(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS1& a, const CLS1& b) { return (a + b).eval(); }), "+");
                m->add(fun([](const CLS1& a, const CLS1& b) { return (a - b).eval(); }), "-");
                m->add(fun([](CLS1& a, const CLS1& b) { return (a += b); }), "+=");

                // Between matrix and scalar
                m->add(fun([](const CLS1& a, const typename CLS1::Scalar b) { return CLS1(a.array() + b); }), "+");
                //m->add(fun([](const typename CLS1::Scalar b, const CLS1& a) { return CLS1(a.array() + b); }), "+");
                m->add(fun([](const CLS1& a, const typename CLS1::Scalar b) { return CLS1(a.array() - b); }), "-");
                //m->add(fun([](const typename CLS1::Scalar b, const CLS1& a) { return CLS1((-a).array() + b); }), "-");

                return m;
            }

            // Unary Addition and subtraction
            template<typename CLS>
            ModulePtr unary_addition_substraction(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS& a) { return a; }), "+");
                m->add(fun([](const CLS& a) { return (-a).eval(); }), "-");
                return m;
            }

            // Scalar multiplication and division
            template<typename CLS, typename Scalar>
            ModulePtr scalar_mult_div(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS& a, const Scalar b) { return (a * b).eval(); }), "*");
                m->add(fun([](const Scalar a, const CLS& b) { return (a * b).eval(); }), "*");
                m->add(fun([](const CLS& a, const Scalar b) { return (a / b).eval(); }), "/");
                m->add(fun([](CLS& a, const Scalar b) { return (a *= b); }), "*=");
                m->add(fun([](CLS& a, const Scalar b) { return (a /= b); }), "/=");
                return m;
            }

            // Transpose and conjugation
            template<typename CLS, const bool withConjugation>
            ModulePtr transpose_conjugation(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS& a) { return CLS(a.transpose()); }), "transpose");
                m->add(fun(static_cast<void (CLS::*)()>(&CLS::transposeInPlace)), "transposeInPlace");    // in-place transpose: void transposeInPlace()

                if (withConjugation) {
                    m->add(fun([](const CLS& a) { return CLS(a.conjugate()); }), "conjugate");
                    m->add(fun([](const CLS& a) { return CLS(a.adjoint()); }), "adjoint");
                    m->add(fun(static_cast<void (CLS::*)()>(&CLS::adjointInPlace)), "adjointInPlace");    // in-place adjoint: void adjointInPlace()
                }
                return m;
            }

            // Matrix multiplication
            template<typename CLS1, typename CLS2 = CLS1>
            ModulePtr matrix_mult(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS1& a, const CLS2& b) { return (a * b).eval(); }), "*");
                m->add(fun([](CLS1& a, const CLS2& b) { return a *= b; }), "*=");
                return m;
            }

            // Dot product (only for vector types)
            template<typename CLS1>
            ModulePtr dot_product(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS1& a, const CLS1& b) { return a.dot(b); }), "dot");
                return m;
            }

            // Basic arithmetic reduction operations
            template<typename CLS>
            ModulePtr arith_reduction(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::sum)), "sum");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::prod)), "prod");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::mean)), "mean");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::minCoeff)), "min");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::maxCoeff)), "max");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::trace)), "trace");
                return m;
            }

            // Coefficient-wise operations
            template<typename CLS>
            ModulePtr coefficient_wise_operations(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS& a) { return a.cwiseAbs().eval(); }), "abs");
                m->add(fun([](const CLS& a) { return a.cwiseAbs2().eval(); }), "abs2");
                m->add(fun([](const CLS& a) { return a.cwiseSqrt().eval(); }), "sqrt");
                m->add(fun([](const CLS& a) { return a.array().log().matrix().eval(); }), "log");
                m->add(fun([](const CLS& a) { return a.array().exp().matrix().eval().eval(); }), "exp");
                m->add(fun([](const CLS& a, double e) { return a.array().pow(e).matrix().eval(); }), "pow");
                m->add(fun([](const CLS& a) { return a.array().square().matrix().eval(); }), "square");
                m->add(fun([](const CLS& a) { return a.array().sin().matrix().eval(); }), "sin");
                m->add(fun([](const CLS& a) { return a.array().cos().matrix().eval(); }), "cos");
                m->add(fun([](const CLS& a) { return a.array().tan().matrix().eval(); }), "tan");
                m->add(fun([](const CLS& a) { return a.array().asin().matrix().eval(); }), "asin");
                m->add(fun([](const CLS& a) { return a.array().acos().matrix().eval(); }), "acos");

                return m;
            }

        }
    }
}
#endif // _CHAISCRIPT_EXTRAS_EIGEN_H


#ifndef DATAOPS_H
#define DATAOPS_H

#include <map>
#include <iterator>
#include <string>
#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>
#include <algorithm>
#include <utility>
#include <functional>
#include <numeric>
#include <math.h>
#include <cmath>
#include <complex>
#include <boost/multi_array.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <typeinfo>

namespace DataOps
{
	template <typename T>
		std::vector<T>& sample_every(std::vector<T>& lhs, const std::vector<T>& rhs, const size_t nskip = 10, const size_t offset = 0)
		{
			if (lhs.size() < 1){
				lhs.resize(int(rhs.size()/nskip),T(0));
			}
			if (lhs.size() > (rhs.size()+offset)/nskip ){
				lhs.resize(size_t((rhs.size()+offset)/nskip));
			}
			for (size_t i = 0; i<lhs.size(); ++i){
				lhs[i] = T(rhs[nskip*i+offset]);
			}
			return lhs;
		}

	template <typename T>
		T* clone(T* lhs,const T* rhs,const size_t n)
		{
			std::copy(rhs, rhs + n, lhs);
			return lhs;
		}

	template <typename T>
		std::vector<T>& clone(std::vector<T> & lhs, const std::vector<T> & rhs)
		{
			lhs.resize(rhs.size());
			std::copy(rhs.begin(),rhs.end(),lhs.begin());
			return lhs;
		}

	template <typename T>
		T& gauss(const T & xin, const T& x0, const T& w,T& a =T(1),T& y0 = T(0)){
			T x = xin-x0;
			return (T) (y0+a*std::exp(- std::pow(x/w,int(2)))) ;
		}

	template <typename X_t,typename Y_t>
		Y_t polynomial(const X_t xin,const std::vector<Y_t> c){
			assert(c.size()>1);
			Y_t x = Y_t(xin) - c.front();
			Y_t y = c[1];
			for (unsigned i=1; i<c.size();++i){
				y += c[i] * std::pow(x,(int)i-1);
			}
			return y;
		}

	template <typename T>
		void fixedfilter(T * vec, const size_t sz){
			T noise(T(.1));
			vec[0] *= T(1);
			vec[sz/2] *= T(1)/(T(1) + noise/10*std::pow(T(sz/2),2));
			for( size_t i=1;i<sz/2;++i){
				vec[i] *= T(1)/(T(1) + noise/T(10)*std::pow(T(i),2));
				vec[sz-i] *= T(1)/(T(1) + noise/T(10)*std::pow(T(i),2));
			}
			return;
		}

	template <typename T>
		bool inwin(const T val, const std::vector<T> win){
			assert(win.size()==2);
			return (val >= win[0] && val < win[1]);
		}

	template <typename T>
		void endstozero(T * vec, const size_t sz, const size_t steps)
		{
			T mean1(0);
			T mean2(0);
			T x1(0);
			T x2(0);
			for (size_t i=0;i<steps;++i){
				mean1 += vec[i];
				x1 += T(i);
				mean2 += vec[sz-i-1];
				x2 += T(i);
			}
			for (size_t i=0; i<sz; ++i){
				vec[i] -= (mean2-mean1)/(x2-x1)*(T(i)-x1) + mean1/T(steps);
			}
		}

	template <typename T>
		void sin2roll(T * vec, const size_t sz, const T center, const T width)
		{
			for (size_t i = 0;i<sz;++i){
				T x = T(M_PI)*(T(i)-center)/(T(2)*width);
				if (std::abs(x) > T(1)){
					vec[i] = T(0);
				} else {
					vec[i] *= std::pow(std::cos(x),int(2));
				}
			}
			return;
		}

	template <typename T>
		void gaussroll(T * vec, const size_t sz, const T center, const T width)
		{
			for (size_t i = 0;i<sz;++i){
				T x = (T(i)-center)/width;
				vec[i] *= std::exp(-1.*std::pow(x,2));
			}
			return;
		}

	template <typename T,typename V>
		V interpolate(const std::map<T,V> &data, T x)
		{
			typedef typename std::map<T,V>::const_iterator MapIterator;
			MapIterator i = data.upper_bound(x);
			MapIterator l = i;
			double slope;
			double span = (double) (x - l->first);
			if (i==data.begin())
			{
				MapIterator u=i;
				++u;
				slope = (double)(u->second - i->second)/(double)(u->first - i->first);
				return (V)( i->second + (slope * span/(u->first - i->first) ) );
			}
			--l;
			if(i==data.end())
			{
				MapIterator ll=l;
				--ll;
				slope = (double)(l->second - ll->second)/(double)(l->first - ll->first);
				return (V)( l->second + (slope * span/(l->first - ll->first) ) ) ;
			}
			slope = (double)( i->second - l->second ) / (double)( i->first - l->first );
			return (V)(l->second + (slope * span/(i->first - l->first)) );
		}

	template <typename T>
		void condense(std::vector< T > & invec,const unsigned newlen)
		{
			double ratio = ((double)invec.size()/(double)newlen);

			if (ratio <= 1){std::cerr << "Cannot condense2(): size mismatch" << std::endl;return;}
			for (unsigned j=1;j<(int)ratio;++j){
				invec[0] += invec[j];
			}
			invec[0] /= ratio;
			for (unsigned i=1;i<newlen;++i){
				invec[i] = invec[(int)(i*ratio)];
				for (unsigned j=1;j<ratio;++j){
					invec[i] += invec[(int)(i*ratio + j)];
				}
				invec[i] /= (double)ratio;
			}
			invec.resize(newlen);

			return;
		}

	template <typename T>
		void condense(std::vector< std::vector < T > > & inmat, const unsigned newlen)
		{
			for (unsigned i=0;i<inmat.size();++i)
				condense(inmat[i],newlen);
			return;
		}

	template <typename T>
		std::istream& operator >> (std::istream & ins, std::vector<T> & record)
		{
			record.clear();
			std::string line;
			getline( ins, line );
			const char head='#';
			if (line.find(head) != std::string::npos){
				return ins;
			}
			std::istringstream iss( (std::string)line );
			T value;
			while (iss >> value){
				record.push_back(value);
			}
			return ins;
		}

	template <typename T>
		std::istream& operator >> (std::istream & ins, std::vector< std::vector<T> > & matrix)
		{
			matrix.clear();
			std::vector<T> record;
			while (ins >> record)
			{
				if (record.size() > 0)
					matrix.push_back( record );
			}
			return ins;
		}
	/*
	   template <typename T, int dim>
	   std::ostream& operator << (std::ostream & outs, boost::multi_array<T,dim> & record)
	   {
	   for (unsigned i=0;i<record.shape()[0];++i){
	   outs << record[i] << "\t";
	   }
	   outs << std::flush;
	   return outs;
	   }
	 */

	template <typename T,typename V>
		std::ostream& operator << (std::ostream & outs, std::pair<T,V> outpair)
		{
			outs << "(" << outpair.first << "," << outpair.second << ")\t" << std::flush;
			return outs;
		}

	template <typename T>
		std::ostream& operator << (std::ostream & outs, std::vector< T > & vec)
		{
			for (unsigned i=0;i<vec.size();++i){
				outs << vec[i] << "\t";
			}
			outs << "\n" << std::flush;
			return outs;
		}

	template <typename T>
		std::ostream& operator << (std::ostream & outs, std::vector< std::vector <T> > & matrix)
		{
			for (unsigned i=0;i<matrix.size();++i){
				for (unsigned j=0;j<matrix[i].size();++j){
					outs << matrix[i][j] << "\t";
				}
				outs << "\n";
			}
			outs << std::flush;
			return outs;
		}

	template <typename T>
		std::complex<T>*& sum(std::complex<T>*& lhs,std::complex<T>* const & rhs,const size_t n)
		{
			for (size_t i = 0;i<n;++i){
				lhs[i] += rhs[i];
			}
			/*
			std::transform(lhs,lhs+n,rhs,[](std::complex<T>* d_lhs, std::complex<T>* d_rhs){
					return *d_lhs + *d_rhs;
					});
			*/
			return lhs;
		}

	template <typename T>
		std::complex<T>* diff(std::complex<T>*& lhs,std::complex<T>* const & rhs,const size_t n)
		{
			for (size_t i = 0;i<n;++i){
				lhs[i] -= rhs[i];
			}
			/*
			std::transform(lhs,lhs+n,rhs,[](std::complex<T>* d_lhs, std::complex<T>* d_rhs){
					return *d_lhs - *d_rhs;
					});
			*/
			return lhs;
		}

	template <typename T>
		std::complex<T>* mul(std::complex<T>*& lhs,std::complex<T>* const & rhs,const size_t n)
		{
			for (size_t i = 0;i<n;++i){
				lhs[i] *= rhs[i];
			}
			/*
			std::transform(lhs,lhs+n,rhs,lhs,[](std::complex<T>* d_lhs, std::complex<T>* d_rhs){
					return (*d_lhs) * (*d_rhs);
					});
			*/
			return lhs;
		}

	template <typename T>
		std::complex<T>* mul(std::complex<T>*& lhs,const T& scale,const size_t n)
		{
			std::transform(lhs, lhs+n, lhs, std::bind2nd(std::multiplies<std::complex<T> >(),scale));
			//std::cerr << "using this complex vec scaling function in DataOps::" << std::endl << std::flush;
			return lhs;
		}
	template <typename T>
		std::complex<T>* mul(std::complex<T>*& lhs,std::complex<T>& scale,const size_t n)
		{
			std::transform(lhs, lhs+n, lhs, std::bind2nd(std::multiplies<std::complex<T> >(),scale));
			return lhs;
		}

	template <typename T>
		std::complex<T>* div(std::complex<T>*& lhs,std::complex<T>* const & rhs,const size_t n)
		{
			for (size_t i=0;i<n;++i){
				lhs[i] /= rhs[i];
			}
			/*
			std::transform(lhs,lhs+n,rhs,lhs,[](std::complex<T>* d_lhs, std::complex<T>* d_rhs){
					return *d_lhs / *d_rhs;
					});
			*/
			return lhs;
		}

	template <typename T>
		inline bool inwin(std::vector<T> win,T val)
		{
			std::sort(win.begin(),win.end());
			return (val >= win.front() && val < win.back());
		}

	template <typename T>
		std::vector<T> & expscaled(const T scale, std::vector<T> & x)
		{
			x *= scale;
			std::transform(x.begin(), x.end(), x.begin(), [&](T xval){return std::exp(xval);});
			return x;
		}

	template <typename T>
		std::vector<std::complex<T> > & operator *= (std::vector<std::complex<T> > & vec,std::complex<T> val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::multiplies<std::complex<T> >(),val));
			return vec;
		}

	template <typename T>
		std::vector<std::complex<T> > & operator *= (std::vector<std::complex<T> > & vec,T val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::multiplies<std::complex<T> >(),val));
			return vec;
		}

	template <typename T>
		std::vector<T> & operator *= (std::vector<T>& vec,T const val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::multiplies<T>(),val));
			return vec;
		}

	//================ HERE HERE HERE HERE ================//
	template <typename T>
		std::vector<T> & operator /= (std::vector<T>& vec,const T val)
		{
			assert(std::abs(val) != T(0) );
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::divides<T>(),val));
			return vec;
		}

	template <typename T>
		std::vector<T> & operator += (std::vector<T>& vec, const T val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::plus<T>(),val));
			return vec;
		}

	template <typename T,typename T2>
		std::vector<T> & operator -= (std::vector<T>& vec, const T2 val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::minus<T>(),val));
			return vec;
		}

	template <typename T>
		std::vector<T>& operator += (std::vector<T>& resvec,const std::vector<T> & srcvec)
		{
			assert(resvec.size() == srcvec.size());
			for (unsigned i=0;i<resvec.size();++i){
				resvec[i] += srcvec[i];
			}
			return resvec;
		}

	template <typename T>
		std::vector<T>& operator -= (std::vector<T>& resvec,const std::vector<T> & srcvec)
		{
			assert(resvec.size() == srcvec.size());
			for (unsigned i=0;i<resvec.size();++i){
				resvec[i] -= srcvec[i];
			}
			return resvec;
		}
	template <typename T>
		std::vector<T>& operator *= (std::vector<T>& resvec,const std::vector<T> & srcvec)
		{
			assert(resvec.size() == srcvec.size());
			for (unsigned i=0;i<resvec.size();++i){
				resvec[i] *= srcvec[i];
			}
			return resvec;
		}
	template <typename T>
		std::vector<T>& operator /= (std::vector<T>& resvec,const std::vector<T> & srcvec)
		{
			assert(resvec.size() == srcvec.size());
			for (unsigned i=0;i<resvec.size();++i){
				resvec[i] /= srcvec[i];
			}
			return resvec;
		}

	template <typename T>
		inline T projection(std::vector<T> &in1,std::vector<T> in2,std::vector<bool> & mask){
			assert(in1.size()==in2.size());
			assert(in1.size() == mask.size());
			T ip(0);
			for (unsigned i=0;i<in1.size();++i){
				if (mask[i]){
					ip += in1[i] * in2[i];
				}
			}
			return ip;
		}

	inline size_t sum(std::vector<bool> & mask){
		size_t sum = 0;
		for (size_t i = 0 ; i<mask.size();++i){
			if (mask[i]==true) sum++;
		}
		return sum;
	}

	template <typename T>
		inline T projection(std::vector<T> &in1,std::vector<T> in2){
			assert(in1.size()==in2.size());
			return std::inner_product(in1.begin(),in1.end(),in2.begin(),0.);
		}

	template <typename T>
		inline void safe_normalize(std::vector<T> & in)
		{
			T ip(0);
			for (unsigned i=0;i<in.size();++i){
				ip += (in[i] * in[i]);
			}
			assert(ip!=T(0));
			T scale = T(1)/std::sqrt(ip);
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::multiplies<T>(),scale) );
		}

	template <typename T>
		inline void sqr_normalize(std::vector<T> & in,std::vector<bool>& mask)
		{
			assert(in.size() == mask.size());
			T ip = T(0);
			for (unsigned i=0;i<in.size();++i)
				if (mask[i]) {ip += (in[i] * in[i]);}
			assert(ip!=T(0));
			T scale = T(1)/std::sqrt(ip);
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::multiplies<T>(),scale) );
		}

	template <typename T>
		inline void sqr_normalize(std::vector<T> & in) {
			T scale = sqrt( std::inner_product(in.begin(), in.end(), in.begin(),T(0)) );
			assert(scale != T(0));
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::divides<T>(),scale) );
		};

	template <typename T>
		inline T removemean(T* in, size_t sz) {
			T sum = std::accumulate(in, in + sz, T(0));
			T mean = sum / T(sz);
			std::transform(in, in + sz, in, std::bind2nd(std::minus<T>(),mean));
			return mean;
		};

	template <typename T>
		inline T removemean(std::vector<T> & in,std::vector<bool> & mask) {
			assert(in.size() == mask.size());
			T sum(0);
			unsigned nvals(0);
			for (unsigned i=0;i<in.size();++i){
				if (mask[i]) {
					sum += in[i];
					++nvals;
				}
			}
			T mean = sum / T(nvals);
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::minus<T>(),mean));
			return mean;
		};

	template <typename T>
		inline T removemean(std::vector<T> & in) {
			T sum = std::accumulate(in.begin(), in.end(), T(0));
			T mean = sum / T(in.size());
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::minus<T>(),mean));
			return mean;
		};

	template <typename T>
		inline T mean(std::vector<T> &in){
			T mean = std::accumulate(in.begin(), in.end(), T(0));
			mean /= T(in.size());
			return mean;
		};

	// WHoah  logarithmic stuff  //

	template <typename T>
		inline void meanstdlog(std::vector<T> & in, T& mean, T& std)
		{
			// I want the log of the values, then sum, then divide by size for mean
			// Diff is the log(values)-mean(log valuse)
			mean = std::log( std::accumulate(in.begin(),in.end(),T(1),std::multiplies<T>()) );
			mean /= T(in.size());
			std::vector<T> diff(in.size());
			for (unsigned i=0;i<diff.size();++i){
				diff[i] = std::log(in[i]) - mean;
			}
			T sq_sum = std::inner_product(diff.begin(),diff.end(),diff.begin(),T(0));
			std = std::sqrt(sq_sum / T(diff.size()));
		}

	template <typename T>
		inline T meanlog(std::vector<T> & in,T offset){
			T mean = offset;
			mean += std::log( std::accumulate(in.begin(),in.end(),T(1),std::multiplies<T>()) );
			mean /= T(in.size());
			return mean;
		};

	template <typename T>
		inline T stdlog(std::vector<T> & in, T mean){
			std::vector<T> diff(in.size());
			for (unsigned i=0;i<diff.size();++i){
				diff[i] = std::log(in[i]) - mean;
			}
			T sq_sum = std::inner_product(diff.begin(),diff.end(),diff.begin(),T(0));
			T stdev = std::sqrt(sq_sum / T(diff.size()));
			return stdev;
		};

	template <typename T>
		void detrend(T * vec, const size_t sz)
		{
			T num(0);
			T den(0);
			T xm = T(sz-1)/T(2);
			T ym = removemean(vec,sz);
			for (size_t i=0;i<sz;++i){
				num += (i-xm)*vec[i];
				den += std::pow((i-xm),int(2));
			}
			T beta = num/den;
			for (size_t i=0;i<sz;++i){
				vec[i] -= beta*T(i) - beta*xm;
			}
			return;
		}

}

#endif