#ifndef TVMTL_MANIFOLD_SPD_HPP
#define TVMTL_MANIFOLD_SPD_HPP

#include <cmath>
#include <complex>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SVD>
#include <unsupported/Eigen/MatrixFunctions>
#include <unsupported/Eigen/KroneckerProduct>

//own includes
#include "enumerators.hpp"
#include "matrix_utils.hpp"

namespace tvmtl {

// Specialization SPD
template <int N>
struct Manifold< SPD, N> {

    public:
	static const MANIFOLD_TYPE MyType;
	static const int manifold_dim ;
	static const int value_dim; // TODO: maybe rename to embedding_dim

	static const bool non_isometric_embedding;

	// Scalar type of manifold
	typedef double scalar_type;
	typedef double dist_type;
	typedef std::complex<double> complex_type;
	typedef std::vector<double> weight_list;

	// Value Typedef
	typedef Eigen::Matrix< scalar_type, N, N>				value_type;
	typedef value_type&							ref_type;
	typedef const value_type&						cref_type;
	typedef std::vector<value_type, Eigen::aligned_allocator<value_type> >	value_list;

	// Tangent space typedefs
	typedef Eigen::Matrix <scalar_type, N * N, N * (N + 1) / 2>   tm_base_type;
	typedef tm_base_type&					    tm_base_ref_type;

	// Derivative Typedefs
	typedef value_type			     deriv1_type;
	typedef deriv1_type&			     deriv1_ref_type;

	typedef Eigen::Matrix<scalar_type, N*N, N*N>				deriv2_type;
	typedef deriv2_type&							deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, N * (N + 1) / 2, N * (N + 1) / 2>	restricted_deriv2_type;

	// Manifold distance functions (for IRLS)
	inline static dist_type dist_squared(cref_type x, cref_type y);
	inline static void deriv1x_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);
	inline static void deriv1y_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);

	inline static void deriv2xx_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2xy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2yy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);

	// Manifold exponentials und logarithms ( for Proximal point)
	template <typename DerivedX, typename DerivedY>
	inline static void exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result);
	inline static void log(cref_type x, cref_type y, ref_type result);

	inline static void convex_combination(cref_type x, cref_type y, double t, ref_type result);

	// Implementations of the Karcher mean
	// Slow list version
	inline static void karcher_mean(ref_type x, const value_list& v, double tol=1e-10, int maxit=15);
	inline static void weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol=1e-10, int maxit=15);
	// Variadic templated version
	template <typename V, class... Args>
	inline static void karcher_mean(V& x, const Args&... args);
	template <typename V>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y);
	template <typename V, class... Args>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y1, const Args&... args);

	// Basis transformation for restriction to tangent space
	inline static void tangent_plane_base(cref_type x, tm_base_ref_type result);

	// Projection
	inline static void projector(ref_type x);

	// Interpolation pre- and postprocessing
	inline static void interpolation_preprocessing(ref_type x);
	inline static void interpolation_postprocessing(ref_type x);

};

/*-----IMPLEMENTATION SPD----------*/

// Static constants, Outside definition to avoid linker error

template <int N>
const MANIFOLD_TYPE Manifold < SPD, N>::MyType = SPD;

template <int N>
const int Manifold < SPD, N>::manifold_dim = N * (N + 1) / 2;

template <int N>
const int Manifold < SPD, N>::value_dim = N * N;

template <int N>
const bool Manifold < SPD, N>::non_isometric_embedding = true;

// Squared SPD distance function
template <int N>
inline typename Manifold < SPD, N>::dist_type Manifold < SPD, N>::dist_squared( cref_type x, cref_type y ){
    #ifdef TV_SPD_DIST_DEBUG
	std::cout << "\nDist2 function with x=\n" << x << "\nand y=\n" << y << std::endl;
    #endif
// NOTE: If x is not strictly spd, using LDLT completely halts the algorithm
/*    value_type sqrtX = x.sqrt();
    Eigen::LDLT<value_type> ldlt;
    ldlt.compute(sqrtX);

    value_type Z = ldlt.solve(y).transpose();
    return ldlt.solve(Z).transpose().log().squaredNorm();	*/
    value_type invsqrt = x.sqrt().inverse();
    return (invsqrt * y * invsqrt).log().squaredNorm();
}

// Derivative of Squared SPD distance w.r.t. first argument
// TODO: Switch to solve() for N>4?
template <int N>
inline void Manifold < SPD, N>::deriv1x_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    value_type invsqrt = x.sqrt().inverse();
    result = -2.0 * invsqrt * (invsqrt * y * invsqrt).log() * invsqrt;
}
// Derivative of Squared SPD distance w.r.t. second argument
template <int N>
inline void Manifold < SPD, N>::deriv1y_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    deriv1x_dist_squared(y, x, result);
}

// Second Derivative of Squared SPD distance w.r.t first argument
template <int N>
inline void Manifold < SPD, N>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    value_type T2, T3, T4;
    T2 = x.sqrt().inverse();
    T3 = T2 * y * T2;
    T4 = T3.log();

    deriv2_type dlog, dsqrt;
    KroneckerDLog(T3, dlog);
    KroneckerDSqrt(x, dsqrt);

    deriv2_type T2T4tId, IdT2T4, T2T2, T2yId, IdT2y;
    T2T4tId = Eigen::kroneckerProduct(T2 * T4.transpose(), value_type::Identity());
    IdT2T4 = Eigen::kroneckerProduct(value_type::Identity(), T2 * T4);
    T2T2 = Eigen::kroneckerProduct(T2, T2);
    T2yId = Eigen::kroneckerProduct(T2 * y, value_type::Identity());
    IdT2y = Eigen::kroneckerProduct(value_type::Identity(), T2 * y);

    result =  2 * (T2T4tId + IdT2T4 + T2T2 * dlog * (T2yId + IdT2y) ) * T2T2 * dsqrt;
}
// Second Derivative of Squared SPD distance w.r.t first and second argument
template <int N>
inline void Manifold < SPD, N>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    value_type isqrtX, T1;
    isqrtX = x.sqrt().eval().inverse();
    T1 = isqrtX * y * isqrtX;

    deriv2_type kp_isqrtX, dlog;
    kp_isqrtX = Eigen::kroneckerProduct(isqrtX, isqrtX);
    KroneckerDLog(T1, dlog);

    result = -2 * kp_isqrtX * dlog * kp_isqrtX;
}
// Second Derivative of Squared SPD distance w.r.t second argument
template <int N>
inline void Manifold < SPD, N>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    deriv2xx_dist_squared(y, x, result);
}

// Exponential and Logarithm Map
template <int N>
template <typename DerivedX, typename DerivedY>
inline void Manifold <SPD, N>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result){
    #ifdef TV_SPD_EXP_DEBUG
	std::cout << "\nEXP function with x=\n" << x << "\nand y=\n" << y << std::endl;
    #endif
    value_type sqrtX = x.sqrt();
    value_type Z = sqrtX.ldlt().solve(y).transpose();
    result = sqrtX * sqrtX.transpose().ldlt().solve(Z).exp() * sqrtX;
}

template <int N>
inline void Manifold <SPD, N>::log(cref_type x, cref_type y, ref_type result){
    #ifdef TV_SPD_LOG_DEBUG
	std::cout << "\nLOG function with x=\n" << x << "\nand y=\n" << y << std::endl;
    #endif
    value_type sqrtX = x.sqrt();
    value_type Z = sqrtX.ldlt().solve(y).transpose();
    result = sqrtX * sqrtX.transpose().ldlt().solve(Z).log() * sqrtX;
}

// Tangent Plane restriction
template <int N>
inline void Manifold <SPD, N>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    int d = value_type::RowsAtCompileTime;
    int k = 0;

    value_type S, T;
    S = x.sqrt();

    for(int i=0; i<d; i++){
	T.setZero();
	T.col(i) = S.col(i);
	T = T * S;

	result.col(k) = Eigen::Map<Eigen::VectorXd>(T.data(), T.size());
	++k;
    }

    for(int i=0; i<d-1; i++)
	for(int j=i+1; j<d; j++){
	    T.setZero();
	    T.col(i) = S.col(j);
	    T.col(j) = S.col(i);
	    T = T * S;

	    result.col(k) = Eigen::Map<Eigen::VectorXd>(T.data(), T.size());
	    ++k;
	}
}

template <int N>
inline void Manifold <SPD, N>::projector(ref_type x){
    // does not exist since SPD is an open set
    // TODO: Eventually implement projection to semi positive definite matrices
}

// Convex geodesic combinations
template <int N>
inline void Manifold <SPD, N>::convex_combination(cref_type x, cref_type y, double t, ref_type result){
    value_type l;
    log(x, y, l);
    exp(x, l * t, result);
}

// Karcher mean implementations
template <int N>
inline void Manifold<SPD, N>::karcher_mean(ref_type x, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += temp;
	}
	exp(x, 0.5 / v.size() * (L + L.transpose()), temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
inline void Manifold<SPD, N>::weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += w[i] * temp;
	}
	exp(x, 0.5 / v.size() * (L + L.transpose()), temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
template <typename V, class... Args>
inline void Manifold<SPD, N>::karcher_mean(V& x, const Args&... args){
    V temp, sum;

    int numArgs = sizeof...(args);
    int k = 0;
    double error = 0.0;
    double tol = 1e-10;
    int maxit = 15;
    do{
	scalar_type m1 = x.sum();
	sum = x;
	variadic_karcher_mean_gradient(sum, args...);
	exp(x, 0.5 / numArgs * (sum + sum.transpose()), temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);
}

template <int N>
template <typename V>
inline void Manifold<SPD, N>::variadic_karcher_mean_gradient(V& x, const V& y){
    V temp;
    log(x, y, temp);
    x = temp;
}

template <int N>
template <typename V, class... Args>
inline void Manifold<SPD, N>::variadic_karcher_mean_gradient(V& x, const V& y1, const Args& ... args){
    V temp1, temp2;
    temp2 = x;

    log(x, y1, temp1);

    variadic_karcher_mean_gradient(temp2, args...);
    temp1 += temp2;
    x = temp1;
}

template <int N>
inline void Manifold<SPD, N>::interpolation_preprocessing(ref_type x){
    value_type t = x.log();
    x = t;
}

template <int N>
inline void Manifold<SPD, N>::interpolation_postprocessing(ref_type x){
    value_type t = ( 0.5 * (x + x.transpose()) ).exp();
    x = t;
}

} // end namespace tvmtl

#endif


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Eigen>

struct PoseData {
    Eigen::Vector3d p;
    Eigen::Quaterniond q;
    std::string filename;
    int index;
};

inline Eigen::Vector3d logmap(const Eigen::Quaterniond &q) {
    Eigen::AngleAxisd aa(q);
    return aa.angle() * aa.axis();
}

inline std::vector<PoseData> read_7scenes_pose(const std::string &path, int pose_num, const std::string &pose_type) {
    std::vector<PoseData> poses;
    // printf("#idx px py pz qx qy qz qw\n");
    for (int idx = 0; idx < pose_num; idx++) {
        char filename[1024];
        if (pose_type == "gt") {
             sprintf(filename, "%s/frame-%06d.pose.txt", path.c_str(), idx);
        } else if (pose_type == "test") {
            sprintf(filename, "%s/frame-%06d.txt", path.c_str(), idx);
        } else {
            std::runtime_error("Unknown pose_type.");
        }
        std::ifstream in(filename);
        Eigen::Matrix4d e;
        for (int r = 0; r < 4; r++) {
            for (int c = 0; c < 4; c++) {
                in >> e(r, c);
            }
        }
        // origin: Twc
        Eigen::Quaterniond qwc(e.block<3, 3>(0, 0));
        Eigen::Vector3d pwc(e.block<3, 1>(0, 3));
        if (std::abs(qwc.norm() - 1.0) > 1.0e-3) {
           fprintf(stderr, "Warning, rotation matrix may not be valid, qwc.norm() %.4e", qwc.norm());
        }
        if (pose_type == "gt") {
            pwc.x() += 0.0245;
        } else if (pose_type == "test") {
            pwc = -(qwc.conjugate() * pwc);
            qwc = qwc.conjugate();
        } else {
            std::runtime_error("Unknown pose_type.");
        }
        PoseData pose;
        pose.q = qwc;
        pose.p = pwc;
        pose.filename = filename;
        pose.index = idx;
        poses.emplace_back(std::move(pose));
        // printf("%05d %.9e %.9e %.9e %.9e %.9e %.9e %.9e\n", idx, pwc.x(), pwc.y(), pwc.z(), qwc.x(), qwc.y(), qwc.z(), qwc.w());
    }
    return poses;
}

int main(int argc, char **argv) {
    std::string gt_dir = argv[1];
    std::string pred_dir = argv[2];
    const int pose_num = 1000;
    auto gt_poses = read_7scenes_pose(gt_dir, pose_num, "gt");
    auto pred_poses = read_7scenes_pose(pred_dir, pose_num, "test");
    assert(pred_poses.size() == gt_poses.size());
    double APE = 0, ARE = 0, Acount = 0;
    std::vector<double> APEs, AREs;
    for (int i = 0; i < pose_num; ++i) {
        const auto &gt_pose = gt_poses[i];
        const auto &pred_pose = pred_poses[i];
        Eigen::Vector3d p_error = gt_pose.p - pred_pose.p;
        Eigen::Vector3d q_error = logmap(gt_pose.q.conjugate() * pred_pose.q);
        APE += p_error.squaredNorm();
        ARE += q_error.squaredNorm();
        APEs.push_back(p_error.norm() * 1e3);
        AREs.push_back(q_error.norm() * 180 / M_PI);
        Acount++;
    }
    Acount = std::max(Acount, 1.0);
    APE = std::sqrt(APE / Acount) * 1e3;
    ARE = std::sqrt(ARE / Acount) * 180 / M_PI;
    printf("APE RMSE: %.3f [mm]\nARE RMSE %.3f[DEG]\n", APE, ARE);
    std::sort(APEs.begin(), APEs.end());
    std::sort(AREs.begin(), AREs.end());
    printf("APE median: %.3f [mm]\nARE median %.3f[DEG]\n", APEs[APEs.size() / 2], AREs[AREs.size() / 2]);
    return 0;
}


/*************************************************************************
	> File Name: lqr_steer_control.cpp
	> Author: TAI Lei
	> Mail: ltai@ust.hk
	> Created Time: Wed Apr 17 11:48:46 2019
 ************************************************************************/

#include <iostream>
#include <limits>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sys/time.h>
#include <Eigen/Eigen>
#include "cubic_spline.h"
#include "motion_model.h"
#include "cpprobotics_types.h"

#define DT 0.1
#define L 0.5
#define KP 1.0
#define MAX_STEER 45.0/180*M_PI

using namespace cpprobotics;

cv::Point2i cv_offset(
    float x, float y, int image_width=2000, int image_height=2000){
  cv::Point2i output;
  output.x = int(x * 100) + 300;
  output.y = image_height - int(y * 100) - image_height/2;
  return output;
};

Vec_f calc_speed_profile(Vec_f rx, Vec_f ry, Vec_f ryaw, float target_speed){
	Vec_f speed_profile(ryaw.size(), target_speed);

	float direction = 1.0;
	for(unsigned int i=0; i < ryaw.size()-1; i++){
		float dyaw = std::abs(ryaw[i+1] - ryaw[i]);
		float switch_point = (M_PI/4.0< dyaw) && (dyaw<M_PI/2.0);

		if (switch_point) direction = direction * -1;
		if (direction != 1.0) speed_profile[i]= target_speed * -1;
		else speed_profile[i]= target_speed;

		if (switch_point) speed_profile[i] = 0.0;
	}

	speed_profile[speed_profile.size()-1] = 0.0;
	return speed_profile;
};

float calc_nearest_index(State state, Vec_f cx, Vec_f cy, Vec_f cyaw, int &ind){
	float mind = std::numeric_limits<float>::max();
	for(unsigned int i=0; i<cx.size(); i++){
		float idx = cx[i] - state.x;
		float idy = cy[i] - state.y;
		float d_e = idx*idx + idy*idy;

		if (d_e<mind){
			mind = d_e;
			ind = i;
		}
	}
	float dxl = cx[ind] - state.x;
	float dyl = cy[ind] - state.y;
	float angle = YAW_P2P(cyaw[ind] - std::atan2(dyl, dxl));
	if (angle < 0) mind = mind * -1;

	return mind;
};

Eigen::Matrix4f solve_DARE(Eigen::Matrix4f A, Eigen::Vector4f B, Eigen::Matrix4f Q, float R){
	Eigen::Matrix4f X = Q;
	int maxiter = 150;
	float eps = 0.01;

	for(int i=0; i<maxiter; i++){
		Eigen::Matrix4f Xn = A.transpose()*X*A-A.transpose()*X*B/(R+B.transpose()*X*B) * B.transpose()*X*A+Q;
		Eigen::Matrix4f error = Xn - X;
		if (error.cwiseAbs().maxCoeff()<eps){
			return Xn;
		}
		X = Xn;
	}

	return X;
};

Eigen::RowVector4f dlqr(Eigen::Matrix4f A, Eigen::Vector4f B, Eigen::Matrix4f Q, float R){
	Eigen::Matrix4f X = solve_DARE(A, B ,Q, R);
	Eigen::RowVector4f K = 1.0/(B.transpose()*X*B + R) * (B.transpose()*X*A);
	return K;
};

float lqr_steering_control(State state, Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, int& ind, float& pe, float& pth_e){
	float e = calc_nearest_index(state, cx, cy, cyaw, ind);

	float k = ck[ind];
	float th_e = YAW_P2P(state.yaw - cyaw[ind]);

	Eigen::Matrix4f A = Eigen::Matrix4f::Zero();
	A(0, 0) = 1.0;
	A(0 ,1) = DT;
	A(1 ,2) = state.v;
	A(2 ,2) = 1.0;
	A(2 ,3) = DT;

	Eigen::Vector4f B = Eigen::Vector4f::Zero();
	B(3) = state.v/L;

	Eigen::Matrix4f Q = Eigen::Matrix4f::Identity();
	float R = 1;

	// gain of lqr
	Eigen::RowVector4f K = dlqr(A, B, Q, R);

	Eigen::Vector4f x = Eigen::Vector4f::Zero();
	x(0) = e;
	x(1) = (e-pe)/DT;
	x(2) = th_e;
	x(3) = (th_e-pth_e)/DT;

	float ff = std::atan2((L*k), (double)1.0);
	float fb = YAW_P2P((-K * x)(0));
	float delta = ff+fb;

	pe = e;
	pth_e = th_e;
	return delta;
};

void update (State& state, float a, float delta){

    if (delta >= MAX_STEER) delta = MAX_STEER;
    if (delta <= - MAX_STEER) delta = - MAX_STEER;

    state.x = state.x + state.v * std::cos(state.yaw) * DT;
    state.y = state.y + state.v * std::sin(state.yaw) * DT;
    state.yaw = state.yaw + state.v / L * std::tan(delta) * DT;
    state.v = state.v + a * DT;

};

void closed_loop_prediction(Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, Vec_f speed_profile, Poi_f goal){
	float T = 500.0;
	float goal_dis = 0.5;
	float stop_speed = 0.05;

	State state(-0.0, -0.0, 0.0, 0.0);

	float time_ = 0.0;
	Vec_f x;
	x.push_back(state.x);
	Vec_f y;
	y.push_back(state.y);
	Vec_f yaw;
	yaw.push_back(state.yaw);
	Vec_f v;
	v.push_back(state.v);
	Vec_f t;
	t.push_back(0.0);

	float e = 0;
	float e_th = 0;
	int ind = 0;

  cv::namedWindow("lqr", cv::WINDOW_NORMAL);
  int count = 0;

	cv::Mat bg(2000, 2000, CV_8UC3, cv::Scalar(255, 255, 255));
	for(unsigned int i=1; i<cx.size(); i++){
		cv::line(
			bg,
			cv_offset(cx[i-1], cy[i-1], bg.cols, bg.rows),
			cv_offset(cx[i], cy[i], bg.cols, bg.rows),
			cv::Scalar(0, 0, 0),
			10);
	}

	while (T >= time_){
		float di = lqr_steering_control(state, cx, cy, cyaw, ck, ind, e, e_th);
		float ai = KP * (speed_profile[ind]-state.v);
	  update(state, ai, di);

		if (std::abs(state.v) <= stop_speed) ind += 1;

		float dx = state.x - goal[0];
		float dy = state.y - goal[1];
		if (std::sqrt(dx*dx + dy*dy) <= goal_dis) {
			std::cout<<("Goal")<<std::endl;
			break;
		}

		cv::circle(
			bg,
			cv_offset(state.x, state.y, bg.cols, bg.rows),
			10, cv::Scalar(0, 0, 255), -1);

		//save image in build/bin/pngs
		// struct timeval tp;
		// gettimeofday(&tp, NULL);
		// long int ms = tp.tv_sec * 1000 + tp.tv_usec / 1000;
		// std::string int_count = std::to_string(ms);
		// cv::imwrite("./pngs/"+int_count+".png", bg);
		cv::imshow("lqr", bg);
		cv::waitKey(5);
	}
};

int main(){
	Vec_f wx({0.0, 6.0,  12.5, 10.0, 7.5, 3.0, -1.0});
	Vec_f wy({0.0, -3.0, -5.0,  6.5, 3.0, 5.0, -2.0});

  Spline2D csp_obj(wx, wy);
	Vec_f r_x;
	Vec_f r_y;
	Vec_f ryaw;
	Vec_f rcurvature;
	Vec_f rs;
	for(float i=0; i<csp_obj.s.back(); i+=0.1){
		std::array<float, 2> point_ = csp_obj.calc_postion(i);
		r_x.push_back(point_[0]);
		r_y.push_back(point_[1]);
		ryaw.push_back(csp_obj.calc_yaw(i));
		rcurvature.push_back(csp_obj.calc_curvature(i));
		rs.push_back(i);
	}
	float target_speed = 10.0 / 3.6;
	Vec_f speed_profile = calc_speed_profile(r_x, r_y, ryaw, target_speed);
	closed_loop_prediction(r_x, r_y, ryaw, rcurvature, speed_profile, {{wx.back(), wy.back()}});

}


#define PY_ARRAY_UNIQUE_SYMBOL phist_PyArray_API
#define NO_IMPORT_ARRAY

#include <string>
#include <cmath>

#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <boost/array.hpp>

#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "seglib/histogram/histogram.hxx"
#include "seglib/histogram/histogram_python.hxx"

namespace python = boost::python;

namespace histogram {

    vigra::NumpyAnyArray jointColorHistogram(

        vigra::NumpyArray<2,  vigra::TinyVector<float,3>  >    img,
        const vigra::TinyVector<float, 3> &                    min,
        const vigra::TinyVector<float, 3> &                    max,
        const vigra::TinyVector<float, 3> &                    bins,
        const size_t                                           r,
        //output
        vigra::NumpyArray<5, float >                           res = vigra::NumpyArray<5, float >()

    ){

        // allocate output
        typedef typename vigra::NumpyArray<5, float >::difference_type Shape5;
        Shape5 shape(img.shape(0),img.shape(1),int(bins[0]),int(bins[1]),int(bins[2]));
        res.reshapeIfEmpty(shape);
        std::fill(res.begin(),res.end(),0.0);
        //
        Shape5 histCoord;
        const vigra::TinyVector<uint,  3>  ones(1,1,1);
        const vigra::TinyVector<float, 2>  radius(r,r);
        const vigra::TinyVector<float, 2>  radius1(r+1,r+1);
        const vigra::TinyVector<float, 3>  fac = ( (bins-ones) / (max-min) );

        vigra::TinyVector<int,2>  start,end,c;

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1]){

            for(int d=0;d<2;++d){
                start[d]   = std::max(int(0),            int(histCoord[d]) - int(r));
                end[d]     = std::min(int(img.shape(d)), int(histCoord[d] + (r+1) ));
            }

            for(c[0]=start[0];c[0]<end[0];++c[0])
            for(c[1]=start[1];c[1]<end[1];++c[1]){

                // get the pixel value at c
                vigra::TinyVector<float, 3>  pixelValue = img(c[0],c[1]);
                pixelValue -= min;
                pixelValue *= fac;

                // (the first two coordinates of hist coord are filled)
                histCoord[2]=int(pixelValue[0]);
                histCoord[3]=int(pixelValue[1]);
                histCoord[4]=int(pixelValue[2]);

                PHIST_ASSERT_OP(histCoord[0],<,img.shape(0));
                PHIST_ASSERT_OP(histCoord[1],<,img.shape(1));

                PHIST_ASSERT_OP(histCoord[2],<,bins[0]);
                PHIST_ASSERT_OP(histCoord[3],<,bins[1]);
                PHIST_ASSERT_OP(histCoord[4],<,bins[2]);

                // increment counter
                res(histCoord[0],histCoord[1],histCoord[2],histCoord[3],histCoord[4])+=1.0;
            }
        }

        // normalizes
        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1]){

            float sum=0;

            for(histCoord[2]=0;histCoord[2]<res.shape(2);++histCoord[2])
            for(histCoord[3]=0;histCoord[3]<res.shape(3);++histCoord[3])
            for(histCoord[4]=0;histCoord[4]<res.shape(4);++histCoord[4]){

                sum+=res(histCoord[0],histCoord[1],histCoord[2],histCoord[3],histCoord[4]);
            }
            for(histCoord[2]=0;histCoord[2]<res.shape(2);++histCoord[2])
            for(histCoord[3]=0;histCoord[3]<res.shape(3);++histCoord[3])
            for(histCoord[4]=0;histCoord[4]<res.shape(4);++histCoord[4]){

                res(histCoord[0],histCoord[1],histCoord[2],histCoord[3],histCoord[4])/=sum;
            }
        }

        return res;
    }

    vigra::NumpyAnyArray batchHistogram(
        vigra::NumpyArray<3, vigra::Multiband<float>  >   img,
        vigra::NumpyArray<1, float  >   min,
        vigra::NumpyArray<1, float  >   max,
        const size_t                    bins,
        const size_t                    r,
        //output
        vigra::NumpyArray<4, float >    res = vigra::NumpyArray<4, float >()
    ){
        const size_t nChannels=img.shape(2);
        // allocate output
        typedef typename vigra::NumpyArray<4, float >::difference_type Shape4;
        Shape4 shape(img.shape(0),img.shape(1),nChannels,bins);
        res.reshapeIfEmpty(shape);
        std::fill(res.begin(),res.end(),0.0);

        // coordinate in the res array (pixel wise histogram)
        // (x,y,c,bin)
        Shape4 histCoord;
        const vigra::TinyVector<float, 2>  radius1(r+1,r+1);
        // channel wise factor
        std::vector<float> fac(nChannels);
        for(size_t channel=0;channel<nChannels;++channel){
            fac[channel]= float(bins-1) / (max(channel)-min(channel));
        }

        vigra::TinyVector<int,2>  start,end,c;

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1]){

            for(int d=0;d<2;++d){
                start[d]   = std::max(int(0),            int(histCoord[d]) - int(r));
                end[d]     = std::min(int(img.shape(d)), int(histCoord[d] + (r+1) ));
            }

            for(c[0]=start[0];c[0]<end[0];++c[0])
            for(c[1]=start[1];c[1]<end[1];++c[1]){

                // iterate over all channels
                for(histCoord[2]=0;histCoord[2]<nChannels;++histCoord[2] ){

                    const float value = img(c[0],c[1],histCoord[2]);

                    histCoord[3] = static_cast<int>((value - min(histCoord[2]) )*fac[histCoord[2]]);

                    /*
                    std::cout<<"\n\n” channel "<<histCoord[2]<<"\n";
                    std::cout<<"value "<< value<<"\n";
                    std::cout<<"mi " << min(histCoord[2])<<"\n";
                    std::cout<<"ma " << max(histCoord[2])<<"\n";
                    std::cout<<"fa " << fac[histCoord[2]]<<"\n";
                    */

                    PHIST_ASSERT_OP(histCoord[3],<,bins);
                    // increment hist
                    res(histCoord[0],histCoord[1],histCoord[2],histCoord[3])+=1.0;
                }
            }
        }

        // normalize

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1])
        for(histCoord[2]=0;histCoord[2]<img.shape(2);++histCoord[2]){

            float sum=0.0;
            for(histCoord[3]=0;histCoord[3]<bins;++histCoord[3]){
                sum+=res(histCoord[0],histCoord[1],histCoord[2],histCoord[3]);
            }
            for(histCoord[3]=0;histCoord[3]<bins;++histCoord[3]){
                res(histCoord[0],histCoord[1],histCoord[2],histCoord[3])/=sum;
            }
        }
        return res;
    }

    /*

    vigra::NumpyAnyArray labelMaskedHistogram(
        // input data (for example an rgb image with xyc order)
        vigra::NumpyArray<3, vigra::Multiband<float>  >         img,
        // input labeling for which are used as mask images
        vigra::NumpyArray<3, vigra::Multiband<vigra::UInt64> >  labelings,
        vigra::NumpyArray<1, UInt64>                            numberOfLabels,
        vigra::NumpyArray<1, float>                             weights
    ){

        const size_t numberOfInputLabelings = labelings.shape(2);

    }
    */

    void export_histogram(){

        python::def("_jointColorHistogram_",vigra::registerConverters(&jointColorHistogram),
            (
                python::arg("img"),
                python::arg("dmin"),
                python::arg("dmax"),
                python::arg("bins"),
                python::arg("r"),
                python::arg("out")=python::object()
            )
        );

        python::def("_batchHistogram_",vigra::registerConverters(&batchHistogram),
            (
                python::arg("img"),
                python::arg("dmin"),
                python::arg("dmax"),
                python::arg("bins"),
                python::arg("r"),
                python::arg("out")=python::object()
            )
        );

    }

}


// Copyright (c) 2020-2021 Franz Alt
// This code is licensed under MIT license (see LICENSE.txt for details).

#include <libcvpg/imageproc/algorithms/hog.hpp>

#include <cmath>
#include <cstdint>
#include <cstring>
#include <exception>

#include <boost/asynchronous/algorithm/then.hpp>

namespace {

template<class image_type>
cvpg::histogram<double> calc_hog_cell(std::shared_ptr<image_type> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, bool l1_normalilze = true);

template<>
cvpg::histogram<double> calc_hog_cell(std::shared_ptr<cvpg::image_gray_8bit> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, bool l1_normalize)
{
    cvpg::histogram<std::size_t> histogram(9);

    const std::uint8_t * raw = image->data(0).get();
    const std::size_t image_width = image->width();

    std::int16_t gx = 0;
    std::int16_t gy = 0;
    double mag = 0.0;
    double angle = 0.0;

    // correct from/to-values to calculate the gradients up to the image borders
    if (from_x > 1)
    {
        --from_x;
    }
    else
    {
        from_x = 1;
    }

    if (to_x < (image->width() - 2))
    {
        ++to_x;
    }
    else
    {
        to_x = image->width() - 2;
    }

    if (from_y > 1)
    {
        --from_y;
    }
    else
    {
        from_y = 1;
    }

    if (to_y < (image->height() - 2))
    {
        ++to_y;
    }
    else
    {
        to_y = image->height() - 2;
    }

    for (std::size_t y = from_y; y <= to_y; ++y)
    {
        for (std::size_t x = from_x; x <= to_x; ++x)
        {
            // calculate the x/y gradients
            gx = static_cast<std::int16_t>(raw[y * image_width + (x + 1)]) - static_cast<std::int16_t>(raw[y * image_width + (x - 1)]);
            gy = static_cast<std::int16_t>(raw[(y + 1) * image_width + x]) - static_cast<std::int16_t>(raw[(y - 1) * image_width + x]);

            // calculate the gradient magnitude
            mag = std::sqrt(static_cast<double>(gx * gx + gy * gy));

            // calculate the absolute gradient angle
            angle = std::fabs((gx == 0) ? 0.0 : atan(static_cast<double>(gy) / static_cast<double>(gx)) * 180.0 / M_PI);

            const std::size_t bin = static_cast<std::size_t>(std::floor(angle / 20.0));

            histogram.at(bin) += static_cast<decltype(histogram)::value_type>(mag);
        }
    }

    cvpg::histogram<double> normalized_histogram(9);

    if (l1_normalize)
    {
        // normalize histogram (L1 norm)
        double sum = 0;

        for (auto const & h : histogram)
        {
            sum += std::fabs(static_cast<double>(h));
        }

        for (std::size_t i = 0; i < histogram.bins(); ++i)
        {
            normalized_histogram.at(i) = static_cast<double>(histogram.at(i)) / sum;
        }
    }
    else
    {
        // normalize histogram (L2 norm)
        double sum = 0;

        for (auto const & h : histogram)
        {
            auto h_ = std::fabs(static_cast<double>(h));
            sum += h_ * h_;
        }

        sum = std::sqrt(sum);

        for (std::size_t i = 0; i < histogram.bins(); ++i)
        {
            normalized_histogram.at(i) = std::min(1.0, static_cast<double>(histogram.at(i)) / sum);
        }
    }

    return normalized_histogram;
}

template<>
cvpg::histogram<double> calc_hog_cell(std::shared_ptr<cvpg::image_rgb_8bit> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, bool l1_normalize)
{
    // TODO implement me!

    return cvpg::histogram<double>(9);
}

template<class image_type>
struct hog_col_task : public boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >
{
    hog_col_task(std::shared_ptr<image_type> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, std::size_t cell_dimension, std::size_t sequential_cells_per_row)
        : boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >("hog_col")
        , m_image(image)
        , m_from_x(from_x)
        , m_to_x(to_x)
        , m_from_y(from_y)
        , m_to_y(to_y)
        , m_cell_dimension(cell_dimension)
        , m_sequential_cells_per_row(sequential_cells_per_row)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cells = (m_to_x - m_from_x + 1) / m_cell_dimension;

        if (cells <= m_sequential_cells_per_row)
        {
            std::vector<cvpg::histogram<double> > hogs;
            hogs.reserve(cells);

            for (std::size_t i = 0; i < cells; ++i)
            {
                hogs.push_back(calc_hog_cell(m_image, m_from_x + i * m_cell_dimension, m_from_x + (i + 1) * m_cell_dimension - 1, m_from_y, m_to_y));
            }

            task_res.set_value(std::move(hogs));
        }
        else
        {
            // determine middle of x-range dependet on the cell dimension
            const std::size_t x_half = m_from_x + (cells / 2) * m_cell_dimension;

            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        auto h = std::move(std::get<1>(cont_res).get());

                        std::vector<cvpg::histogram<double> > hogs(std::move(std::get<0>(cont_res).get()));
                        hogs.insert(hogs.end(), h.begin(), h.end());

                        task_res.set_value(std::move(hogs));
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_col_task<image_type>(m_image, m_from_x, x_half - 1, m_from_y, m_to_y, m_cell_dimension, m_sequential_cells_per_row),
                hog_col_task<image_type>(m_image, x_half, m_to_x, m_from_y, m_to_y, m_cell_dimension, m_sequential_cells_per_row)
            );
        }
    }

private:
    std::shared_ptr<image_type> m_image;

    std::size_t m_from_x;
    std::size_t m_to_x;
    std::size_t m_from_y;
    std::size_t m_to_y;

    std::size_t m_cell_dimension;

    std::size_t m_sequential_cells_per_row;
};

template<class image_type>
struct hog_row_task : public boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >
{
    hog_row_task(std::shared_ptr<image_type> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, std::size_t cell_dimension, std::size_t sequential_cells_per_row)
        : boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >("hog_row")
        , m_image(image)
        , m_from_x(from_x)
        , m_to_x(to_x)
        , m_from_y(from_y)
        , m_to_y(to_y)
        , m_cell_dimension(cell_dimension)
        , m_sequential_cells_per_row(sequential_cells_per_row)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cells = (m_to_y - m_from_y + 1) / m_cell_dimension;

        if (cells <= 1)
        {
            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        task_res.set_value(std::move(std::get<0>(cont_res).get()));
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_col_task<image_type>(m_image, m_from_x, m_to_x, m_from_y, m_to_y, m_cell_dimension, m_sequential_cells_per_row)
            );
        }
        else
        {
            // determine middle of y-range dependet on the cell dimension
            const std::size_t y_half = m_from_y + (cells / 2) * m_cell_dimension;

            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        auto h = std::move(std::get<1>(cont_res).get());

                        std::vector<cvpg::histogram<double> > hogs(std::move(std::get<0>(cont_res).get()));
                        hogs.insert(hogs.end(), h.begin(), h.end());

                        task_res.set_value(std::move(hogs));
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_row_task<image_type>(m_image, m_from_x, m_to_x, m_from_y, y_half - 1, m_cell_dimension, m_sequential_cells_per_row),
                hog_row_task<image_type>(m_image, m_from_x, m_to_x, y_half, m_to_y, m_cell_dimension, m_sequential_cells_per_row)
            );
        }
    }

private:
    std::shared_ptr<image_type> m_image;

    std::size_t m_from_x;
    std::size_t m_to_x;
    std::size_t m_from_y;
    std::size_t m_to_y;

    std::size_t m_cell_dimension;

    std::size_t m_sequential_cells_per_row;
};

template<class image_type>
struct hog_task : public boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >
{
    hog_task(image_type image, std::size_t cell_dimension, std::size_t sequential_cells_per_row = 4)
        : boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >("hog")
        , m_image(std::make_shared<image_type>(std::forward<image_type>(image)))
        , m_cell_dimension(cell_dimension)
        , m_sequential_cells_per_row(sequential_cells_per_row)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        // determine the amount of rows and columns dependent on the desired cell dimension
        const std::size_t cols = m_image->width() / m_cell_dimension;
        const std::size_t rows = m_image->height() / m_cell_dimension;

        boost::asynchronous::create_callback_continuation(
            [task_res = std::move(task_res)](auto cont_res) mutable
            {
                try
                {
                    task_res.set_value(std::move(std::get<0>(cont_res).get()));
                }
                catch (...)
                {
                    task_res.set_exception(std::current_exception());
                }
            },
            hog_row_task<image_type>(m_image, 0, m_image->width() - 1, 0, m_image->height() - 1, m_cell_dimension, m_sequential_cells_per_row)
        );
    }

private:
    std::shared_ptr<image_type> m_image;

    std::size_t m_cell_dimension;

    std::size_t m_sequential_cells_per_row;
};

void paint_hog_cell(std::shared_ptr<cvpg::image_gray_8bit> image, cvpg::histogram<double> const & histogram, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y)
{
    auto raw = image->data(0).get();
    const std::size_t image_width = image->width();

    const std::size_t half_x = from_x + (to_x - from_x) / 2;
    const std::size_t half_y = from_y + (to_y - from_y) / 2;

    auto draw_line_from_center =
        [raw, image_width, h_x = half_x, h_y = half_y](std::int32_t dx, std::int32_t dy, double increment)
        {
            // calculate start point
            const std::int32_t x_s = h_x - abs(dx);
            const std::int32_t y_s = h_y - abs(dy);

            // calculate end point
            const std::int32_t x_e = h_x + abs(dx);
            const std::int32_t y_e = h_y + abs(dy);

            if (abs(dx) > abs(dy))
            {
                // calculate the slope of the line
                const std::int32_t s = dy / dx;

                std::size_t y = y_s;

                for (std::size_t x = x_s; x <= x_e; ++x, y += s)
                {
                    std::uint8_t * r = raw + y * image_width + x;

                    *r = std::min(
                            static_cast<std::uint8_t>(255),
                            static_cast<std::uint8_t>(*r + increment * 255.0)
                         );
                }
            }
            else
            {
                // calculate the slope of the line
                const std::int32_t s = dx / dy;

                std::size_t x = x_s;

                for (std::size_t y = y_s; y <= y_e; ++y, x += s)
                {
                    std::uint8_t * r = raw + y * image_width + x;

                    *r = std::min(
                            static_cast<std::uint8_t>(255),
                            static_cast<std::uint8_t>(*r + increment * 255.0)
                         );
                }
            }
        };

    for (std::size_t i = 0; i < histogram.bins(); ++i)
    {
        auto const & h = histogram.at(i);

        if (h != 0.0)
        {
            if (i == 0)
            {
                // case: angle is at range [0..20) degree
                draw_line_from_center(0, 2, h);
            }
            else if (i == 1)
            {
                // case: angle it at range [20..40) degree
                draw_line_from_center(1, 2, h);
            }
            else if (i == 2)
            {
                // case: angle it at range [40..60) degree
                draw_line_from_center(2, 2, h);
            }
            else if (i == 3)
            {
                // case: angle it at range [60..80) degree
                draw_line_from_center(2, 1, h);
            }
            else if (i == 4)
            {
                // case: angle it at range [80..100) degree
                draw_line_from_center(2, 0, h);
            }
            else if (i == 5)
            {
                // case: angle it at range [100..120) degree
                draw_line_from_center(2, -1, h);
            }
            else if (i == 6)
            {
                // case: angle it at range [120..140) degree
                draw_line_from_center(2, -2, h);
            }
            else if (i == 7)
            {
                // case: angle it at range [140..160) degree
                draw_line_from_center(1, -2, h);
            }
            else if (i == 8)
            {
                // case: angle it at range [160..180) degree
                draw_line_from_center(0, -2, h);
            }
        }
    }
}

struct hog_image_col_task : public boost::asynchronous::continuation_task<void>
{
    hog_image_col_task(std::shared_ptr<cvpg::image_gray_8bit> image, std::shared_ptr<std::vector<cvpg::histogram<double> > > histograms, std::size_t from_cell_col, std::size_t to_cell_col, std::size_t from_cell_row, std::size_t to_cell_row, std::size_t cell_dimension)
        : boost::asynchronous::continuation_task<void>("hog_image_col")
        , m_image(std::move(image))
        , m_histograms(std::move(histograms))
        , m_from_cell_col(from_cell_col)
        , m_to_cell_col(to_cell_col)
        , m_from_cell_row(from_cell_row)
        , m_to_cell_row(to_cell_row)
        , m_cell_dimension(cell_dimension)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cells = m_to_cell_col - m_from_cell_col + 1;

        if (cells <= 1)
        {
            const std::size_t cells_per_row = m_image->width() / m_cell_dimension;

            // determine histogram for current cell
            cvpg::histogram<double> const & h = m_histograms->at(m_from_cell_row * cells_per_row + m_from_cell_col);

            const std::size_t from_x = m_from_cell_col * m_cell_dimension;
            const std::size_t to_x = from_x + m_cell_dimension - 1;
            const std::size_t from_y = m_from_cell_row * m_cell_dimension;
            const std::size_t to_y = from_y + m_cell_dimension - 1;

            paint_hog_cell(m_image, h, from_x, to_x, from_y, to_y);

            task_res.set_value();
        }
        else
        {
            // determine middle of col-range
            const std::size_t col_half = m_from_cell_col + (cells / 2);

            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        std::get<0>(cont_res).get();
                        std::get<1>(cont_res).get();

                        task_res.set_value();
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_image_col_task(m_image, m_histograms, m_from_cell_col, col_half - 1, m_from_cell_row, m_to_cell_row, m_cell_dimension),
                hog_image_col_task(m_image, m_histograms, col_half, m_to_cell_col, m_from_cell_row, m_to_cell_row, m_cell_dimension)
            );
        }
    }

private:
    std::shared_ptr<cvpg::image_gray_8bit> m_image;

    std::shared_ptr<std::vector<cvpg::histogram<double> > > m_histograms;

    std::size_t m_from_cell_col;
    std::size_t m_to_cell_col;
    std::size_t m_from_cell_row;
    std::size_t m_to_cell_row;

    std::size_t m_cell_dimension;
};

struct hog_image_row_task : public boost::asynchronous::continuation_task<void>
{
    hog_image_row_task(std::shared_ptr<cvpg::image_gray_8bit> image, std::shared_ptr<std::vector<cvpg::histogram<double> > > histograms, std::size_t from_cell_col, std::size_t to_cell_col, std::size_t from_cell_row, std::size_t to_cell_row, std::size_t cell_dimension)
        : boost::asynchronous::continuation_task<void>("hog_image_row")
        , m_image(std::move(image))
        , m_histograms(std::move(histograms))
        , m_from_cell_col(from_cell_col)
        , m_to_cell_col(to_cell_col)
        , m_from_cell_row(from_cell_row)
        , m_to_cell_row(to_cell_row)
        , m_cell_dimension(cell_dimension)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cells = m_to_cell_row - m_from_cell_row + 1;

        if (cells <= 1)
        {
            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        std::get<0>(cont_res).get();

                        task_res.set_value();
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_image_col_task(m_image, m_histograms, m_from_cell_col, m_to_cell_col, m_from_cell_row, m_to_cell_row, m_cell_dimension)
            );
        }
        else
        {
            // determine middle of row-range
            const std::size_t row_half = m_from_cell_row + (cells / 2);

            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        std::get<0>(cont_res).get();
                        std::get<1>(cont_res).get();

                        task_res.set_value();
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_image_row_task(m_image, m_histograms, m_from_cell_col, m_to_cell_col, m_from_cell_row, row_half - 1, m_cell_dimension),
                hog_image_row_task(m_image, m_histograms, m_from_cell_col, m_to_cell_col, row_half, m_to_cell_row, m_cell_dimension)
            );
        }
    }

private:
    std::shared_ptr<cvpg::image_gray_8bit> m_image;

    std::shared_ptr<std::vector<cvpg::histogram<double> > > m_histograms;

    std::size_t m_from_cell_col;
    std::size_t m_to_cell_col;
    std::size_t m_from_cell_row;
    std::size_t m_to_cell_row;

    std::size_t m_cell_dimension;
};

struct hog_image_task : public boost::asynchronous::continuation_task<cvpg::image_gray_8bit>
{
    hog_image_task(std::vector<cvpg::histogram<double> > histograms, std::size_t cells_per_row, std::size_t cell_dimension)
        : boost::asynchronous::continuation_task<cvpg::image_gray_8bit>("hog_image")
        , m_histograms(std::make_shared<std::vector<cvpg::histogram<double> > >(std::move(histograms)))
        , m_cells_per_row(cells_per_row)
        , m_cell_dimension(cell_dimension)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cell_cols = m_cells_per_row;
        const std::size_t cell_rows = m_histograms->size() / cell_cols;

        // create a result image ...
        auto image = std::make_shared<cvpg::image_gray_8bit>(cell_cols * m_cell_dimension, cell_rows * m_cell_dimension);

        // ... and paint it black
        memset(image->data(0).get(), 0, image->width() * image->height());

        boost::asynchronous::create_callback_continuation(
            [task_res = std::move(task_res), image](auto cont_res) mutable
            {
                try
                {
                    std::get<0>(cont_res).get();

                    task_res.set_value(std::move(*image));
                }
                catch (...)
                {
                    task_res.set_exception(std::current_exception());
                }
            },
            hog_image_row_task(image, m_histograms, 0, cell_cols - 1, 0, cell_rows - 1, m_cell_dimension)
        );
    }

private:
    std::shared_ptr<std::vector<cvpg::histogram<double> > > m_histograms;

    std::size_t m_cells_per_row;

    std::size_t m_cell_dimension;
};

}

namespace cvpg::imageproc::algorithms {

boost::asynchronous::detail::callback_continuation<std::vector<histogram<double> > > hog(image_gray_8bit image, std::size_t cell_dimension)
{
    return boost::asynchronous::top_level_callback_continuation<std::vector<histogram<double> > >(
               hog_task(std::move(image), cell_dimension)
           );
}

boost::asynchronous::detail::callback_continuation<std::vector<histogram<double> > > hog(image_rgb_8bit image, std::size_t cell_dimension)
{
    return boost::asynchronous::top_level_callback_continuation<std::vector<histogram<double> > >(
               hog_task(std::move(image), cell_dimension)
           );
}

boost::asynchronous::detail::callback_continuation<image_gray_8bit> hog_image(std::vector<histogram<double> > histograms, std::size_t cells_per_row, std::size_t cell_dimension)
{
    return boost::asynchronous::top_level_callback_continuation<image_gray_8bit>(
               hog_image_task(std::move(histograms), cells_per_row, cell_dimension)
           );
}

boost::asynchronous::detail::callback_continuation<image_gray_8bit> hog_image(image_gray_8bit image)
{
    const std::size_t cell_dimension = 8;

    auto cells_per_row = image.width() / cell_dimension;

    return boost::asynchronous::then(
             hog(std::move(image), cell_dimension),
             [cells_per_row, cell_dimension](auto cont_res)
             {
                 return hog_image(std::move(cont_res.get()), cells_per_row, cell_dimension);
             }
           );
}

boost::asynchronous::detail::callback_continuation<image_gray_8bit> hog_image(image_rgb_8bit image)
{
    const std::size_t cell_dimension = 8;

    auto cells_per_row = image.width() / cell_dimension;

    return boost::asynchronous::then(
             hog(std::move(image), cell_dimension),
             [cells_per_row, cell_dimension](auto cont_res)
             {
                 return hog_image(std::move(cont_res.get()), cells_per_row, cell_dimension);
             }
           );
}

} // namespace cvpg::imageproc::algorithms


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__MPC__DLTV_OCP_HPP_
#define CBR_CONTROL__MPC__DLTV_OCP_HPP_

#include <Eigen/Dense>

#include <cbr_utils/utils.hpp>

#include <utility>

#include "ocp_common.hpp"

namespace cbr
{

/* ---------------------------------------------------------------------------------------------- */
/*               Continuous Time Varying Linear Optimal Control Problem Discretizer               */
/* ---------------------------------------------------------------------------------------------- */

template<typename cltv_pb_t, std::size_t _nPts = 100, std::size_t exp_order = 4>
class DltvOcp
{
public:
  // Must be defined in ctlv_pb problem
  constexpr static std::size_t nx = cltv_pb_t::nx;
  constexpr static std::size_t nu = cltv_pb_t::nu;
  constexpr static std::size_t nPts = _nPts;

  using problem_t = cltv_pb_t;

  // Create some useful aliases
  using state_t = Eigen::Matrix<double, nx, 1>;
  using input_t = Eigen::Matrix<double, nu, 1>;
  using time_t = Eigen::Matrix<double, nPts, 1>;
  using A_t = Eigen::Matrix<double, nx, nx>;
  using B_t = Eigen::Matrix<double, nx, nu>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  // Get return type of problem functions
  using Ar_t = std::result_of_t<decltype(&cltv_pb_t::get_A)(cltv_pb_t, double)>;
  using Br_t = std::result_of_t<decltype(&cltv_pb_t::get_B)(cltv_pb_t, double)>;
  using Qr_t = std::result_of_t<decltype(&cltv_pb_t::get_Q)(cltv_pb_t, double)>;
  using QTr_t = std::result_of_t<decltype(&cltv_pb_t::get_QT)(cltv_pb_t)>;
  using Rr_t = std::result_of_t<decltype(&cltv_pb_t::get_R)(cltv_pb_t, double)>;

  /* -------------------------------------------------------------------------- */
  /*                                  Optionals                                 */
  /* -------------------------------------------------------------------------- */

  // Check existance of get_E function
  constexpr static bool has_E_approx = std::experimental::is_detected_v<
    ocp_detail::has_E_continuous, cltv_pb_t>;
  constexpr static bool has_E = std::experimental::is_detected_exact_v<
    state_t, ocp_detail::has_E_continuous, cltv_pb_t>||
    std::experimental::is_detected_exact_v<
    const state_t &, ocp_detail::has_E_continuous, cltv_pb_t>;
  using Er_t = std::experimental::detected_or_t<
    state_t, ocp_detail::has_E_continuous, cltv_pb_t>;
  static_assert(
    !(has_E_approx && !has_E),
    "Detected get_E function doesn't have a correct return type. "
    "It must be an nx*1 Eigen::Matrix (or a const reference to one)");

  // Check existance of get_q function
  constexpr static bool has_q_approx = std::experimental::is_detected_v<
    ocp_detail::has_q_continuous, cltv_pb_t>;
  constexpr static bool has_q = std::experimental::is_detected_exact_v<
    state_t, ocp_detail::has_q_continuous, cltv_pb_t>||
    std::experimental::is_detected_exact_v<
    const state_t &, ocp_detail::has_q_continuous, cltv_pb_t>;
  using qr_t = std::experimental::detected_or_t<
    state_t, ocp_detail::has_q_continuous, cltv_pb_t>;
  static_assert(
    !(has_q_approx && !has_q),
    "Detected get_q function doesn't have a correct return type. "
    "It must be an nx*1 Eigen::Matrix (or a const reference to one)");

  // Check existance of get_qT function
  constexpr static bool has_qT_approx = std::experimental::is_detected_v<
    ocp_detail::has_qT_continuous, cltv_pb_t>;
  constexpr static bool has_qT = std::experimental::is_detected_exact_v<
    state_t, ocp_detail::has_qT_continuous, cltv_pb_t>||
    std::experimental::is_detected_exact_v<
    const state_t &, ocp_detail::has_qT_continuous, cltv_pb_t>;

  using qTr_t = std::experimental::detected_or_t<
    state_t, ocp_detail::has_qT_continuous, cltv_pb_t>; \
  static_assert(
    !(has_qT_approx && !has_qT),
    "Detected get_qT function doesn't have a correct return type. "
    "It must be an nx*1 Eigen::Matrix (or a const reference to one)");

  // Check existance of get_r function
  constexpr static bool has_r_approx = std::experimental::is_detected_v<
    ocp_detail::has_r_continuous, cltv_pb_t>;
  constexpr static bool has_r = std::experimental::is_detected_exact_v<
    input_t, ocp_detail::has_r_continuous, cltv_pb_t>||
    std::experimental::is_detected_exact_v<
    const input_t &, ocp_detail::has_r_continuous, cltv_pb_t>;
  using rr_t = std::experimental::detected_or_t<
    input_t, ocp_detail::has_r_continuous, cltv_pb_t>;
  static_assert(
    !(has_r_approx && !has_r),
    "Detected get_r function doesn't have a correct return type. "
    "It must be an nu*1 Eigen::Matrix (or a const reference to one)");

  // Check problem dimensions
  static_assert(nx > 0, "Number of states must be > 0.");
  static_assert(nu > 0, "Number of inputs must be > 0.");
  static_assert(nPts > 1, "Number of trajectory points must be > 1.");
  static_assert(exp_order < 20, "Exponential order must be < 20.");

  // Check return type of problem functions
  static_assert(
    std::is_same_v<std::decay_t<Ar_t>, A_t>,
    "The get_A method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Br_t>, B_t>,
    "The get_B method of the problem must return an nx*nu Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Qr_t>, Q_t>,
    "The get_Q method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<qr_t>, state_t>,
    "The get_q method of the problem must return an nx*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<QTr_t>, Q_t>,
    "The get_QT method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<qTr_t>, state_t>,
    "The get_qT method of the problem must return an nx*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Rr_t>, R_t>,
    "The get_R method of the problem must return an nu*nu Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<rr_t>, input_t>,
    "The get_r method of the problem must return an nu*1 Eigen::Matrix (or a reference to one).");

public:
  DltvOcp() = delete;
  DltvOcp(const DltvOcp &) = default;
  DltvOcp(DltvOcp &&) = default;
  DltvOcp & operator=(const DltvOcp &) = default;
  DltvOcp & operator=(DltvOcp &&) = default;

  explicit DltvOcp(const cltv_pb_t & pb)
  : cltv_pb_(pb),
    dt_{compute_dt()}
  {}

  explicit DltvOcp(cltv_pb_t && pb)
  : cltv_pb_(std::move(pb)),
    dt_{compute_dt()}
  {}

  template<typename T1>
  DltvOcp(T1 && pb)
  : cltv_pb_(std::forward<T1>(pb)),
    dt_{compute_dt()}
  {}

  void get_x0(Eigen::Ref<state_t> x0) const
  {
    cltv_pb_.get_x0(x0);
  }

  void get_state_lb(std::size_t k, Eigen::Ref<state_t> state_lb) const
  {
    cltv_pb_.get_state_lb(indexToTime(k), state_lb);
  }

  void get_state_ub(std::size_t k, Eigen::Ref<state_t> state_ub) const
  {
    cltv_pb_.get_state_ub(indexToTime(k), state_ub);
  }

  void get_input_lb(std::size_t k, Eigen::Ref<input_t> input_lb) const
  {
    cltv_pb_.get_input_lb(indexToTime(k), input_lb);
  }

  void get_input_ub(std::size_t k, Eigen::Ref<input_t> input_ub) const
  {
    cltv_pb_.get_input_ub(indexToTime(k), input_ub);
  }

  A_t get_A(std::size_t k) const
  {
    // define identity for order = 0
    A_t expA = A_t::Identity();

    if constexpr (exp_order > 0) {
      const A_t Adt = dt_ * cltv_pb_.get_A(indexToTime(k));
      expA += Adt;
      if constexpr (exp_order > 1) {
        double c = 1.;
        A_t Adtp = Adt;
        for (std::size_t i = 2; i <= exp_order; i++) {
          Adtp *= Adt;
          c /= static_cast<double>(i);
          expA += c * Adtp;
        }
      }
    }

    return expA;
  }

  B_t get_B(std::size_t k) const
  {
    // define identity for order = 0
    A_t expA = A_t::Identity();

    if constexpr (exp_order > 0) {
      const A_t Adt = dt_ * cltv_pb_.get_A(indexToTime(k));
      expA += Adt / 2.;
      if constexpr (exp_order > 1) {
        double c = 0.5;
        A_t Adtp = Adt / 2.;
        for (std::size_t i = 2; i <= exp_order; i++) {
          Adtp *= Adt;
          c /= static_cast<double>(i + 1);
          expA += c * Adtp;
        }
      }
    }

    B_t expB = expA * cltv_pb_.get_B(indexToTime(k)) * dt_;

    return expB;
  }

  template<typename T = std::size_t>
  state_t get_E(std::enable_if_t<has_E, T> k)
  {
    // define identity for order = 0
    A_t expA = A_t::Identity();

    if constexpr (exp_order > 0) {
      const A_t Adt = dt_ * cltv_pb_.get_A(indexToTime(k));
      expA += Adt / 2.;
      if constexpr (exp_order > 1) {
        double c = 0.5;
        A_t Adtp = Adt / 2.;
        for (std::size_t i = 2; i <= exp_order; i++) {
          Adtp *= Adt;
          c /= static_cast<double>(i + 1);
          expA += c * Adtp;
        }
      }
    }

    state_t expE = expA * cltv_pb_.get_E(indexToTime(k)) * dt_;
    return expE;
  }

  Q_t get_Q(std::size_t k) const
  {
    return dt_ * cltv_pb_.get_Q(indexToTime(k));
  }

  template<typename T = std::size_t>
  state_t get_q(std::enable_if_t<has_q, T> k) const
  {
    return dt_ * cltv_pb_.get_q(indexToTime(k));
  }

  R_t get_R(std::size_t k) const
  {
    return dt_ * cltv_pb_.get_R(indexToTime(k));
  }

  template<typename T = std::size_t>
  input_t get_r(std::enable_if_t<has_r, T> k) const
  {
    return dt_ * cltv_pb_.get_r(indexToTime(k));
  }

  Q_t get_QT() const
  {
    double T;
    cltv_pb_.get_T(T);
    return cltv_pb_.get_QT();
  }

  template<typename T = void *>
  state_t get_qT([[maybe_unused]] std::enable_if_t<has_qT, T> k = nullptr) const
  {
    double TT;
    cltv_pb_.get_T(TT);
    return cltv_pb_.get_qT();
  }

  cltv_pb_t & problem()
  {
    return cltv_pb_;
  }

  double indexToTime(std::size_t k) const
  {
    return static_cast<double>(k) * dt_;
  }

protected:
  double compute_dt()
  {
    double T;
    cltv_pb_.get_T(T);
    return T / static_cast<double>(nPts - 1);
  }

protected:
  cltv_pb_t cltv_pb_{};
  double dt_{};
};

// Class template argument deduction guides
template<typename T>
DltvOcp(T)->DltvOcp<T>;

template<typename T1, typename T2>
DltvOcp(T1, T2)->DltvOcp<T1, T2::nPts, T2::expOrder>;

}  // namespace cbr

#endif  // CBR_CONTROL__MPC__DLTV_OCP_HPP_


//          Copyright Gavin Band 2008 - 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 <iostream>
#include <iomanip>
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/function.hpp>
#include "config/config.hpp"
#if HAVE_CBLAS
	#include "cblas.h"
#endif
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "genfile/VariantIdentifyingData.hpp"
#include "genfile/VariantDataReader.hpp"
#include "genfile/vcf/get_set_eigen.hpp"
#include "appcontext/get_current_time_as_string.hpp"
#include "components/RelatednessComponent/PCALoadingComputer.hpp"
#include "components/RelatednessComponent/LapackEigenDecomposition.hpp"
#include "components/RelatednessComponent/mean_centre_genotypes.hpp"

// #define DEBUG_PCA_LOADING_COMPUTER 1

namespace {
	template< typename Vector1, typename Vector2, typename NonMissingVector >
	double compute_correlation( Vector1 const& v1, Vector2 const& v2, NonMissingVector const& non_missingness_indicator ) {
		assert( v1.size() == v2.size() ) ;
		double non_missingness = non_missingness_indicator.sum() ;
		double mean1 = 0.0 ;
		double mean2 = 0.0 ;
		for( int i = 0; i < v1.size(); ++i ) {
			if( non_missingness_indicator( i )) {
				mean1 += v1(i) / non_missingness ;
				mean2 += v2(i) / non_missingness ;
			}
		}

		double covariance = 0.0 ;
		double variance1 = 0.0 ;
		double variance2 = 0.0 ;
		for( int i = 0; i < v1.size(); ++i ) {
			if( non_missingness_indicator( i )) {
				covariance += ( v1(i) - mean1 ) * ( v2(i) - mean2 ) ;
				variance1 += ( v1(i) - mean1 ) * ( v1(i) - mean1 ) ;
				variance2 += ( v2(i) - mean2 ) * ( v2(i) - mean2 ) ;
			}
		}

		// We should divide the covariance by N-1 and also
		// divide each variance by the same quantity.
		// But this washes out in the ratio.

		return covariance / std::sqrt( variance1 * variance2 ) ;
	}

	std::string eigenvector_column_names( std::size_t N, std::string const& string1, std::string const& string2, std::size_t i ) {
		if( i < N ) {
			return string1 + genfile::string_utils::to_string( i+1 ) ;
		}
		else {
			return string2 + genfile::string_utils::to_string( i+1 -N ) ;
		}
	}
}

PCALoadingComputer::PCALoadingComputer( int number_of_loadings ):
	m_number_of_loadings( number_of_loadings ),
	m_number_of_snps( 1 )
{}

void PCALoadingComputer::set_UDUT( std::size_t number_of_snps, Matrix const& udut ) {
	assert( udut.cols() == udut.rows() + 1 ) ;
	int n = std::min( int( m_number_of_loadings ), int( udut.rows() ) ) ;
	m_D = udut.block( 0, 0, n, 1 ) ;
	m_sqrt_D_inverse = 1 / m_D.array().sqrt() ;
	m_U = udut.block( 0, 1, udut.rows(), n ) ;
	m_number_of_snps = number_of_snps ;
}

void PCALoadingComputer::begin_processing_snps( std::size_t number_of_samples, genfile::SNPDataSource::Metadata const& ) {
	assert( number_of_samples = std::size_t( m_U.rows() )) ;
	m_genotype_calls.resize( number_of_samples ) ;
	m_non_missingness.resize( number_of_samples ) ;
}

void PCALoadingComputer::processed_snp( genfile::VariantIdentifyingData const& snp, genfile::VariantDataReader& data_reader ) {
	data_reader.get(
		":genotypes:",
		genfile::vcf::get_threshholded_calls( m_genotype_calls, m_non_missingness, 0.9, 0, 0, 1, 2 )
	) ;
	assert( m_genotype_calls.size() == m_U.rows() ) ;
	assert( m_non_missingness.size() == m_U.rows() ) ;
	// setup the storage
	m_loading_vectors.resize( 2 * m_D.rows() ) ;
	m_loading_vectors.setConstant( std::numeric_limits< double >::quiet_NaN() ) ;
	double const allele_frequency = m_genotype_calls.sum() / ( 2.0 * m_non_missingness.sum() ) ;
	if( m_non_missingness.sum() > 0 && allele_frequency > 0.001 ) {
		//std::cerr << "pre-mean genotypes are: " << m_genotype_calls.head( 20 ).transpose() << "...\n" ;
		pca::mean_centre_genotypes( &m_genotype_calls, m_non_missingness, allele_frequency ) ;
		m_genotype_calls /= std::sqrt( 2.0 * allele_frequency * ( 1.0 - allele_frequency ) ) ;

#if DEBUG_PCA_LOADING_COMPUTER
		//std::cerr << "                    SNP: " << snp << ", allele frequency = " << allele_frequency << ".\n" ;
		//std::cerr << std::resetiosflags( std::ios::floatfield ) << std::setprecision( 5 ) ;
		//std::cerr << "pre-scale genotypes are: " << m_genotype_calls.head( 20 ).transpose() << "...\n" ;
		std::cerr << "          genotypes are: " << m_genotype_calls.head( 20 ).transpose() << "...\n" ;
		std::cerr << "     non-missingness is: " << m_non_missingness.head( 20 ).transpose() << "...\n" ;
		std::cerr << "                   U is: " << m_U.block(0,0,10,10) << "...\n" ;
		std::cerr << "                   D is: " << m_D << "...\n" ;
#endif // DEBUG_PCA_LOADING_COMPUTER

		//
		// Let X  be the L\times n matrix (L SNPs, n samples) of (mean-centred, scaled) genotypes.  We want
		// to compute the row of the matrix S of unit eigenvectors of the variance-covariance matrix
		// (1/L) X X^t that corresponds to the current SNP.
		// The matrix S is given by
		//
		//       S = (1/√L) X U D^{-½}
		//
		// where
		//       (1/L) X^t X = U D U^t
		// is the eigenvalue decomposition of (1/L) X^t X that we are passed in via set_UDUT (and L is the number of SNPs).
		//
		// This is true since then
		//
		// S^t S = D^{-½} U^t (1/L) X^t X U D^{-½} = id
		//
		// (so columns of S are orthogonal) while
		//
		// (1/L X X^t) S = (1/L√L) X X^t X U D^{-½}
		//           = (1/√L) X U D U^t U D^{-½}
		//           = (1/√L) X U D^½
		//           = SD
		//
		// (so columns of S are eigenvectors with eigenvalues given by D.)
		//
#if 0
		m_loading_vectors.segment( 0, m_U.cols() ) =
			( m_genotype_calls.transpose() * m_U ) * m_D.array().sqrt().matrix().asDiagonal()
			/ ( ( m_D.transpose().array() * m_number_of_snps ).sqrt() ) ;
#else
		m_loading_vectors.segment( 0, m_U.cols() ) =
			( m_genotype_calls.transpose() * m_U ) * m_sqrt_D_inverse.asDiagonal() ;
		m_loading_vectors /= std::sqrt( m_number_of_snps ) ;
#endif
		// We also wish to compute the correlation between the SNP and the PCA component.
		// With S as above, the PCA components are the projections of columns of X onto columns of S.
		// If we want samples to correspond to columns, this is
		//   S^t X
		// which can be re-written
		//   sqrt(L) U D^{1/2}
		// i.e. we may as well compute the correlation with columns of U.
		if( m_non_missingness.sum() > 10 ) {
			for( int i = 0; i < m_U.cols(); ++i ) {
				m_loading_vectors( m_U.cols() + i ) = compute_correlation( m_genotype_calls, m_U.col( i ), m_non_missingness ) ;
			}
		}
	}
	send_results(
		snp,
		m_non_missingness.sum(),
		allele_frequency,
		m_loading_vectors,
		boost::bind(
			&eigenvector_column_names,
			m_U.cols(),
			"eigenvector_",
			"correlation_",
			_1
		)
	) ;
}

void PCALoadingComputer::send_results_to( ResultCallback callback ) {
	m_result_signal.connect( callback ) ;
}

void PCALoadingComputer::send_results( genfile::VariantIdentifyingData const& snp, double const N, double const frequency, Eigen::VectorXd const& data, GetNames get_names ) {
	m_result_signal( snp, N, frequency, data, get_names ) ;
}

std::string PCALoadingComputer::get_metadata() const {
	using namespace genfile::string_utils ;
	return "Number of SNPs: " + to_string( m_number_of_snps ) + "\n"
		+ "Number of samples: " + to_string( m_U.rows() ) + "\n"
		+ "These loadings represent unit eigenvectors of the variance-covariance matrix\n"
		+ "    1/L X X^t\n"
		+ "where X is the LxN matrix of genotypes at L SNPs and N samples (normalised across rows.)" ;
}

void PCALoadingComputer::end_processing_snps() {}


#include <iostream>
#include <string>
#include <Eigen/Dense>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>
#include <unistd.h>
#include <sys/wait.h>
#include "H5Cpp.h"
#include "rapidjson/document.h"
#include "rapidjson/writer.h"
#include "rapidjson/stringbuffer.h"

using std::cout;
using std::endl;

using namespace H5;
using namespace rapidjson;
using Eigen::tanh;
using Eigen::MatrixXd;

inline double min(double a, double b) { return(((a)<(b))?(a):(b));}
inline double max(double a, double b) { return(((a)>(b))?(a):(b));}

double hard_sigmoid(double x){
    return(max(0.0, min(1.0, x*0.2+0.5)));
}

double sigmoid(double x){
    return 1.0 / (1.0 + exp(-x));
}

inline MatrixXd Mult(MatrixXd input1, MatrixXd input2){
    return(input1.array() * input2.array());
}

char* read_model(const char *filename)
{
    char *arch_json;

    H5File file(filename, H5F_ACC_RDWR);
    Attribute attr(file.openAttribute("model_config"));
    DataType type(attr.getDataType());

    attr.read(type, &arch_json);
    return arch_json;
}

inline MatrixXd readCSV(const char* filename, int row_n, int col_n){

    std::ifstream file(filename);
    int col_flag = 0;

    MatrixXd m(row_n, col_n);

    if(row_n == 1){
        col_flag = 1;
        m.resize(col_n, row_n);
    }

    std::string line;

    int row = 0;
    int col = 0;

    if(file.is_open()){
        while(std::getline(file, line)){
            char *ptr = (char *)line.c_str();
            int len = line.length();

            col = 0;

            char *start = ptr;
            for(int i = 0; i < len; i++){
                if(ptr[i] == ','){
                    m(row, col++) = atof(start);
                    start = ptr + i + 1;
                }
            }
            m(row, col) = atof(start);

            row++;
        }
        file.close();
    }

    if(col_flag)
        return(m.transpose());

    return(m);
}

class Embedding{
    public:
        int row;
        int col;
        int out_size;
        MatrixXd W;

        Embedding(){

        }

        Embedding(const char* filename, int row, int col){
            this->row = row;
            this->col = col;
            this->out_size = col;

            W.resize(this->row, this->col);
            this->W = readCSV(filename, this->row, this->col);

        }

        MatrixXd operator()(MatrixXd input){
            MatrixXd Out(input.cols(), this->out_size);
            for(int t=0; t < Out.rows(); t++)
                Out.row(t) = W.row( input(0, t) );

            return(Out);
        }
};

class LSTM{
    public:
        int inp_size;
        int out_size;
        MatrixXd kernel;
        MatrixXd recurrent_kernel;
        MatrixXd bias;

        LSTM(){

        }

        LSTM(const char* kernel_filename, const char* recurrent_kernel_filename, const char* bias_filename, int inp_size, int out_size){
            this->inp_size = inp_size;
            this->out_size = out_size;

            kernel.resize(this->inp_size, this->out_size * 4);
            kernel = readCSV(kernel_filename, this->inp_size, this->out_size * 4);

            recurrent_kernel.resize(this->out_size, this->out_size * 4);
            recurrent_kernel = readCSV(recurrent_kernel_filename, this->out_size, this->out_size * 4);

            bias.resize(1, this->out_size * 4);
            bias = readCSV(bias_filename, 1, this->out_size * 4);

        }

        MatrixXd operator()(MatrixXd input){
            int LSTM_OUT = this->out_size;
            int MAXLEN   = input.rows();

            MatrixXd C_t = MatrixXd::Zero(1, this->out_size);
            MatrixXd h_t = MatrixXd::Zero(1, this->out_size);

            for(int t = 0; t < MAXLEN; t++){
                MatrixXd tmp_out(1, 4*LSTM_OUT);
                MatrixXd i_t(1, LSTM_OUT);
                MatrixXd f_t(1, LSTM_OUT);
                MatrixXd o_t(1, LSTM_OUT);
                MatrixXd g_t(1, LSTM_OUT);

                // IFCO
                tmp_out = input.row(t) * kernel + h_t * recurrent_kernel + bias;

                i_t = (tmp_out.block(0, 0*LSTM_OUT, 1, LSTM_OUT)).unaryExpr(&hard_sigmoid);
                f_t = (tmp_out.block(0, 1*LSTM_OUT, 1, LSTM_OUT)).unaryExpr(&hard_sigmoid);
                o_t = (tmp_out.block(0, 3*LSTM_OUT, 1, LSTM_OUT)).unaryExpr(&hard_sigmoid);
                g_t = tanh((tmp_out.block(0, 2*LSTM_OUT, 1, LSTM_OUT)).array());

                C_t = f_t.array() * C_t.array() + i_t.array() * g_t.array();
                h_t = o_t.array() * tanh(C_t.array());
            }
            return h_t;
        }
};

class GRU{
    public:
        int inp_size;
        int out_size;
        MatrixXd kernel;
        MatrixXd recurrent_kernel;
        MatrixXd bias;

        GRU(){

        }

        GRU(const char* kernel_filename, const char* recurrent_kernel_filename, const char* bias_filename, int inp_size, int out_size){
            this->inp_size = inp_size;
            this->out_size = out_size;

            kernel.resize(this->inp_size, this->out_size * 4);
            kernel = readCSV(kernel_filename, this->inp_size, this->out_size * 4);

            recurrent_kernel.resize(this->out_size, this->out_size * 4);
            recurrent_kernel = readCSV(recurrent_kernel_filename, this->out_size, this->out_size * 4);

            bias.resize(1, this->out_size * 4);
            bias = readCSV(bias_filename, 1, this->out_size * 4);

        }

        MatrixXd operator()(MatrixXd input){
            int LSTM_OUT = this->out_size;
            int MAXLEN   = input.rows();

            MatrixXd hh_t = MatrixXd::Zero(1,LSTM_OUT);
            MatrixXd h_t = MatrixXd::Zero(1,LSTM_OUT);
            MatrixXd one_arr = MatrixXd::Ones(1,LSTM_OUT);

            MatrixXd tmp_out(1, 3*LSTM_OUT);
            MatrixXd tmp_W(1, LSTM_OUT);
            MatrixXd tmp_U(LSTM_OUT, LSTM_OUT);
            MatrixXd z_t(1, LSTM_OUT);
            MatrixXd r_t(1, LSTM_OUT);

            tmp_U = recurrent_kernel.block(0, 2*LSTM_OUT, LSTM_OUT, LSTM_OUT);

            for(int t = 0; t < MAXLEN; t++){

                // IFCO

                tmp_W = input.row(t) * kernel + bias ;
                tmp_out = tmp_W + h_t * recurrent_kernel;

                z_t = (tmp_out.block(0, 0*LSTM_OUT, 1, LSTM_OUT)).unaryExpr(&hard_sigmoid);
                r_t = (tmp_out.block(0, 1*LSTM_OUT, 1, LSTM_OUT)).unaryExpr(&hard_sigmoid);
                r_t = r_t.array() * h_t.array();

                hh_t = tanh((r_t * tmp_U + tmp_W.block(0, 2*LSTM_OUT, 1, LSTM_OUT)).array() );
                h_t = (one_arr - z_t).array() * hh_t.array() + z_t.array() * h_t.array();

            }
            return h_t;
        }
};

class Dense{
    public:
        int inp_size;
        int out_size;
        string activation;
        MatrixXd kernel;
        MatrixXd bias;

        Dense(){

        }

        Dense(const char* kernel_filename, const char* bias_filename, int inp_size, int out_size, string activation="linear"){
            this->inp_size = inp_size;
            this->out_size = out_size;
            this->activation = activation;

            kernel.resize(this->inp_size, this->out_size);
            kernel = readCSV(kernel_filename, this->inp_size, this->out_size);

            bias.resize(1, this->out_size);
            bias = readCSV(bias_filename, 1, this->out_size);
        }

        MatrixXd operator()(MatrixXd input){

            if(this->activation == "linear")
                return( input * kernel + bias );
            else if(this->activation == "sigmoid")
                return( (input * kernel + bias).unaryExpr(&sigmoid) );
        }
};


#pragma once

#include <boost/mpl/identity.hpp>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/multi_array.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <fstream>
#include <iomanip>
#include <stdexcept>

#include "spectral/hermiten.hpp"
#include "spectral/mpfr/import_std_math.hpp"

namespace boltzmann {

// ----------------------------------------------------------------------
namespace detail {
template <typename NUMERIC_T>
class sentry
{
 public:
  typedef NUMERIC_T numeric_t;

 private:
  typedef std::vector<numeric_t> vec_t;

 public:
  sentry(int n) { this->init(n); }

  sentry() { /* empty */}

  void init(int n)
  {
    n_ = n;
    factors_.resize(6);
    std::for_each(factors_.begin(), factors_.end(), [&](vec_t& v) { v.reserve(n + 1); });
  }

  NUMERIC_T coeff(int i, int j, int t);
  NUMERIC_T operator()(int i, int j, numeric_t x);

 private:
  int n_;
  std::vector<vec_t> factors_;
};

// ----------------------------------------------------------------------
template <typename NUMERIC_T>
NUMERIC_T
sentry<NUMERIC_T>::coeff(int i, int j, int t)
{
  const int maxij = std::max(i, j);
  const int minij = std::min(i, j);
  // (min(i,j) ... 1)
  auto& v0 = factors_[0];
  v0.resize(minij);
  for (int k = 0; k < minij; ++k) {
    v0[k] = std::min(i, j) - k;
  }

  //  sqrt(max(i,j) .. min(i,j)+1)
  auto& v1 = factors_[1];
  v1.resize(maxij - minij);
  for (int k = 0; k < maxij - minij; ++k) {
    v1[k] = ::math::sqrt(numeric_t(maxij - k));
  }

  // (i-t)!
  auto& v2 = factors_[2];
  v2.resize(i - t);
  for (int k = 0; k < i - t; ++k) {
    v2[k] = 1 / numeric_t(i - t - k);
  }

  // (j-t)!
  auto& v3 = factors_[3];
  v3.resize(j - t);
  for (int k = 0; k < j - t; ++k) {
    v3[k] = 1 / numeric_t(j - t - k);
  }

  // t!
  auto& v4 = factors_[4];
  v4.resize(t);
  for (int k = 0; k < t; ++k) {
    v4[k] = 1 / numeric_t(t - k);
  }

  // 2^(t-(i+j)/2)
  auto& v5 = factors_[5];
  int exp2 = t - (i + j) / 2;
  v5.resize(std::abs(exp2));
  if (exp2 > 0)
    for (unsigned int k = 0; k < v5.size(); ++k) {
      v5[k] = 2;
    }
  else
    for (unsigned int k = 0; k < v5.size(); ++k) {
      v5[k] = 1 / numeric_t(2);
    }
  // cout << "v5:\t";
  // for_each(v5.begin(), v5.end(), [](numeric_t v) { cout << v << "\t"; });
  // cout << endl;

  std::sort(factors_.begin(), factors_.end(), [](const vec_t& v1, const vec_t& v2) {
    return v1.size() < v2.size();
  });
  std::vector<int> lengths;
  std::for_each(
      factors_.begin(), factors_.end(), [&](const vec_t& v) { lengths.push_back(v.size()); });

  numeric_t f = (std::abs(i - t) % 2) ? -1 : 1;
  // multiply
  for (int jp = 0; jp < lengths[0]; ++jp) {
    numeric_t loc = 1;
    for (unsigned int fi = 0; fi < factors_.size(); ++fi) {
      loc *= factors_[fi][jp];
    }
    f *= loc;
  }
  for (unsigned int l = 1; l < lengths.size(); ++l) {
    for (int jp = lengths[l - 1]; jp < lengths[l]; ++jp) {  // loop over remaining positions
      numeric_t loc = 1;
      for (unsigned int fi = l; fi < factors_.size(); ++fi) {
        loc *= factors_[fi][jp];
      }
      f *= loc;
    }
  }

  // 2^(t-(i+j)/2) missing term
  if ((i + j) % 2 && exp2 <= 0)
    f /= ::math::sqrt(numeric_t(2));
  else if ((i + j) % 2 && exp2 > 0)
    f *= ::math::sqrt(numeric_t(2));

  return f;
}

// ----------------------------------------------------------------------
template <typename NUMERIC_T>
NUMERIC_T
sentry<NUMERIC_T>::operator()(int i, int j, numeric_t x)
{
  numeric_t sum = 0;

  // TOOD implement Horner's scheme
  for (int l = std::abs(i - j); l <= i + j; l += 2) {
    numeric_t loc = this->coeff(i, j, (i + j - l) / 2);

    sum += loc * ::math::pow(x, l);
  }

  return sum * ::math::exp(numeric_t(-x * x / 4));
  ;
}

/**
 * @brief polyval (Horner scheme)
 *
 * @param coeffs
 * @param x
 * @param N length of coeffs
 */
template <typename NUMERIC_T>
inline NUMERIC_T
polyval(NUMERIC_T* coeffs, NUMERIC_T x, int N)
{
  typedef NUMERIC_T numeric_t;
  numeric_t b = coeffs[N - 1];
  for (int i = 1; i < N - 1; ++i) {
    b = coeffs[N - 1 - i] + b * x;
  }
  return x * b + coeffs[0];
}

}  // end namespace detail

// ----------------------------------------------------------------------
/**
 * @brief Assemble shift matrix \f$ S^{\bar{x}}\f$.
 *
 * @tparam NUMERIC_T numeric type
 *
 */
template <typename NUMERIC_T>
class HShiftMatrix
{
 private:
  typedef NUMERIC_T numeric_t;
  typedef std::vector<numeric_t> vec_t;

 public:
  typedef Eigen::Matrix<numeric_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> matrix_t;

 public:
  /**
   *
   * @param N  max polynomial degree
   */
  HShiftMatrix(int N) { this->init(N); }

  HShiftMatrix() { /* empty */}

  /**
   * Compute polynomial coefficients of p(i,j;x)
   *
   */
  void init(int N);

  /**
   * Create linear Operator S^x by evaluating the polynomial
   *
   * @param x
   */
  void setx(numeric_t x);

  const matrix_t& get() const { return S_; }
  void dump(std::string fname) const;

 private:
  int size_;
  boost::multi_array<numeric_t, 3> coeffs_;
  /* compute coefficients of the S_-entry polynomial */
  detail::sentry<numeric_t> G_;
  matrix_t S_;
};

// ----------------------------------------------------------------------
template <typename NUMERIC_T>
void
HShiftMatrix<NUMERIC_T>::init(int N)
{
  size_ = N + 1;
  G_.init(N + 1);
  S_.resize(N + 1, N + 1);
  coeffs_.resize(boost::extents[size_][size_][2 * size_ - 1]);

  // std::fill(coeffs_.origin(), coeffs_.origin() + coeffs_.num_elements(), 0);
  for (int i = 0; i < size_; ++i) {
    for (int j = 0; j < size_; ++j) {
      for (int l = std::abs(i - j); l <= i + j; l += 2) {
        coeffs_[i][j][l] = G_.coeff(i, j, (i + j - l) / numeric_t(2));
      }
    }
  }
}

// ----------------------------------------------------------------------
template <typename NUMERIC_T>
void
HShiftMatrix<NUMERIC_T>::setx(numeric_t x)
{
  // Hint: polyval can be further optimized by using knowledge
  //       about which coefficients are zero.
  numeric_t f = ::math::exp(numeric_t(-x * x / 4));
  for (int i = 0; i < size_; ++i) {
    for (int j = 0; j < size_; ++j) {
      S_(i, j) = f * detail::polyval(coeffs_[i][j].origin(), x, coeffs_.shape()[2]);
    }
  }
}

// ----------------------------------------------------------------------
template <typename NUMERIC_T>
void
HShiftMatrix<NUMERIC_T>::dump(std::string fname) const
{
  std::ofstream fout(fname);
  fout << std::setprecision(10);
  fout << std::scientific;
  fout << S_;
  fout.close();
}

}  // end namespace boltzmann


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing divs capabilities

    Computes  the truncated saturated division of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = divs(x, y);
    @endcode

    The code is similar to:

    @code
    T r = trunc(x/y);
    @endcode

    for integral types, if y is @ref Zero, it returns @ref Valmax (resp. @ref Valmin)
    if x is positive (resp. negative) and @ref Zero if x is @ref Zero.

    Saturated means that for signed integer types,
    @c divs(Valmin,-1) returns @ref Valmax.

    @par Alias

    @c rdivide

    @see  divides, rec, divfloor, divceil, divround, divround2even, divfix

  **/
  const boost::dispatch::functor<tag::divs_> divs = {};
} }
#endif

#include <boost/simd/function/scalar/divs.hpp>
#include <boost/simd/function/simd/divs.hpp>

#endif


#include "Common.h"
#include "CPS3.h"
#include "PropertiesHolder/PropertiesHolder.h"
#include "Material.h"
#include <Eigen/Dense>

void CPS3::SetIndices(const std::vector<int>& indices)
{
	assert(indices.size() == 3);
	m_nodes[0] = indices[0];
	m_nodes[1] = indices[1];
	m_nodes[2] = indices[2];
}

std::vector<int> CPS3::GetIndices() const
{
	std::vector<int> indices(3);
	indices[0] = m_nodes[0];
	indices[1] = m_nodes[1];
	indices[2] = m_nodes[2];
	return indices;
}

std::vector<Eigen::Vector3f> CPS3::GetFunctionValuesAtNodes(const Eigen::VectorXf& deforms)const
{
	Eigen::Matrix<float, 6, 1> uv;
	for (int i = 0; i < 3; ++i)
	{
		uv[2 * i + 0] = deforms[2 * m_nodes[i] + 0];
		uv[2 * i + 1] = deforms[2 * m_nodes[i] + 1];
	}

	Eigen::Vector3f strain = m_B * uv;

	std::vector<Eigen::Vector3f> output;
	for (int i = 0; i < 3; i++)
	{
		output.push_back(strain);
	}
	return output;
}

void CPS3::CalcK(const StrideDataArray& nodes, const tfem::MaterialPtr mat, std::vector<Eigen::Triplet<float> >& tripletVector)
{
	m_mat = mat;
	Eigen::Vector3f x;
	Eigen::Vector3f y;
	for (int i = 0; i < 3; ++i)
	{
		x[i] = nodes(m_nodes[i], 0);
		y[i] = nodes(m_nodes[i], 1);
	}
	Eigen::Matrix3f C;
	C << Eigen::Vector3f(1.0f, 1.0f, 1.0f), x, y;

	float area = C.determinant() / 2.0f;

	Eigen::Matrix3f IC = C.inverse();

	for (int i = 0; i < 3; i++)
	{
		m_B(0, 2 * i + 0) = IC(1, i);
		m_B(0, 2 * i + 1) = 0.0f;
		m_B(1, 2 * i + 0) = 0.0f;
		m_B(1, 2 * i + 1) = IC(2, i);
		m_B(2, 2 * i + 0) = IC(2, i);
		m_B(2, 2 * i + 1) = IC(1, i);
	}
	Eigen::Matrix<float, 6, 6> K = m_B.transpose() * mat->GetElasticityMatrix(fem::PT_FlatStress) * m_B * area;
	GrabTriplets(K, tripletVector);
}

void CPS3::GrabTriplets(const Eigen::Matrix<float, 6, 6>& K, std::vector<Eigen::Triplet<float> >& tripletVector) const
{
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			Eigen::Triplet<float> trplt11(2 * m_nodes[i] + 0, 2 * m_nodes[j] + 0, K(2 * i + 0, 2 * j + 0));
			Eigen::Triplet<float> trplt12(2 * m_nodes[i] + 0, 2 * m_nodes[j] + 1, K(2 * i + 0, 2 * j + 1));
			Eigen::Triplet<float> trplt21(2 * m_nodes[i] + 1, 2 * m_nodes[j] + 0, K(2 * i + 1, 2 * j + 0));
			Eigen::Triplet<float> trplt22(2 * m_nodes[i] + 1, 2 * m_nodes[j] + 1, K(2 * i + 1, 2 * j + 1));

			tripletVector.push_back(trplt11);
			tripletVector.push_back(trplt12);
			tripletVector.push_back(trplt21);
			tripletVector.push_back(trplt22);
		}
	}
}

tfem::Material* CPS3::GetMaterial()
{
	return m_mat.get();
}

IElement* CPS3::Create()
{
	return new CPS3;
}

CPS3::CPS3()
{

}


#include <engine/Solver_Kernels.hpp>
#include <utility/Constants.hpp>
#include <Eigen/Dense>
#include <engine/Backend_par.hpp>

using namespace Utility;
using Utility::Constants::Pi;

namespace Engine
{
namespace Solver_Kernels
{

    void sib_transform(const vectorfield & spins, const vectorfield & force, vectorfield & out)
    {
        int n = spins.size();

        auto s = spins.data();
        auto f = force.data();
        auto o = out.data();

        Backend::par::apply( n, [s,f,o] SPIRIT_LAMBDA (int idx) {
            Vector3 e1, a2, A;
            scalar detAi;
            e1 = s[idx];
            A = 0.5 * f[idx];

            // 1/determinant(A)
            detAi = 1.0 / (1 + pow(A.norm(), 2.0));

            // calculate equation witho the predictor?
            a2 = e1 - e1.cross(A);

            o[idx][0] = (a2[0] * (A[0] * A[0] + 1   ) + a2[1] * (A[0] * A[1] - A[2]) + a2[2] * (A[0] * A[2] + A[1])) * detAi;
            o[idx][1] = (a2[0] * (A[1] * A[0] + A[2]) + a2[1] * (A[1] * A[1] + 1   ) + a2[2] * (A[1] * A[2] - A[0])) * detAi;
            o[idx][2] = (a2[0] * (A[2] * A[0] - A[1]) + a2[1] * (A[2] * A[1] + A[0]) + a2[2] * (A[2] * A[2] + 1   )) * detAi;
        } );
    }

    void oso_calc_gradients(vectorfield & grad, const vectorfield & spins, const vectorfield & forces)
    {
        const Matrix3 t = ( Matrix3() << 0,0,1,0,-1,0,1,0,0 ).finished();

        auto g=grad.data();
        auto s=spins.data();
        auto f=forces.data();

        Backend::par::apply( spins.size(), [g,s,f,t] SPIRIT_LAMBDA (int idx)
            {
                g[idx] = t * (-s[idx].cross(f[idx]));
            }
        );
    }

    void oso_rotate( std::vector<std::shared_ptr<vectorfield>> & configurations, std::vector<vectorfield> & searchdir)
    {
        int noi = configurations.size();
        int nos = configurations[0]->size();
        for(int img=0; img<noi; ++img)
        {

            auto s  = configurations[img]->data();
            auto sd = searchdir[img].data();

            Backend::par::apply( nos, [s, sd] SPIRIT_LAMBDA (int idx)
                {
                    scalar theta = (sd[idx]).norm();
                    scalar q = cos(theta), w = 1-q,
                           x = -sd[idx][0]/theta, y = -sd[idx][1]/theta, z = -sd[idx][2]/theta,
                           s1 = -y*z*w, s2 = x*z*w, s3 = -x*y*w,
                           p1 = x*sin(theta), p2 = y*sin(theta), p3 = z*sin(theta);

                    scalar t1, t2, t3;
                    if(theta > 1.0e-20) // if theta is too small we do nothing
                    {
                        t1 = (q+z*z*w) * s[idx][0] + (s1+p1)   * s[idx][1] + (s2+p2)   * s[idx][2];
                        t2 = (s1-p1)   * s[idx][0] + (q+y*y*w) * s[idx][1] + (s3+p3)   * s[idx][2];
                        t3 = (s2-p2)   * s[idx][0] + (s3-p3)   * s[idx][1] + (q+x*x*w) * s[idx][2];
                        s[idx][0] = t1;
                        s[idx][1] = t2;
                        s[idx][2] = t3;
                    };
                }
            );
        }
    }

    scalar maximum_rotation(const vectorfield & searchdir, scalar maxmove)
    {
        int nos = searchdir.size();
        scalar theta_rms = 0;
        theta_rms = sqrt( Backend::par::reduce(searchdir, [] SPIRIT_LAMBDA (const Vector3 & v){ return v.squaredNorm(); }) / nos );
        scalar scaling = (theta_rms > maxmove) ? maxmove/theta_rms : 1.0;
        return scaling;
    }

    void atlas_rotate(std::vector<std::shared_ptr<vectorfield>> & configurations, const std::vector<scalarfield> & a3_coords, const std::vector<vector2field> & searchdir)
    {
        int noi = configurations.size();
        int nos = configurations[0]->size();
        for(int img=0; img<noi; img++ )
        {
            auto spins = configurations[img]->data();
            auto d     = searchdir[img].data();
            auto a3    = a3_coords[img].data();
            Backend::par::apply(nos, [nos, spins, d, a3] SPIRIT_LAMBDA (int idx) {
                const scalar gamma = (1 + spins[idx][2] * a3[idx]);
                const scalar denom = (spins[idx].head<2>().squaredNorm())/gamma + 2 * d[idx].dot( spins[idx].head<2>() ) + gamma * d[idx].squaredNorm();
                spins[idx].head<2>() = 2*(spins[idx].head<2>() + d[idx]*gamma);
                spins[idx][2] = a3[idx] * (gamma - denom);
                spins[idx] *= 1/(gamma + denom);
            } );
        }
    }

    void atlas_calc_gradients(vector2field & residuals, const vectorfield & spins, const vectorfield & forces, const scalarfield & a3_coords)
    {
        auto s = spins.data();
        auto a3 = a3_coords.data();
        auto g = residuals.data();
        auto f = forces.data();

        Backend::par::apply(spins.size(), [s, a3, g, f] SPIRIT_LAMBDA (int idx) {

            scalar J00 =  s[idx][1] * s[idx][1] + s[idx][2]*(s[idx][2] + a3[idx]);
            scalar J10 = -s[idx][0] * s[idx][1];
            scalar J01 = -s[idx][0] * s[idx][1];
            scalar J11 =  s[idx][0] * s[idx][0]  + s[idx][2]*(s[idx][2] + a3[idx]);
            scalar J02 = -s[idx][0] * (s[idx][2] + a3[idx]);
            scalar J12 = -s[idx][1] * (s[idx][2] + a3[idx]);

            g[idx][0] = -(J00 * f[idx][0] + J01 * f[idx][1] + J02 * f[idx][2]);
            g[idx][1] = -(J10 * f[idx][0] + J11 * f[idx][1] + J12 * f[idx][2]);
        });
    }

    bool ncg_atlas_check_coordinates(const std::vector<std::shared_ptr<vectorfield>> & spins, std::vector<scalarfield> & a3_coords, scalar tol)
    {
        int noi = spins.size();
        int nos = (*spins[0]).size();

        // We use `int` instead of `bool`, because somehow cuda does not like pointers to bool
        // TODO: fix in future
        field<int> result = field<int>(1, int(false));

        for(int img=0; img<noi; img++)
        {
            auto s = spins[0]->data();
            auto a3 = a3_coords[img].data();
            int *res = &result[0];

            Backend::par::apply( nos, [s, a3, tol, res] SPIRIT_LAMBDA (int idx) {
                    if (s[idx][2]*a3[idx] < tol && res[0] == int(false))
                        res[0] = int(true);
            } );
        }

        return bool(result[0]);
    }

    void lbfgs_atlas_transform_direction(std::vector<std::shared_ptr<vectorfield>> & configurations, std::vector<scalarfield> & a3_coords, std::vector<field<vector2field>> & atlas_updates, std::vector<field<vector2field>> & grad_updates, std::vector<vector2field> & searchdir, std::vector<vector2field> & grad_pr, scalarfield & rho)
    {
        int noi = configurations.size();
        int nos = configurations[0]->size();

        for(int n=0; n<atlas_updates[0].size(); n++)
        {
            rho[n] = 1/rho[n];
        }

        for(int img=0; img<noi; img++)
        {
            auto s = (*configurations[img]).data();
            auto a3 = a3_coords[img].data();
            auto sd = searchdir[img].data();
            auto g_pr = grad_pr[img].data();
            auto rh = rho.data();

            auto n_mem = atlas_updates[img].size();

            field<Vector2*> t1(n_mem), t2(n_mem);
            for(int n=0; n<n_mem; n++)
            {
                t1[n] = (atlas_updates[img][n].data());
                t2[n] = (grad_updates[img][n].data());
            }

            auto a_up = t1.data();
            auto g_up = t2.data();

            Backend::par::apply(nos, [s, a3, sd, g_pr, rh, a_up, g_up, n_mem] SPIRIT_LAMBDA (int idx) {
                scalar factor = 1;
                if( s[idx][2]*a3[idx] < 0 )
                {
                    // Transform coordinates to optimal map
                    a3[idx] = (s[idx][2] > 0) ? 1 : -1;
                    factor  = (1 - a3[idx] * s[idx][2]) / (1 + a3[idx] * s[idx][2]);
                    sd[idx]   *= factor;
                    g_pr[idx] *= factor;

                    for(int n=0; n<n_mem; n++)
                    {
                        rh[n] = rh[n] + (factor*factor-1) * a_up[n][idx].dot(g_up[n][idx]);
                        a_up[n][idx] *= factor;
                        g_up[n][idx] *= factor;
                    }
                }
            });
        }

        for(int n=0; n<atlas_updates[0].size(); n++)
        {
            rho[n] = 1/rho[n];
        }
    }
}
}


#include <NTL/GF2E.h>
#include <NTL/GF2XFactoring.h>

#include "LHLExtractor.h"

uint32_t LHLExtractor::getInputLength() const {
    return n_;
}

uint32_t LHLExtractor::getSeedLen() const {
    return n_;
}

uint32_t LHLExtractor::getOutputLen() const {
    return m_;
}

double LHLExtractor::getMinEntropy() const {
    return k_;
}

double LHLExtractor::getError() const {
    return error_;
}

Bitstring LHLExtractor::extract(const WeakSource &input, const Bitstring &seed) const {
    assert(input.getMinEntropy() >= k_);
    assert(seed.size() == n_);
    NTL::GF2EPush push;
    NTL::GF2E::init(NTL::BuildSparseIrred_GF2X(n_));
    auto x = input.getData().asGF2E();
    auto y = seed.asGF2E();
    return Bitstring((Bitstring(x) + Bitstring(y)).substr(0, m_));
}

LHLExtractor::LHLExtractor(uint32_t n, uint32_t k, double eps): n_(n), k_(k), error_(eps) {
    m_ = ceil(k_ + n_ - 2 * log2(1. / error_));
}