#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <math.h>
#include <Eigen/Dense>
#include <float.h>
#include "fcm.h"
#include "record.h"

extern vector<traj_base> traj;
extern vector<centers_fcm_base> centers_fcm;

FCM::FCM(double m, double epsilon) {
  m_epsilon = epsilon;
  m_m = m;
  m_membership = nullptr;
  m_data = nullptr;
  m_cluster_center = nullptr;
  m_num_clusters = 0;
  m_num_dimensions = 0;
}

FCM::~FCM() {
  if (m_data != nullptr) {
    delete m_data;
    m_data = nullptr;
  }

  if (m_membership != nullptr) {
    delete m_membership;
    m_membership = nullptr;
  }

  if (m_cluster_center != nullptr) {
    delete m_cluster_center;
    m_cluster_center = nullptr;
  }
}

double FCM::update_membership() {

  long k, i;
  float new_uik;
  double max_diff = 0.0, diff;

  if (m_data == nullptr || m_data->rows() == 0) {
    throw std::logic_error("ERROR: data should not be empty when updating the membership");
  }

  if (m_membership == nullptr || m_membership->rows() == 0 || m_membership->rows() != m_data->rows()) {
    this->init_membership();
  }
  if (m_num_clusters == 0) {
    throw std::logic_error("ERROR: the number of clusters should be set");
  }

  for (i = 0; i < m_num_clusters; i++) {
    for (k = 0; k < m_data->rows(); k++) {
      //cout << "point: " << k << " and cluster" << i <<endl;
      //cout << "\nwill ask for the new new_uik"<< endl;
      new_uik = this->compute_membership_point(i, k);
      diff = new_uik - (*m_membership)(k, i); // We need the membership inversed which is more natural for us
      if (diff > max_diff) {
        max_diff = diff;
      }
      (*m_membership)(k, i) = new_uik;
    }
  }
  return max_diff;
}

void FCM::compute_centers() {
  long i, j, k;
  double numerator, denominator;
  MatrixXf t;
  t.resize(m_data->rows(), m_num_clusters);
  if (m_data == nullptr || m_data->rows() == 0) {
    throw std::logic_error("ERROR: number of rows is zero");
    return;
  }
  for (i = 0; i < m_data->rows(); i++) { // compute (u^m) for each cluster for each point
    for (j = 0; j < m_num_clusters; j++) {
      t(i, j) = float(pow((*m_membership)(i, j), m_m));
    }
  }
  for (j = 0; j < m_num_clusters; j++) { // loop for each cluster
    for (k = 0; k < m_num_dimensions; k++) { // for each dimension
      numerator = 0.0;
      denominator = 0.0;
      for (i = 0; i < m_data->rows(); i++) {
        numerator += t(i, j) * (*m_data)(i, k);
        denominator += t(i, j);
      }
      (*m_cluster_center)(j, k) = float(numerator / denominator);
    }
  }
}

double FCM::get_dist(long i, long k) {
  /*
   * distance which is denoted in the paper as d
   * k is the data point
   * i is the cluster center point
  */
  //cout<<"get_dist: point: "<<k<<" and cluster "<<i<<endl;
  long j;
  double sqsum = 0.0;
  if (m_num_clusters == 0) {
    throw std::logic_error("ERROR: number of clusters should not be zero\n");
  }
  if (m_num_dimensions == 0) {
    throw std::logic_error("ERROR: number of dimensions should not be zero\n");
  }
  for (j = 0; j < m_num_dimensions; j++) {
    sqsum += pow(((*m_data)(k, j) - (*m_cluster_center)(i, j)), 2);
  }
  return sqrt(sqsum);
}

float FCM::compute_membership_point(long i, long k) {
  /*
   * i the cluster
   * k is the data point
  */
  //cout << __func__ <<"  num of cluster: "<<m_num_clusters<<endl;
  long j;
  double t, seg = 0.0;
  double exp = 2 / (m_m - 1);
  double dik, djk;
  if (m_num_clusters == 0) {
    throw std::logic_error("ERROR: number of clusters should not be zero\n");
  }
  for (j = 0; j < m_num_clusters; j++) {
    //std::cout << i << "  " << j << "  " << k << std::endl;
    dik = this->get_dist(i, k);
    djk = this->get_dist(j, k);
    //std::cout << dik << "  " << djk << std::endl;
    if (djk == 0) {
      djk = DBL_MIN;
    }
    t = dik / djk;
    t = pow(t, exp);
    //cout << "cluster: " << i << "data: " << k << " - " << "t: "<<t<<endl;
    seg += t;
  }
  //std::cin.get();

  //cout << "seg: "<<seg << " u: "<<(1.0/seg)<<endl;
  return float(1.0 / seg);
}

void FCM::set_data(MatrixXf *data) {
  if (m_data != nullptr) {
    delete m_data;
  }
  if (data->rows() == 0) {
    throw std::logic_error("ERROR: seting empty data");
  }
  m_data = data;
  m_num_dimensions = m_data->cols();
}

void FCM::set_membership(MatrixXf *membership) {
  if (m_data == 0) {
    throw std::logic_error("ERROR: the data should present before setting up the membership");
  }
  if (m_num_clusters == 0) {
    if (membership->cols() == 0) {
      throw std::logic_error("ERROR: the number of clusters is 0 and the membership matrix is empty");
    }
    else {
      this->set_num_clusters(membership->cols());
    }
  }
  if (m_membership != nullptr) {
    delete m_membership;
  }
  m_membership = membership;
  if (m_membership->rows() == 0) {
    m_membership->resize(m_data->rows(), m_num_clusters);
  }
}

void FCM::init_membership() {
  long i, j;
  double mem;
  if (m_num_clusters == 0) {
    throw std::logic_error("ERROR: the number of clusters is 0");
  }
  if (m_data == nullptr) {
    throw std::logic_error("ERROR: the data should present before setting up the membership");
  }
  if (m_membership != nullptr) {
    delete m_membership;
  }
  m_membership = new MatrixXf;
  m_membership->resize(m_data->rows(), m_num_clusters);
  mem = 1.0 / m_num_clusters;
  for (j = 0; j < m_num_clusters; j++) {
    for (i = 0; i < m_data->rows(); i++) {
      (*m_membership)(i, j) = float(mem);
    }
  }
}

void FCM::set_num_clusters(long num_clusters) {
  m_num_clusters = num_clusters;
  if (m_cluster_center) {
    delete m_cluster_center;
  }
  m_cluster_center = new MatrixXf;
  m_cluster_center->resize(m_num_clusters, m_num_dimensions);
}

MatrixXf * FCM::get_data() {
  return m_data;
}

MatrixXf * FCM::get_membership() {
  return m_membership;
}

MatrixXf * FCM::get_cluster_center() {
  return m_cluster_center;
}

// DUNN INDEX //
double d_eu_distance(vector<double> v1, vector<double> v2) {
  double d_eu = 0.0;
  for (int n = 0; n < v1.size(); ++n)
    d_eu += pow((v1[n] - v2[n]), 2);

  if (d_eu > 0) return sqrt(d_eu);
  else return 0.0;
}
//-------------------------------------------------------------------
double intra_distance(int i) {
  double max_dist = 0.0;
  for (int n = 0; n < traj.size(); ++n)
    if (traj[n].means_class == i)
      for (int m = 0; m < traj.size(); ++m)
        if (traj[m].means_class == i)
        {
          vector<double> v1 = { traj[n].average_speed, traj[n].v_max, traj[n].v_min, traj[n].sinuosity };
          vector<double> v2 = { traj[m].average_speed, traj[m].v_max, traj[m].v_min, traj[m].sinuosity };
          double dist = d_eu_distance(v1, v2);
          if (max_dist < dist)
            max_dist = dist;
        }

  return max_dist;
}
//-------------------------------------------------------------------
double inter_distance(int n, int m) {
  double min_dist = 1e6;
  for (auto &t1: traj)
    if (t1.means_class == n)
      for (auto &t2:traj)
        if (t2.means_class == m)
        {
          vector<double> v1 = { t1.average_speed, t1.v_max, t1.v_min, t1.sinuosity };
          vector<double> v2 = { t2.average_speed, t2.v_max, t2.v_min, t2.sinuosity };
          double dist = d_eu_distance(v1, v2);
          if (min_dist > dist)
            min_dist = dist;
        }

  return min_dist;
}
//-------------------------------------------------------------------
double measure_dunn_index() {
  double num=  1e6;
  double den = 0.0;

  for (auto &c : centers_fcm) {
    double intra_dist_ci = intra_distance(c.idx);
    if (intra_dist_ci > den)
      den = intra_dist_ci;
  }
  std::cout << "den measured" << std::endl;

  for (int n=0; n<centers_fcm.size()-1; ++n)
    for (int m = n + 1; m < centers_fcm.size(); ++m) {
      double inter_n_m = inter_distance(n, m);
      if (inter_n_m < num)
        num = inter_n_m;
    }

  std::cout << "num measured " << std::endl;
  return (num/den);
}
//-------------------------------------------------------------------


#include <armadillo>
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/matrix_proxy.hpp"
#include "viennacl/vector_proxy.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/tools/timer.hpp"
#include "viennacl/forwards.h"
#include "comfi.hpp"

using namespace viennacl::linalg;
using namespace arma;

/* sp_mat comfi::routines::computeRi(const vcl_vec &xn_vcl, const comfi::types::Operators &op) */
/*{*/
/*  vec xn(num_of_elem);*/
/*  viennacl::fast_copy(xn_vcl, xn);*/

/*  const uint nnzp = 12;*/
/*  umat  Avi = zeros<umat>(nnzp, num_of_grid);*/
/*  umat  Avj = zeros<umat>(nnzp, num_of_grid);*/
/*  mat   Avv = zeros<mat> (nnzp, num_of_grid);*/

/*  #pragma omp parallel for schedule(static)*/
/*  for (uint index=0; index<num_of_grid; index++)*/
/*  {*/
/*    // indexing*/
/*    const unsigned int  i=(index)%nx; // find i and c++ index ii*/
/*    const unsigned int  j=(index)/nx; //find j and c++ index jj*/

/*    const int           ij=ind(i, j);*/
/*    int                 ip1j=ind(i+1, j);*/
/*    int                 im1j=ind(i-1, j);*/
/*    int                 ijp1=ind(i, j+1);*/
/*    int                 ijm1=ind(i, j-1);*/

/*    //const double nuin   = mhdsim::sol::nu_in(Nnij, 0.5*(Tpij+Tnij));*/
/*    //const double nuni   = mhdsim::sol::nu_in(Nnij, 0.5*(Tpij+Tnij))*Npij/Nnij;*/
/*    const double nuni   = collisionrate;*/
/*    const double nuin   = nuni*Nnij/Npij;*/

/*    //const double resij = mhdsim::sol::resistivity(Npij, Nnij, Tpij, Tnij);*/

/*    //const double irate = mhdsim::sol::ionization_coeff(Tnij);*/
/*    //const double rrate = mhdsim::sol::recomb_coeff(Tpij);*/

/*    // collect*/
/*    Avi.col(index) = vi;*/
/*    Avj.col(index) = vj;*/
/*    Avv.col(index) = vv;*/
/*  }*/

/*  return comfi::util::syncSpMat(Avi, Avj, Avv);*/
/*}*/

vcl_mat comfi::routines::build_eig_matrix_z(const vcl_mat &xn,
                                            comfi::types::Context &ctx) {
  vcl_mat eig_matrix(ctx.num_of_grid(), ctx.num_of_eq);
  const vcl_mat p_fast = comfi::routines::fast_speed_z(xn, ctx);
  const vcl_mat GLM_eig = viennacl::scalar_matrix<double>(p_fast.size1(), p_fast.size2(), ctx.c_h());
  const vcl_mat V_z = element_fabs(viennacl::linalg::element_div(ctx.v_NVz(xn), ctx.v_Np(xn)));
  const vcl_mat U_z = element_fabs(viennacl::linalg::element_div(ctx.v_NUz(xn), ctx.v_Nn(xn)));
  const vcl_mat c_a = viennacl::linalg::element_div(ctx.v_Bz(xn), viennacl::linalg::element_sqrt(ctx.v_Np(xn)));
  const vcl_mat c_s = comfi::routines::sound_speed_p(xn, ctx);
  const vcl_mat c_sn = comfi::routines::sound_speed_n(xn, ctx);

  //ctx.v_Np(eig_matrix) = -GLM_eig;
  ctx.v_Np(eig_matrix) = V_z+p_fast;
  ctx.v_NVx(eig_matrix) = V_z+p_fast;
  ctx.v_NVp(eig_matrix) = V_z+c_a;
  ctx.v_NVz(eig_matrix) = V_z+p_fast;
  ctx.v_Bx(eig_matrix) = V_z;
  ctx.v_Bp(eig_matrix) = V_z+c_s;
  ctx.v_Bz(eig_matrix) = V_z+c_a;
  ctx.v_Ep(eig_matrix) = V_z+p_fast;
  ctx.v_Nn(eig_matrix) = U_z+c_sn;
  ctx.v_NUx(eig_matrix) = U_z+c_sn;
  ctx.v_NUp(eig_matrix) = U_z+c_sn;
  ctx.v_NUz(eig_matrix) = U_z+c_sn;
  ctx.v_En(eig_matrix) = U_z+c_sn;
  ctx.v_GLM(eig_matrix) = GLM_eig;

  return eig_matrix;
}

vcl_mat comfi::routines::build_eig_matrix_x(const vcl_mat &xn,
                                            comfi::types::Context &ctx) {
  vcl_mat eig_matrix(ctx.num_of_grid(), ctx.num_of_eq);
  const vcl_mat p_fast = comfi::routines::fast_speed_x(xn, ctx);
  const vcl_mat GLM_eig = viennacl::scalar_matrix<double>(p_fast.size1(), p_fast.size2(), ctx.c_h());
  const vcl_mat V_x = viennacl::linalg::element_div(ctx.v_NVx(xn), ctx.v_Np(xn));
  const vcl_mat U_x = viennacl::linalg::element_div(ctx.v_NUx(xn), ctx.v_Nn(xn));
  const vcl_mat c_a = viennacl::linalg::element_div(ctx.v_Bx(xn), viennacl::linalg::element_sqrt(ctx.v_Np(xn)));
  const vcl_mat c_s = comfi::routines::sound_speed_p(xn, ctx);
  const vcl_mat c_sn = comfi::routines::sound_speed_n(xn, ctx);

  //ctx.v_Np(eig_matrix) = -GLM_eig;
  ctx.v_Np(eig_matrix) = V_x+p_fast;
  ctx.v_NVx(eig_matrix) = V_x-p_fast;
  ctx.v_NVp(eig_matrix) = V_x-c_a;
  ctx.v_NVz(eig_matrix) = V_x-c_s;
  ctx.v_Bx(eig_matrix) = V_x;
  ctx.v_Bp(eig_matrix) = V_x+c_s;
  ctx.v_Bz(eig_matrix) = V_x+c_a;
  ctx.v_Ep(eig_matrix) = V_x+p_fast;
  ctx.v_Nn(eig_matrix) = U_x+c_sn;
  ctx.v_NUx(eig_matrix) = U_x-c_sn;
  ctx.v_NUp(eig_matrix) = U_x;
  ctx.v_NUz(eig_matrix) = U_x-c_sn;
  ctx.v_En(eig_matrix) = U_x+c_sn;
  ctx.v_GLM(eig_matrix) = GLM_eig;

  return eig_matrix;
}

vcl_mat comfi::routines::Re_MUSCL(const vcl_mat &xn, comfi::types::Context &ctx)
{
  const vcl_mat xn_ip1 = comfi::operators::ip1(xn, ctx);
  const vcl_mat xn_im1 = comfi::operators::im1(xn, ctx);
  const vcl_mat xn_jp1 = comfi::operators::jp1(xn, ctx);
  const vcl_mat xn_jm1 = comfi::operators::jm1(xn, ctx);

  const vcl_mat dxn_iph = xn_ip1-xn;
  const vcl_mat dxn_imh = xn-xn_im1;
  const vcl_mat dxn_jph = xn_jp1-xn;
  const vcl_mat dxn_jmh = xn-xn_jm1;

  /*
  viennacl::ocl::program & fluxl_prog  = viennacl::ocl::current_context().get_program("fluxl");
  viennacl::ocl::kernel  & fluxl = fluxl_prog.get_kernel("fluxl");

  const vcl_mat r_i   = element_div(dxn_imh, dxn_iph);
  const vcl_mat r_ip1 = element_div(dxn_iph, (comfi::operators::ip1(xn_ip1, ctx)-xn_ip1));
  const vcl_mat r_im1 = element_div((xn_im1-comfi::operators::im1(xn_im1, ctx)), dxn_imh);
  const vcl_mat r_j   = element_div(dxn_jmh, dxn_jph);
  const vcl_mat r_jp1 = element_div(dxn_jph, (comfi::operators::jp1(xn_jp1, ctx)-xn_jp1));
  const vcl_mat r_jm1 = element_div((xn_jm1-comfi::operators::jm1(xn_jm1, ctx)), dxn_jmh);
  vcl_mat phi_i(xn.size1(), xn.size2()), phi_ip1(xn.size1(), xn.size2()), phi_im1(xn.size1(), xn.size2()),
          phi_j(xn.size1(), xn.size2()), phi_jp1(xn.size1(), xn.size2()), phi_jm1(xn.size1(), xn.size2());
  viennacl::ocl::enqueue(fluxl(r_i, phi_i,
                               cl_uint(r_i.size1()*r_i.size2())));
  viennacl::ocl::enqueue(fluxl(r_ip1, phi_ip1,
                               cl_uint(r_i.size1()*r_i.size2())));
  viennacl::ocl::enqueue(fluxl(r_im1, phi_im1,
                               cl_uint(r_i.size1()*r_i.size2())));
  viennacl::ocl::enqueue(fluxl(r_j, phi_j,
                               cl_uint(r_i.size1()*r_i.size2())));
  viennacl::ocl::enqueue(fluxl(r_jp1, phi_jp1,
                               cl_uint(r_i.size1()*r_i.size2())));
  viennacl::ocl::enqueue(fluxl(r_jm1, phi_jm1,
                               cl_uint(r_i.size1()*r_i.size2())));

  vcl_mat Lxn_iph = xn     + 0.5*element_prod(phi_i, dxn_imh);
  vcl_mat Lxn_imh = xn_im1 + 0.5*element_prod(phi_im1, dxn_imh);
  vcl_mat Rxn_iph = xn_ip1 - 0.5*element_prod(phi_ip1, dxn_iph);
  vcl_mat Rxn_imh = xn     - 0.5*element_prod(phi_i, dxn_iph);
  vcl_mat Lxn_jph = xn     + 0.5*element_prod(phi_j, dxn_jmh);
  vcl_mat Lxn_jmh = xn_jm1 + 0.5*element_prod(phi_jm1, dxn_jmh);
  vcl_mat Rxn_jph = xn_jp1 - 0.5*element_prod(phi_jp1, dxn_jph);
  vcl_mat Rxn_jmh = xn     - 0.5*element_prod(phi_j, dxn_jph);
  */

  static const vcl_mat eps = viennacl::scalar_matrix<double>(xn.size1(), xn.size2(), 1.e-100);
  const vcl_mat r_i   = element_div(dxn_imh, dxn_iph+eps);
  const vcl_mat r_ip1 = element_div(dxn_iph, (comfi::operators::ip1(xn_ip1, ctx)-xn_ip1)+eps);
  const vcl_mat r_im1 = element_div((xn_im1-comfi::operators::im1(xn_im1, ctx)), dxn_imh+eps);
  const vcl_mat r_j   = element_div(dxn_jmh, dxn_jph+eps);
  const vcl_mat r_jp1 = element_div(dxn_jph, (comfi::operators::jp1(xn_jp1, ctx)-xn_jp1)+eps);
  const vcl_mat r_jm1 = element_div((xn_jm1-comfi::operators::jm1(xn_jm1, ctx)), dxn_jmh+eps);
  //extrapolated cell edge variables
  vcl_mat Lxn_iph = xn     + 0.5*element_prod(comfi::routines::fluxl(r_i), dxn_imh);
  vcl_mat Lxn_imh = xn_im1 + 0.5*element_prod(comfi::routines::fluxl(r_im1), dxn_imh);
  vcl_mat Rxn_iph = xn_ip1 - 0.5*element_prod(comfi::routines::fluxl(r_ip1), dxn_iph);
  vcl_mat Rxn_imh = xn     - 0.5*element_prod(comfi::routines::fluxl(r_i), dxn_iph);
  vcl_mat Lxn_jph = xn     + 0.5*element_prod(comfi::routines::fluxl(r_j), dxn_jmh);
  vcl_mat Lxn_jmh = xn_jm1 + 0.5*element_prod(comfi::routines::fluxl(r_jm1), dxn_jmh);
  vcl_mat Rxn_jph = xn_jp1 - 0.5*element_prod(comfi::routines::fluxl(r_jp1), dxn_jph);
  vcl_mat Rxn_jmh = xn     - 0.5*element_prod(comfi::routines::fluxl(r_j), dxn_jph);

   // BOUNDARY CONDITIONS
  //mhdsim::routines::bottomBC(Lxn_jmh,Rxn_jmh,t,op,bg);
  //comfi::routines::bottombc_shock_tube(Lxn_jmh, Rxn_jmh, ctx);
  //comfi::routines::topbc_shock_tube(Lxn_jph, Rxn_jph, ctx);
  //comfi::routines::topbc_soler(Lxn_jph, Rxn_jph, op);
  //mhdsim::routines::topbc_driver(Lxn_jph, Rxn_jph, t, op);
  //comfi::routines::bottombc_soler(Lxn_jmh, Rxn_jmh, op);

  /* // Fast mode speed eigenvalues */
  /* vcl_mat Leig_iph_p = comfi::routines::fast_speed_x_mat(Lxn_iph, ctx); */
  /* vcl_mat Reig_iph_p = comfi::routines::fast_speed_x_mat(Rxn_iph, ctx); */
  /* vcl_mat Leig_jph_p = comfi::routines::fast_speed_z_mat(Lxn_jph, ctx); */
  /* vcl_mat Reig_jph_p = comfi::routines::fast_speed_z_mat(Rxn_jph, ctx); */
  /* vcl_mat Leig_imh_p = comfi::routines::fast_speed_x_mat(Lxn_imh, ctx); */
  /* vcl_mat Reig_imh_p = comfi::routines::fast_speed_x_mat(Rxn_imh, ctx); */
  /* vcl_mat Leig_jmh_p = comfi::routines::fast_speed_z_mat(Lxn_jmh, ctx); */
  /* vcl_mat Reig_jmh_p = comfi::routines::fast_speed_z_mat(Rxn_jmh, ctx); */

  /* viennacl::ocl::program & eig_prog  = viennacl::ocl::current_context().get_program("element_max"); */
  /* viennacl::ocl::kernel  & element_max = eig_prog.get_kernel("element_max"); */

  /* vcl_mat a_imh_p(Leig_iph_p.size1(), Leig_iph_p.size2()); */
  /* viennacl::ocl::enqueue(element_max(Leig_imh_p, Reig_imh_p, */
  /*                                    a_imh_p, */
  /*                                    cl_uint(Leig_imh_p.size1()))); */
  /* vcl_mat a_iph_p(Leig_iph_p.size1(), Leig_iph_p.size2()); */
  /* viennacl::ocl::enqueue(element_max(Leig_iph_p, Reig_iph_p, */
  /*                                    a_iph_p, */
  /*                                    cl_uint(Leig_iph_p.size1()))); */
  /* vcl_mat a_jmh_p(Leig_iph_p.size1(), Leig_iph_p.size2()); */
  /* viennacl::ocl::enqueue(element_max(Leig_jmh_p, Reig_jmh_p, */
  /*                                    a_jmh_p, */
  /*                                    cl_uint(Leig_jmh_p.size1()))); */
  /* vcl_mat a_jph_p(Leig_iph_p.size1(), Leig_iph_p.size2()); */
  /* viennacl::ocl::enqueue(element_max(Leig_jph_p, Reig_jph_p, */
  /*                                    a_jph_p, */
  /*                                    cl_uint(Leig_jph_p.size1()))); */
  /* Leig_iph_p = element_fabs(element_div(ctx.v_NVx(Lxn_iph), ctx.v_Np(Lxn_iph))); */
  /* viennacl::ocl::enqueue(element_max(Leig_iph_p, a_iph_p, */
  /*                                    a_iph_p, */
  /*                                    cl_uint(Leig_iph_p.size1()))); */

  /* Reig_iph_p = element_fabs(element_div(ctx.v_NVx(Rxn_iph), ctx.v_Np(Rxn_iph))); */
  /* viennacl::ocl::enqueue(element_max(Reig_iph_p, a_iph_p, */
  /*                                    a_iph_p, */
  /*                                    cl_uint(Reig_iph_p.size1()))); */
  /* Leig_jph_p = element_fabs(element_div(ctx.v_NVz(Lxn_jph), ctx.v_Np(Lxn_jph))); */
  /* viennacl::ocl::enqueue(element_max(Leig_jph_p, a_jph_p, */
  /*                                    a_jph_p, */
  /*                                    cl_uint(Leig_jph_p.size1()))); */
  /* Reig_jph_p = element_fabs(element_div(ctx.v_NVz(Rxn_jph), ctx.v_Np(Rxn_jph))); */
  /* viennacl::ocl::enqueue(element_max(Reig_jph_p, a_jph_p, */
  /*                                    a_jph_p, */
  /*                                    cl_uint(Reig_jph_p.size1()))); */
  /* Leig_imh_p = element_fabs(element_div(ctx.v_NVx(Lxn_imh), ctx.v_Np(Lxn_imh))); */
  /* viennacl::ocl::enqueue(element_max(Leig_imh_p, a_imh_p, */
  /*                                    a_imh_p, */
  /*                                    cl_uint(Leig_imh_p.size1()))); */
  /* Reig_imh_p = element_fabs(element_div(ctx.v_NVx(Rxn_imh), ctx.v_Np(Rxn_imh))); */
  /* viennacl::ocl::enqueue(element_max(Reig_imh_p, a_imh_p, */
  /*                                    a_imh_p, */
  /*                                    cl_uint(Reig_imh_p.size1()))); */
  /* Leig_jmh_p = element_fabs(element_div(ctx.v_NVz(Lxn_jmh), ctx.v_Np(Lxn_jmh))); */
  /* viennacl::ocl::enqueue(element_max(Leig_jmh_p, a_jmh_p, */
  /*                                    a_jmh_p, */
  /*                                    cl_uint(Leig_jmh_p.size1()))); */
  /* Reig_jmh_p = element_fabs(element_div(ctx.v_NVz(Rxn_jmh), ctx.v_Np(Rxn_jmh))); */
  /* viennacl::ocl::enqueue(element_max(Reig_jmh_p, a_jmh_p, */
  /*                                    a_jmh_p, */
  /*                                    cl_uint(Reig_jmh_p.size1()))); */

  /* vcl_mat Leig_iph_n = comfi::routines::sound_speed_neutral_mat(Lxn_iph, ctx); */
  /* vcl_mat Reig_iph_n = comfi::routines::sound_speed_neutral_mat(Rxn_iph, ctx); */
  /* vcl_mat Leig_jph_n = comfi::routines::sound_speed_neutral_mat(Lxn_jph, ctx); */
  /* vcl_mat Reig_jph_n = comfi::routines::sound_speed_neutral_mat(Rxn_jph, ctx); */
  /* vcl_mat Leig_imh_n = comfi::routines::sound_speed_neutral_mat(Lxn_imh, ctx); */
  /* vcl_mat Reig_imh_n = comfi::routines::sound_speed_neutral_mat(Rxn_imh, ctx); */
  /* vcl_mat Leig_jmh_n = comfi::routines::sound_speed_neutral_mat(Lxn_jmh, ctx); */
  /* vcl_mat Reig_jmh_n = comfi::routines::sound_speed_neutral_mat(Rxn_jmh, ctx); */

  /* vcl_mat a_imh_n(Leig_iph_n.size1(), Leig_iph_n.size2()); */
  /* if (ctx.bc_left != comfi::types::DIMENSIONLESS) { */
  /*   viennacl::ocl::enqueue(element_max(Leig_imh_n, Reig_imh_n, */
  /*                                      a_imh_n, */
  /*                                      cl_uint(Leig_imh_n.size1()))); */
  /* } */
  /* vcl_mat a_iph_n(Leig_iph_n.size1(), Leig_iph_n.size2()); */
  /* if (ctx.bc_right != comfi::types::DIMENSIONLESS) { */
  /*   viennacl::ocl::enqueue(element_max(Leig_iph_n, Reig_iph_n, */
  /*                                      a_iph_n, */
  /*                                      cl_uint(Leig_iph_n.size1()))); */
  /* } */
  /* vcl_mat a_jmh_n(Leig_iph_n.size1(), Leig_iph_n.size2()); */
  /* if (ctx.bc_down != comfi::types::DIMENSIONLESS) { */
  /*   viennacl::ocl::enqueue(element_max(Leig_jmh_n, Reig_jmh_n, */
  /*                                      a_jmh_n, */
  /*                                      cl_uint(Leig_jmh_n.size1()))); */
  /* } */
  /* vcl_mat a_jph_n(Leig_iph_n.size1(), Leig_iph_n.size2()); */
  /* if (ctx.bc_up != comfi::types::DIMENSIONLESS) { */
  /*   viennacl::ocl::enqueue(element_max(Leig_jph_n, Reig_jph_n, */
  /*                                      a_jph_n, */
  /*                                      cl_uint(Leig_jph_n.size1()))); */
  /* } */

  /* Reig_jph_n = element_fabs(element_div(ctx.v_NUz(Rxn_jph), ctx.v_Nn(Rxn_jph))); */
  /* viennacl::ocl::enqueue(element_max(Reig_jph_n, a_jph_n, */
  /*                                    a_jph_n, */
  /*                                    cl_uint(Reig_jph_n.size1()))); */
  /* Reig_jmh_n = element_fabs(element_div(ctx.v_NUz(Rxn_jmh), ctx.v_Nn(Rxn_jmh))); */
  /* viennacl::ocl::enqueue(element_max(Reig_jmh_n, a_jmh_n, */
  /*                                    a_jmh_n, */
  /*                                    cl_uint(Reig_jmh_n.size1()))); */
  /* Reig_imh_n = element_fabs(element_div(ctx.v_NUx(Rxn_imh), ctx.v_Nn(Rxn_imh))); */
  /* viennacl::ocl::enqueue(element_max(Reig_imh_n, a_imh_n, */
  /*                                    a_imh_n, */
  /*                                    cl_uint(Reig_imh_n.size1()))); */
  /* Reig_iph_n = element_fabs(element_div(ctx.v_NUx(Rxn_iph), ctx.v_Nn(Rxn_iph))); */
  /* viennacl::ocl::enqueue(element_max(Reig_iph_n, a_iph_n, */
  /*                                    a_iph_n, */
  /*                                    cl_uint(Reig_iph_n.size1()))); */
  /* Leig_jph_n = element_fabs(element_div(ctx.v_NUz(Lxn_jph), ctx.v_Nn(Lxn_jph))); */
  /* viennacl::ocl::enqueue(element_max(Leig_jph_n, a_jph_n, */
  /*                                    a_jph_n, */
  /*                                    cl_uint(Leig_jph_n.size1()))); */
  /* Leig_jmh_n = element_fabs(element_div(ctx.v_NUz(Lxn_jmh), ctx.v_Nn(Lxn_jmh))); */
  /* viennacl::ocl::enqueue(element_max(Leig_jmh_n, a_jmh_n, */
  /*                                    a_jmh_n, */
  /*                                    cl_uint(Leig_jmh_n.size1()))); */
  /* Leig_imh_n = element_fabs(element_div(ctx.v_NUx(Lxn_imh), ctx.v_Nn(Lxn_imh))); */
  /* viennacl::ocl::enqueue(element_max(Leig_imh_n, a_imh_n, */
  /*                                    a_imh_n, */
  /*                                    cl_uint(Leig_imh_n.size1()))); */
  /* Leig_iph_n = element_fabs(element_div(ctx.v_NUx(Lxn_iph), ctx.v_Nn(Lxn_iph))); */
  /* viennacl::ocl::enqueue(element_max(Leig_iph_n, a_iph_n, */
  /*                                    a_iph_n, */
  /*                                    cl_uint(Leig_iph_n.size1()))); */

  const vcl_mat a_imh = element_fabs(build_eig_matrix_x(0.5*(Lxn_imh+Rxn_imh), ctx));
  const vcl_mat a_iph = element_fabs(build_eig_matrix_x(0.5*(Lxn_iph+Rxn_iph), ctx));
  const vcl_mat a_jmh = element_fabs(build_eig_matrix_z(0.5*(Lxn_jmh+Rxn_jmh), ctx));
  const vcl_mat a_jph = element_fabs(build_eig_matrix_z(0.5*(Lxn_jph+Rxn_jph), ctx));

  // LAX-FRIEDRICHS FLUX
  const vcl_mat Fximh = 0.5*(comfi::routines::Fx(Lxn_imh, xn, ctx)+comfi::routines::Fx(Rxn_imh, xn, ctx))
                             -element_prod(a_imh, (Rxn_imh-Lxn_imh));
  const vcl_mat Fxiph = 0.5*(comfi::routines::Fx(Lxn_iph, xn, ctx)+comfi::routines::Fx(Rxn_iph, xn, ctx))
                             -element_prod(a_iph, (Rxn_iph-Lxn_iph));
  const vcl_mat Fzjmh = 0.5*(comfi::routines::Fz(Lxn_jmh, xn, ctx)+comfi::routines::Fz(Rxn_jmh, xn, ctx))
                             -element_prod(a_jmh, (Rxn_jmh-Lxn_jmh));
  const vcl_mat Fzjph = 0.5*(comfi::routines::Fz(Lxn_jph, xn, ctx)+comfi::routines::Fz(Rxn_jph, xn, ctx))
                             -element_prod(a_jph, (Rxn_jph-Lxn_jph));

  return -1.0*(Fxiph-Fximh)/ctx.dx
         -1.0*(Fzjph-Fzjmh)/ctx.dz;
         //+ prod(op.f2V, v_collission_source)
         //- prod(op.f2U, v_collission_source)
         //+ prod(op.f2U, u_collission_source)
         //- prod(op.f2V, u_collission_source);
         //+ prod(op.f2V, me_nu_J)
         //+ prod(op.f2V, vsource)
         //- prod(op.f2U, me_nu_J)
         //- prod(op.f2U, vsource)
         //- prod(op.SG, xn)
         //+ boundaryconditions
         //+ prod(op.s2Np, isource)
         //- prod(op.s2Nn, isource)
         //+ prod(op.s2Tp, TpdivV)/3.0;
         //+ prod(op.s2Tn, TndivU)/3.0;
         //+ prod(op.s2Tp, nuin_dV2)/3.0
         //+ prod(op.s2Tn, nuni_dV2)/3.0
         //- two_thirds*prod(op.s2Tn, L)
         //+ prod(op.f2B, gNpxJxBoverN2) // Hall term source term
         //- prod(op.f2B,gradrescrossJ);
}

vcl_mat comfi::routines::computeRHS_RK4(const vcl_mat &xn, comfi::types::Context &ctx)
{
  const double dt = ctx.dt();
  // RK-4
  const vcl_mat k1 = Re_MUSCL(xn, ctx)*dt; //return xn+k1;
  //const vcl_mat k2 = Re_MUSCL(xn+0.5*k1,t+0.5*dt,op,bg)*dt;
  const vcl_mat k2 = Re_MUSCL(xn+0.5*k1, ctx)*dt;
  //const vcl_mat k3 = Re_MUSCL(xn+0.5*k2,t+0.5*dt,op,bg)*dt;
  const vcl_mat k3 = Re_MUSCL(xn+0.5*k2, ctx)*dt;
  //const vcl_mat k4 = Re_MUSCL(xn+k3,t+dt,op,bg)*dt;
  const vcl_mat k4 = Re_MUSCL(xn+k3, ctx)*dt;

  vcl_mat result = xn + (k1+2.0*k2+2.0*k3+k4)/6.0;

  // GLM exact solution
  ctx.v_GLM(result) *= std::exp(-ctx.alpha_p*ctx.dt()*ctx.c_h()/ctx.ds);

  return result;
}

vcl_mat comfi::routines::computeRHS_Euler(const vcl_mat &xn, comfi::types::Context &ctx)
{
  // Simple Eulerian Steps
  vcl_mat result = xn + comfi::routines::Re_MUSCL(xn, ctx)*ctx.dt();

  // GLM exact solution
  ctx.v_GLM(result) *= std::exp(-ctx.alpha_p*ctx.dt()*ctx.c_h()/ctx.ds);

  return result;
}

/*
vcl_vec comfi::routines::computeRHS_BDF2(const vcl_vec &xn,
                                         const vcl_vec &xn1,
                                         const vcl_sp_mat &Ri,
                                         const double alpha,
                                         const double beta,
                                         const double dt,
                                         const double t,
                                         comfi::types::Operators &op,
                                         const comfi::types::BgData &bg)
{
  // BDF2
  static double dtn1 = dt;

  const vcl_vec Rin = prod(Ri,xn);
  const vcl_vec Re = comfi::routines::Re_MUSCL(xn,t,op,bg);
  const vcl_vec xnpRedt = xn+Re*dt;
  const vcl_vec result = xnpRedt
                       + alpha*dt*(((xn-xn1)/dtn1)-Re)
                       + beta*dt*Rin;

  //GLM
  const double a = 0.1;
  vcl_vec glm = prod(op.GLMs,xnpRedt);
  glm *= std::exp(-a*op.ch/(ds/dt));

  dtn1=dt;
  return prod(op.ImGLM,result) + prod(op.s2GLM,glm);
}
*/

vcl_mat comfi::routines::Fx(const vcl_mat &xn, const vcl_mat &xn_ij, comfi::types::Context &ctx)
{
  vcl_mat F = viennacl::zero_matrix<double>(xn.size1(), xn.size2());

  const vcl_mat V_x = element_div(ctx.v_NVx(xn), ctx.v_Np(xn));
  const vcl_mat U_x = element_div(ctx.v_NUx(xn), ctx.v_Nn(xn));
  const vcl_mat V_z = element_div(ctx.v_NVz(xn), ctx.v_Np(xn));
  const vcl_mat U_z = element_div(ctx.v_NUz(xn), ctx.v_Nn(xn));
  const vcl_mat V_p = element_div(ctx.v_NVp(xn), ctx.v_Np(xn));
  const vcl_mat U_p = element_div(ctx.v_NUp(xn), ctx.v_Nn(xn));

  // Local speed flux -> quantity*Vz
  ctx.v_Np(F) = element_prod(ctx.v_Np(xn), V_x);
  ctx.v_Nn(F) = element_prod(ctx.v_Nn(xn), U_x);
  ctx.v_NVx(F) = element_prod(ctx.v_NVx(xn), V_x);
  ctx.v_NVz(F) = element_prod(ctx.v_NVz(xn), V_x);
  ctx.v_NVp(F) = element_prod(ctx.v_NVp(xn), V_x);
  ctx.v_NUx(F) = element_prod(ctx.v_NUx(xn), U_x);
  ctx.v_NUz(F) = element_prod(ctx.v_NUz(xn), U_x);
  ctx.v_NUp(F) = element_prod(ctx.v_NUp(xn), U_x);
  ctx.v_Ep(F) = element_prod(ctx.v_Ep(xn), V_x);
  ctx.v_En(F) = element_prod(ctx.v_En(xn), U_x);

  // Induction VB-BV
  ctx.v_Bz(F) = element_prod(V_x, ctx.v_Bz(xn)) - element_prod(ctx.v_Bx(xn), V_z);
  ctx.v_Bp(F) = element_prod(V_x, ctx.v_Bp(xn)) - element_prod(ctx.v_Bx(xn), V_p);

  // General Lagrange Multiplier
  ctx.v_Bx(F) = ctx.v_GLM(xn);

  // Thermal pressure
  vcl_mat Pp = comfi::routines::pressure_p(xn, ctx);
  ctx.v_NVx(F) += Pp;
  vcl_mat Pn = comfi::routines::pressure_n(xn, ctx);
  ctx.v_NUx(F) += Pn;

  // Magnetic pressure
  const vcl_mat pmag = 0.5*(element_prod(ctx.v_Bx(xn), ctx.v_Bx(xn))
                            + element_prod(ctx.v_Bz(xn), ctx.v_Bz(xn))
                            + element_prod(ctx.v_Bp(xn), ctx.v_Bp(xn)));
  ctx.v_NVx(F) += pmag;
  ctx.v_NVz(F) -= element_prod(ctx.v_Bz(xn), ctx.v_Bz(xn));
  ctx.v_NVx(F) -= element_prod(ctx.v_Bz(xn), ctx.v_Bx(xn));
  ctx.v_NVp(F) -= element_prod(ctx.v_Bz(xn), ctx.v_Bp(xn));

  // Energy flux
  vcl_mat bdotv = element_prod(V_x, ctx.v_Bx(xn)) + element_prod(V_z, ctx.v_Bz(xn)) + element_prod(V_p, ctx.v_Bp(xn));
  ctx.v_Ep(F) += element_prod(Pp+pmag, V_x) - element_prod(ctx.v_Bx(xn), bdotv);
  ctx.v_En(F) += element_prod(Pn, U_x);

  // Flux part of GLM
  ctx.v_GLM(F) = ctx.c_h()*ctx.c_h()*ctx.v_Bx(xn);

  return F;
}

vcl_mat comfi::routines::Fz(const vcl_mat &xn, const vcl_mat &xn_ij, comfi::types::Context &ctx)
{
  vcl_mat F = viennacl::zero_matrix<double>(xn.size1(), xn.size2());

  const vcl_mat V_x = element_div(ctx.v_NVx(xn), ctx.v_Np(xn));
  const vcl_mat U_x = element_div(ctx.v_NUx(xn), ctx.v_Nn(xn));
  const vcl_mat V_z = element_div(ctx.v_NVz(xn), ctx.v_Np(xn));
  const vcl_mat U_z = element_div(ctx.v_NUz(xn), ctx.v_Nn(xn));
  const vcl_mat V_p = element_div(ctx.v_NVp(xn), ctx.v_Np(xn));
  const vcl_mat U_p = element_div(ctx.v_NUp(xn), ctx.v_Nn(xn));

  // Local speed flux -> quantity*Vz
  ctx.v_Np(F) = element_prod(ctx.v_Np(xn), V_z);
  ctx.v_Nn(F) = element_prod(ctx.v_Nn(xn), U_z);
  ctx.v_NVx(F) = element_prod(ctx.v_NVx(xn), V_z);
  ctx.v_NVz(F) = element_prod(ctx.v_NVz(xn), V_z);
  ctx.v_NVp(F) = element_prod(ctx.v_NVp(xn), V_z);
  ctx.v_NUx(F) =  element_prod(ctx.v_NUx(xn), U_z);
  ctx.v_NUz(F) = element_prod(ctx.v_NUz(xn), U_z);
  ctx.v_NUp(F) = element_prod(ctx.v_NUp(xn), U_z);
  ctx.v_Ep(F) = element_prod(ctx.v_Ep(xn), V_z);
  ctx.v_En(F) = element_prod(ctx.v_En(xn), U_z);

  // Induction VB-BV
  ctx.v_Bx(F) = element_prod(V_z, ctx.v_Bx(xn)) - element_prod(ctx.v_Bz(xn), V_x);
  ctx.v_Bp(F) = element_prod(V_z, ctx.v_Bp(xn)) - element_prod(ctx.v_Bz(xn), V_p);

  // General Lagrange Multiplier
  ctx.v_Bz(F) = ctx.v_GLM(xn);

  // Thermal pressure
  vcl_mat Pp = comfi::routines::pressure_p(xn, ctx);
  ctx.v_NVz(F) += Pp;
  vcl_mat Pn = comfi::routines::pressure_n(xn, ctx);
  ctx.v_NUz(F) += Pn;

  // Magnetic pressure
  const vcl_mat pmag = 0.5*(element_prod(ctx.v_Bx(xn), ctx.v_Bx(xn))
                            +element_prod(ctx.v_Bz(xn), ctx.v_Bz(xn))
                            +element_prod(ctx.v_Bp(xn), ctx.v_Bp(xn)));
  ctx.v_NVz(F) += pmag;
  ctx.v_NVz(F) -= element_prod(ctx.v_Bz(xn), ctx.v_Bz(xn));
  ctx.v_NVx(F) -= element_prod(ctx.v_Bz(xn), ctx.v_Bx(xn));
  ctx.v_NVp(F) -= element_prod(ctx.v_Bz(xn), ctx.v_Bp(xn));

  // Energy flux
  vcl_mat bdotv = element_prod(V_x, ctx.v_Bx(xn))
                  + element_prod(V_z, ctx.v_Bz(xn))
                  + element_prod(V_p, ctx.v_Bp(xn));
  ctx.v_Ep(F) += element_prod(Pp+pmag, V_z) - element_prod(ctx.v_Bz(xn), bdotv);
  ctx.v_En(F) += element_prod(Pn, U_z);

  // Flux part of GLM
  ctx.v_GLM(F) = ctx.c_h()*ctx.c_h()*ctx.v_Bz(xn);

  return F;
}

vcl_mat comfi::routines::pressure_n(const vcl_mat &xn, comfi::types::Context &ctx) {
  vcl_mat Pn = ctx.v_NUx(xn);
  Pn = element_prod(Pn, ctx.v_NUx(xn));
  Pn = Pn + element_prod(ctx.v_NUz(xn), ctx.v_NUz(xn));
  Pn = Pn + element_prod(ctx.v_NUp(xn), ctx.v_NUp(xn));
  Pn = 0.5*element_div(Pn, ctx.v_Nn(xn));
  Pn  = element_fabs((ctx.gammamono-1.0)*(ctx.v_En(xn)-Pn));
  return Pn;
}

vcl_mat comfi::routines::sound_speed_p(const vcl_mat &xn, comfi::types::Context &ctx) {
  const vcl_mat Pp = comfi::routines::pressure_p(xn, ctx);
  return element_sqrt(element_div(ctx.gammamono*Pp, ctx.v_Np(xn)));
}

vcl_mat comfi::routines::sound_speed_n(const vcl_mat &xn, comfi::types::Context &ctx) {
  const vcl_mat Pn = comfi::routines::pressure_n(xn, ctx);
  return element_sqrt(element_div(ctx.gammamono*Pn, ctx.v_Nn(xn)));
}

vcl_mat comfi::routines::pressure_p(const vcl_mat &xn, comfi::types::Context &ctx)
{
  using namespace viennacl::linalg;
  // Calculate pressures by total energy
  vcl_mat Pp = ctx.v_NVx(xn);
  Pp = element_prod(Pp, ctx.v_NVx(xn));
  Pp = Pp + element_prod(ctx.v_NVz(xn), ctx.v_NVz(xn));
  Pp = Pp + element_prod(ctx.v_NVp(xn), ctx.v_NVp(xn));
  Pp = 0.5*element_div(Pp, ctx.v_Np(xn));

  Pp = Pp + 0.5*element_prod(ctx.v_Bz(xn), ctx.v_Bz(xn));
  Pp = Pp + 0.5*element_prod(ctx.v_Bx(xn), ctx.v_Bx(xn));
  Pp = Pp + 0.5*element_prod(ctx.v_Bp(xn), ctx.v_Bp(xn));

  Pp = element_fabs((ctx.gammamono-1.0)*(ctx.v_Ep(xn)-Pp));

  return Pp;
}

vcl_mat comfi::routines::fast_speed_x(const vcl_mat &xn, comfi::types::Context &ctx) {
  using namespace viennacl::linalg;
  // Calculate pressures by total energy
  vcl_mat k_e = element_prod(ctx.v_NVx(xn), ctx.v_NVx(xn));
  k_e = k_e + element_prod(ctx.v_NVz(xn), ctx.v_NVz(xn));
  k_e = k_e + element_prod(ctx.v_NVp(xn), ctx.v_NVp(xn));
  k_e = 0.5*element_div(k_e, ctx.v_Np(xn));

  vcl_mat b_e = 0.5*element_prod(ctx.v_Bx(xn), ctx.v_Bx(xn));
  b_e = b_e + 0.5*element_prod(ctx.v_Bz(xn), ctx.v_Bz(xn));
  b_e = b_e + 0.5*element_prod(ctx.v_Bp(xn), ctx.v_Bp(xn));

  const vcl_mat Ep = element_fabs(ctx.v_Ep(xn));
  const vcl_mat Pp  = element_fabs((ctx.gammamono-1.0)*(Ep - k_e - b_e));

  const vcl_mat cps2 = ctx.gammamono*(element_div(Pp, ctx.v_Np(xn)));
  const vcl_mat cps = element_sqrt(cps2);
  const vcl_mat ca2 = element_div(2.0*b_e, ctx.v_Np(xn));
  const vcl_mat cax = element_div(ctx.v_Bx(xn), element_sqrt(ctx.v_Np(xn)));
  const vcl_mat cpsca = element_prod(cps, cax);
  const vcl_mat cpsca2 = element_prod(cpsca, cpsca);

  const vcl_mat cp = 0.5*(element_sqrt(2.0*(cps2 + ca2 + element_sqrt(element_prod(cps2+ca2,cps2+ca2)-4.0*cpsca2))));

  return cp;
}

vcl_mat comfi::routines::fast_speed_z(const vcl_mat &xn, comfi::types::Context &ctx) {
  using namespace viennacl::linalg;
  // Calculate pressures by total energy
  vcl_mat k_e = element_prod(ctx.v_NVx(xn), ctx.v_NVx(xn));
  k_e = k_e + element_prod(ctx.v_NVz(xn), ctx.v_NVz(xn));
  k_e = k_e + element_prod(ctx.v_NVp(xn), ctx.v_NVp(xn));
  k_e = 0.5*element_div(k_e, ctx.v_Np(xn));

  vcl_mat b_e = 0.5*element_prod(ctx.v_Bx(xn), ctx.v_Bx(xn));
  b_e = b_e + 0.5*element_prod(ctx.v_Bz(xn), ctx.v_Bz(xn));
  b_e = b_e + 0.5*element_prod(ctx.v_Bp(xn), ctx.v_Bp(xn));

  const vcl_mat Ep = element_fabs(ctx.v_Ep(xn));
  const vcl_mat Pp  = element_fabs((ctx.gammamono-1.0)*(Ep - k_e - b_e));

  const vcl_mat cps2 = ctx.gammamono*(element_div(Pp, ctx.v_Np(xn)));
  const vcl_mat cps = element_sqrt(cps2);
  const vcl_mat ca2 = element_div(2.0*b_e, ctx.v_Np(xn));
  const vcl_mat caz = element_div(ctx.v_Bz(xn), element_sqrt(ctx.v_Np(xn)));
  const vcl_mat cpsca = element_prod(cps, caz);
  const vcl_mat cpsca2 = element_prod(cpsca, cpsca);

  const vcl_mat cp = 0.5*(element_sqrt(2.0*(cps2 + ca2 + element_sqrt(element_prod(cps2+ca2,cps2+ca2)-4.0*cpsca2))));

  return cp;
}

vcl_vec comfi::routines::polyval(const arma::vec &p, const vcl_vec &x)
{
  vcl_vec b = viennacl::zero_vector<double>(x.size());

  for (int i = 0; i < p.size(); i++)
  {
    const vcl_vec a = viennacl::scalar_vector<double>(x.size(), p(i));
    b = a + element_prod(b, x);
  }

  return b;
}

vcl_mat comfi::routines::fluxl(const vcl_mat &r) {
  static const vcl_mat ones = viennacl::scalar_matrix<double>(r.size1(), r.size2(), 1.0);
  const vcl_mat r2 = element_prod(r, r);
  // Ospre
  return 1.5*element_div(r2+r, r2+r+ones);
  // Van Albada
  //return element_div(r2+r, r2+ones);
  // Van Leer
  //const vcl_mat absr = element_fabs(r);
  //return element_div(r+absr, ones+absr);
}

void comfi::routines::bottombc_shock_tube(vcl_mat &Lxn, vcl_mat &Rxn, comfi::types::Context &ctx) {
  uint ij = inds(0, 0, ctx);

  Rxn(ij, ctx.n_n) = 0.125;
  Rxn(ij, ctx.n_p) = 0.125;
  Rxn(ij, ctx.E_p) = 0.1/(ctx.gammamono-1.0);
  Rxn(ij, ctx.E_n) = 0.1/(ctx.gammamono-1.0);
  Rxn(ij, ctx.Ux) = 0.0;
  Rxn(ij, ctx.Uz) = 0.0;
  Rxn(ij, ctx.Up) = 0.0;
  Rxn(ij, ctx.Vx) = 0.0;
  Rxn(ij, ctx.Vz) = 0.0;
  Rxn(ij, ctx.Vp) = 0.0;

  Lxn(ij, ctx.n_n) = 0.125;
  Lxn(ij, ctx.n_p) = 0.125;
  Lxn(ij, ctx.E_p) = 0.1/(ctx.gammamono-1.0);
  Lxn(ij, ctx.E_n) = 0.1/(ctx.gammamono-1.0);
  Lxn(ij, ctx.Ux) = 0.0;
  Lxn(ij, ctx.Uz) = 0.0;
  Lxn(ij, ctx.Up) = 0.0;
  Lxn(ij, ctx.Vx) = 0.0;
  Lxn(ij, ctx.Vz) = 0.0;
  Lxn(ij, ctx.Vp) = 0.0;
}

void comfi::routines::topbc_shock_tube(vcl_mat &Lxn, vcl_mat &Rxn, comfi::types::Context &ctx) {
  uint ij = inds(0, ctx.nz-1, ctx);

  Rxn(ij, ctx.n_n) = 1.0;
  Rxn(ij, ctx.n_p) = 1.0;
  Rxn(ij, ctx.E_p) = 1.0/(ctx.gammamono-1.0);
  Rxn(ij, ctx.E_n) = 1.0/(ctx.gammamono-1.0);
  Rxn(ij, ctx.Ux) = 0.0;
  Rxn(ij, ctx.Uz) = 0.0;
  Rxn(ij, ctx.Up) = 0.0;
  Rxn(ij, ctx.Vx) = 0.0;
  Rxn(ij, ctx.Vz) = 0.0;
  Rxn(ij, ctx.Vp) = 0.0;

  Lxn(ij, ctx.n_n) = 1.0;
  Lxn(ij, ctx.n_p) = 1.0;
  Lxn(ij, ctx.E_p) = 1.0/(ctx.gammamono-1.0);
  Lxn(ij, ctx.E_n) = 1.0/(ctx.gammamono-1.0);
  Lxn(ij, ctx.Ux) = 0.0;
  Lxn(ij, ctx.Uz) = 0.0;
  Lxn(ij, ctx.Up) = 0.0;
  Lxn(ij, ctx.Vx) = 0.0;
  Lxn(ij, ctx.Vz) = 0.0;
  Lxn(ij, ctx.Vp) = 0.0;
}

/*
vim: tabstop=2
vim: shiftwidth=2
vim: smarttab
vim: expandtab
*/


#include "WLS.h"
#include <stdio.h>
#include <stdlib.h>
#include <Eigen/Sparse>

void WeightedLeastSquare(cv::Mat& resImg, const cv::Mat& img_guide, const cv::Mat& img_color,
	float alpha, float lamda)
{

	float epsilon = 0.0001f;

	cv::Mat grayImgF = cv::Mat::zeros(img_guide.size(), CV_32FC1);
	cv::cvtColor(img_guide, grayImgF, CV_BGR2GRAY);

	cv::Mat gradWeightX = cv::Mat::zeros(img_guide.size(), CV_32FC1);
	cv::Mat gradWeightY = cv::Mat::zeros(img_guide.size(), CV_32FC1);

#pragma omp parallel for
	for (int y = 0; y < img_guide.rows - 1; ++y)
	{
		for (int x = 0; x < img_guide.cols - 1; ++x)
		{
			if (x + 1 < img_guide.cols)
			{
				float gx = grayImgF.at<float>(y, x + 1) - grayImgF.at<float>(y, x);
				gradWeightX.at<float>(y, x) = lamda / (pow(abs(gx), alpha) + epsilon);
			}
			if (y + 1 < img_guide.rows)
			{
				float gy = grayImgF.at<float>(y + 1, x) - grayImgF.at<float>(y, x);
				gradWeightY.at<float>(y, x) = lamda / (pow(abs(gy), alpha) + epsilon);
			}
		}
	}
	//prepare
	int width = img_color.cols;
	int height = img_color.rows;
	int size = width * height;
	int n = width * height;

	//matrix
	Eigen::SparseMatrix<float> A(n,n);
	Eigen::VectorXi nnzVec = Eigen::VectorXi::Constant(n, 4 + 1);
	Eigen::VectorXf bs[3], xs[3];
	A.reserve(nnzVec);
	bs[0].resize(n);
	bs[1].resize(n);
	bs[2].resize(n);

	xs[0].resize(n);
	xs[1].resize(n);
	xs[2].resize(n);

	for (int y = 0; y < height; y++)
	{
		for (int x = 0; x < width; x++)
		{
			float a[5];
			a[0] = a[1] = a[2] = a[3] = a[4] = 0.0f;

			int ii = y * width + x;
			if (y - 1 >= 0) // top
			{
				const float gyw = gradWeightY.at<float>(y - 1, x);
				a[2] += 1.0f * gyw;
				a[0] -= 1.0f * gyw;
				A.insert(ii, ii - width) = a[0];
			}
			if (x - 1 >= 0) // left
			{
				const float gxw = gradWeightX.at<float>(y, x - 1);
				a[2] += 1.0f * gxw;
				a[1] -= 1.0f * gxw;
				A.insert(ii, ii - 1) = a[1];
			}
			if (x + 1 < width) // right
			{
				const float gxw = gradWeightX.at<float>(y, x);
				a[2] += 1.0f * gxw;
				a[3] -= 1.0f * gxw;
				A.insert(ii, ii + 1) = a[3];
			}
			if (y + 1 < height) // bottom
			{
				const float gyw = gradWeightY.at<float>(y, x);
				a[2] += 1.0f * gyw;
				a[4] -= 1.0f * gyw;
				A.insert(ii, ii + width) = a[4];
			}

			// data term
			a[2] += 1.f;
			A.insert(ii, ii) = a[2];

			const cv::Vec3f& col = img_color.at<cv::Vec3f>(y, x);
			xs[0][ii] = 0.0f;
			xs[1][ii] = 0.0f;
			xs[2][ii] = 0.0f;
			bs[0][ii] = (float)col[0];
			bs[1][ii] = (float)col[1];
			bs[2][ii] = (float)col[2];
		}
	}

#pragma omp parallel for
	for (int ch = 0; ch < 3; ++ch)
	{
		Eigen::SimplicialLLT<Eigen::SparseMatrix<float> > solver;
		solver.compute(A);
		xs[ch] = solver.solve(bs[ch]);
	}

	//paste
	resImg = cv::Mat(height, width, CV_32FC3);

#pragma omp parallel for
	for (int y = 0; y < height; y++)
	{
		for (int x = 0; x < width; x++)
		{
			resImg.at<cv::Vec3f>(y, x) = cv::Vec3f(
				xs[0][y * width + x],
				xs[1][y * width + x],
				xs[2][y * width + x]);

		}
	}

}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/arch/common/detail/generic/sinh_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/any.hpp>
#include <boost/simd/function/average.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( sinhcosh_
                             , (typename A0, typename X)
                             , bd::cpu_
                             , bs::pack_< bd::floating_<A0>, X>
                             )
  {
    using result_t = std::pair<A0, A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 const& a0) const
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 1 sinh is computed using a polynomial(float)
      // respectively rational(double) approx from cephes.
      // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
      // computed
      // *  in the first case sinh is (e-rec(e))/2 and cosh (e+rec(e))/2
      // *  in the second     sinh and cosh are (e/2)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2 defined in Maxshlog
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      auto lt1= is_less(x, One<A0>());
      A0 bts = bitofsign(a0);
      A0 s = Zero<A0>();
      if( bs::any(lt1))
      {
        s = detail::sinh_kernel<A0>::compute(x, sqr(x));
      }
      auto test1 = is_greater(x, Maxlog<A0>()-Log_2<A0>());
      A0 fac = if_else(test1, Half<A0>(), One<A0>());
      A0 tmp = exp(x*fac);
      A0 tmp1 = Half<A0>()*tmp;
      A0 rtmp = rec(tmp);
      A0 r = if_else(test1, tmp1*tmp, tmp1-Half<A0>()*rtmp);
      return { bitwise_xor(if_else(lt1, s, r), bts), if_else(test1, r, bs::average(tmp, rtmp))};
    }
  };

} } }

#endif


/******************************************************************
 * Created by Wei on 3/12/17.
 *
 * This is an implementation of this paper:
 * "Sparse Inverse Covariance Estimation with the Graphical Lasso"
 *
 * All names of parameters comply with the notaions in the paper.
 ******************************************************************/

#include "GraphicalLasso.hpp"

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>
#include <map>

#ifdef BAZEL
#include "Models/LinearRegression.hpp"
#include "Models/Model.hpp"
#else
#include "../Models/LinearRegression.hpp"
#include "../Models/Model.hpp"
#endif

using namespace Eigen;
using namespace std;

GraphicalLasso::GraphicalLasso(const unordered_map<string, string>& opts) {
    try {
        tolerance = stod(opts.at("tolerance"));
    } catch(std::out_of_range& oor) {
        tolerance = default_tolerance;
    }
    try {
        learningRate = stod(opts.at("learning_rate"));
    } catch(std::out_of_range& oor) {
        learningRate = default_learning_rate;
    }
}

GraphicalLasso::GraphicalLasso() {
    learningRate = default_learning_rate;
    tolerance = default_tolerance;
}

/*void GraphicalLasso::stop() {
    shouldStop = true;
}*/

void GraphicalLasso::setTolerance(float tol) {
    tolerance = tol;
}

void GraphicalLasso::assertReadyToRun() {
    return;    // there is no data that cannot be inferred
}

void GraphicalLasso::setUpRun() {
    mtx.lock();
    isRunning = true;
    progress = 0.0;
    shouldStop = false;
}

void GraphicalLasso::finishRun() {
    isRunning = false;
    progress = 1.0;
    mtx.unlock();
}

void GraphicalLasso::setLearningRate(float lr) {
    learningRate = lr;
}

MatrixXf GraphicalLasso::matrixRaiseToHalf(MatrixXf& matrix) {
    BDCSVD<MatrixXf> svd(matrix, ComputeThinU | ComputeThinV);
    MatrixXf D = svd.singularValues().array().sqrt().matrix().asDiagonal();
    MatrixXf U = svd.matrixU();
    return U * D * U.transpose();
}

void GraphicalLasso::run(shared_ptr<LinearRegression> model) {
    MatrixXf X = model->getX();
    int num_samples = X.rows();
    int num_features = X.cols();
    stdNormalize(X);
    MatrixXf S = X.transpose() * X / num_samples;
    MatrixXf W = S;
    MatrixXf W_inv = Math::getInstance().pseudoInverse(W);
    MatrixXf W_inv_old = W_inv;
    float regularizer = model->getL1_reg();

    for (int epoch = 0; epoch < maxIteration; epoch++) {
        progress = static_cast<float>(epoch) / maxIteration;
        for (int idx = 0; idx < num_features; idx++) {
            // partition matrixes into blocks
            vector<MatrixXf> S_blocks = partitionBlocks(S);
            MatrixXf W_11 = partitionBlocks(W)[0];
            MatrixXf W_inv_11 = partitionBlocks(W_inv)[0];

            // find W_11 ^ (1/2)
            MatrixXf W_11_half = matrixRaiseToHalf(W_11);
            MatrixXf b = Math::getInstance().pseudoInverse(W_11_half) * S_blocks[1];

            // run linear regression with lasso
            MatrixXf beta = fit(model, W_11_half, b);
            MatrixXf W_12 = W_11 * beta;

            // recover W_inv (W_inv corresponds to theta in the paper)
            float S_22 = S_blocks.back()(0, 0);
            float W_22 = S_22 + regularizer;
            float W_inv_22 = 1 / (W_22 - (W_12.transpose() * beta)(0, 0));
            MatrixXf W_inv_12 = -W_inv_22 * beta;

            W_inv = composeBlocks(W_inv_11, W_inv_12, W_inv_22);
            W = composeBlocks(W_11, W_12, W_22);
            S = composeBlocks(S_blocks[0], S_blocks[1], S_blocks[2](0, 0));
        }
        float diff = (W_inv - W_inv_old).norm();
        if (diff < tolerance) {
            break;
        } else {
            W_inv_old = W_inv;
        }
    }
    for (long col = 0; col < W_inv.cols(); col++) {
        for (long row = 0; row < W_inv.rows(); row++) {
            W_inv(row, col) = abs(W_inv(row, col)) < 1e-5 ? 0 : 1;
        }
    }
    model->updateBeta(W_inv);
}

MatrixXf GraphicalLasso::fit(shared_ptr<LinearRegression> model, MatrixXf& X, MatrixXf& Y) {
    model->setL1_reg(model->getL1_reg()*10);
    model->setX(X);
    model->setY(Y);
    model->initBeta();
    float residue = model->cost();
    VectorXf grad;
    VectorXf in;
    long epoch = 0;
    while (!shouldStop && epoch < maxIteration && residue > tolerance && !shouldStop) {
        grad = model->proximal_derivative();
        in = model->getBeta() - learningRate * grad;
        model->updateBeta(model->proximal_operator(in, learningRate));
        residue = model->cost();
        epoch++;
    }
    return model->getBeta();
}

void GraphicalLasso::stdNormalize(MatrixXf& matrix) {
    RowVectorXf mean = matrix.colwise().mean();
    RowVectorXf std = ((matrix.rowwise() - mean).array().square().colwise().sum() /
                       (matrix.rows())).sqrt();
    matrix = (matrix.rowwise() - mean).array().rowwise() / std.array();
}

vector<MatrixXf> GraphicalLasso::partitionBlocks(MatrixXf& matrix) {
    int num_rows = matrix.rows();
    int num_cols = matrix.cols();
    MatrixXf m_11 = matrix.block(0, 0, num_rows - 1, num_cols - 1);
    MatrixXf m_12 = matrix.block(0, num_cols - 1, num_rows - 1, 1);
    MatrixXf m_22 = matrix.block(num_rows - 1, num_cols - 1, 1, 1);
    return {m_11, m_12, m_22};
}

MatrixXf GraphicalLasso::composeBlocks(MatrixXf& m_11,
                                       MatrixXf& m_12,
                                       float m_22) {
    int num_rows = m_11.rows() + 1;
    int num_cols = m_11.cols() + 1;
    MatrixXf result(num_rows, num_cols);
    result.block(1, 1, num_rows - 1, num_cols - 1) = m_11;
    result.block(1, 0, num_rows - 1, 1) = m_12;
    result.block(0, 1, 1, num_cols - 1) = m_12.transpose();
    result(0, 0) = m_22;
    return result;
}


// Expand an orientation set expressed as a set of grids into a set of
// explicit orientations.
//
// Written by Charles Karney
// Copyright (c) 2006 Sarnoff Corporation. All rights reserved.
//
// For more information, see
//
//    https://github.com/cffk/orientation
//
// Compile with, e.g.,
//
//    g++ -O2 -o ExpandSet ExpandSet.cpp
//
// Run with
//
//   ./ExpandSet [-e] < grid-file > orientation-file
//
// If -e is specified, the orientations are written as Euler angles,
// otherwise they are written as quaternions.  Format of the grid file:
//
//     Any number of initial comment lines beginning with #
//     A line containing "format grid"
//     A line containing: delta sigma ntot ncell nent maxrad coverage
//     nent lines containing: k l m weight radius mult
//
// Here k >= l >= m >= 0.  delta and sigma are used to define the grid.
// ntot is the total number of orientations, ncell = ntot/24 is the
// number of orientations per cell of the 48-cell.  maxrad is the
// covering radius of the set and radius is the radius of the Voronoi
// cell.  Both are measured in degrees.  coverage is the coverage of the
// set, i.e., how much overlap there is when caps of radius maxrad are
// placed at each point; coverage = 1 means no overlap.
//
// For each triplet, [k l m], generate mult distinct permutations by
// changing the order and the signs of the elements.  Each [k l m] is
// converted to a point in a truncated cube [x y z] =
// [pind(k/2,delta,sigma) pind(l/2,delta,sigma) pind(m/2,delta,sigma)]
// Each [x y z] is converted to a unit quaternion via p = [1 x y z]; q =
// p/|p| to give ncell orientations.  Finally, the 24 rotational cube
// symmetries are applied to the results to yield ntot = orientations.
// The weights are normalized such that sum mult weight = sum mult =
// ncell

// Format of the orientation file
//
//     Any number of initial comment lines beginning with #
//     A line containing "format quaternion" or "format euler"
//     A line containing: ntot maxrad coverage
//     ntot lines containing: q0 q1 q2 q3 weight # for quaternions
//     ntot lines containing: alpha beta gamma weight # for euler.
//
// The weights are normalized such that sum weight = ntot.

#pragma once

#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <string>
#include <vector>
// todo: wrap and adapt this code to make numpy array of quats

// Windows doesn't define M_PI in the standard header?
#if !defined(M_PI)
#define M_PI 3.1415926535897932384626433832795028841971694
#endif

namespace rpxdock {
namespace sampling {
namespace orientations {

using namespace std;

// Minimal quaternion class
class Quaternion {
 public:
  double w, x, y, z;
  Quaternion(double ww = 1, double xx = 0, double yy = 0, double zz = 0)
      : w(ww), x(xx), y(yy), z(zz) {}
  void Normalize() {
    double t = w * w + x * x + y * y + z * z;
    assert(t > 0);
    t = 1 / sqrt(t);
    w *= t;
    x *= t;
    y *= t;
    z *= t;
    return;
  }
  void Canonicalize() {
    Normalize();
    // Make first biggest element positive
    double mag = w;
    if (abs(x) > abs(mag)) mag = x;
    if (abs(y) > abs(mag)) mag = y;
    if (abs(z) > abs(mag)) mag = z;
    if (mag < 0) {
      w *= -1;
      x *= -1;
      y *= -1;
      z *= -1;
    }
    return;
  }
  // a.Times(b) returns a * b
  Quaternion Times(const Quaternion &q) const {
    double mw = w * q.w - x * q.x - y * q.y - z * q.z,
           mx = w * q.x + x * q.w + y * q.z - z * q.y,
           my = w * q.y + y * q.w + z * q.x - x * q.z,
           mz = w * q.z + z * q.w + x * q.y - y * q.x;
    return Quaternion(mw, mx, my, mz);
  }
  void Print(ostream &s) const;
  void PrintEuler(ostream &s) const;
};

// Class to hold a set of orientations and weights
class PackSet {
 public:
  Quaternion Orientation(size_t i) const { return m_v[i]; }
  double Weight(size_t i) const { return m_w[i]; }
  size_t Number() const { return m_v.size(); }
  void Add(const Quaternion &q, double w = 1) {
    Quaternion v(q);
    v.Canonicalize();
    m_v.push_back(v);
    m_w.push_back(w);
  }
  void Clear() {
    m_v.clear();
    m_w.clear();
  }
  void Print(ostream &s, bool euler = false, size_t prec = 6) const {
    for (size_t i = 0; i < Number(); ++i) {
      if (euler)
        m_v[i].PrintEuler(s);
      else
        m_v[i].Print(s);
      s << " " << fixed << setprecision(prec) << setw(prec + 2) << m_w[i]
        << endl;
    }
  }

 private:
  vector<Quaternion> m_v;
  vector<double> m_w;
};

// The triple of grid indices
class Triple {
 public:
  int a, b, c;
  Triple(int aa, int bb, int cc) : a(aa), b(bb), c(cc) {}
};

// Generate the permutations and sign changes for a Triple.
class Permute {
 public:
  Permute(Triple x) {
    assert(x.a >= x.b && x.b >= x.c && x.c >= 0);
    m_arr.push_back(x);
    size_t n = 1;
    // Do the sign changes
    if (x.a != 0) {
      for (size_t i = 0; i < n; ++i)
        m_arr.push_back(Triple(-m_arr[i].a, m_arr[i].b, m_arr[i].c));
      n *= 2;
    }
    if (x.b != 0) {
      for (size_t i = 0; i < n; ++i)
        m_arr.push_back(Triple(m_arr[i].a, -m_arr[i].b, m_arr[i].c));
      n *= 2;
    }
    if (x.c != 0) {
      for (size_t i = 0; i < n; ++i)
        m_arr.push_back(Triple(m_arr[i].a, m_arr[i].b, -m_arr[i].c));
      n *= 2;
    }
    if (x.a == x.b && x.b == x.c) return;
    // With at least two distinct indices we can rotate the set thru 3
    // permuations.
    for (size_t i = 0; i < n; ++i) {
      m_arr.push_back(Triple(m_arr[i].b, m_arr[i].c, m_arr[i].a));
      m_arr.push_back(Triple(m_arr[i].c, m_arr[i].a, m_arr[i].b));
    }
    n *= 3;
    if (x.a == x.b || x.b == x.c) return;
    // With three distinct indices we can in addition interchange the
    // first two indices (to yield all 6 permutations of 3 indices).
    for (size_t i = 0; i < n; ++i) {
      m_arr.push_back(Triple(m_arr[i].b, m_arr[i].a, m_arr[i].c));
    }
    n *= 2;
  }
  size_t Number() const { return m_arr.size(); }
  Triple Member(size_t i) const { return m_arr[i]; }

 private:
  vector<Triple> m_arr;
};

// The rotational symmetries of the cube.  (Not normalized, since
// PackSet.Add does this.)
static double CubeSyms[24][4] = {
    {1, 0, 0, 0},
    // 180 deg rotations about 3 axes
    {0, 1, 0, 0},
    {0, 0, 1, 0},
    {0, 0, 0, 1},
    // +/- 120 degree rotations about 4 leading diagonals
    {1, 1, 1, 1},
    {1, 1, 1, -1},
    {1, 1, -1, 1},
    {1, 1, -1, -1},
    {1, -1, 1, 1},
    {1, -1, 1, -1},
    {1, -1, -1, 1},
    {1, -1, -1, -1},
    // +/- 90 degree rotations about 3 axes
    {1, 1, 0, 0},
    {1, -1, 0, 0},
    {1, 0, 1, 0},
    {1, 0, -1, 0},
    {1, 0, 0, 1},
    {1, 0, 0, -1},
    // 180 degree rotations about 6 face diagonals
    {0, 1, 1, 0},
    {0, 1, -1, 0},
    {0, 1, 0, 1},
    {0, 1, 0, -1},
    {0, 0, 1, 1},
    {0, 0, 1, -1},
};

// Convert from index to position.  The sinh scaling tries to compensate
// for the bunching up that occurs when [1 x y z] is projected onto the
// unit sphere.
double pind(double ind, double delta, double sigma) {
  return (sigma == 0) ? ind * delta : sinh(sigma * ind * delta) / sigma;
}

auto read_karney_orientations(std::string file_content) {
  Eigen::MatrixX4d quats;
  Eigen::VectorXd cover;

  std::istringstream in(file_content);

  bool euler = false;
  assert(in.good());
  string line;
  while (in.peek() == '#') {
    getline(in, line);
    // cout << line << endl;
  }
  assert(in.good());
  getline(in, line);
  assert(line == "format grid");
  double delta, sigma, maxrad, coverage;
  size_t ncell, ntot, nent;
  in >> delta >> sigma >> ntot >> ncell >> nent >> maxrad >> coverage;
  // Use extra digit of precision with weights and radii.  This also
  // triggers a memory minimizing expansion.
  const bool fine = delta < 0.05;
  quats.resize(ntot, 4);
  cover.resize(ntot);
  int quats_i = 0;
  PackSet s;
  size_t ncell1 = 0;
  for (size_t n = 0; n < nent; ++n) {
    int k, l, m;
    size_t mult;
    double r, w;
    assert(in.good());
    in >> k >> l >> m >> w >> r >> mult;
    Permute p(Triple(k, l, m));
    assert(mult == p.Number());
    for (size_t i = 0; i < mult; ++i) {
      Triple t = p.Member(i);
      s.Add(Quaternion(1.0, pind(0.5 * t.a, delta, sigma),
                       pind(0.5 * t.b, delta, sigma),
                       pind(0.5 * t.c, delta, sigma)),
            w);
    }
    ncell1 += mult;
    if (fine) {
      // Skip n = 0; that's already included.
      for (size_t n = 1; n < 24; ++n) {
        Quaternion q(CubeSyms[n][0], CubeSyms[n][1], CubeSyms[n][2],
                     CubeSyms[n][3]);
        for (size_t i = 0; i < mult; ++i)
          s.Add(q.Times(s.Orientation(i)), s.Weight(i));
      }
      // s.Print(cout, euler, fine ? 7 : 6);
      for (size_t i = 0; i < s.Number(); ++i) {
        quats(quats_i, 0) = s.Orientation(i).w;
        quats(quats_i, 1) = s.Orientation(i).x;
        quats(quats_i, 2) = s.Orientation(i).y;
        quats(quats_i, 3) = s.Orientation(i).z;
        cover[quats_i++] = s.Weight(i);
      }
      s.Clear();
    }
  }
  assert(in.good());
  assert(ncell1 == ncell);
  if (!fine) {
    size_t nc = s.Number();
    assert(nc == ncell);
    for (size_t n = 1; n < 24; ++n) {
      Quaternion q(CubeSyms[n][0], CubeSyms[n][1], CubeSyms[n][2],
                   CubeSyms[n][3]);
      for (size_t i = 0; i < nc; ++i)
        s.Add(q.Times(s.Orientation(i)), s.Weight(i));
    }
    assert(s.Number() == ntot);
    // s.Print(cout, euler, fine ? 7 : 6);
    for (size_t i = 0; i < s.Number(); ++i) {
      quats(quats_i, 0) = s.Orientation(i).w;
      quats(quats_i, 1) = s.Orientation(i).x;
      quats(quats_i, 2) = s.Orientation(i).y;
      quats(quats_i, 3) = s.Orientation(i).z;
      cover[quats_i++] = s.Weight(i);
    }
    s.Clear();
  }
  return std::make_tuple(quats, cover);
}

void Quaternion::Print(ostream &s) const {
  s << fixed << setprecision(9) << setw(12) << w << " ";
  s << setw(12) << x << " ";
  s << setw(12) << y << " ";
  s << setw(12) << z;
}

void Quaternion::PrintEuler(ostream &s) const {
  // Print out orientation as a set of Euler angles, following the
  // convention given in
  //
  //    http://www.mhl.soton.ac.uk/research/help/Euler/index.html
  //
  // Rotation by Euler angles [a,b,c] is defined as rotation by c about
  // z axis, followed by rotation by b about y axis. followed by
  // rotation by a about z axis (again).
  //
  // Convert to rotation matrix (assume quaternion is already
  // normalized)
  double
      // m00 = 1 - 2*y*y - 2*z*z,
      m01 = 2 * x * y - 2 * z * w,
      m02 = 2 * x * z + 2 * y * w,
      // m10 =     2*x*y + 2*z*w,
      m11 = 1 - 2 * x * x - 2 * z * z, m12 = 2 * y * z - 2 * x * w,
      m20 = 2 * x * z - 2 * y * w, m21 = 2 * y * z + 2 * x * w,
      m22 = 1 - 2 * x * x - 2 * y * y;
  // Taken from Ken Shoemake, "Euler Angle Conversion", Graphics Gems
  // IV, Academic 1994.
  //
  //    http://vered.rose.utoronto.ca/people/david_dir/GEMS/GEMS.html
  double sy = sqrt(m02 * m02 + m12 * m12);
  //  double sy = sqrt(m10*m10 + m20*m20);
  double a, b, c;
  b = atan2(sy, m22);
  if (sy > 16 * numeric_limits<double>::epsilon()) {
    a = atan2(m12, m02);
    c = atan2(m21, -m20);
  } else {
    a = atan2(-m01, m11);
    c = 0;
  }
  s << fixed << setprecision(9) << setw(12) << a << " " << setw(12) << b << " "
    << setw(12) << c;

#if !defined(NDEBUG)
  // Sanity check.  Convert from Euler angles back to a quaternion, q
  Quaternion q =
      Quaternion(cos(a / 2), 0, 0, sin(a / 2))
          .  // a about z
      Times(Quaternion(cos(b / 2), 0, sin(b / 2), 0)
                .                                              // b about y
            Times(Quaternion(cos(c / 2), 0, 0, sin(c / 2))));  // c about z
  // and check that q is parallel to *this.
  double t = abs(q.w * w + q.x * x + q.y * y + q.z * z);
  assert(t > 1 - 16 * numeric_limits<double>::epsilon());
#endif
}

}  // namespace orientations
}  // namespace sampling
}  // namespace rpxdock


/**
   \file gsl_simplex.hpp
   \brief a simple wrapper for the GSL_SIMPLEX method
   \author Junhua Gu
 */

#ifndef GSL_SIMPLEX_METHOD
#define GSL_SIMPLEX_METHOD
#define OPT_HEADER
#include <core/optimizer.hpp>
//#include <blitz/array.h>
#include <vector>
#include <limits>
#include <cassert>
#include <cmath>
#include <algorithm>
#include <gsl/gsl_multimin.h>
#include <iostream>

namespace opt_utilities
{

  /**
     \brief object function of the gsl simplex function
   */
  template <typename rT,typename pT>
  double gsl_func_adapter(const gsl_vector* v,void* params)
  {
    pT temp;
    temp.resize(v->size);
    for(size_t i=0;i<get_size(temp);++i)
      {
	set_element(temp,i,gsl_vector_get(v,i));
      }
    return ((func_obj<rT,pT>*)params)->eval(temp);
  }

  /**
     \brief wrapper for the gsl simplex optimization method
     \tparam return type of the object function
     \tparam param type of the object function
  */
  template <typename rT,typename pT>
  class gsl_simplex
    :public opt_method<rT,pT>
  {
  public:
    typedef pT array1d_type;
    typedef rT T;
  private:
    func_obj<rT,pT>* p_fo;
    optimizer<rT,pT>* p_optimizer;

    //typedef blitz::Array<rT,2> array2d_type;

  private:
    array1d_type start_point;
    array1d_type end_point;

  private:
    rT threshold;
  private:
    rT func(const pT& x)
    {
      assert(p_fo!=0);
      return p_fo->eval(x);
    }

    const char* do_get_type_name()const
    {
      return "gsl simplex";
    }
  public:
    gsl_simplex()
      :threshold(1e-4)
    {}

    virtual ~gsl_simplex()
    {
    };

    gsl_simplex(const gsl_simplex<rT,pT>& rhs)
      :p_fo(rhs.p_fo),p_optimizer(rhs.p_optimizer),
       start_point(rhs.start_point),
       end_point(rhs.end_point),
       threshold(rhs.threshold)
    {
    }

    gsl_simplex<rT,pT>& operator=(const gsl_simplex<rT,pT>& rhs)
    {
      threshold=rhs.threshold;
      p_fo=rhs.p_fo;
      p_optimizer=rhs.p_optimizer;
      opt_eq(start_point,rhs.start_point);
      opt_eq(end_point,rhs.end_point);
    }

    opt_method<rT,pT>* do_clone()const
    {
      return new gsl_simplex<rT,pT>(*this);
    }

    void do_set_start_point(const array1d_type& p)
    {
      start_point.resize(get_size(p));
      opt_eq(start_point,p);

    }

    array1d_type do_get_start_point()const
    {
      return start_point;
    }

    void do_set_precision(rT t)
    {
      threshold=t;
    }

    rT do_get_precision()const
    {
      return threshold;
    }

    void do_set_optimizer(optimizer<rT,pT>& o)
    {
      p_optimizer=&o;
      p_fo=p_optimizer->ptr_func_obj();
    }

    pT do_optimize()
    {
      const gsl_multimin_fminimizer_type *T =
	gsl_multimin_fminimizer_nmsimplex;
      gsl_multimin_fminimizer *s = NULL;
      gsl_vector *ss, *x;
      gsl_multimin_function minex_func;

      size_t iter = 0;
      int status;
      double size;

      /* Starting point */
      x = gsl_vector_alloc (get_size(start_point));
      //      gsl_vector_set (x, 0, 5.0);
      //gsl_vector_set (x, 1, 7.0);
      for(size_t i=0;i!=get_size(start_point);++i)
	{
	  gsl_vector_set(x,i,get_element(start_point,i));
	}

      /* Set initial step sizes to 1 */
      ss = gsl_vector_alloc (get_size(start_point));
      gsl_vector_set_all (ss, 1.0);

      //foo f;
      /* Initialize method and iterate */
      minex_func.n = get_size(start_point);
      minex_func.f = &gsl_func_adapter<double,std::vector<double> >;
      minex_func.params = (void *)p_fo;

      s = gsl_multimin_fminimizer_alloc (T, get_size(start_point));
      gsl_multimin_fminimizer_set (s, &minex_func, x, ss);

      do
	{
	  iter++;
	  status = gsl_multimin_fminimizer_iterate(s);

	  if (status)
	    {
	      break;
	    }
	  //std::cerr<<"threshold="<<threshold<<std::endl;
	  size = gsl_multimin_fminimizer_size (s);
	  status = gsl_multimin_test_size (size, threshold);

	  if (status == GSL_SUCCESS)
	    {
	      //printf ("converged to minimum at\n");
	    }

	  //printf ("%5d %10.3e %10.3ef f() = %7.3f size = %.3f\n",
	  //iter,
	  //gsl_vector_get (s->x, 0),
	  //gsl_vector_get (s->x, 1),
	  //  s->fval, size);
	}
      while (status == GSL_CONTINUE);

      /*
	foo f;
	gsl_vector_set (x, 0, 0.0);
	gsl_vector_set (x, 1, 0.0);
	cout<<"fdsa ";
	cout<<gsl_func_adapter<double,vector<double> >(x,(void*)&f)<<endl;;

      */

      end_point.resize(get_size(start_point));
      for(size_t i=0;i<get_size(start_point);++i)
	{
	  set_element(end_point,i,gsl_vector_get(s->x,i));
	}

      gsl_vector_free(x);
      gsl_vector_free(ss);
      gsl_multimin_fminimizer_free (s);

      return end_point;
    }
  };

}

#endif
//EOF


#include <iostream>
#include <vector>
#include <functional>
#include <cassert>
#include <cmath>
#include <chrono>
#include <NTL/matrix.h>
#include <NTL/ZZ_pX.h> // includes ZZ_p.h and ZZ.h as well

using std::vector;
using namespace std::chrono;
using NTL::Mat;
using NTL::ZZ_p;
using NTL::ZZ_pX;
using NTL::ZZ;

// TODO: add return type to final algorithm

// Reference: https://www.dropbox.com/home/urop2019/literature?preview=Bostan%2C+Gaudry%2C+Schost+-+Linear+recurrences+with+polynomial+coefficients+and+application+to+integer+factorization+and+Cartier-Manin+operator.pdf
// Page 1781 (Lemma 1)
void invert_all(vector<ZZ_p> &out, vector<ZZ_p> &a);
// Page 1781 (Lemma 2)
void find_delta(vector<ZZ_p> &out, ZZ &p);
void find_delta(vector<ZZ_p> &out, ZZ &p, ZZ_p &a);
// Page 1784-5 (Theorem 5)
void shift_values(vector<ZZ_p> &out, vector<ZZ_p> &values, ZZ_p &a, ZZ_p &b, ZZ &p);
void shift_values(vector<Mat<ZZ_p>> &out, vector<Mat<ZZ_p>> &values, ZZ_p &a, ZZ_p &b, ZZ &p);
// Page 1786-8 (Section 4)
void multieval_prod(vector<Mat<ZZ_p>> &out, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p);
void multieval_prod(vector<Mat<ZZ_p>> &out, ZZ_p &k, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p);
// Page 1792 (Equation (5))
void matrix_factorial(Mat<ZZ_p> &out, long n, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p);
// Following functions for testing:
// matrix definition
void A(Mat<ZZ_p> &out, ZZ_p &x);
// print contents of a vector
template <typename T>
void print(vector<T> vec);

/*
 * Evaluate M(x), returned in out
 */
void A(Mat<ZZ_p> &out, ZZ_p &x){
    ZZ_p zero;
    zero.init(x.modulus());
    zero = 0;
    ZZ_p one;
    one.init(x.modulus());
    one = 1;
    /*out.SetDims(2, 2);
    out.put(0, 0, x+1);
    out.put(0, 1, zero);
    out.put(1, 0, one);
    out.put(1, 1, one);
    */
    out.SetDims(1, 1);
    out.put(0, 0, x);

}

int main(){
    std::function<void (Mat<ZZ_p>&, ZZ_p&)> A_func = A;

    Mat<ZZ_p> answer;
    for(long i = 1073741824; i <= 1073741824; i*=2){
        ZZ p;
        NextPrime(p, ZZ(i));
        long p1;
        conv(p1, p-1);
        mul(p, p, p);
        uint64_t start = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
        matrix_factorial(answer, p1, A_func, p);
        uint64_t time = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count() - start;

        std::cout << "final answer: " << answer << std::endl;
        //std::cout << time << ", ";
    }
    std::cout << std::endl;

}

template<typename T>
void print(vector<T> vec){
    for(long i = 0; i < vec.size(); i++){
        std::cout << vec[i];
        if(i < vec.size()-1){
            std::cout << ", ";
        }
    }
    std::cout << std::endl;
}

/*
 * Steps:
 * [DONE] Create function that take inputs a0, a1, ..., an-1 and outputs 1/a0, ..., 1/an-1
 * [DONE] Create function that takes input d and outputs 1/prod(0-j), 1/prod(1-j), ..., 1/prod(d-j)
 * [DONE] Create function that takes input a, d and outputs prod(a+0-j), prod(a+1-j), ..., prod(a+d-j)
 * [DONE] Create function that take inputs M(a), M(a+b), ..., M(a+kb) and outputs M(a+i), M(a+b+i), ..., M(a+kb+i)
 * [DONE] Create function to calculate M_k(0), M_k(k), ..., M_k(k^2)
 * [DONE] Create main function to calculate M(1)...M(k^2) = M_k(0)M_k(k)...M_k(k^2-k)
 */

/*
 * Takes a list of elements a and returns their inversions mod p
 */
void invert_all(vector<ZZ_p> &out, vector<ZZ_p> &a){
    assert(out.size() == a.size());

    long n = a.size();
    vector<ZZ_p> inverses(n);

    vector<ZZ_p> accum_prod(n); // a0, a0a1, ..., a0a1...an-1

    accum_prod[0] = a[0];
    for(long i = 1; i < n; i++){
        mul(accum_prod[i], accum_prod[i-1], a[i]);
    }

    inv(accum_prod[n-1], accum_prod[n-1]);
    for(long i = n-1; i > 0; i--){
        mul(inverses[i], accum_prod[i], accum_prod[i-1]); // 1/ai = 1/(a1...ai) * a1...a(i-1)
        mul(accum_prod[i-1], accum_prod[i], a[i]); // a1...a(i-1) -> 1/(a1...a(i-1))
    }
    inverses[0] = accum_prod[0];

    out = inverses;
}

/*
 * d = out.size()-1;
 * Returns 1/delta(i, d) = 1/prod(i-j) where the product goes from j=0 to d skipping i.
 */
void find_delta(vector<ZZ_p> &out, ZZ &p){
    long d = out.size() - 1;
    vector<ZZ_p> ints(d);
    for(long i = 0; i < ints.size(); i++){
        ints[i].init(p);
        ints[i] = i+1;
    }
    vector<ZZ_p> inv_ints(d);
    invert_all(inv_ints, ints);

    vector<ZZ_p> deltas(d+1);
    deltas[0] = inv_ints[0]; // = 1
    for(long i = 1; i < inv_ints.size(); i++){
        mul(deltas[0], deltas[0], inv_ints[i]);
    } // deltas[0] = 1/d!

    if(d%2 == 1){
        deltas[0] = -deltas[0]; // deltas[0] = 1/((-1)^d*d!)
    }

    for(long i = 1; i < deltas.size(); i++){
        mul(deltas[i], deltas[i-1], i-1-d);
        mul(deltas[i], deltas[i], inv_ints[i-1]);
    }

    out = deltas;
}

/*
 * d = out.size()-1;
 * Returns Delta(a, i, d) = prod(a+i-j) where the product goes from j=0 to d.
 */
void find_delta(vector<ZZ_p> &out, ZZ_p &a, ZZ &p){
    long d = out.size() - 1;

    vector<ZZ_p> ints(d);
    for(long i = 0; i < ints.size(); i++){
        ints[i].init(p);
        ints[i] = a-d+i;
    }
    vector<ZZ_p> inv_ints(d);
    invert_all(inv_ints, ints);

    vector<ZZ_p> Deltas(d+1);
    Deltas[0] = a;
    for(long i = 0; i < inv_ints.size(); i++){
        mul(Deltas[0], Deltas[0], ints[i]);
    } // Deltas[0] = (a-d)...(a-1)(a)
    for(long i = 1; i < Deltas.size(); i++){
        mul(Deltas[i], Deltas[i-1], a+i);
        mul(Deltas[i], Deltas[i], inv_ints[i-1]);
    }

    out = Deltas;
}

/*
 * Takes in the values of polynomials F(r), F(r+b), ..., F(r+db) and outputs F(r+a), F(r+b+a), ... F(r+db+a)
 * Requires deg(F) <= d to work mathematically, since it will interpolate based on the given evaluations of F.
 */
void shift_values(vector<ZZ_p> &out, vector<ZZ_p> &values, ZZ_p &a, ZZ_p &b, ZZ &p){
    assert(out.size() == values.size());

    long d = values.size() - 1;
    vector<ZZ_p> P(d+1);
    find_delta(P, p);

    ZZ_pX Px;
    for(long i = 0; i < values.size(); i++){
        mul(P[i], P[i], values[i]);
        SetCoeff(Px, i, P[i]);
    }

    ZZ_p shift;
    shift.init(p);
    inv(shift, b);
    mul(shift, shift, a); // shift = a/b

    vector<ZZ_p> S(2*d+1);
    for(long i = 0; i < S.size(); i++){
        S[i] = shift + i - d;
    }
    invert_all(S, S);

    ZZ_pX Sx;
    for(long i = 0; i < S.size(); i++){
        SetCoeff(Sx, i, S[i]);
    }

    ZZ_pX PS;
    mul(PS, Px, Sx);

    find_delta(out, shift, p);
    for(long i = 0; i < out.size(); i++){
        mul(out[i], out[i], coeff(PS, i+d));
    }
}

/*
 * Takes in the values of matrices of polynomials M(r), M(r+b), ..., M(r+db) and outputs M(r+a), M(r+b+a), ... M(r+db+a)
 * Requires the maximal degree of any element of M(x) to be <= d to successfully interpolate
 */
void shift_values(vector<Mat<ZZ_p>> &out, vector<Mat<ZZ_p>> &values, ZZ_p &a, ZZ_p &b, ZZ &p){
    assert(out.size() == values.size());
    assert(values[0].NumRows() == values[0].NumCols());
    for(long i = 0; i < out.size(); i++){
        assert(values[i].NumRows() == values[0].NumRows());
        assert(values[i].NumCols() == values[0].NumCols());
        out[i].SetDims(values[0].NumRows(), values[0].NumCols());
    }

    for(long row = 0; row < values[0].NumRows(); row++){
        for(long col = 0; col < values[0].NumCols(); col++){
            vector<ZZ_p> vals(values.size());
            for(long i = 0; i < values.size(); i++){
                vals[i] = values[i].get(row, col);
            }
            vector<ZZ_p> shifted_vals(vals.size());
            shift_values(shifted_vals, vals, a, b, p);
            for(long i = 0; i < shifted_vals.size(); i++){
                out[i].put(row, col, shifted_vals[i]);
            }
        }
    }
}

/*
 * k := out.size()-1
 * Calculates M_k(x) at 0, k, ..., k^2 recursively
 * M_k(x) = M(x+1)M(x+2)...M(x+k)
 * To calculate M_m(x) at 0, k, ..., mk, use the value M_m/2(x) at 0, k, ..., (m/2)k
 */
void multieval_prod(vector<Mat<ZZ_p>> &out, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p){
    ZZ_p kp;
    kp.init(p);
    kp = out.size()-1;
    multieval_prod(out, kp, A, p);
}
/*
 * m := out.size()-1
 */
void multieval_prod(vector<Mat<ZZ_p>> &out, ZZ_p &k, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p){
    long m = out.size()-1;

    if(m == 1){
        ZZ_p one;
        one.init(p);
        one = 1;
        ZZ_p kone = k+1;
        A(out[0], one);
        A(out[1], kone);
        return;
    }

    vector<Mat<ZZ_p>> lower_layer(m/2+1);
    multieval_prod(lower_layer, k, A, p);

    vector<Mat<ZZ_p>> ll_extend(lower_layer.size());
    ZZ_p m21k = k*(m/2+1);
    shift_values(ll_extend, lower_layer, m21k, k, p);

    vector<Mat<ZZ_p>> ll_total;
    ll_total.reserve(lower_layer.size() + ll_extend.size());
    ll_total.insert(ll_total.end(), lower_layer.begin(), lower_layer.end());
    ll_total.insert(ll_total.end(), ll_extend.begin(), ll_extend.end());

    vector<Mat<ZZ_p>> ll_shift(ll_total.size());
    ZZ_p m2;
    m2.init(p);
    m2 = m/2;
    shift_values(ll_shift, ll_total, m2, k, p);

    for(long i = 0; i < out.size(); i++){
        mul(out[i], ll_shift[i], ll_total[i]);
    }
    if(m % 2 == 1){
        for(long i = 0; i < out.size(); i++){
            ZZ_p ikm = k*i + m;
            Mat<ZZ_p> extra;
            A(extra, ikm);
            mul(out[i], extra, out[i]);
        }
    }

}

/*
 * Calculate M(1)M(2)...M(n) mod p
 */
void matrix_factorial(Mat<ZZ_p> &out, long n, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p){
    long rtn = sqrt(n);

    vector<Mat<ZZ_p>> seg_prods(rtn+1);
    multieval_prod(seg_prods, A, p);

    if(n < rtn*rtn + rtn){
        for(long i = seg_prods.size()-2; i > 0; i--){
            mul(seg_prods[i-1], seg_prods[i], seg_prods[i-1]);
        }
        for(long i = rtn*rtn+1; i <= n; i++){
            Mat<ZZ_p> extra;
            ZZ_p x;
            x.init(p);
            x = i;
            A(extra, x);
            mul(seg_prods[0], extra, seg_prods[0]);
        }
    }
    else{
        for(long i = seg_prods.size()-1; i > 0; i--){
            mul(seg_prods[i-1], seg_prods[i], seg_prods[i-1]);
        }
        for(long i = rtn*rtn+rtn+1; i <= n; i++){
            Mat<ZZ_p> extra;
            ZZ_p x;
            x.init(p);
            x = i;
            A(extra, x);
            mul(seg_prods[0], extra, seg_prods[0]);
        }
    }
    out = seg_prods[0];
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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.
// ----------------------------------------------------------------------------

#include "TransformationEstimation.h"

#include <Eigen/Geometry>
#include <Core/Geometry/PointCloud.h>
#include <Core/Utility/Eigen.h>

namespace open3d{

double TransformationEstimationPointToPoint::ComputeRMSE(
        const PointCloud &source, const PointCloud &target,
        const CorrespondenceSet &corres) const
{
    if (corres.empty()) return 0.0;
    double err = 0.0;
    for (const auto &c : corres) {
        err += (source.points_[c[0]] - target.points_[c[1]]).squaredNorm();
    }
    return std::sqrt(err / (double)corres.size());
}

Eigen::Matrix4d TransformationEstimationPointToPoint::ComputeTransformation(
        const PointCloud &source, const PointCloud &target,
        const CorrespondenceSet &corres) const
{
    if (corres.empty()) return Eigen::Matrix4d::Identity();
    Eigen::MatrixXd source_mat(3, corres.size());
    Eigen::MatrixXd target_mat(3, corres.size());
    for (size_t i = 0; i < corres.size(); i++) {
        source_mat.block<3, 1>(0, i) = source.points_[corres[i][0]];
        target_mat.block<3, 1>(0, i) = target.points_[corres[i][1]];
    }
    return Eigen::umeyama(source_mat, target_mat, with_scaling_);
}

double TransformationEstimationPointToPlane::ComputeRMSE(
        const PointCloud &source, const PointCloud &target,
        const CorrespondenceSet &corres) const
{
    if (corres.empty() || target.HasNormals() == false) return 0.0;
    double err = 0.0, r;
    for (const auto &c : corres) {
        r = (source.points_[c[0]] - target.points_[c[1]]).dot(
                target.normals_[c[1]]);
        err += r * r;
    }
    return std::sqrt(err / (double)corres.size());
}

Eigen::Matrix4d TransformationEstimationPointToPlane::ComputeTransformation(
        const PointCloud &source, const PointCloud &target,
        const CorrespondenceSet &corres) const
{
    if (corres.empty() || target.HasNormals() == false)
        return Eigen::Matrix4d::Identity();

    auto compute_jacobian_and_residual = [&]
            (int i, Eigen::Vector6d &J_r, double &r) {
        const Eigen::Vector3d &vs = source.points_[corres[i][0]];
        const Eigen::Vector3d &vt = target.points_[corres[i][1]];
        const Eigen::Vector3d &nt = target.normals_[corres[i][1]];
        r = (vs - vt).dot(nt);
        J_r.block<3, 1>(0, 0) = vs.cross(nt);
        J_r.block<3, 1>(3, 0) = nt;
    };

    Eigen::Matrix6d JTJ;
    Eigen::Vector6d JTr;
    std::tie(JTJ, JTr) = ComputeJTJandJTr<Eigen::Matrix6d, Eigen::Vector6d>(
            compute_jacobian_and_residual, (int)corres.size());

    bool is_success;
    Eigen::Matrix4d extrinsic;
    std::tie(is_success, extrinsic) =
            SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr);

    return is_success ? extrinsic : Eigen::Matrix4d::Identity();
}

}    // namespace open3d


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/four_point_homography.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <glog/logging.h>
#include <vector>

#include "theia/sfm/pose/util.h"

namespace theia {
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

namespace {

inline Matrix<double, 2, 9> CreateActionConstraint(const Vector2d& img1_point,
                                                   const Vector2d& img2_point) {
  Matrix<double, 2, 9> constraint;
  constraint << Eigen::RowVector3d::Zero(), -img1_point.transpose(), -1.0,
      img1_point.transpose() * img2_point.y(), img2_point.y(),
      img1_point.transpose(), 1.0, Eigen::RowVector3d::Zero(),
      -img1_point.transpose() * img2_point.x(), -img2_point.x();

  return constraint;
}

}  // namespace

// Normalized DLT method to compute the homography H that maps image points in
// image_1 to image_2 via x' = Hx (where x is in image 1 and x' is in image
// 2). The DLT algorithm implemented is from Algorithm 4.2 in Hartley and
// Zisserman (page 109).
bool FourPointHomography(const std::vector<Vector2d>& image_1_points,
                         const std::vector<Vector2d>& image_2_points,
                         Matrix3d* homography) {
  CHECK_GE(image_1_points.size(), 4);
  CHECK_EQ(image_1_points.size(), image_2_points.size());

  // Normalize the image points.
  std::vector<Vector2d> norm_image_1_points, norm_image_2_points;
  Matrix3d norm_image_1_mat, norm_image_2_mat;
  NormalizeImagePoints(image_1_points, &norm_image_1_points, &norm_image_1_mat);
  NormalizeImagePoints(image_2_points, &norm_image_2_points, &norm_image_2_mat);

  // Create the constraint matrix based on x' = Hx (Eq. 4.1 in Hartley and
  // Zisserman).
  Matrix<double, Eigen::Dynamic, 9> action_matrix(
      2 * image_1_points.size(), 9);
  for (int i = 0; i < image_1_points.size(); i++) {
    action_matrix.block<2, 9>(2 * i, 0) =
        CreateActionConstraint(norm_image_1_points[i], norm_image_2_points[i]);
  }

  const Matrix<double, 9, 1> null_vector =
      (action_matrix.transpose() * action_matrix).jacobiSvd(Eigen::ComputeFullV)
          .matrixV().rightCols<1>();

  *homography = norm_image_2_mat.inverse() *
                Eigen::Map<const Matrix3d>(null_vector.data()).transpose() *
                norm_image_1_mat;
  return true;
}

}  // namespace theia


#ifndef CANNON_GEOM_TRAJECTORY
#define CANNON_GEOM_TRAJECTORY

/*!
 * \file cannon/geom/trajectory.hpp
 * \brief File containing Trajectory class definition.
 */

#include <vector>

#include <Eigen/Dense>

#include <cannon/utils/class_forward.hpp>

using namespace Eigen;

namespace cannon {

  namespace math {
    CANNON_CLASS_FORWARD(MultiSpline);
  }

  using namespace cannon::math;

  namespace geom {

    /*!
     * \brief Class representing a time-parameterized trajectory.
     */
    class Trajectory {
      public:

        /*!
         * \brief Default constructor.
         */
        Trajectory();

        /*!
         * \brief Constructor taking vector of states and times.
         */
        Trajectory(const std::vector<VectorXd> &states,
                   const std::vector<double> times);

        /*!
         * \brief Get the state of this trajectory at the input time. This
         * trajectory is treated as a piecewise-linear function over time.
         *
         * \param t The time to evaluate the trajectory at.
         *
         * \returns The linearly interpolated state of this trajectory at the
         * input time.
         */
        VectorXd operator()(double t) const;

        /*!
         * \brief Add a new state at the end of this trajectory, at the input
         * time.
         *
         * \param state The state to add.
         * \param time The time for the new state. Should be greater than any
         * time in the trajectory so far.
         */
        void push_back(const Ref<const VectorXd>& state, double time);

        /*!
         * \brief Get the length (total time) of this trajectory.
         *
         * \returns Length of trajectory.
         */
        double length() const;

        /*!
         * \brief Get the size (number of states) in this trajectory.
         *
         * \returns Trajectory size.
         */
        size_t size() const;

        /*!
         * \brief Get a spline that interpolates this trajectory.
         *
         * \returns A spline which smoothly interpolates this trajectory.
         */
        MultiSpline interp() const;

        /*!
         * \brief Save this trajectory to a file.
         *
         * \param path The path to save this trajectory to.
         */
        void save(const std::string& path);

        /*!
         * \brief Load a trajectory from a file. Overwrites any existing points
         * in this trajectory.
         *
         * \param path The path to load a trajectory from.
         */
        void load(const std::string& path);

      private:

        /*!
         * \brief Find the closest idx such that times_[idx] <= t
         *
         * \param t Input value to locate
         *
         * \returns Index of closest time
         */
        unsigned int find_closest_t_(double t) const;

        std::vector<VectorXd> states_; //!< States in trajectory
        std::vector<double> times_; //!< Times for states in trajectory

    };

    /*!
     * \brief Class representing a time-parameterized trajectory with controls.
     */
    class ControlledTrajectory {
      public:

        /*!
         * \brief Default constructor.
         */
        ControlledTrajectory();

        /*!
         * \brief Constructor taking vector of states, vector of controls, and times.
         */
        ControlledTrajectory(const std::vector<VectorXd> &states,
                             const std::vector<VectorXd> &controls,
                             const std::vector<double> times);

        /*!
         * \brief Get the trajectory of states from this controlled trajectory.
         *
         * \returns A trajectory of the states making up this controlled trajectory.
         */
        Trajectory state_traj() const;

        /*!
         * \brief Get the state and control of this controlled trajectory at the
         * input time. This trajectory is treated as a piecewise-linear function
         * over time.
         *
         * \param t The time to evaluate the trajectory at.
         *
         * \returns A pair containing the linearly interpolated state and
         * control of this trajectory at the input time.
         */
        std::pair<VectorXd, VectorXd> operator()(double t) const;

        /*!
         * \brief Add a new state and control at the end of this trajectory, at
         * the input time.
         *
         * \param state The state to add.
         * \param control The control to add.
         * \param time The time for the new state. Should be greater than any
         * time in the trajectory so far.
         */
        void push_back(const Ref<const VectorXd> &state,
                       const Ref<const VectorXd> &control, double time);

        /*!
         * \brief Get the length (total time) of this trajectory.
         *
         * \returns Length of trajectory.
         */
        double length() const;

        /*!
         * \brief Get the size (number of states) of this trajectory.
         *
         * \returns Trajectory size.
         */
        size_t size() const;

        /*!
         * \brief Save this trajectory to a file.
         *
         * \param path The path to save this trajectory to.
         */
        void save(const std::string& path);

        /*!
         * \brief Load a trajectory from a file. Overwrites any existing points
         * in this trajectory.
         *
         * \param path The path to load a trajectory from.
         */
        void load(const std::string& path);

      private:

        /*!
         * \brief Find the closest idx such that times_[idx] <= t
         *
         * \param t Input value to locate
         *
         * \returns Index of closest time
         */
        unsigned int find_closest_t_(double t) const;

        std::vector<VectorXd> states_; //!< States in trajectory
        std::vector<VectorXd> controls_; //!< Controls in trajectory
        std::vector<double> times_; //!< Times for states in trajectory

    };

  }
}

#endif /* ifndef CANNON_GEOM_TRAJECTORY */