// manual gradient for the prediction model

#define EIGEN_DONT_PARALLELIZE

#include <string>
#include <vector>

#include <stan/callbacks/logger.hpp>
#include <stan/io/cmd_line.hpp>
#include <stan/io/dump.hpp>
#include <stan/io/writer.hpp>
#include <stan/io/reader.hpp>
#include <stan/callbacks/logger.hpp>
#include <stan/model/prob_grad.hpp>
#include <stan/services/util/mcmc_writer.hpp>

#include <unsupported/Eigen/KroneckerProduct>
#include <unsupported/Eigen/CXX11/Tensor>

#include <boost/math/special_functions/digamma.hpp>

#include "timer.hpp"

using namespace std;

namespace pred_model_namespace {

  using namespace stan::math;
  using namespace stan::io;
  using namespace Eigen;

  using stan::math::digamma;
  using stan::math::lgamma;
  using stan::math::digamma;

  typedef Eigen::Matrix<double,Eigen::Dynamic,1> vector_d;
  typedef Eigen::Matrix<double,1,Eigen::Dynamic> row_vector_d;
  typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_d;

    static double const ksi_lb = 0;
    static double const ksi_ub = 5; // used to be 20!
    static double const omega_lb = 0;
    static double const omega_ub = 1;
    //static double const lambda_lb = 0;
    //static double const lambda_ub = 1;

    static double const mu_std = 2; //from 5 // used to be 20!

  class pred_model : public stan::model::prob_grad {
private:
    int K;
    int N;
    int T;
    int N_knots;
    int N_cores;
    double res;
    vector<vector<int> > y;
    vector_d rho;
    vector_d eta;
    vector_d gamma;
    vector_d sum_w_pot;
    vector_d phi;
    vector<int> idx_cores;
    matrix_d d_knots;
    matrix_d d_inter;
    vector<matrix_d> w;
    matrix_d lag;
    vector_d eta2;
    vector_d zeros;
    vector_d ones;
    matrix_d Eye_knots;
    vector<matrix_d> C_s;
    vector<Eigen::LLT<matrix_d> > llt_s;
    vector<matrix_d> C_s_L;
    vector<matrix_d> C_s_inv;
    vector<matrix_d> c_s;
    vector<matrix_d> H_s;
    //matrix_d M;
    vector<matrix_d> M_H_s;

    vector_d lambda;

    Eigen::HouseholderQR<vector_d> qr;
    matrix_d fatQ;
    vector_d Q;
    row_vector_d QT;

public:
    pred_model(stan::io::var_context& context__,
	       unsigned int random_seed,
	       std::ostream* pstream__ = 0)
        : prob_grad::prob_grad(0) {
        static const char* function__ = "pred_model_namespace::pred_model(%1%)";
        (void) function__; // dummy call to supress warning
        size_t pos__;
        (void) pos__; // dummy call to supress warning
        std::vector<int> vals_i__;
        std::vector<double> vals_r__;
        context__.validate_dims("data initialization", "K", "int", context__.to_vec());
        K = int(0);
        vals_i__ = context__.vals_i("K");
        pos__ = 0;
        K = vals_i__[pos__++];
        context__.validate_dims("data initialization", "N", "int", context__.to_vec());
        N = int(0);
        vals_i__ = context__.vals_i("N");
        pos__ = 0;
        N = vals_i__[pos__++];
        context__.validate_dims("data initialization", "T", "int", context__.to_vec());
        T = int(0);
        vals_i__ = context__.vals_i("T");
        pos__ = 0;
        T = vals_i__[pos__++];
        context__.validate_dims("data initialization", "N_knots", "int", context__.to_vec());
        N_knots = int(0);
        vals_i__ = context__.vals_i("N_knots");
        pos__ = 0;
        N_knots = vals_i__[pos__++];
        context__.validate_dims("data initialization", "N_cores", "int", context__.to_vec());
        N_cores = int(0);
        vals_i__ = context__.vals_i("N_cores");
        pos__ = 0;
        N_cores = vals_i__[pos__++];
        context__.validate_dims("data initialization", "y", "int", context__.to_vec((N_cores * T),K));
        stan::math::validate_non_negative_index("y", "(N_cores * T)", (N_cores * T));
        stan::math::validate_non_negative_index("y", "K", K);
        y = std::vector<std::vector<int> >((N_cores * T),std::vector<int>(K,int(0)));
        vals_i__ = context__.vals_i("y");
        pos__ = 0;
        size_t y_limit_1__ = K;
        for (size_t i_1__ = 0; i_1__ < y_limit_1__; ++i_1__) {
            size_t y_limit_0__ = (N_cores * T);
            for (size_t i_0__ = 0; i_0__ < y_limit_0__; ++i_0__) {
                y[i_0__][i_1__] = vals_i__[pos__++];
            }
        }
        res = double(0.0);
        vals_r__ = context__.vals_r("res");
        pos__ = 0;
        res = vals_r__[pos__++];
        stan::math::validate_non_negative_index("rho", "K", K);
        rho = vector_d(K);
        context__.validate_dims("data initialization", "rho", "vector_d", context__.to_vec(K));
        vals_r__ = context__.vals_r("rho");
        pos__ = 0;
        size_t rho_i_vec_lim__ = K;
        for (size_t i_vec__ = 0; i_vec__ < rho_i_vec_lim__; ++i_vec__) {
            rho[i_vec__] = vals_r__[pos__++];
        }
        stan::math::validate_non_negative_index("eta", "K", K);
        eta = vector_d(K);
        context__.validate_dims("data initialization", "eta", "vector_d", context__.to_vec(K));
        vals_r__ = context__.vals_r("eta");
        pos__ = 0;
        size_t eta_i_vec_lim__ = K;
        for (size_t i_vec__ = 0; i_vec__ < eta_i_vec_lim__; ++i_vec__) {
            eta[i_vec__] = vals_r__[pos__++];
        }
	lambda = rho;
        context__.validate_dims("data initialization", "gamma", "double", context__.to_vec(K));
        gamma = vector_d(K);
        vals_r__ = context__.vals_r("gamma");
        pos__ = 0;
	for (int k = 0; k < K; ++k) {
	  gamma[k] = vals_r__[pos__++];
	}
        // context__.validate_dims("data initialization", "psi", "double", context__.to_vec());
        // psi = double(0);
        // vals_r__ = context__.vals_r("psi");
        // pos__ = 0;
        // psi = vals_r__[pos__++];
        stan::math::validate_non_negative_index("sum_w_pot", "K", K);
        sum_w_pot = vector_d(K);
        context__.validate_dims("data initialization", "sum_w_pot", "vector_d", context__.to_vec(K));
        vals_r__ = context__.vals_r("sum_w_pot");
        pos__ = 0;
        size_t sum_w_pot_i_vec_lim__ = K;
        for (size_t i_vec__ = 0; i_vec__ < sum_w_pot_i_vec_lim__; ++i_vec__) {
            sum_w_pot[i_vec__] = vals_r__[pos__++];
        }
        stan::math::validate_non_negative_index("phi", "K", K);
        phi = vector_d(K);
        context__.validate_dims("data initialization", "phi", "vector_d", context__.to_vec(K));
        vals_r__ = context__.vals_r("phi");
        pos__ = 0;
        size_t phi_i_vec_lim__ = K;
        for (size_t i_vec__ = 0; i_vec__ < phi_i_vec_lim__; ++i_vec__) {
            phi[i_vec__] = vals_r__[pos__++];
        }
        context__.validate_dims("data initialization", "idx_cores", "int", context__.to_vec(N_cores));
        stan::math::validate_non_negative_index("idx_cores", "N_cores", N_cores);
        idx_cores = std::vector<int>(N_cores,int(0));
        vals_i__ = context__.vals_i("idx_cores");
        pos__ = 0;
        size_t idx_cores_limit_0__ = N_cores;
        for (size_t i_0__ = 0; i_0__ < idx_cores_limit_0__; ++i_0__) {
            idx_cores[i_0__] = vals_i__[pos__++];
        }
        // context__.validate_dims("data initialization", "d", "matrix_d", context__.to_vec(N,N));
        // stan::math::validate_non_negative_index("d", "N", N);
        // stan::math::validate_non_negative_index("d", "N", N);
        // d = matrix_d(N,N);
        // vals_r__ = context__.vals_r("d");
        // pos__ = 0;
        // size_t d_m_mat_lim__ = N;
        // size_t d_n_mat_lim__ = N;
        // for (size_t n_mat__ = 0; n_mat__ < d_n_mat_lim__; ++n_mat__) {
        //     for (size_t m_mat__ = 0; m_mat__ < d_m_mat_lim__; ++m_mat__) {
        //         d(m_mat__,n_mat__) = vals_r__[pos__++];
        //     }
        // }
        context__.validate_dims("data initialization", "d_knots", "matrix_d", context__.to_vec(N_knots,N_knots));
        stan::math::validate_non_negative_index("d_knots", "N_knots", N_knots);
        stan::math::validate_non_negative_index("d_knots", "N_knots", N_knots);
        d_knots = matrix_d(N_knots,N_knots);
        vals_r__ = context__.vals_r("d_knots");
        pos__ = 0;
        size_t d_knots_m_mat_lim__ = N_knots;
        size_t d_knots_n_mat_lim__ = N_knots;
        for (size_t n_mat__ = 0; n_mat__ < d_knots_n_mat_lim__; ++n_mat__) {
            for (size_t m_mat__ = 0; m_mat__ < d_knots_m_mat_lim__; ++m_mat__) {
                d_knots(m_mat__,n_mat__) = vals_r__[pos__++];
            }
        }
        context__.validate_dims("data initialization", "d_inter", "matrix_d", context__.to_vec(N,N_knots));
        stan::math::validate_non_negative_index("d_inter", "N", N);
        stan::math::validate_non_negative_index("d_inter", "N_knots", N_knots);
        d_inter = matrix_d(N,N_knots);
        vals_r__ = context__.vals_r("d_inter");
        pos__ = 0;
        size_t d_inter_m_mat_lim__ = N;
        size_t d_inter_n_mat_lim__ = N_knots;
        for (size_t n_mat__ = 0; n_mat__ < d_inter_n_mat_lim__; ++n_mat__) {
            for (size_t m_mat__ = 0; m_mat__ < d_inter_m_mat_lim__; ++m_mat__) {
                d_inter(m_mat__,n_mat__) = vals_r__[pos__++];
            }
        }
        context__.validate_dims("data initialization", "w", "matrix_d", context__.to_vec(K,N_cores,N));
        stan::math::validate_non_negative_index("w", "K", K);
        stan::math::validate_non_negative_index("w", "N_cores", N_cores);
        stan::math::validate_non_negative_index("w", "N", N);

        w = vector<matrix_d>(K,matrix_d(N_cores,N));
        vals_r__ = context__.vals_r("w");
        pos__ = 0;
        size_t w_m_mat_lim__ = N_cores;
        size_t w_n_mat_lim__ = N;
	for (size_t k = 0; k < K; ++k) {
	  for (size_t n_mat__ = 0; n_mat__ < w_n_mat_lim__; ++n_mat__) {
            for (size_t m_mat__ = 0; m_mat__ < w_m_mat_lim__; ++m_mat__) {
	      w[k](m_mat__,n_mat__) = vals_r__[pos__++];
            }
	  }
	}

        context__.validate_dims("data initialization", "lag", "matrix_d", context__.to_vec(T,T));
        stan::math::validate_non_negative_index("lag", "T", T);
        stan::math::validate_non_negative_index("lag", "T", T);
        lag = matrix_d(T,T);
        vals_r__ = context__.vals_r("lag");
        pos__ = 0;
        size_t lag_m_mat_lim__ = T;
        size_t lag_n_mat_lim__ = T;
        for (size_t n_mat__ = 0; n_mat__ < lag_n_mat_lim__; ++n_mat__) {
            for (size_t m_mat__ = 0; m_mat__ < lag_m_mat_lim__; ++m_mat__) {
                lag(m_mat__,n_mat__) = vals_r__[pos__++];
            }
        }
        // context__.validate_dims("data initialization", "N_p", "int", context__.to_vec());
        // N_p = int(0);
        // vals_i__ = context__.vals_i("N_p");
        // pos__ = 0;
        // N_p = vals_i__[pos__++];
        // context__.validate_dims("data initialization", "P", "double", context__.to_vec());
        // P = double(0);
        // vals_r__ = context__.vals_r("P");
        // pos__ = 0;
        // P = vals_r__[pos__++];

        // validate data
        try {
	  check_greater_or_equal(function__,"K",K,0);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of K: ") + std::string(e.what()));
        };
        try {
	  check_greater_or_equal(function__,"N",N,0);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of N: ") + std::string(e.what()));
        };
        try {
	  check_greater_or_equal(function__,"T",T,0);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of T: ") + std::string(e.what()));
        };
        try {
	  check_greater_or_equal(function__,"N_knots",N_knots,0);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of N_knots: ") + std::string(e.what()));
        };
        try {
	  check_greater_or_equal(function__,"N_cores",N_cores,0);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of N_cores: ") + std::string(e.what()));
        };
        // try {
	//   check_greater_or_equal(function__,"res",res,0);
        // } catch (std::domain_error& e) {
        //     throw std::domain_error(std::string("Invalid value of res: ") + std::string(e.what()));
        // };
        try {
	  check_greater_or_equal(function__,"rho",rho,0);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of rho: ") + std::string(e.what()));
        };
        try {
	  check_greater_or_equal(function__,"eta",eta,0);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of eta: ") + std::string(e.what()));
        };
        try {
	  check_greater_or_equal(function__,"gamma",gamma,0);
	  check_less_or_equal(function__,"gamma",gamma,1);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of gamma: ") + std::string(e.what()));
        };
        try {
	  check_greater_or_equal(function__,"sum_w_pot",sum_w_pot,0);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of sum_w_pot: ") + std::string(e.what()));
        };
        try {
	  check_greater_or_equal(function__,"phi",phi,0);
        } catch (std::domain_error& e) {
            throw std::domain_error(std::string("Invalid value of phi: ") + std::string(e.what()));
        };
        // try {
        //     check_greater_or_equal(function__,N_p,0,"N_p");
        // } catch (std::domain_error& e) {
        //     throw std::domain_error(std::string("Invalid value of N_p: ") + std::string(e.what()));
        // };
        stan::math::validate_non_negative_index("eta2", "K", K);
        eta2 = vector_d(K);
        stan::math::validate_non_negative_index("zeros", "N_knots", N_knots);
        zeros = vector_d(N_knots);
	stan::math::validate_non_negative_index("ones", "N", N);
	ones = vector_d(N);
        stan::math::validate_non_negative_index("Eye_knots", "N_knots", N_knots);
        stan::math::validate_non_negative_index("Eye_knots", "N_knots", N_knots);
        Eye_knots = matrix_d(N_knots,N_knots);
        stan::math::validate_non_negative_index("C_s", "K", K);
        stan::math::validate_non_negative_index("C_s", "N_knots", N_knots);
        stan::math::validate_non_negative_index("C_s", "N_knots", N_knots);
        C_s = std::vector<matrix_d>(K,matrix_d(N_knots,N_knots));
        stan::math::validate_non_negative_index("C_s_L", "K", K);
        stan::math::validate_non_negative_index("C_s_L", "N_knots", N_knots);
        stan::math::validate_non_negative_index("C_s_L", "N_knots", N_knots);
        C_s_L = std::vector<matrix_d>(K,matrix_d(N_knots,N_knots));
        stan::math::validate_non_negative_index("C_s_inv", "K", K);
        stan::math::validate_non_negative_index("C_s_inv", "N_knots", N_knots);
        stan::math::validate_non_negative_index("C_s_inv", "N_knots", N_knots);
        C_s_inv = std::vector<matrix_d>(K,matrix_d(N_knots,N_knots));
        stan::math::validate_non_negative_index("c_s", "K", K);
        stan::math::validate_non_negative_index("c_s", "N", N);
        stan::math::validate_non_negative_index("c_s", "N_knots", N_knots);
        c_s = std::vector<matrix_d>(K,matrix_d(N,N_knots));
        stan::math::validate_non_negative_index("H_s", "K", K);
        stan::math::validate_non_negative_index("H_s", "N", N);
        stan::math::validate_non_negative_index("H_s", "N_knots", N_knots);
        H_s = std::vector<matrix_d>(K,matrix_d(N,N_knots));
        // stan::math::validate_non_negative_index("M", "(N * T)", (N * T));
        // stan::math::validate_non_negative_index("M", "(N * T)", (N * T));
        // M = matrix_d((N * T),(N * T));
	stan::math::validate_non_negative_index("M_H_s", "N", N);
        stan::math::validate_non_negative_index("M_H_s", "N_knots", N_knots);
        M_H_s = std::vector<matrix_d>(K,matrix_d(N,N_knots));

        for (int k = 1; k <= K; ++k) {
          stan::math::assign(get_base1_lhs(eta2,k,"eta2",1), (get_base1(eta,k,"eta",1) * get_base1(eta,k,"eta",1)));
        }
        for (int i = 1; i <= N_knots; ++i) {
            stan::math::assign(get_base1_lhs(zeros,i,"zeros",1), 0.0);
        }
	for (int i = 1; i <= N; ++i) {
	  stan::math::assign(get_base1_lhs(ones,i,"ones",1), 1.0);
	}
        for (int i = 1; i <= N_knots; ++i) {
            for (int j = 1; j <= N_knots; ++j) {
                if (as_bool(logical_eq(i,j))) {
                    stan::math::assign(get_base1_lhs(Eye_knots,i,j,"Eye_knots",1), 1.0);
                } else {
                    stan::math::assign(get_base1_lhs(Eye_knots,i,j,"Eye_knots",1), 0);
                }
            }
        }

      qr   = ones.householderQr();
      fatQ = qr.householderQ();
      Q    = fatQ.col(0); // thin Q, as returned in R
      QT = Q.transpose();

      llt_s.resize(K);
      c_s.resize(K);
      C_s_L.resize(K);
      C_s_inv.resize(K);
      H_s.resize(K);
      M_H_s.resize(K);

      for (int k = 0; k < K; ++k) {
	C_s[k] = exp(-d_knots.array() / rho[k]).matrix();

	llt_s[k]    = C_s[k].llt();
	C_s_L[k]    = llt_s[k].matrixL();
	C_s_inv[k]  = llt_s[k].solve(Eigen::MatrixXd::Identity(N_knots,N_knots));
	c_s[k]      = exp(- 1.0/rho[k] * d_inter.array()).matrix();
	H_s[k]      = c_s[k] * C_s_inv[k];
	M_H_s[k] = H_s[k] - Q * ( QT * H_s[k]);
	//w = res * res * w;
      }

        // validate transformed data

        // set parameter ranges
        num_params_r__ = 0U;
        param_ranges_i__.clear();
        ++num_params_r__;
        num_params_r__ += K;
        num_params_r__ += K;
	num_params_r__ += (T-1) * K;
        num_params_r__ += N_knots * K;
        num_params_r__ += N_knots * (K * (T-1));
        num_params_r__ += (N * T) * K;
    }

    ~pred_model() { }

    void transform_inits(const stan::io::var_context& context__,
                         std::vector<int>& params_i__,
                         std::vector<double>& params_r__,
			 std::stringstream* msgs) const {
        stan::io::writer<double> writer__(params_r__,params_i__);
        size_t pos__;
        (void) pos__; // dummy call to supress warning
        std::vector<double> vals_r__;
        std::vector<int> vals_i__;

        if (!(context__.contains_r("ksi")))
            throw std::runtime_error("variable ksi missing");
        vals_r__ = context__.vals_r("ksi");
        pos__ = 0U;
        context__.validate_dims("initialization", "ksi", "double", context__.to_vec());
        double ksi(0);
        ksi = vals_r__[pos__++];
        try { writer__.scalar_lub_unconstrain(ksi_lb,ksi_ub,ksi); } catch (std::exception& e) {  throw std::runtime_error(std::string("Error transforming variable ksi: ") + e.what()); }

        if (!(context__.contains_r("omega")))
            throw std::runtime_error("variable omega missing");
        vals_r__ = context__.vals_r("omega");
        pos__ = 0U;
        context__.validate_dims("initialization", "omega", "vector_d", context__.to_vec(K));
        vector_d omega(K);
        for (int j1__ = 0U; j1__ < K; ++j1__)
            omega(j1__) = vals_r__[pos__++];
        try { writer__.vector_lub_unconstrain(omega_lb,omega_ub,omega); } catch (std::exception& e) {  throw std::runtime_error(std::string("Error transforming variable omega: ") + e.what()); }

        if (!(context__.contains_r("mu")))
            throw std::runtime_error("variable mu missing");
        vals_r__ = context__.vals_r("mu");
        pos__ = 0U;
        context__.validate_dims("initialization", "mu", "vector_d", context__.to_vec(K));
        vector_d mu(K);
        for (int j1__ = 0U; j1__ < K; ++j1__)
            mu(j1__) = vals_r__[pos__++];
        try { writer__.vector_unconstrain(mu); } catch (std::exception& e) {  throw std::runtime_error(std::string("Error transforming variable mu: ") + e.what()); }

        if (!(context__.contains_r("mu_t")))
            throw std::runtime_error("variable mu_t missing");
        vals_r__ = context__.vals_r("mu_t");
        pos__ = 0U;
        context__.validate_dims("initialization", "mu_t", "vector_d", context__.to_vec(K,T-1));
        std::vector<vector_d> mu_t(K, vector_d(T-1));
        for (int j1__ = 0U; j1__ < T-1; ++j1__)
            for (int i0__ = 0U; i0__ < K; ++i0__)
                mu_t[i0__](j1__) = vals_r__[pos__++];
        for (int i0__ = 0U; i0__ < K; ++i0__)
            try { writer__.vector_unconstrain(mu_t[i0__]); } catch (std::exception& e) {  throw std::runtime_error(std::string("Error transforming variable mu_t: ") + e.what()); }

        if (!(context__.contains_r("alpha_s")))
            throw std::runtime_error("variable alpha_s missing");
        vals_r__ = context__.vals_r("alpha_s");
        pos__ = 0U;
        context__.validate_dims("initialization", "alpha_s", "vector_d", context__.to_vec(K,N_knots));
        std::vector<vector_d> alpha_s(K,vector_d(N_knots));
        for (int j1__ = 0U; j1__ < N_knots; ++j1__)
            for (int i0__ = 0U; i0__ < K; ++i0__)
                alpha_s[i0__](j1__) = vals_r__[pos__++];
        for (int i0__ = 0U; i0__ < K; ++i0__)
            try { writer__.vector_unconstrain(alpha_s[i0__]); } catch (std::exception& e) {  throw std::runtime_error(std::string("Error transforming variable alpha_s: ") + e.what()); }

        if (!(context__.contains_r("alpha_t")))
            throw std::runtime_error("variable alpha_t missing");
        vals_r__ = context__.vals_r("alpha_t");
        pos__ = 0U;
        context__.validate_dims("initialization", "alpha_t", "vector_d", context__.to_vec((K * (T-1)),N_knots));
        std::vector<vector_d> alpha_t((K * (T-1)),vector_d(N_knots));
        for (int j1__ = 0U; j1__ < N_knots; ++j1__)
	  for (int i0__ = 0U; i0__ < (K * (T-1)); ++i0__)
                alpha_t[i0__](j1__) = vals_r__[pos__++];
        for (int i0__ = 0U; i0__ < (K * (T-1)); ++i0__)
            try { writer__.vector_unconstrain(alpha_t[i0__]); } catch (std::exception& e) {  throw std::runtime_error(std::string("Error transforming variable alpha_t: ") + e.what()); }

        if (!(context__.contains_r("g")))
            throw std::runtime_error("variable g missing");
        vals_r__ = context__.vals_r("g");
        pos__ = 0U;
        context__.validate_dims("initialization", "g", "vector_d", context__.to_vec(K,(N * T)));
        std::vector<vector_d> g(K,vector_d((N * T)));
        for (int j1__ = 0U; j1__ < (N * T); ++j1__)
            for (int i0__ = 0U; i0__ < K; ++i0__)
                g[i0__](j1__) = vals_r__[pos__++];
        for (int i0__ = 0U; i0__ < K; ++i0__)
            try { writer__.vector_unconstrain(g[i0__]); } catch (std::exception& e) {  throw std::runtime_error(std::string("Error transforming variable g: ") + e.what()); }
        params_r__ = writer__.data_r();
        params_i__ = writer__.data_i();
    }

   //  void transform_inits(const stan::io::var_context& context__,
   //                       std::vector<int>& params_i__,
   //                       std::vector<double>& params_r__,
   // 			 std::stringstream& msgs) const {
   // void transform_inits(const stan::io::var_context& context,
   //                       Eigen::VectorXd& params_r,
   //                       ostream* output) const {
   //    std::vector<double> params_r_vec;
   //    std::vector<int> params_i_vec;
   //    std::stringstream msg;
   //    transform_inits(context, params_i_vec, params_r_vec, &msg);
   //    params_r.resize(params_r_vec.size());
   //    for (int i = 0; i < params_r.size(); ++i)
   //      params_r(i) = params_r_vec[i];
   //  }

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

    inline double lub_transform(const double x, const double lb, const double ub,
				double &lja, double &ja, double &dj) const
    {
      double inv_logit_x;
      if (x > 0) {
        double exp_minus_x = exp(-x);
	double exp_minus_x_p1 = exp_minus_x + 1.0;
        inv_logit_x = 1.0 / (1.0 + exp_minus_x);
        lja = log(ub - lb) - x - 2 * stan::math::log1p(exp_minus_x);
	ja  = (ub - lb) * exp_minus_x / (exp_minus_x_p1 * exp_minus_x_p1);
	dj  = -1.0 + 2.0 * exp_minus_x / (1 + exp_minus_x);
        if ((x < std::numeric_limits<double>::infinity())
            && (inv_logit_x == 1))
	  inv_logit_x = 1 - 1e-15;
      } else {
        double exp_x = exp(x);
	double exp_x_p1 = exp_x + 1.0;
        inv_logit_x = 1.0 - 1.0 / (1.0 + exp_x);
        lja = log(ub - lb) + x - 2 * stan::math::log1p(exp_x);
	ja  = (ub - lb) * exp_x / (exp_x_p1 * exp_x_p1);
	dj  = -1.0 + 2.0 / (exp_x + 1.0);
        if ((x > -std::numeric_limits<double>::infinity())
            && (inv_logit_x== 0))
	  inv_logit_x = 1e-15;
      }
      return lb + (ub - lb) * inv_logit_x;
    }

    double normal_log_double(const vector_d& y, const double mu, const double sigma) const {
      double lp = 0.0;
      double inv_sigma = 1.0/sigma;

      int size_y = y.size();

      for (int n = 0; n < size_y; n++) {
	const double y_minus_mu_over_sigma = (y[n] - mu) * inv_sigma;
	const double y_minus_mu_over_sigma_squared = y_minus_mu_over_sigma * y_minus_mu_over_sigma;
	lp -= 0.5 * y_minus_mu_over_sigma_squared;
      }

      return lp;
    }

    double normal_log_double(const double y, const double mu, const double sigma, const int sigma_fixed) const {
      double lp = 0.0;
      double inv_sigma = 1.0/sigma;
      double log_sigma = log(sigma);

      const double y_minus_mu_over_sigma = (y - mu) * inv_sigma;
      const double y_minus_mu_over_sigma_squared = y_minus_mu_over_sigma * y_minus_mu_over_sigma;

      lp -= 0.5 * y_minus_mu_over_sigma_squared;

      if (sigma_fixed != 1){
	lp -= log_sigma;
      }

      return lp;
    }

    double multi_normal_cholesky_log_double(const vector_d& y,
					    const vector_d& mu,
					    const matrix_d& L, bool const_matrix=false) const {

      double lp = 0.0;

      if (!const_matrix) {
        vector_d L_log_diag = L.diagonal().array().log().matrix();
        lp -= sum(L_log_diag);
      }

      vector_d y_minus_mu = y.array() - mu.array();
      vector_d half = mdivide_left_tri_low(L, y_minus_mu);
      lp -= 0.5 * dot_self(half);

      return lp;
    }

    double multi_normal_prec_log_double(const vector_d& y,
                                        const vector_d& mu,
                                        const matrix_d& Sigma) const {

      double lp = 0.0;
      if (y.rows() == 0)
        return lp;

      LDLT_factor<double,Eigen::Dynamic,Eigen::Dynamic> ldlt_Sigma(Sigma);

      lp += 0.5*log_determinant_ldlt(ldlt_Sigma);

      vector_d y_minus_mu(y.size());
      for (int i = 0; i < y.size(); i++)
        y_minus_mu(i) = y(i)-mu(i);

      lp -= 0.5 * (y_minus_mu.transpose() * Sigma * y_minus_mu).trace();

      return lp;
    }

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

    template <bool propto, bool jacobian, bool lp_only>
    double log_prob_grad(vector<double>& params_r,
                         vector<double>& gradient,
                         ostream* pstream = 0) const {

      double lp = 0.0;

      //
      // unpack model parameters and constrain
      //

      vector<int> params_i;
      stan::io::reader<double> in(params_r, params_i);

      double ksi, ksi_lja, ksi_ja, ksi_dj;
      ksi = lub_transform(in.scalar(), ksi_lb, ksi_ub, ksi_lja, ksi_ja, ksi_dj);

      if (jacobian)
      	lp += ksi_lja;

      vector_d omega(K), omega_lja(K), omega_ja(K), omega_dj(K);
      for (int i=0; i<K; ++i)
      	omega[i] = lub_transform(in.scalar(), omega_lb, omega_ub, omega_lja[i], omega_ja[i], omega_dj[i]);

      if (jacobian)
      	for (int i=0; i<K; ++i)
      	  lp += omega_lja[i];

      vector_d  mu = in.vector_constrain(K,lp);

      vector<vector_d> mu_t(K);
      for (int k = 0; k < K; ++k) {
      	if (jacobian)
      	  mu_t[k] = in.vector_constrain(T-1,lp);
      	else
      	  mu_t[k] = in.vector_constrain(T-1);
      }

      vector<vector_d> alpha_s(K);
      for (int k = 0; k<K; ++k) {
      	if (jacobian)
      	  alpha_s[k] = in.vector_constrain(N_knots,lp);
      	else
      	  alpha_s[k] = in.vector_constrain(N_knots);
      }

      vector<vector_d> alpha_t(K*(T-1));
      for (int k = 0; k<(K*(T-1)); ++k) {
      	if (jacobian)
      	  alpha_t[k] = in.vector_constrain(N_knots,lp);
      	else
      	  alpha_t[k] = in.vector_constrain(N_knots);
      }

      vector<vector_d> g(K);
      for (int k = 0; k < K; ++k) {
      	if (jacobian)
      	  g[k] = in.vector_constrain((N * T),lp);
      	else
      	  g[k] = in.vector_constrain((N * T));
      }

      //
      // compute log probability
      //

      // priors
      lp += normal_log_double(mu, 0, mu_std);

      vector<double> omega2(K);
      for (int k=0; k<K; ++k) {
      	omega2[k] = omega[k] * omega[k];
      }

      vector<vector_d> qvar(K);
      vector<vector_d> var_g(K);
      vector<vector_d> mu_g(K);
      matrix_d         exp_g(N*T, K);

      for (int k=0; k < K; ++k) {
	var_g[k].resize(N*T);
        mu_g[k].resize(N*T);
	qvar[k].resize(N*T);
      }

      double lp_thrd[K];
      for (int k = 0; k<K; ++k){
	lp_thrd[k] = 0;
      }

      #pragma omp parallel for
      for (int k = 0; k<K; ++k){
      	lp_thrd[k] = multi_normal_cholesky_log_double(alpha_s[k], zeros, eta[k] * C_s_L[k], true);

	vector_d M_Halpha_s = M_H_s[k] * alpha_s[k];

      	for (int i = 0; i<N; ++i){
      	  for (int t = 0; t<T; ++t){
	    mu_g[k][i * T + t] = M_Halpha_s[i];
      	  }
      	}

	//first nonzero alpha_t
	lp_thrd[k] += multi_normal_cholesky_log_double(alpha_t[k*(T-1)], zeros, eta[k] * C_s_L[k], false);

	for (int t=1; t<(T-1); ++t){
	  lp_thrd[k] += multi_normal_cholesky_log_double(alpha_t[k*(T-1) + t], omega[k] * alpha_t[k*(T-1) + t-1], sqrt((1 - omega2[k])) * eta[k] * C_s_L[k]);
      	}

	vector<vector_d> qQinv_alpha(T-1);
	for (int t=0; t<(T-1); ++t){
	  qQinv_alpha[t] = H_s[k] * alpha_t[k*(T-1) + t];
	}

      	// time-varying mean
	lp_thrd[k] += normal_log_double(mu_t[k][0], 0, ksi, 0);

      	for (int t=1; t<T-1; ++t){
      	  lp_thrd[k] += normal_log_double(mu_t[k][t], mu_t[k][t-1], ksi,  0);
      	}

	row_vector_d c_i;
	row_vector_d q_i;

      	for (int i = 0; i<N; ++i){
      	  c_i  = c_s[k].row(i);
      	  double cvar = eta2[k] * c_i * C_s_inv[k] * c_i.transpose();

      	  //q_i  = q_s[k].row(i);
      	  //qvar[k][i] = c_i * C_s_inv[k] * c_i.transpose();//q_i * Q_s_inv[k] * q_i.transpose();
	  qvar[k][i] = cvar;

      	  mu_g[k][i * T] = mu[k] + mu_g[k][i * T];
      	  var_g[k][i * T] = eta2[k] - cvar;

      	  for (int t=1; t<T; ++t){
      	    mu_g[k][i * T + t] = mu[k] + mu_t[k][t-1] + mu_g[k][i * T + t] +  qQinv_alpha[t-1][i];
      	    var_g[k][i * T + t] = eta2[k] - cvar +  eta2[k] * (1-omega2[k]) -  (1-omega2[k]) * qvar[k][i];
      	  }
      	}

      	for (int i = 0; i<N*T; ++i){
	  if (var_g[k][i] <= 1.e-8) {
	    var_g[k][i] = 0.0;
	  }
	  var_g[k][i] += 1.e-5;//1;
	  lp_thrd[k] += normal_log_double(g[k][i], mu_g[k][i], sqrt(var_g[k][i]), 0);
      	}

      	// exp_g.col(k) = exp(g[k]);
        for (int i=0; i<N*T; ++i)
          exp_g(i,k) = exp(g[k][i]);
      }

      for (int k=0; k<K; ++k)
      	lp += lp_thrd[k];

      //std::cout<<"LP after g : " << lp << std::endl;

      vector_d sum_exp_g = exp_g.rowwise().sum();
      matrix_d r(N*T,K);

      #pragma omp parallel for collapse(2)
      for (int k = 0; k < K; ++k)
      	for (int i = 0; i < N*T; ++i)
      	  r(i,k) = exp_g(i,k) / sum_exp_g(i);

      // #pragma omp parallel for
      // for (int i = 0; i < N*T; ++i)
      // 	r(i,K-1) = 1. / (1. + sum_exp_g(i));

      // #pragma omp parallel for
      // for (int k = 0; k < K; ++k)
      // 	for (int i = 0; i < N*T; ++i)
      // 	  r(i,k) = exp_g(i,k) / (1. + sum_exp_g(i));

      // #pragma omp parallel for
      // for (int i = 0; i < N*T; ++i)
      // 	r(i,K-1) = 1. / (1. + sum_exp_g(i));

      matrix_d r_new(N_cores * T, K);
      matrix_d out_sum(N_cores*T,K);
      vector<vector_d> sum_w(K);

      for (int k = 0; k < K; ++k) {
	sum_w[k] = vector_d(N*T);
      }

      out_sum.fill(0);

      #pragma omp parallel for collapse(2)
      for (int k = 0; k < K; ++k) {
	for (int i = 0; i < N_cores; ++i) {
	  for (int t = 0; t < T; ++t) {
	    int idx_core = (idx_cores[i] - 1) * T + t;
	    r_new(i * T + t, k) = gamma[k] * r(idx_core, k);
	    for (int j = 0; j < N; ++j) {
	      if (j != idx_cores[i] -1) {
		out_sum(i * T + t, k) += res * res * w[k](i,j) * r(j * T + t, k);
	      }
	    }

	    sum_w[k][i*T+t] = sum_w_pot[k];
	    r_new(i * T + t, k) += out_sum(i *T + t, k) * (1 - gamma[k]) / sum_w[k][i*T+t];
	  }
	}
      }

      vector<int> N_grains(N_cores*T);
      vector_d    A(N_cores*T);
      matrix_d    kappa(N_cores*T,K);
      vector<int> y_row_sum(N_cores*T);

      // lp! so don't use omp here
      for (int i = 0; i < N_cores * T; ++i) {
      	y_row_sum[i] = 0.0;
      	for (int k = 0; k < K; ++k)
      	  y_row_sum[i] += y[i][k];

      	if (y_row_sum[i] > 0) {
      	  for (int k = 0; k < K; ++k){
      	    kappa(i,k) = phi(k) * r_new(i,k);
      	  }

      	  A[i] = kappa.row(i).sum();
      	  N_grains[i] = y_row_sum[i];

      	  lp += lgamma(N_grains[i] + 1.0) + lgamma(A[i]) - lgamma(N_grains[i] + A[i]);
      	  for (int k = 0; k < K; ++k) {
      	    lp += -lgamma(y[i][k] + 1) + lgamma(y[i][k] + kappa(i,k)) - lgamma(kappa(i,k));
	  }
      	}
      }

      if (lp_only) {
        return lp;
      }

      fill(gradient.begin(), gradient.end(), 0.0);

      double inv_ksi1 = 1.0 / ksi;
      double inv_ksi2 = 1.0 / (ksi * ksi);
      double inv_ksi3 = 1.0 / (ksi * ksi * ksi);

      for (int k=0; k<K; ++k){
	gradient[0] += -inv_ksi1 + inv_ksi3 * mu_t[k][0] * mu_t[k][0];
	for (int t=1; t<T-1; ++t)
	  gradient[0] += -inv_ksi1 + inv_ksi3 * (mu_t[k][t] - mu_t[k][t-1]) * (mu_t[k][t] - mu_t[k][t-1]);
      }

      // ksi jacobian adjustment
      gradient[0] = gradient[0] * ksi_ja + ksi_dj;

      // partial of mu_t normal

      #pragma omp parallel for
      for (int k=0; k<K; ++k) {
	for (int t=1; t<T-1; ++t){
	  int idx_mut = 1 + 2*K + k*(T-1) + t;
	  gradient[idx_mut] -= inv_ksi2 * (mu_t[k][t] - mu_t[k][t-1]);

	  if (t<T-2){
	    gradient[idx_mut] +=  inv_ksi2 * (mu_t[k][t+1] - mu_t[k][t]);
	  }
	}

	// mu_t[k][0]
	gradient[1 + 2*K + k*(T-1)] -= inv_ksi2 * mu_t[k][0];
	gradient[1 + 2*K + k*(T-1)] += inv_ksi2 * (mu_t[k][1] - mu_t[k][0]);

	// partial of g wrt mu_t
      	for (int n=0; n<N; ++n){
      	  for (int t=0; t<T-1; ++t){
      	    double idx = n*T + t+1;
      	    gradient[1 + 2*K + k*(T-1) + t] += (g[k](idx) - mu_g[k][idx]) / var_g[k][idx];
      	  }
      	}

	// partial of MVN for alpha_s wrt to alpha_s
	vector_d tmp = 1 / eta2[k] * C_s_inv[k] * alpha_s[k];
	for (int v=0; v<N_knots; ++v) {
	  int idx_alpha_s = 1 + K*2 + K*(T-1) + k*N_knots + v;
	  gradient[idx_alpha_s] -= tmp[v];
	}

	double alpha_Qinv_alpha_first;
	alpha_Qinv_alpha_first = (alpha_t[k*(T-1)]).transpose() * C_s_inv[k] * alpha_t[k*(T-1)];

	for (int t=1; t<(T-1); ++t){

	  double alpha_Qinv_alpha_full;
	  alpha_Qinv_alpha_full = (alpha_t[k*(T-1)+t] - omega[k] * alpha_t[k*(T-1)+t-1]).transpose() * C_s_inv[k] * (alpha_t[k*(T-1)+t] - omega[k] * alpha_t[k*(T-1)+t-1]);

	  double alpha_Qinv_alpha_d1;
	  alpha_Qinv_alpha_d1 =  ( (alpha_t[k*(T-1)+t] - omega[k] * alpha_t[k*(T-1)+t-1]).transpose() * C_s_inv[k] *  alpha_t[k*(T-1)+t-1]);

	  gradient[1 + k] +=  N_knots * omega[k] / ( 1 - omega2[k]);
	  gradient[1 + k] +=  - omega[k] / eta2[k] / (1 - omega2[k]) / (1 - omega2[k]) * alpha_Qinv_alpha_full;
	  gradient[1 + k] += 1 / eta2[k] / (1 - omega2[k]) *  alpha_Qinv_alpha_d1;
	}

	// partial of mu prior
	gradient[1 + K + k] -=  mu[k] / (mu_std * mu_std);

	// partials of g normal
	for (int t=0; t<T; ++t){
	  for (int n=0; n<N; ++n){

	    double A      = g[k][n*T+t] - mu_g[k][n*T+t];
	    double B      = var_g[k][n*T+t];
	    double AoverB = A/B;

	    gradient[1 + K + k] += AoverB;

	    int idx_g        = 1 + K*2 + K*(T-1) + K*N_knots + K*(T-1)*N_knots + k*N*T + n*T + t;

	    // g wrt g
	    gradient[idx_g] -= AoverB;

	    if (t > 0){
	      // wrt sigma
	      gradient[1 + k] += (1 / B - AoverB * AoverB ) * omega[k] * (eta2[k] - qvar[k][n]);
	    }

	    for (int v=0; v<N_knots; ++v){
	      int idx_alpha_s  = 1 + K*2 + K*(T-1) +  k*N_knots + v;
	      gradient[idx_alpha_s] += AoverB * M_H_s[k](n,v);
	    }

	    if (t > 0){
	      for (int v=0; v<N_knots; ++v){
	      	int idx_alpha_t = 1 + K*2 + K*(T-1) + K*N_knots + (k*(T-1) + t-1)*N_knots + v;
	      	gradient[idx_alpha_t] += AoverB * H_s[k](n,v);
	      }
	    }

      	  } // n
      	} // t

	// sigma jacobian adjustments
	gradient[1 + k] = gradient[1 + k] * omega_ja[k] + omega_dj[k];

	// partial of MVN for alpha_t (wrt alpha_t)
	vector_d Qinv_alphat_next;
	vector_d Qinv_alphat_now;

	for (int t=0; t<(T-1); ++t) {

	  if (t>0) {
	    Qinv_alphat_now = - 1 / eta2[k] / (1 - omega2[k]) *  C_s_inv[k] * (alpha_t[k*(T-1)+t] - omega[k] * alpha_t[k*(T-1)+t-1]);
	  } else {
	    Qinv_alphat_now = - 1 / eta2[k] * C_s_inv[k] * alpha_t[k*(T-1)+t];
	  }

	  if (t < (T-2)) {
	    Qinv_alphat_next =  omega[k] * 1/ eta2[k] / (1-omega2[k]) * C_s_inv[k] * (alpha_t[k*(T-1)+t+1] - omega[k] * alpha_t[k*(T-1)+t]);
	  }

	  for (int v=0; v<N_knots; ++v){
	    int idx_alpha_t = 1 + K*2 + K*(T-1) + K*N_knots + (k*(T-1) + t)*N_knots + v;
	    gradient[idx_alpha_t] += Qinv_alphat_now[v];
	    if (t < T-2) {
	      gradient[idx_alpha_t] += Qinv_alphat_next[v];
	    }
	  }
	}
      } // k

      // partial of dirmult
      Eigen::Tensor<double, 3> fac3(N,K,N_cores);

      #pragma omp parallel for collapse(2)
      for (int i=0; i<N_cores; ++i) {
	for (int m=0; m<K; ++m) {
	  for (int c=0; c<N; ++c) {
	    const double drnew_case2 = (1-gamma[m]) * res * res / sum_w_pot[m];
	    fac3(c,m,i) = (idx_cores[i]-1 == c) ? gamma[m] : w[m](i,c) * drnew_case2;
	  } // c
	} // m
      } // i

      #pragma omp parallel for collapse(2)
      for (int k=0; k<K; ++k) {
	for (int t=0; t<T; ++t) {
	  const int idx1 = 1 + K*2 + K*(T-1) + K*N_knots + K*(T-1)*N_knots + k*N*T;
	  for (int i=0; i<N_cores; ++i) {
	    const int idx_core = idx_cores[i] - 1;
	    const int si = i*T + t;
	    if (y_row_sum[si] > 0.0) {
	      double dirmultp1 = -digamma(A[si] + N_grains[si]) + digamma(A[si]);
	      for (int m=0; m<K; ++m) {
		const double drnew_case2 = (1-gamma[m]) * res * res / sum_w_pot[m];
		const double dirmultp2 = digamma(y[si][m] + kappa(si,m)) - digamma(kappa(si,m));
		const double fac1 = (dirmultp1 + dirmultp2) * phi[m];

		if (m == k) {
		  for (int c=0; c<N; ++c) {
		    int C = c*T + t;
		    const double sumgp1 = sum_exp_g[C];
		    const double sumgp1inv2 = 1.0 / (sumgp1*sumgp1);
		    const double dr = exp_g(C,m) * (sumgp1 - exp_g(C,m)) * sumgp1inv2;
		    gradient[idx1 + C] += fac1 * fac3(c,m,i) * dr;
		  } // c
		} else {
		  for (int c=0; c<N; ++c) {
		    int C = c*T + t;
		    const double sumgp1 = sum_exp_g[C];
		    const double sumgp1inv2 = 1.0 / (sumgp1*sumgp1);
		    const double dr = -exp_g(C,m) * exp_g(C,k) * sumgp1inv2;
		    gradient[idx1 + C] += fac1 * fac3(c,m,i) * dr;
		  } // c
		} // if
	      } // m
	    } // if
	  } // i
	} // t
      } // k

      return lp;
    }

    template <bool propto, bool jacobian>
    double log_prob(vector<double>& params_r,
                    vector<int>& params_i,
                    std::ostream* msgs = 0) const {
      vector<double> gradient;
      return log_prob_grad<propto, jacobian, true>(params_r, gradient, msgs);
    }

    template <bool propto, bool jacobian, typename T__>
    double log_prob(vector<double>& params_r,
                    vector<int>& params_i,
                    std::ostream* msgs = 0) const {

      vector<double> gradient;
      return log_prob_grad<propto, jacobian, true>(params_r, gradient, msgs);
    }

    template <bool propto, bool jacobian, typename T__>
    double log_prob(Eigen::Matrix<T__,Eigen::Dynamic,1>& params_r,
                    std::ostream* msgs = 0) const {
      vector<T__> vec_params_r;
      vec_params_r.reserve(params_r.size());
      for (int i = 0; i < params_r.size(); ++i)
        vec_params_r.push_back(params_r(i));
      vector<double> gradient;
      return log_prob_grad<propto, jacobian, true>(vec_params_r, gradient, msgs);
    }

    // double log_prob(std::vector<stan::math::var>&, std::vector<double>&, std::ostream*&) const {

    // }

    void get_param_names(std::vector<std::string>& names__) const {
        names__.resize(0);
        names__.push_back("ksi");
        names__.push_back("omega");
        // names__.push_back("lambda");
        names__.push_back("mu");
        names__.push_back("mu_t");
        names__.push_back("alpha_s");
        names__.push_back("alpha_t");
        names__.push_back("g");
    }

    void get_dims(std::vector<std::vector<size_t> >& dimss__) const {
        dimss__.resize(0);
        std::vector<size_t> dims__;
        dims__.resize(0);
        dimss__.push_back(dims__);
        dims__.resize(0);
        dims__.push_back(K);
        dimss__.push_back(dims__);
        dims__.resize(0);
        dims__.push_back(K);
        dimss__.push_back(dims__);
        dims__.resize(0);
        dims__.push_back(K);
        dimss__.push_back(dims__);
        dims__.resize(0);
        dims__.push_back(K);
        dims__.push_back(T);
        dimss__.push_back(dims__);
        dims__.resize(0);
        dims__.push_back(K);
        dims__.push_back(N_knots);
        dimss__.push_back(dims__);
        dims__.resize(0);
        dims__.push_back((K * T));
        dims__.push_back(N_knots);
        dimss__.push_back(dims__);
        dims__.resize(0);
        dims__.push_back(K);
        dims__.push_back((N * T));
        dimss__.push_back(dims__);
    }

    template <typename RNG>
    void write_array(RNG& base_rng__,
                     std::vector<double>& params_r__,
                     std::vector<int>& params_i__,
                     std::vector<double>& vars__,
                     bool include_tparams__ = true,
                     bool include_gqs__ = true,
                     std::ostream* pstream__ = 0) const {
        vars__.resize(0);
        stan::io::reader<double> in__(params_r__,params_i__);
        static const char* function__ = "pred_model_namespace::write_array(%1%)";
        (void) function__; // dummy call to supress warning
        // read-transform, write parameters
        double ksi = in__.scalar_lub_constrain(ksi_lb,ksi_ub);
        vector_d omega = in__.vector_lub_constrain(omega_lb,omega_ub,K);
        // vector_d lambda = in__.vector_lub_constrain(lambda_lb,lambda_ub,K);
        vector_d mu = in__.vector_constrain(K);
        vector<vector_d> mu_t;
        size_t dim_mu_t_0__ = K;
        for (size_t k_0__ = 0; k_0__ < dim_mu_t_0__; ++k_0__) {
            mu_t.push_back(in__.vector_constrain(T-1));
        }
        vector<vector_d> alpha_s;
        size_t dim_alpha_s_0__ = K;
        for (size_t k_0__ = 0; k_0__ < dim_alpha_s_0__; ++k_0__) {
            alpha_s.push_back(in__.vector_constrain(N_knots));
        }
        vector<vector_d> alpha_t;
        size_t dim_alpha_t_0__ = (K * (T-1));
        for (size_t k_0__ = 0; k_0__ < dim_alpha_t_0__; ++k_0__) {
            alpha_t.push_back(in__.vector_constrain(N_knots));
        }
        vector<vector_d> g;
        size_t dim_g_0__ = K;
        for (size_t k_0__ = 0; k_0__ < dim_g_0__; ++k_0__) {
            g.push_back(in__.vector_constrain((N * T)));
        }
        vars__.push_back(ksi);
        for (int k_0__ = 0; k_0__ < K; ++k_0__) {
            vars__.push_back(omega[k_0__]);
        }
        // for (int k_0__ = 0; k_0__ < K; ++k_0__) {
        //     vars__.push_back(lambda[k_0__]);
        // }
        for (int k_0__ = 0; k_0__ < K; ++k_0__) {
            vars__.push_back(mu[k_0__]);
        }
        for (int k_1__ = 0; k_1__ < T-1; ++k_1__) {
            for (int k_0__ = 0; k_0__ < K; ++k_0__) {
                vars__.push_back(mu_t[k_0__][k_1__]);
            }
        }
        for (int k_1__ = 0; k_1__ < N_knots; ++k_1__) {
            for (int k_0__ = 0; k_0__ < K; ++k_0__) {
                vars__.push_back(alpha_s[k_0__][k_1__]);
            }
        }
        for (int k_1__ = 0; k_1__ < N_knots; ++k_1__) {
	  for (int k_0__ = 0; k_0__ < (K * (T-1)); ++k_0__) {
                vars__.push_back(alpha_t[k_0__][k_1__]);
            }
        }
        for (int k_1__ = 0; k_1__ < (N * T); ++k_1__) {
            for (int k_0__ = 0; k_0__ < K; ++k_0__) {
                vars__.push_back(g[k_0__][k_1__]);
            }
        }

        if (!include_tparams__) return;
        // declare and define transformed parameters
        double lp__ = 0.0;
        (void) lp__; // dummy call to supress warning
        stan::math::accumulator<double> lp_accum__;

        // validate transformed parameters

        // write transformed parameters

        if (!include_gqs__) return;
        // declare and define generated quantities

        // validate generated quantities

        // write generated quantities
    }

    template <typename RNG>
    void write_array(RNG& base_rng,
                     Eigen::Matrix<double,Eigen::Dynamic,1>& params_r,
                     Eigen::Matrix<double,Eigen::Dynamic,1>& vars,
                     bool include_tparams = true,
                     bool include_gqs = true,
                     std::ostream* pstream = 0) const {
      std::vector<double> params_r_vec(params_r.size());
      for (int i = 0; i < params_r.size(); ++i)
        params_r_vec[i] = params_r(i);
      std::vector<double> vars_vec;
      std::vector<int> params_i_vec;
      write_array(base_rng,params_r_vec,params_i_vec,vars_vec,include_tparams,include_gqs,pstream);
      vars.resize(vars_vec.size());
      for (int i = 0; i < vars.size(); ++i)
        vars(i) = vars_vec[i];
    }

    static std::string model_name() {
        return "pred_model";
    }

    void constrained_param_names(std::vector<std::string>& param_names__,
                                 bool include_tparams__ = true,
                                 bool include_gqs__ = true) const {
        std::stringstream param_name_stream__;
        param_name_stream__.str(std::string());
        param_name_stream__ << "ksi";
        param_names__.push_back(param_name_stream__.str());
        for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
            param_name_stream__.str(std::string());
            param_name_stream__ << "omega" << '.' << k_0__;
            param_names__.push_back(param_name_stream__.str());
        }
        // for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
        //     param_name_stream__.str(std::string());
        //     param_name_stream__ << "lambda" << '.' << k_0__;
        //     param_names__.push_back(param_name_stream__.str());
        // }
        for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
            param_name_stream__.str(std::string());
            param_name_stream__ << "mu" << '.' << k_0__;
            param_names__.push_back(param_name_stream__.str());
        }
        for (int k_1__ = 1; k_1__ <= T-1; ++k_1__) {
            for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
                param_name_stream__.str(std::string());
                param_name_stream__ << "mu_t" << '.' << k_0__ << '.' << k_1__;
                param_names__.push_back(param_name_stream__.str());
            }
        }
        for (int k_1__ = 1; k_1__ <= N_knots; ++k_1__) {
            for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
                param_name_stream__.str(std::string());
                param_name_stream__ << "alpha_s" << '.' << k_0__ << '.' << k_1__;
                param_names__.push_back(param_name_stream__.str());
            }
        }
        for (int k_1__ = 1; k_1__ <= N_knots; ++k_1__) {
	  for (int k_0__ = 1; k_0__ <= (K * (T-1)); ++k_0__) {
                param_name_stream__.str(std::string());
                param_name_stream__ << "alpha_t" << '.' << k_0__ << '.' << k_1__;
                param_names__.push_back(param_name_stream__.str());
            }
        }
        for (int k_1__ = 1; k_1__ <= (N * T); ++k_1__) {
            for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
                param_name_stream__.str(std::string());
                param_name_stream__ << "g" << '.' << k_0__ << '.' << k_1__;
                param_names__.push_back(param_name_stream__.str());
            }
        }

        if (!include_gqs__ && !include_tparams__) return;

        if (!include_gqs__) return;
    }

    void unconstrained_param_names(std::vector<std::string>& param_names__,
                                   bool include_tparams__ = true,
                                   bool include_gqs__ = true) const {
        std::stringstream param_name_stream__;
        param_name_stream__.str(std::string());
        param_name_stream__ << "ksi";
        param_names__.push_back(param_name_stream__.str());
        for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
            param_name_stream__.str(std::string());
            param_name_stream__ << "omega" << '.' << k_0__;
            param_names__.push_back(param_name_stream__.str());
        }
        // for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
        //     param_name_stream__.str(std::string());
        //     param_name_stream__ << "lambda" << '.' << k_0__;
        //     param_names__.push_back(param_name_stream__.str());
        // }
        for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
            param_name_stream__.str(std::string());
            param_name_stream__ << "mu" << '.' << k_0__;
            param_names__.push_back(param_name_stream__.str());
        }
        for (int k_1__ = 1; k_1__ <= T-1; ++k_1__) {
            for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
                param_name_stream__.str(std::string());
                param_name_stream__ << "mu_t" << '.' << k_0__ << '.' << k_1__;
                param_names__.push_back(param_name_stream__.str());
            }
        }
        for (int k_1__ = 1; k_1__ <= N_knots; ++k_1__) {
            for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
                param_name_stream__.str(std::string());
                param_name_stream__ << "alpha_s" << '.' << k_0__ << '.' << k_1__;
                param_names__.push_back(param_name_stream__.str());
            }
        }
        for (int k_1__ = 1; k_1__ <= N_knots; ++k_1__) {
	  for (int k_0__ = 1; k_0__ <= (K * (T-1)); ++k_0__) {
                param_name_stream__.str(std::string());
                param_name_stream__ << "alpha_t" << '.' << k_0__ << '.' << k_1__;
                param_names__.push_back(param_name_stream__.str());
            }
        }
        for (int k_1__ = 1; k_1__ <= (N * T); ++k_1__) {
            for (int k_0__ = 1; k_0__ <= K; ++k_0__) {
                param_name_stream__.str(std::string());
                param_name_stream__ << "g" << '.' << k_0__ << '.' << k_1__;
                param_names__.push_back(param_name_stream__.str());
            }
        }

        if (!include_gqs__ && !include_tparams__) return;

        if (!include_gqs__) return;
    }

}; // model

} // namespace

typedef pred_model_namespace::pred_model stan_model;

namespace stan {
  namespace model {

    using namespace std;

    template <bool propto, bool jacobian_adjust_transform>
    double log_prob_grad(const stan_model& model,
                         std::vector<double>& params_r,
                         std::vector<int>& params_i,
                         std::vector<double>& gradient,
                         std::ostream* msgs = 0) {
      gradient.resize(params_r.size());
      return model.log_prob_grad<propto, jacobian_adjust_transform, false>(params_r, gradient, msgs);
    }

    void gradient(const stan_model& model,
                  const Eigen::Matrix<double, Eigen::Dynamic, 1>& x,
                  double& f,
                  Eigen::Matrix<double, Eigen::Dynamic, 1>& grad_f,
		  std::ostream* msgs = 0) {
      vector<double> params_r(x.rows());
      vector<double> grad(x.rows());
      for (size_t i = 0; i < params_r.size(); i++)
        params_r[i] = x[i];
      paleon::Timer timer_lpg(1);
      timer_lpg.tic(0);
      f = model.log_prob_grad<true, true, false>(params_r, grad, msgs);
      timer_lpg.toc(0);
      timer_lpg.echo("=> gradient ");
      for (size_t i = 0; i < params_r.size(); i++)
        grad_f[i] = grad[i];
    }

    void gradient(const stan_model& model,
                  const Eigen::Matrix<double, Eigen::Dynamic, 1>& x,
                  double& f,
                  Eigen::Matrix<double, Eigen::Dynamic, 1>& grad_f,
		  callbacks::logger& logger) {
      gradient(model, x, f, grad_f);
    }
  }
}


#include <stdio.h>
#include <stdlib.h>
#include <armadillo>
#include <iostream>
#include <string>

mat niceRead(string arxeio)
{
  ifstream myFile;

  // choose data
  try
  {
    myFile.open(arxeio.c_str());
  }
  catch(const std::exception& e)
  {
    cout<< "Failed to open file data "<<arxeio<< endl;
  }

  //get size
  int DIM = 7;
  double num = 0.0;
  int lenght =0;
  while (myFile >> num)
  {
    lenght++;
  }
  myFile.close();
  lenght = int(lenght/DIM);
  // fix size
  mat m(DIM,lenght);

  // open again to get data

    int i = -1;

    FILE *fp1;
     int ar0,ar1,ar2,ar3,ar4,ar5,ar6,k;

     fp1=fopen(arxeio.c_str(),"r");
     if (fp1==NULL)
     {
        puts("Error opening file");
        exit(2);
     }

      while (k=fscanf(fp1,"%d %d %d %d %d %d %d", &ar0, &ar1, &ar2, &ar3, &ar4, &ar5, &ar6)>=1)
     {

	      i++;
        m(0,i) = ar0;
        m(1,i) = ar1;
        m(2,i) = ar2;
        m(3,i) = ar3;
        m(4,i) = ar4;
        m(5,i) = ar5;
        m(6,i) = ar6;

         printf("%d %d %d %d %d %d %d", &ar0, &ar1, &ar2, &ar3, &ar4, &ar5, &ar6));

     }
     fclose(fp1);

    return m;
}


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

#define BOOST_TEST_MODULE emsa1_encoding_test

#include <iostream>
#include <unordered_map>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <nil/crypto3/pkpad/emsa/emsa1.hpp>

#include <nil/crypto3/pkpad/algorithms/encode.hpp>
#include <nil/crypto3/pkpad/algorithms/verify.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/curves/secp_r1.hpp>

#include <nil/crypto3/hash/sha1.hpp>
#include <nil/crypto3/hash/sha2.hpp>

using namespace nil::crypto3;

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename algebra::fields::detail::element_fp<FieldParams> &e) {
    os << e.data;
}

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<typename FieldParams>
            struct print_log_value<typename algebra::fields::detail::element_fp<FieldParams>> {
                void operator()(std::ostream &os, typename algebra::fields::detail::element_fp<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost

const char *test_data = "../../../../libs/pkpad/test/data/emsa.json";

boost::property_tree::ptree string_data(std::string test_name) {
    boost::property_tree::ptree string_data;
    boost::property_tree::read_json(test_data, string_data);
    return string_data.get_child(test_name);
}

template<typename ElementType>
struct field_element_init;

template<typename FieldParams>
struct field_element_init<algebra::fields::detail::element_fp<FieldParams>> {
    using element_type = algebra::fields::detail::element_fp<FieldParams>;

    template<typename ElementData>
    static inline element_type process(const ElementData &element_data) {
        return element_type(typename element_type::integral_type(element_data.second.data()));
    }
};

template<typename FieldValueType, typename Padding, typename DataSet>
void test_emsa(const DataSet &array_element) {
    auto text = array_element.first;
    std::vector<uint8_t> data(text.begin(), text.end());
    FieldValueType out = pubkey::encode<Padding>(array_element.first);
    FieldValueType etalon_out = field_element_init<FieldValueType>::process(array_element);
    BOOST_CHECK_EQUAL(out, etalon_out);
}

// test data generated by botan
BOOST_AUTO_TEST_SUITE(emsa_test_suite)

BOOST_AUTO_TEST_CASE(emsa1_bls12_fr_single_range_encode) {
    using curve_type = algebra::curves::bls12_381;
    using field_type = typename curve_type::scalar_field_type;
    using field_value_type = typename field_type::value_type;
    using hash_type = hashes::sha1;
    using padding_type = pubkey::padding::emsa1<field_value_type, hash_type>;

    std::vector<std::uint8_t> in {0, 0, 0, 0, 1};
    field_value_type out = pubkey::encode<padding_type>(in);
    print_field_element(std::cout, out);
}

BOOST_AUTO_TEST_CASE(emsa1_secp256r1_fr_manual_conformity_test) {
    using curve_type = algebra::curves::secp256r1;
    using field_type = typename curve_type::scalar_field_type;
    using field_value_type = typename field_type::value_type;
    using integral_type = typename field_type::integral_type;
    using hash_type = hashes::sha2<256>;
    using padding_type = pubkey::padding::emsa1<field_value_type, hash_type>;

    std::string text("This is a tasty burger!");
    std::vector<uint8_t> data(text.data(), text.data() + text.length());
    field_value_type out = pubkey::encode<padding_type>(data);
    field_value_type etalon_out =
        integral_type("111474717792720247796999809655932432881783035037226574051829933946736885398526");
    BOOST_CHECK_EQUAL(out, etalon_out);
}

BOOST_DATA_TEST_CASE(emsa1_sha256_secp256r1_fr_conformity_test, string_data("emsa1_sha256_secp256r1_fr"), array_element) {
    using curve_type = algebra::curves::secp256r1;
    using field_type = typename curve_type::scalar_field_type;
    using field_value_type = typename field_type::value_type;
    using hash_type = hashes::sha2<256>;
    using padding_type = pubkey::padding::emsa1<field_value_type, hash_type>;

    test_emsa<field_value_type, padding_type>(array_element);
}

BOOST_DATA_TEST_CASE(emsa1_sha512_secp256r1_fr_conformity_test, string_data("emsa1_sha512_secp256r1_fr"), array_element) {
    using curve_type = algebra::curves::secp256r1;
    using field_type = typename curve_type::scalar_field_type;
    using field_value_type = typename field_type::value_type;
    using hash_type = hashes::sha2<512>;
    using padding_type = pubkey::padding::emsa1<field_value_type, hash_type>;

    test_emsa<field_value_type, padding_type>(array_element);
}

BOOST_AUTO_TEST_SUITE_END()


//////////////////////////////////////////////////////////////////////////////////
// distribution::common::concept::aggregate::unnormalized_functions.hpp         //
//                                                                              //
//  (C) Copyright 2009 Erwann Rogard                                            //
//  Use, modification and distribution are subject to the                       //
//  Boost Software License, Version 1.0. (See accompanying file                 //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_COMMON_CONCEPT_AGGREGATE_UNNORMALIZED_FUNCTIONS_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_COMMON_CONCEPT_AGGREGATE_UNNORMALIZED_FUNCTIONS_HPP_ER_2009
#include <boost/mpl/empty_base.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_common/concept/unary/derivative_log_unnormalized_pdf.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace common{
namespace concept{

    template<
        typename B
    >
    class UnnormalizedFunctions : public
        LogUnnormalizedPdf<
            DerivativeLogUnnormalizedPdf<
                B
            >
        >
    {
    };

}// concept
}// common
}// distribution

}// statistics
}// detail
}// boost

#endif


/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2011-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_TOOLBOX_LINALG_FUNCTIONS_GENERAL_MPOWER_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_FUNCTIONS_GENERAL_MPOWER_HPP_INCLUDED

#include <nt2/toolbox/linalg/functions/mpower.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/include/functions/tofloat.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/is_flint.hpp>
#include <nt2/include/functions/is_ltz.hpp>
#include <nt2/include/functions/is_even.hpp>
#include <nt2/include/functions/modf.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/isdiagonal.hpp>
#include <nt2/include/functions/schur.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/complexify.hpp>
#include <nt2/include/functions/power.hpp>
#include <nt2/include/functions/round.hpp>
#include <nt2/include/functions/trunc.hpp>
#include <nt2/include/functions/inv.hpp>
#include <nt2/include/functions/sum.hpp>
#include <nt2/include/functions/sqrtm.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/expm.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/length.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/include/functions/conj.hpp>
#include <nt2/include/functions/size.hpp>
#include <nt2/include/functions/norm.hpp>
#include <nt2/include/functions/isscalar.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/sdk/complex/meta/is_complex.hpp>
#include <boost/mpl/bool.hpp>
namespace nt2{ namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::mpower_, tag::cpu_
                              , (A0)(N0)(A1)(N1)
                              , ((node_<A0, nt2::tag::mpower_, N0, nt2::container::domain>))
                              ((node_<A1, nt2::tag::tie_ , N1, nt2::container::domain>))
                            )
  {
    typedef void                                                    result_type;
    typedef typename boost::proto::result_of::child_c<A1&,0>::type         Out0;
    typedef typename boost::proto::result_of::child_c<A0&,0>::type          In0;
    typedef typename boost::proto::result_of::child_c<A0&,1>::type          In1;
    typedef typename A0::value_type                                  value_type;
    typedef std::complex<value_type>                                  cplx_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0, A1& a1 ) const
    {
      const In0& a  = boost::proto::child_c<0>(a0);
      const In1& b  = boost::proto::child_c<1>(a0);
      const Out0& r  = boost::proto::child_c<0>(a1);
      bool s0 =  nt2::isscalar(a);
      bool s1 =  nt2::isscalar(b);
      if (s0 && s1)
      {
        value_type aa = a,  bb = b;
        doit0(aa, bb, r);

      }
      else if(s0)
      {
        value_type aa = a;
        doit1(aa, b, r);
      }
      else if(s1)
      {
        value_type bb = b;
        doit2(a, bb, r);
      }
    }
  private:
    BOOST_FORCEINLINE static void doit0(const value_type& a, value_type& b, Out0& r)
    {
      r =  nt2::pow(a, b);
    }
    template < class T >
    BOOST_FORCEINLINE static void doit1(const value_type& a, T& b, Out0& r)
    {
      r.resize(extent(b));
      r =  nt2::expm(nt2::log(a)*b);
    }
    template < class T >
    BOOST_FORCEINLINE static void doit2(const T& a, value_type& b, Out0& r)
    {
      r.resize(extent(a));
      typedef typename A0::index_type        index_type;
      typedef table<value_type, index_type> result_type;
      typedef table<cplx_type>                  ct_type;
      bool is_ltz_b = is_ltz(b);
      if(is_ltz(b)) b = -b;
      value_type m = nt2::trunc(b);
      value_type f = b-m;
      ct_type q, t;
      nt2::tie(q, t) = schur(nt2::complexify(a),'N'); // t is complex schur form.        result_type e, v;
      if (isdiagonal(t))
      {
        t = nt2::from_diag(nt2::pow(diag_of(t), m));
        if(is_ltz_b) t = nt2::inv(t);
        BOOST_AUTO_TPL(z, nt2::mtimes(q, nt2::mtimes(t, nt2::trans(nt2::conj(q)))));
        transtype(r, z, typename nt2::meta::is_complex<value_type>::type());
        return;
      }
      else
      { //use iterative method
        r = nt2::eye(nt2::size(a), meta::as_<value_type>());
        result_type rf = r;
        if (m)
        {
          result_type a00 = a;
          while (m >= nt2::One<value_type>())
          {
            if (nt2::is_odd(m))
            {
              r =  nt2::mtimes(a00, r);
            }
            a00 =  nt2::mtimes(a00, a00);
            m =  nt2::trunc(m/2); //Half<value_type>(); or >> 1
          }
        }
        if(!f)
        {
          if(is_ltz_b) r = nt2::inv(r);
          return;
        }
        else
        {
          result_type a00 = nt2::sqrtm(a);
          value_type thresh = nt2::Half<value_type>();
          while (f > Zero<value_type>())
          {
            if (f >= thresh)
            {
              rf = nt2::mtimes(rf, a00);
              f -= thresh;
            }
            thresh *= nt2::Half<value_type>();
            a00 =  nt2::sqrtm(a00);
          }
        }
        r= nt2::mtimes(r, rf);
        if(is_ltz_b) r = nt2::inv(r);
      }
    }
    template < class T1, class T2>
      BOOST_FORCEINLINE static void transtype(T1& r, T2& z, boost::mpl::true_ const &)
    {
      r = z;
    }
    template < class T1, class T2 >
      BOOST_FORCEINLINE static void transtype(T1& r, T2& z, boost::mpl::false_ const &)
    {
      r =  real(z);
    }

  };

} }

#endif


#include "Sound.h"
#include<SFML\Audio.hpp>
#include <Eigen\Dense>
#include <algorithm>
#include <iostream>

#define SECOND 3
#define SAMPLING_HZ 44100
#define BUFFER_LENGTH (SECOND * SAMPLING_HZ)
#define SOUND_HZ 14706

static const int SR = 44100;
static const int TS = 3;
double normalizeScale_damping = -1.0;
double normalizeScale_no_dampin = -1.0;
namespace sound {

	void soundplay_sfml(std::vector<double> const& force, modalmodel& modal, massspringsystem& sys)
	{
		//std::vector<double> const &force, modalmodel &modal, massspringsystem &sys
		const unsigned SAMPLES = 44100;
		const unsigned SAMPLE_RATE = 44100;
		const unsigned AMPLITUDE = 32767;

		sf::Int16 raw[SR * TS] = { 0 };
		const int totticks = SR * TS;

		const std::vector<double>& omegaD = modal.damped_omega();  //no damping
		const std::vector<double>& c = modal.damping_co();
		const std::vector<pointmass*> mass = sys.getmasses();
		for (int i = 0; i < modal.num_modes(); i++)
		{
			const double SS = force[i] / omegaD[i] / mass[i]->mass;
			for (int ti = 0; ti < totticks; ti++)
			{
				const double ts = static_cast<double>(ti) / static_cast<double>(SR);
				const double amp = exp(-c[i] * 0.5 * ts);
				if (amp < 1e-5) break;  //cut amplitude
				raw[ti] += sin(omegaD[i] * ts) * amp * SS * SHRT_MAX;

			}
		}

		sf::SoundBuffer Buffer;

		if (!Buffer.loadFromSamples(raw, totticks, 1, SAMPLE_RATE)) {
			std::cerr << "Loading failed!" << std::endl;
			return;
		}

		sf::Sound Sound;
		Sound.setBuffer(Buffer);
		Sound.setLoop(false);
		Sound.play();
		sf::sleep(sf::seconds(0.1));
		Sound.stop();
		//while (1) {
		//	sf::sleep(sf::milliseconds(100));
		//}
	}

	short Sinewave(double time, double freq, double amplitude)
	{
		short result;
		double tpc = 44100 / freq;   //ticks per cycle for sampling  self-define sampling rate based on the frequency
		double cycles = time / tpc;
		double rad = two_PI * cycles;
		short tmpamplitude = 32767 * amplitude;   //amplitude can be 0-1

		result = tmpamplitude * sin(rad);

		return result;
	}
	//
	void soundplay_al(Eigen::MatrixXd& S, std::vector<double>frequency, std::vector<double> ampli)
	{
		ALCdevice* device;
		ALCcontext* context;
		ALshort data[BUFFER_LENGTH];
		ALuint buffer, source;

		//int i;

		// Initialization
		device = alcOpenDevice(NULL);
		if (device)
		{
			context = alcCreateContext(device, NULL);
			alcMakeContextCurrent(context);
		}
		alGetError();
		alGenBuffers(1, &buffer);

		if (alGetError() != AL_NO_ERROR)
		{
			std::cout << "error" << std::endl;
			return;
		}
		memset(data, 0, sizeof(data));   //zero +
		// Generate sine wave data    //
		for (int j = 0; j < frequency.size(); j++)
		{
			for (int i = 0; i < BUFFER_LENGTH; ++i)
			{
				data[i] += sin(2 * EIGEN_PI * frequency[j] * i / BUFFER_LENGTH) * ampli[j] * SHRT_MAX;   //normallized amplitude

			}
		}

		// Output looping sine wave
		//for (int i = 0; i < frequency.size(); i++)
	//	{
		alBufferData(buffer, AL_FORMAT_STEREO16, data, sizeof(data), BUFFER_LENGTH * 2);
		alGenSources(1, &source);
		alGetError();  //clear error code
		alSourcei(source, AL_BUFFER, buffer);
		alSourcei(source, AL_LOOPING, AL_FALSE);
		//alsourcef(source[i],AL_SEC_OFFSET)

	//}

		alSourcePlay(source);
		ALint s = AL_PLAYING;
		while (s == AL_PLAYING)
		{
			alGetSourcei(source, AL_SOURCE_STATE, &s);
			Sleep(3000);   //50 ms
			alSourceStop(source);
		}
		//alSourcePlay(source2);
		// Wait to exit
		//printf("Press any key to exit.");
		//getchar();
		// Finalization

		alDeleteSources(1, &source);
		alDeleteBuffers(1, &buffer);
		alcMakeContextCurrent(NULL);
		alcDestroyContext(context);
		alcCloseDevice(device);

	}

	//currently one force,damping
	void soundplay_al_new(std::vector<double> const& force, modalmodel& modal, massspringsystem& sys)
	{
		ALCdevice* device;
		ALCcontext* context;
		ALshort data[BUFFER_LENGTH];
		//double tmpdata[BUFFER_LENGTH * 2];
		ALuint buffer, source;
		const std::vector<double>& omegaD = modal.damped_omega();
		const std::vector<double>& c = modal.damping_co();
		const std::vector<double> mass = sys.masses_3d();     //masses

		const int totticks = SR * TS;   //sample the sound for 3 second

		// Initialization
		device = alcOpenDevice(NULL);
		if (device)
		{
			context = alcCreateContext(device, NULL);
			alcMakeContextCurrent(context);
		}
		alGetError();
		alGenBuffers(1, &buffer);

		if (alGetError() != AL_NO_ERROR)
		{
			std::cout << "error" << std::endl;
			return;
		}

		memset(data, 0, sizeof(data));   //zero +
										 // Generate sine wave data    //
		//std::cout << "Number of Modes: " << modal.num_modes()<<std::endl;
		//std::cout << mass.size() << std::endl << std::endl;
		for (int j = 0; j < modal.num_modes(); j++)
		{
			//std::cout <<omegaD[j] << std::endl;
			//Currently not condiering distance between cam and object
			const double SS = force[j] / omegaD[j] / mass[j];   //need to be updated
			//std::cout << "ss" << std::endl << std::endl;
			for (int i = 0; i < BUFFER_LENGTH; ++i)
			{
				const double ts = static_cast<double>(i) / static_cast<double>(SR);
				const double amp = exp(-c[j] * 0.5 * ts);
				//if (amp < 1e-5) break;  //cut amplitude
				data[i] += sin(omegaD[j] * ts) * amp * SS * SHRT_MAX;   //normallized amplitude
				//data[2*i+1] += -sin(omegaD[j] * ts) * amp*SS*SHRT_MAX;   //normallized amplitude
			}
		}

		if (normalizeScale_damping < 0.)    // no necessary
		{
			double amptmp = 0;
			for (int ti = 0; ti < totticks * 2; ++ti)
			{
				amptmp = max(amptmp, abs(data[ti]));
				//std::cout << abs(tmpdata[ti]) << std::endl;
			}
			//std::cout <<"Max: "<< amptmp <<std::endl;
			normalizeScale_damping = 1 /amptmp;
			//std::cout << normalizeScale_damping << std::endl;
		}
		for (int ti = 0; ti < totticks*2; ++ti)
		{
			data[ti] = data[ti]/normalizeScale_damping*SHRT_MAX;   //SHRT Avoiding all zero values
		}
		// Output looping sine wave
		//for (int i = 0; i < frequency.size(); i++)
		//
		alBufferData(buffer, AL_FORMAT_MONO16, data, sizeof(data), SAMPLING_HZ);   //stereo shortens the sound (two channels)
		alGenSources(1, &source);
		alGetError();  //clear error code
		alSourcei(source, AL_BUFFER, buffer);
		alSourcei(source, AL_LOOPING, AL_FALSE);
		//alsourcef(source[i],AL_SEC_OFFSET)

		//}

		alSourcePlay(source);
		ALint s = AL_PLAYING;
		while (s == AL_PLAYING)
		{
			alGetSourcei(source, AL_SOURCE_STATE, &s);
			Sleep(10);   //50 ms
			//alSourceStop(source);
		}
		//alGetError();
		//alSourcePlay(source2);
		// Wait to exit
		//printf("Press any key to exit.");
		//getchar();
		// Finalization

		alDeleteSources(1, &source);
		alDeleteBuffers(1, &buffer);
		alcMakeContextCurrent(NULL);
		alcDestroyContext(context);
		alcCloseDevice(device);

	}

	void soundplay_al_new_nodamping(std::vector<double> const& force, modalmodel& modal, massspringsystem& sys)
	{
		ALCdevice* device;
		ALCcontext* context;
		ALshort data[BUFFER_LENGTH * 2];
		//double tmpdata[BUFFER_LENGTH * 2];
		ALuint buffer, source;
		const std::vector<double>& omegaD = modal.omega_all();  //no damping
		const std::vector<double>& c = modal.damping_co();
		const std::vector<pointmass*> mass = sys.getmasses();

		const int totticks = SR * TS;   //sample the sound for 3 second

										// Initialization
		device = alcOpenDevice(NULL);
		if (device)
		{
			context = alcCreateContext(device, NULL);
			alcMakeContextCurrent(context);
		}
		alGetError();
		alGenBuffers(1, &buffer);

		if (alGetError() != AL_NO_ERROR)
		{
			std::cout << "error" << std::endl;
			return;
		}

		memset(data, 0, sizeof(data));   //zero +
										 // Generate sine wave data    //
		for (int j = 0; j < modal.num_modes(); j++)
		{
			//std::cout <<omegaD[j] << std::endl;
			//Currently not condiering distance between cam and object
			const double SS = force[j] / omegaD[j] / mass[j]->mass;
			for (int i = 0; i < BUFFER_LENGTH; ++i)
			{
				const double ts = static_cast<double>(i) / static_cast<double>(SR);
				const double amp = 1.0 * SS; //exp(-c[j] * 0.5*ts);
				//if (amp < 1e-16) continue;  //cut amplitude,  need to be identified
				data[i] += sin(omegaD[j] * ts) * amp * SHRT_MAX;   //normallized amplitude
			}
		}

		if (normalizeScale_damping < 0.)
		{
			double amptmp = 0;
			for (int ti = 0; ti < totticks * 2; ++ti)
			{
				amptmp = max(amptmp, abs(data[ti]));
				//std::cout << abs(tmpdata[ti]) << std::endl;
			}
			//std::cout << amptmp << std::endl;
			normalizeScale_damping = 0.6 /amptmp;
			//std::cout << normalizeScale_damping << std::endl;
		}
		for (int ti = 0; ti < totticks * 2; ++ti)
		{
			data[ti] *= normalizeScale_damping * SHRT_MAX;   //SHRT Avoiding all zero values
		}
		// Output looping sine wave
		//for (int i = 0; i < frequency.size(); i++)
		//	{
		ALint channels = 5;
		//alGetBufferi(buffer, AL_CHANNELS, &channels);
		//std::cout << "ppp " << channels << std::endl;
		alBufferData(buffer, AL_FORMAT_STEREO16, data, sizeof(data), SAMPLING_HZ);
		alGenSources(1, &source);
		alGetError();  //clear error code
		alSourcei(source, AL_BUFFER, buffer);

		alSourcei(source, AL_LOOPING, AL_FALSE);

		//alsourcef(source[i],AL_SEC_OFFSET)

		//}

		alSourcePlay(source);
		ALint s = AL_PLAYING;
		while (s == AL_PLAYING)
		{
			alGetSourcei(source, AL_SOURCE_STATE, &s);
			Sleep(10);   //50 ms
			//std::cout << "spspsp " << pp << std::endl;
			//alSourceStop(source);
		}
		//alSourcePlay(source2);
		// Wait to exit
		//printf("Press any key to exit.");
		//getchar();
		// Finalization

		alDeleteSources(1, &source);
		alDeleteBuffers(1, &buffer);
		alcMakeContextCurrent(NULL);
		alcDestroyContext(context);
		alcCloseDevice(device);

	}

	void soundplay_al_one_frequency(std::vector<double> const& force, modalmodel& modal, massspringsystem& sys, int tag)
	{
		ALCdevice* device;
		ALCcontext* context;
		ALshort data[BUFFER_LENGTH];
		//double tmpdata[BUFFER_LENGTH * 2];
		ALuint buffer, source;
		const std::vector<double>& omegaD = modal.damped_omega();
		const std::vector<double>& c = modal.damping_co();
		const std::vector<pointmass*> mass = sys.getmasses();

		const int totticks = SR * TS;   //sample the sound for 3 second

										// Initialization
		device = alcOpenDevice(NULL);
		if (device)
		{
			context = alcCreateContext(device, NULL);
			alcMakeContextCurrent(context);
		}
		alGetError();
		alGenBuffers(1, &buffer);

		if (alGetError() != AL_NO_ERROR)
		{
			std::cout << "error" << std::endl;
			return;
		}

		memset(data, 0, sizeof(data));   //zero +
										 // Generate sine wave data    //
										 //std::cout << "Number of Modes: " << modal.num_modes()<<std::endl;
			//std::cout <<omegaD[j] << std::endl;
			//Currently not condiering distance between cam and object
		const double SS = force[tag] / omegaD[tag] / mass[tag]->mass;

		for (int i = 0; i < BUFFER_LENGTH; ++i)
		{
			const double ts = static_cast<double>(i) / static_cast<double>(SR);
			const double amp = exp(-c[tag] * 0.5 * ts);
			if (amp < 1e-5) break;  //cut amplitude
			data[i] += sin(ts * omegaD[tag]) * amp * SS * SHRT_MAX;   //

		}

		if (normalizeScale_damping < 0.)    // no necessary
		{
		double amptmp = 0;
		for (int ti = 0; ti < totticks * 2; ++ti)
		{
		amptmp = max(amptmp, abs(data[ti]));
		//std::cout << abs(tmpdata[ti]) << std::endl;
		}
		//std::cout <<"Max: "<< amptmp <<std::endl;
		normalizeScale_damping = 1 /amptmp;
		//std::cout << normalizeScale_damping << std::endl;
		}
		for (int ti = 0; ti < totticks*2; ++ti)
		{
		data[ti] = data[ti]/normalizeScale_damping*SHRT_MAX;   //SHRT Avoiding all zero values
		}
		// Output looping sine wave
		//for (int i = 0; i < frequency.size(); i++)
		//	{
		alBufferData(buffer, AL_FORMAT_STEREO16, data, sizeof(data), SAMPLING_HZ);
		alGenSources(1, &source);
		alGetError();  //clear error code
		alSourcei(source, AL_BUFFER, buffer);
		alSourcei(source, AL_LOOPING, AL_FALSE);
		//alsourcef(source[i],AL_SEC_OFFSET)

		//}

		alSourcePlay(source);
		ALint s = AL_PLAYING;
		while (s == AL_PLAYING)
		{
			alGetSourcei(source, AL_SOURCE_STATE, &s);
			Sleep(10);   //50 ms
						 //alSourceStop(source);
		}
		//alGetError();
		//alSourcePlay(source2);
		// Wait to exit
		//printf("Press any key to exit.");
		//getchar();
		// Finalization

		alDeleteSources(1, &source);
		alDeleteBuffers(1, &buffer);
		alcMakeContextCurrent(NULL);
		alcDestroyContext(context);
		alcCloseDevice(device);

	}
	void soundplay_al_one_frequency_nodamping(std::vector<double> const& force, modalmodel& modal, massspringsystem& sys, int tag)
	{
		const unsigned SAMPLES = 44100;
		const unsigned SAMPLE_RATE = 44100;
		const unsigned AMPLITUDE = 32767;

		sf::Int16 raw[SR * TS] = { 0 };
		const int totticks = SR * TS;

		const std::vector<double>& omegaD = modal.omega_all();  //no damping
		const std::vector<double>& c = modal.damping_co();
		const std::vector<pointmass*> mass = sys.getmasses();
		for (int i = 0; i < modal.num_modes(); i++)
		{
			const double SS = force[i] / omegaD[i] / mass[i]->mass;
			for (int ti = 0; ti < totticks; ti++)
			{
				const double ts = static_cast<double>(ti) / static_cast<double>(SR);
				const double amp = 1.0 * SS; //exp(-c[j] * 0.5*ts);
				//if (amp < 1e-5) break;  //cut amplitude
				raw[ti] += sin(2 * EIGEN_PI * omegaD[i] * ti / BUFFER_LENGTH) * amp * SHRT_MAX;

			}
		}

		sf::SoundBuffer Buffer;

		if (!Buffer.loadFromSamples(raw, totticks, 2, SAMPLE_RATE)) {
			std::cerr << "Loading failed!" << std::endl;
			return;
		}

		sf::Sound Sound;
		Sound.setBuffer(Buffer);
		Sound.setLoop(false);
		Sound.play();
		sf::sleep(sf::seconds(2));
		Sound.stop();
	}
	void sound_test()
	{
		const unsigned SAMPLES = 44100;
		const unsigned SAMPLE_RATE = 44100;
		const unsigned AMPLITUDE = 30000;

		sf::Int16 raw[SAMPLES * 3];

		const double TWO_PI = 6.28318;
		const double increment = 440. / 44100;
		double x = 0;
		for (unsigned i = 0; i < SAMPLES * 3; i++) {
			raw[i] = AMPLITUDE * sin(x * TWO_PI);
			x += increment;
		}

		sf::SoundBuffer Buffer;
		if (!Buffer.loadFromSamples(raw, SAMPLES * 3, 1, SAMPLE_RATE)) {
			std::cerr << "Loading failed!" << std::endl;
			return;
		}

		sf::Sound Sound;
		Sound.setBuffer(Buffer);
		Sound.setLoop(false);
		Sound.play();
		sf::sleep(sf::seconds(3));
		Sound.stop();

	}
}


#include <PnC/WBC/WBDC/WBDC.hpp>
#include <Utils/IO/IOUtilities.hpp>
#include <Eigen/LU>
#include <Eigen/SVD>

WBDC::WBDC(const std::vector<bool> & act_list,
        const Eigen::MatrixXd * Jci): WBC(act_list, Jci){
    myUtils::pretty_constructor(3, "WBDC");
    Sf_ = Eigen::MatrixXd::Zero(6, num_qdot_);
    Sf_.block(0,0, 6, 6).setIdentity();
}

void WBDC::updateSetting(const Eigen::MatrixXd & A,
        const Eigen::MatrixXd & Ainv,
        const Eigen::VectorXd & cori,
        const Eigen::VectorXd & grav,
        void* extra_setting){
    A_ = A;
    Ainv_ = Ainv;
    cori_ = cori;
    grav_ = grav;
    b_updatesetting_ = true;
}

bool WBDC::_CheckNullSpace(const Eigen::MatrixXd & Npre){
    Eigen::MatrixXd M_check = Sf_ * A_ * Npre;
    Eigen::JacobiSVD< Eigen::MatrixXd > svd(M_check, Eigen::ComputeThinU | Eigen::ComputeThinV);
    //myUtils::pretty_print(svd.singularValues(), std::cout, "svd singular value");

    for(int i(0); i<svd.singularValues().rows(); ++i){
        if(svd.singularValues()[i] > 0.00001) {
            printf("non singular!!\n");
            myUtils::pretty_print(svd.singularValues(), std::cout, "svd singular value");
            return false;
        }else{
            //printf("small enough singular value: %f\n", svd.singularValues()[i]);
        }
    }
    return true;
}

void WBDC::makeTorque(const std::vector<Task*> & task_list,
        const std::vector<ContactSpec*> & contact_list,
        Eigen::VectorXd & cmd,
        void* extra_input){
    _PrintDebug(1);
    if(!b_updatesetting_) { printf("[Wanning] WBDC setting is not done\n"); }

    if(extra_input) data_ = static_cast<WBDC_ExtraData*>(extra_input);

    // Internal Constraint Check
    Nci_ = Eigen::MatrixXd::Identity(num_qdot_, num_qdot_);

    if(b_internal_constraint_) {
        Eigen::MatrixXd JciBar;
        _WeightedInverse(Jci_, Ainv_, JciBar);
        Nci_ -= JciBar * Jci_;
    }

    _PrintDebug(2);
    // Contact Setting
    _ContactBuilding(contact_list);
    Eigen::MatrixXd JcN = Jc_ * Nci_;
    Eigen::MatrixXd check = JcN - Jc_;

    _PrintDebug(3);
    Eigen::MatrixXd JcN_Bar;
    _WeightedInverse(JcN, Ainv_, JcN_Bar);
    Eigen::MatrixXd Npre = Eigen::MatrixXd::Identity(num_qdot_, num_qdot_)
        - JcN_Bar * JcN;

    _PrintDebug(4);
    // First Task Check
    Task* task = task_list[0];
    Eigen::MatrixXd Jt, JtPre, JtPreBar;
    Eigen::VectorXd JtDotQdot, xddot, qddot_pre;
    qddot_pre = JcN_Bar * ( - JcDotQdot_ );

    _PrintDebug(5);
    if(!task->isTaskSet()){ printf("1st task is not set!\n"); exit(0); }
    task->getTaskJacobian(Jt);
    task->getTaskJacobianDotQdot(JtDotQdot);
    task->getCommand(xddot);
    dim_first_task_ = task->getDim();

    JtPre = Jt * Npre;
    _WeightedInverse(JtPre, Ainv_, JtPreBar);
    Npre = Npre * ( Eigen::MatrixXd::Identity(num_qdot_, num_qdot_)
            - JtPreBar * JtPre);

    _PrintDebug(6);
    //_CheckNullSpace(Npre);
    // Optimization
    _PrintDebug(7);
    _OptimizationPreparation();
    // Set equality constraints
    Eigen::MatrixXd dyn_CE(dim_eq_cstr_, dim_opt_);
    Eigen::VectorXd dyn_ce0(dim_eq_cstr_);
    _PrintDebug(8);
    dyn_CE.block(0,0, dim_eq_cstr_, dim_first_task_) = Sf_ * A_ * JtPreBar;
    _PrintDebug(9);
    dyn_CE.block(0, dim_first_task_, dim_eq_cstr_, dim_rf_) = -Sf_ * JcN.transpose();
    _PrintDebug(10);
    dyn_ce0 = Sf_ * (A_ * (JtPreBar * (xddot - JtDotQdot - Jt * qddot_pre) + qddot_pre)
            + cori_ + grav_);
    _PrintDebug(11);
    for(int i(0); i< dim_eq_cstr_; ++i){
        for(int j(0); j<dim_opt_; ++j){
            CE[j][i] = dyn_CE(i,j);
        }
        ce0[i] = dyn_ce0[i];
    }

    _PrintDebug(12);
    // Set inequality constraints
    _SetInEqualityConstraint();
    _PrintDebug(13);

    // Optimization
    double f = solve_quadprog(G, g0, CE, ce0, CI, ci0, z);
    _PrintDebug(14);
    Eigen::VectorXd delta(dim_first_task_);
    for(int i(0); i<dim_first_task_; ++i) { delta[i] = z[i]; }
    qddot_pre = qddot_pre + JtPreBar * (xddot + delta - JtDotQdot - Jt * qddot_pre);
    _PrintDebug(15);

    // First Qddot is found
    // Stack The last Task
    for(int i(1); i<task_list.size(); ++i){
        task = task_list[i];

        if(!task->isTaskSet()){ printf("%d th task is not set!\n", i); exit(0); }
        task->getTaskJacobian(Jt);
        task->getTaskJacobianDotQdot(JtDotQdot);
        task->getCommand(xddot);

        JtPre = Jt * Npre;
        _WeightedInverse(JtPre, Ainv_, JtPreBar);

        qddot_pre = qddot_pre + JtPreBar * (xddot - JtDotQdot - Jt * qddot_pre);
        //std::cout << i << " th task additional acceleration" << std::endl;
        //std::cout << JtPreBar * (xddot - JtDotQdot - Jt * qddot_pre) << std::endl;

        Npre = Npre * ( Eigen::MatrixXd::Identity(num_qdot_, num_qdot_)
                - JtPreBar * JtPre);
    }
    _PrintDebug(16);

    _GetSolution(qddot_pre, cmd);

    _PrintDebug(17);
    data_->opt_result_ = Eigen::VectorXd(dim_opt_);
    for(int i(0); i<dim_opt_; ++i){
        data_->opt_result_[i] = z[i];
    }
    _PrintDebug(18);
    //std::cout << "f: " << f << std::endl;
    //std::cout << "x: " << z << std::endl;
    //std::cout << "cmd: "<<cmd<<std::endl;
    //myUtils::pretty_print(Sf_, std::cout, "Sf");
    //myUtils::pretty_print(qddot_pre, std::cout, "qddot_pre");
    //myUtils::pretty_print(JcN, std::cout, "JcN");
    //myUtils::pretty_print(Nci_, std::cout, "Nci");
    //vx eq_check = dyn_CE * data_->opt_result_;
    //myUtils::pretty_print(dyn_ce0, std::cout, "dyn ce0");
    //myUtils::pretty_print(eq_check, std::cout, "eq_check");

    //myUtils::pretty_print(Jt, std::cout, "Jt");
    //myUtils::pretty_print(JtDotQdot, std::cout, "Jtdotqdot");
    //myUtils::pretty_print(xddot, std::cout, "xddot");

    //printf("G:\n");
    //std::cout<<G<<std::endl;
    //printf("g0:\n");
    //std::cout<<g0<<std::endl;

    //printf("CE:\n");
    //std::cout<<CE<<std::endl;
    //printf("ce0:\n");
    //std::cout<<ce0<<std::endl;

    //printf("CI:\n");
    //std::cout<<CI<<std::endl;
    //printf("ci0:\n");
    //std::cout<<ci0<<std::endl;

    // if(f > 1.e5){
    //   std::cout << "f: " << f << std::endl;
    //   std::cout << "x: " << z << std::endl;
    //   std::cout << "cmd: "<<cmd<<std::endl;

    //   printf("G:\n");
    //   std::cout<<G<<std::endl;
    //   printf("g0:\n");
    //   std::cout<<g0<<std::endl;

    //   printf("CE:\n");
    //   std::cout<<CE<<std::endl;
    //   printf("ce0:\n");
    //   std::cout<<ce0<<std::endl;

    //   printf("CI:\n");
    //   std::cout<<CI<<std::endl;
    //   printf("ci0:\n");
    //   std::cout<<ci0<<std::endl;
    // }

}

void WBDC::_SetInEqualityConstraint(){
    Eigen::MatrixXd dyn_CI(dim_ieq_cstr_, dim_opt_); dyn_CI.setZero();
    Eigen::VectorXd dyn_ci0(dim_ieq_cstr_);

    dyn_CI.block(0, dim_first_task_, dim_rf_cstr_, dim_rf_) = Uf_;
    dyn_ci0 = uf_ieq_vec_;

    for(int i(0); i< dim_ieq_cstr_; ++i){
        for(int j(0); j<dim_opt_; ++j){
            CI[j][i] = dyn_CI(i,j);
        }
        ci0[i] = -dyn_ci0[i];
    }
    // myUtils::pretty_print(dyn_CI, std::cout, "WBDC: CI");
    // myUtils::pretty_print(dyn_ci0, std::cout, "WBDC: ci0");
}

void WBDC::_ContactBuilding(const std::vector<ContactSpec*> & contact_list){
    Eigen::MatrixXd Uf;
    Eigen::VectorXd uf_ieq_vec;
    // Initial
    Eigen::MatrixXd Jc;
    Eigen::VectorXd JcDotQdot;
    contact_list[0]->getContactJacobian(Jc);
    contact_list[0]->getJcDotQdot(JcDotQdot);
    Jc_ = Jc;

    JcDotQdot_ = JcDotQdot;
    contact_list[0]->getRFConstraintMtx(Uf_);
    contact_list[0]->getRFConstraintVec(uf_ieq_vec_);

    dim_rf_ = contact_list[0]->getDim();
    dim_rf_cstr_ = contact_list[0]->getDimRFConstratint();

    int dim_new_rf, dim_new_rf_cstr;

    for(int i(1); i<contact_list.size(); ++i){
        contact_list[i]->getContactJacobian(Jc);
        contact_list[i]->getJcDotQdot(JcDotQdot);
        dim_new_rf = contact_list[i]->getDim();
        dim_new_rf_cstr = contact_list[i]->getDimRFConstratint();

        // Jc append
        Jc_.conservativeResize(dim_rf_ + dim_new_rf, num_qdot_);
        Jc_.block(dim_rf_, 0, dim_new_rf, num_qdot_) = Jc;

        // JcDotQdot append
        JcDotQdot_.conservativeResize(dim_rf_ + dim_new_rf, 1);
        JcDotQdot_.tail(dim_new_rf) = JcDotQdot;

        // Uf
        contact_list[i]->getRFConstraintMtx(Uf);
        Uf_.conservativeResize(dim_rf_cstr_ + dim_new_rf_cstr, dim_rf_ + dim_new_rf);
        Uf_.block(0, dim_rf_, dim_rf_cstr_, dim_new_rf).setZero();
        Uf_.block(dim_rf_cstr_, 0, dim_new_rf_cstr, dim_rf_).setZero();
        Uf_.block(dim_rf_cstr_, dim_rf_, dim_new_rf_cstr, dim_new_rf) = Uf;

        // Uf inequality vector
        contact_list[i]->getRFConstraintVec(uf_ieq_vec);
        uf_ieq_vec_.conservativeResize(dim_rf_cstr_ + dim_new_rf_cstr);
        uf_ieq_vec_.tail(dim_new_rf_cstr) = uf_ieq_vec;

        // Increase reaction force dimension
        dim_rf_ += dim_new_rf;
        dim_rf_cstr_ += dim_new_rf_cstr;
    }
    // myUtils::pretty_print(Jc_, std::cout, "WBDC: Jc");
    // myUtils::pretty_print(JcDotQdot_, std::cout, "WBDC: JcDot Qdot");
    // myUtils::pretty_print(Uf_, std::cout, "WBDC: Uf");
}

void WBDC::_GetSolution(const Eigen::VectorXd & qddot, Eigen::VectorXd & cmd){
    Eigen::VectorXd Fr(dim_rf_);
    for(int i(0); i<dim_rf_; ++i) Fr[i] = z[i + dim_first_task_];
    Eigen::VectorXd tot_tau = A_ * qddot + cori_ + grav_ - (Jc_* Nci_).transpose() * Fr;

    //cmd = tot_tau.tail(num_act_joint_);
    Eigen::MatrixXd UNci = Sa_ * Nci_;
    Eigen::MatrixXd UNciBar;
    // only work in 3D case
    Eigen::MatrixXd UNci_trc = UNci.block(0,6, num_act_joint_, num_qdot_-6);
    Eigen::VectorXd tot_tau_trc = tot_tau.tail(num_qdot_-6);
    Eigen::MatrixXd UNciBar_trc;
    Eigen::MatrixXd eye(num_qdot_-6, num_qdot_-6);
    eye.setIdentity();
    _WeightedInverse(UNci_trc, eye, UNciBar_trc);
    //mx    Ainv_trc = Ainv_.block(6,6, num_qdot_-6, num_qdot_-6);
    //_WeightedInverse(UNci_trc, Ainv_trc, UNciBar_trc, 0.0001);
    cmd = UNciBar_trc.transpose() * tot_tau_trc;
    //cmd = (UNci_trc.inverse()).transpose()* tot_tau_trc;
    //myUtils::pretty_print(UNci, std::cout, "UNci");
    //myUtils::pretty_print(UNci_trc, std::cout, "UNci+trc");
    //myUtils::pretty_print(UNciBar_trc, std::cout, "UNciBar_trc");
    //myUtils::pretty_print(tot_tau_trc, std::cout, "tot tau trc");
    //_WeightedInverse(UNci, Ainv_, UNciBar);
    //cmd = UNciBar.transpose() * tot_tau;
    // myUtils::pretty_print(result, std::cout, "opt result");
    //myUtils::pretty_print(tot_tau, std::cout, "tot tau result");
    //myUtils::pretty_print(cmd, std::cout, "final command");
}

void WBDC::_OptimizationPreparation(){
    dim_opt_ = dim_rf_ + dim_first_task_;
    dim_eq_cstr_ = 6;
    dim_ieq_cstr_ = dim_rf_cstr_;

    G.resize(dim_opt_, dim_opt_);
    g0.resize(dim_opt_);
    CE.resize(dim_opt_, dim_eq_cstr_);
    ce0.resize(dim_eq_cstr_);
    CI.resize(dim_opt_, dim_ieq_cstr_);
    ci0.resize(dim_ieq_cstr_);

    for(int i(0); i<dim_opt_; ++i){
        for(int j(0); j<dim_opt_; ++j){
            G[i][j] = 0.;
        }
        g0[i] = 0.;
    }
    // Set Cost
    for (int i(0); i < dim_opt_; ++i){
        G[i][i] = data_->cost_weight[i];
    }
}