#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
*/


/**
 * @file AdaBoost_test.cpp
 * @author Udit Saxena
 *
 * Tests for AdaBoost class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/adaboost/adaboost.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"

using namespace arma;
using namespace mlpack;
using namespace mlpack::adaboost;
using namespace mlpack::tree;
using namespace mlpack::perceptron;

BOOST_AUTO_TEST_SUITE(AdaBoostTest);

/**
 * This test case runs the AdaBoost.mh algorithm on the UCI Iris dataset.  It
 * checks whether the hamming loss breaches the upperbound, which is provided by
 * ztAccumulator.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundIris)
{
  arma::mat inputData;

  if (!data::Load("iris.csv", inputData))
    BOOST_FAIL("Cannot load test dataset iris.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("iris_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for iris iris_labels.txt");

  const size_t numClasses = max(labels.row(0)) + 1;

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptronIter iterations.
  int perceptronIter = 400;

  Perceptron<> p(inputData, labels.row(0), numClasses, perceptronIter);

  // Define parameters for AdaBoost.
  size_t iterations = 100;
  double tolerance = 1e-10;
  AdaBoost<> a(tolerance);
  double ztProduct = a.Train(inputData, labels.row(0), numClasses, p,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  // Check that ztProduct is finite.
  BOOST_REQUIRE_EQUAL(std::isfinite(ztProduct), true);
  BOOST_REQUIRE_LE(hammingLoss, ztProduct);
}

/**
 * This test case runs the AdaBoost.mh algorithm on the UCI Iris dataset.  It
 * checks if the error returned by running a single instance of the weak learner
 * is worse than running the boosted weak learner using adaboost.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorIris)
{
  arma::mat inputData;

  if (!data::Load("iris.csv", inputData))
    BOOST_FAIL("Cannot load test dataset iris.csv!");

  arma::Mat<size_t> labels;

  if (!data::Load("iris_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for iris iris_labels.txt");

  const size_t numClasses = max(labels.row(0)) + 1;

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptronIter iterations.
  int perceptronIter = 400;

  arma::Row<size_t> perceptronPrediction(labels.n_cols);
  Perceptron<> p(inputData, labels.row(0), numClasses, perceptronIter);
  p.Classify(inputData, perceptronPrediction);

  size_t countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != perceptronPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for AdaBoost.
  size_t iterations = 100;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), numClasses, p, iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE_LE(error, weakLearnerErrorRate);
}

/**
 * This test case runs the AdaBoost.mh algorithm on the UCI Vertebral Column
 * dataset.  It checks whether the hamming loss breaches the upperbound, which
 * is provided by ztAccumulator.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundVertebralColumn)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  const size_t numClasses = max(labels.row(0)) + 1;

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptronIter iterations.
  size_t perceptronIter = 800;
  Perceptron<> p(inputData, labels.row(0), numClasses, perceptronIter);

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(tolerance);
  double ztProduct = a.Train(inputData, labels.row(0), numClasses, p,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  // Check that ztProduct is finite.
  BOOST_REQUIRE_EQUAL(std::isfinite(ztProduct), true);
  BOOST_REQUIRE_LE(hammingLoss, ztProduct);
}

/**
 * This test case runs the AdaBoost.mh algorithm on the UCI Vertebral Column
 * dataset.  It checks if the error returned by running a single instance of the
 * weak learner is worse than running the boosted weak learner using adaboost.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorVertebralColumn)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  const size_t numClasses = max(labels.row(0)) + 1;

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptronIter iterations.
  size_t perceptronIter = 800;

  Row<size_t> perceptronPrediction(labels.n_cols);
  Perceptron<> p(inputData, labels.row(0), numClasses, perceptronIter);
  p.Classify(inputData, perceptronPrediction);

  size_t countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != perceptronPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), numClasses, p, iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE_LE(error, weakLearnerErrorRate);
}

/**
 * This test case runs the AdaBoost.mh algorithm on non-linearly separable
 * dataset.  It checks whether the hamming loss breaches the upperbound, which
 * is provided by ztAccumulator.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundNonLinearSepData)
{
  arma::mat inputData;
  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;
  if (!data::Load("train_labels_nonlinsep.txt", labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  const size_t numClasses = max(labels.row(0)) + 1;

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptronIter iterations.
  size_t perceptronIter = 800;
  Perceptron<> p(inputData, labels.row(0), numClasses, perceptronIter);

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(tolerance);
  double ztProduct = a.Train(inputData, labels.row(0), numClasses, p,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  // Check that ztProduct is finite.
  BOOST_REQUIRE_EQUAL(std::isfinite(ztProduct), true);
  BOOST_REQUIRE_LE(hammingLoss, ztProduct);
}

/**
 * This test case runs the AdaBoost.mh algorithm on a non-linearly separable
 * dataset.  It checks if the error returned by running a single instance of the
 * weak learner is worse than running the boosted weak learner using AdaBoost.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorNonLinearSepData)
{
  arma::mat inputData;
  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;
  if (!data::Load("train_labels_nonlinsep.txt", labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  const size_t numClasses = max(labels.row(0)) + 1;

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptronIter iterations.
  size_t perceptronIter = 800;

  Row<size_t> perceptronPrediction(labels.n_cols);
  Perceptron<> p(inputData, labels.row(0), numClasses, perceptronIter);
  p.Classify(inputData, perceptronPrediction);

  size_t countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != perceptronPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), numClasses, p, iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE_LE(error, weakLearnerErrorRate);
}

/**
 * This test case runs the AdaBoost.mh algorithm on the UCI Iris dataset.  It
 * checks whether the Hamming loss breaches the upper bound, which is provided
 * by ztAccumulator.  This uses decision stumps as the weak learner.
 */
BOOST_AUTO_TEST_CASE(HammingLossIris_DS)
{
  arma::mat inputData;
  if (!data::Load("iris.csv", inputData))
    BOOST_FAIL("Cannot load test dataset iris.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("iris_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for iris_labels.txt");

  // Define your own weak learner, decision stumps in this case.
  const size_t numClasses = 3;
  const size_t inpBucketSize = 6;
  arma::Row<size_t> labelsvec = labels.row(0);
  ID3DecisionStump ds(inputData, labelsvec, numClasses, inpBucketSize);

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<ID3DecisionStump> a(tolerance);
  double ztProduct = a.Train(inputData, labelsvec, numClasses, ds,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  // Check that ztProduct is finite.
  BOOST_REQUIRE_EQUAL(std::isfinite(ztProduct), true);
  BOOST_REQUIRE_LE(hammingLoss, ztProduct);
}

/**
 * This test case runs the AdaBoost.mh algorithm on a non-linearly separable
 * dataset.  It checks if the error returned by running a single instance of the
 * weak learner is worse than running the boosted weak learner using adaboost.
 * This is for the weak learner: decision stumps.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorIris_DS)
{
  arma::mat inputData;
  if (!data::Load("iris.csv", inputData))
    BOOST_FAIL("Cannot load test dataset iris.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("iris_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for iris_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, decision stumps in this case.
  const size_t numClasses = 3;
  const size_t inpBucketSize = 6;
  arma::Row<size_t> labelsvec = labels.row(0);

  arma::Row<size_t> dsPrediction(labels.n_cols);

  ID3DecisionStump ds(inputData, labelsvec, numClasses, inpBucketSize);
  ds.Classify(inputData, dsPrediction);

  size_t countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != dsPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;

  AdaBoost<ID3DecisionStump> a(inputData, labelsvec, numClasses, ds,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE_LE(error, weakLearnerErrorRate);
}

/**
 * This test case runs the AdaBoost.mh algorithm on the UCI Vertebral Column
 * dataset.  It checks if the error returned by running a single instance of the
 * weak learner is worse than running the boosted weak learner using adaboost.
 * This is for the weak learner: decision stumps.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundVertebralColumn_DS)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // Define your own weak learner, decision stumps in this case.
  const size_t numClasses = 3;
  const size_t inpBucketSize = 6;
  arma::Row<size_t> labelsvec = labels.row(0);

  ID3DecisionStump ds(inputData, labelsvec, numClasses, inpBucketSize);

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;

  AdaBoost<ID3DecisionStump> a(tolerance);
  double ztProduct = a.Train(inputData, labelsvec, numClasses, ds,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  // Check that ztProduct is finite.
  BOOST_REQUIRE_EQUAL(std::isfinite(ztProduct), true);
  BOOST_REQUIRE_LE(hammingLoss, ztProduct);
}

/**
 * This test case runs the AdaBoost.mh algorithm on the UCI Vertebral Column
 * dataset.  It checks if the error returned by running a single instance of the
 * weak learner is worse than running the boosted weak learner using adaboost.
 * This is for the weak learner: decision stumps.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorVertebralColumn_DS)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // Define your own weak learner, decision stumps in this case.
  const size_t numClasses = 3;
  const size_t inpBucketSize = 6;
  arma::Row<size_t> dsPrediction(labels.n_cols);
  arma::Row<size_t> labelsvec = labels.row(0);

  ID3DecisionStump ds(inputData, labelsvec, numClasses, inpBucketSize);
  ds.Classify(inputData, dsPrediction);

  size_t countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != dsPrediction(i))
      countWeakLearnerError++;

  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<ID3DecisionStump> a(inputData, labelsvec, numClasses, ds,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE_LE(error, weakLearnerErrorRate);
}

/**
 * This test case runs the AdaBoost.mh algorithm on non-linearly separable
 * dataset.  It checks whether the hamming loss breaches the upperbound, which
 * is provided by ztAccumulator.  This is for the weak learner: decision stumps.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundNonLinearSepData_DS)
{
  arma::mat inputData;
  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;
  if (!data::Load("train_labels_nonlinsep.txt", labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  // Define your own weak learner, decision stumps in this case.
  const size_t numClasses = 2;
  const size_t inpBucketSize = 6;
  arma::Row<size_t> labelsvec = labels.row(0);

  ID3DecisionStump ds(inputData, labelsvec, numClasses, inpBucketSize);

  // Define parameters for Adaboost.
  size_t iterations = 50;
  double tolerance = 1e-10;

  AdaBoost<ID3DecisionStump> a(tolerance);
  double ztProduct = a.Train(inputData, labelsvec, numClasses, ds,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  // Check that ztProduct is finite.
  BOOST_REQUIRE_EQUAL(std::isfinite(ztProduct), true);
  BOOST_REQUIRE_LE(hammingLoss, ztProduct);
}

/**
 * This test case runs the AdaBoost.mh algorithm on a non-linearly separable
 * dataset.  It checks if the error returned by running a single instance of the
 * weak learner is worse than running the boosted weak learner using adaboost.
 * This for the weak learner: decision stumps.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorNonLinearSepData_DS)
{
  arma::mat inputData;
  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;
  if (!data::Load("train_labels_nonlinsep.txt", labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  // Define your own weak learner, decision stumps in this case.
  const size_t numClasses = 2;
  const size_t inpBucketSize = 3;
  arma::Row<size_t> labelsvec = labels.row(0);

  arma::Row<size_t> dsPrediction(labels.n_cols);

  ID3DecisionStump ds(inputData, labelsvec, numClasses, inpBucketSize);
  ds.Classify(inputData, dsPrediction);

  size_t countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != dsPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for AdaBoost.
  size_t iterations = 500;
  double tolerance = 1e-23;

  AdaBoost<ID3DecisionStump > a(inputData, labelsvec, numClasses, ds,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels;
  a.Classify(inputData, predictedLabels);

  size_t countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if (labels(i) != predictedLabels(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE_LE(error, weakLearnerErrorRate);
}

/**
 * This test case runs the AdaBoost.mh algorithm on the UCI Vertebral Column
 * dataset.  It tests the Classify function and checks for a satisfactory error
 * rate.
 */
BOOST_AUTO_TEST_CASE(ClassifyTest_VERTEBRALCOL)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptronIter iterations.
  size_t perceptronIter = 1000;

  arma::mat testData;

  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2_test.csv!");

  arma::Mat<size_t> trueTestLabels;

  if (!data::Load("vc2_test_labels.txt", trueTestLabels))
    BOOST_FAIL("Cannot load labels for vc2_test_labels.txt");

  const size_t numClasses = max(labels.row(0)) + 1;

  Row<size_t> perceptronPrediction(labels.n_cols);
  Perceptron<> p(inputData, labels.row(0), numClasses, perceptronIter);
  p.Classify(inputData, perceptronPrediction);

  // Define parameters for AdaBoost.
  size_t iterations = 100;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), numClasses, p, iterations, tolerance);

  arma::Row<size_t> predictedLabels1(testData.n_cols),
                    predictedLabels2(testData.n_cols);
  arma::mat probabilities;

  a.Classify(testData, predictedLabels1);
  a.Classify(testData, predictedLabels2, probabilities);

  BOOST_REQUIRE_EQUAL(probabilities.n_cols, testData.n_cols);
  BOOST_REQUIRE_EQUAL(probabilities.n_rows, numClasses);

  for (size_t i = 0; i < predictedLabels1.n_cols; ++i)
    BOOST_REQUIRE_EQUAL(predictedLabels1[i], predictedLabels2[i]);

  arma::colvec pRow;
  arma::uword maxIndex = 0;

  for (size_t i = 0; i < predictedLabels1.n_cols; i++)
  {
    pRow = probabilities.unsafe_col(i);
    pRow.max(maxIndex);
    BOOST_REQUIRE_EQUAL(predictedLabels1(i), maxIndex);
    BOOST_REQUIRE_CLOSE(arma::accu(probabilities.col(i)), 1, 1e-5);
  }

  size_t localError = 0;
  for (size_t i = 0; i < trueTestLabels.n_cols; i++)
    if (trueTestLabels(i) != predictedLabels1(i))
      localError++;

  double lError = (double) localError / trueTestLabels.n_cols;
  BOOST_REQUIRE_LE(lError, 0.30);
}

/**
 * This test case runs the AdaBoost.mh algorithm on a non linearly separable
 * dataset.  It tests the Classify function and checks for a satisfactory error
 * rate.
 */
BOOST_AUTO_TEST_CASE(ClassifyTest_NONLINSEP)
{
  arma::mat inputData;
  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;
  if (!data::Load("train_labels_nonlinsep.txt", labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  // Define your own weak learner; in this test decision stumps are used.
  const size_t numClasses = 2;
  const size_t inpBucketSize = 3;
  arma::Row<size_t> labelsvec = labels.row(0);

  arma::mat testData;

  if (!data::Load("test_nonlinsep.txt", testData))
    BOOST_FAIL("Cannot load test dataset test_nonlinsep.txt!");

  arma::Mat<size_t> trueTestLabels;

  if (!data::Load("test_labels_nonlinsep.txt", trueTestLabels))
    BOOST_FAIL("Cannot load labels for test_labels_nonlinsep.txt");

  arma::Row<size_t> dsPrediction(labels.n_cols);

  ID3DecisionStump ds(inputData, labelsvec, numClasses, inpBucketSize);

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<ID3DecisionStump > a(inputData, labelsvec, numClasses, ds,
      iterations, tolerance);

  arma::Row<size_t> predictedLabels1(testData.n_cols),
                    predictedLabels2(testData.n_cols);
  arma::mat probabilities;

  a.Classify(testData, predictedLabels1);
  a.Classify(testData, predictedLabels2, probabilities);

  BOOST_REQUIRE_EQUAL(probabilities.n_cols, testData.n_cols);

  for (size_t i = 0; i < predictedLabels1.n_cols; ++i)
    BOOST_REQUIRE_EQUAL(predictedLabels1[i], predictedLabels2[i]);

  arma::colvec pRow;
  arma::uword maxIndex = 0;

  for (size_t i = 0; i < predictedLabels1.n_cols; i++)
  {
    pRow = probabilities.unsafe_col(i);
    pRow.max(maxIndex);
    BOOST_REQUIRE_EQUAL(predictedLabels1(i), maxIndex);
    BOOST_REQUIRE_CLOSE(arma::accu(probabilities.col(i)), 1, 1e-5);
  }

  size_t localError = 0;
  for (size_t i = 0; i < trueTestLabels.n_cols; i++)
    if (trueTestLabels(i) != predictedLabels1(i))
      localError++;

  double lError = (double) localError / trueTestLabels.n_cols;
  BOOST_REQUIRE_LE(lError, 0.30);
}

/**
 * This test case runs the AdaBoost.mh algorithm on the UCI Iris Dataset.  It
 * trains it on two thirds of the Iris dataset (iris_train.csv), and tests on
 * the remaining third of the dataset (iris_test.csv).  It tests the Classify()
 * function and checks for a satisfactory error rate.
 */
BOOST_AUTO_TEST_CASE(ClassifyTest_IRIS)
{
  arma::mat inputData;
  if (!data::Load("iris_train.csv", inputData))
    BOOST_FAIL("Cannot load test dataset iris_train.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("iris_train_labels.csv", labels))
    BOOST_FAIL("Cannot load labels for iris_train_labels.csv");
  const size_t numClasses = max(labels.row(0)) + 1;

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptronIter iterations.
  size_t perceptronIter = 800;

  Perceptron<> p(inputData, labels.row(0), numClasses, perceptronIter);

  // Define parameters for AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), numClasses, p, iterations, tolerance);

  arma::mat testData;
  if (!data::Load("iris_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset iris_test.csv!");

  arma::Row<size_t> predictedLabels(testData.n_cols);
  a.Classify(testData, predictedLabels);

  arma::Mat<size_t> trueTestLabels;
  if (!data::Load("iris_test_labels.csv", trueTestLabels))
    BOOST_FAIL("Cannot load test dataset iris_test_labels.csv!");

  arma::Row<size_t> predictedLabels1(testData.n_cols),
                    predictedLabels2(testData.n_cols);
  arma::mat probabilities;

  a.Classify(testData, predictedLabels1);
  a.Classify(testData, predictedLabels2, probabilities);

  BOOST_REQUIRE_EQUAL(probabilities.n_cols, testData.n_cols);

  for (size_t i = 0; i < predictedLabels1.n_cols; ++i)
    BOOST_REQUIRE_EQUAL(predictedLabels1[i], predictedLabels2[i]);

  arma::colvec pRow;
  arma::uword maxIndex = 0;

  for (size_t i = 0; i < predictedLabels1.n_cols; i++)
  {
    pRow = probabilities.unsafe_col(i);
    pRow.max(maxIndex);
    BOOST_REQUIRE_EQUAL(predictedLabels1(i), maxIndex);
    BOOST_REQUIRE_CLOSE(arma::accu(probabilities.col(i)), 1, 1e-5);
  }

  size_t localError = 0;
  for (size_t i = 0; i < trueTestLabels.n_cols; i++)
    if (trueTestLabels(i) != predictedLabels1(i))
      localError++;
  double lError = (double) localError / labels.n_cols;
  BOOST_REQUIRE_LE(lError, 0.30);
}

/**
 * Ensure that the Train() function works like it is supposed to, by building
 * AdaBoost on one dataset and then re-training on another dataset.
 */
BOOST_AUTO_TEST_CASE(TrainTest)
{
  // First train on the iris dataset.
  arma::mat inputData;
  if (!data::Load("iris_train.csv", inputData))
    BOOST_FAIL("Cannot load test dataset iris_train.csv!");

  arma::Mat<size_t> labels;
  if (!data::Load("iris_train_labels.csv", labels))
    BOOST_FAIL("Cannot load labels for iris_train_labels.csv");

  const size_t numClasses = max(labels.row(0)) + 1;

  size_t perceptronIter = 800;
  Perceptron<> p(inputData, labels.row(0), numClasses, perceptronIter);

  // Now train AdaBoost.
  size_t iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), numClasses, p, iterations, tolerance);

  // Now load another dataset...
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  const size_t newNumClasses = max(labels.row(0)) + 1;

  Perceptron<> p2(inputData, labels.row(0), newNumClasses, perceptronIter);

  a.Train(inputData, labels.row(0), newNumClasses, p2, iterations, tolerance);

  // Load test set to see if it trained on vc2 correctly.
  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2_test.csv!");

  arma::Mat<size_t> trueTestLabels;
  if (!data::Load("vc2_test_labels.txt", trueTestLabels))
    BOOST_FAIL("Cannot load labels for vc2_test_labels.txt");

  // Define parameters for AdaBoost.
  arma::Row<size_t> predictedLabels(testData.n_cols);
  a.Classify(testData, predictedLabels);

  int localError = 0;
  for (size_t i = 0; i < trueTestLabels.n_cols; i++)
    if (trueTestLabels(i) != predictedLabels(i))
      localError++;

  double lError = (double) localError / trueTestLabels.n_cols;

  BOOST_REQUIRE_LE(lError, 0.30);
}

BOOST_AUTO_TEST_CASE(PerceptronSerializationTest)
{
  // Build an AdaBoost object.
  mat data = randu<mat>(10, 500);
  Row<size_t> labels(500);
  for (size_t i = 0; i < 250; ++i)
    labels[i] = 0;
  for (size_t i = 250; i < 500; ++i)
    labels[i] = 1;

  Perceptron<> p(data, labels, 2, 800);
  AdaBoost<> ab(data, labels, 2, p, 50, 1e-10);

  // Now create another dataset to train with.
  mat otherData = randu<mat>(5, 200);
  Row<size_t> otherLabels(200);
  for (size_t i = 0; i < 100; ++i)
    otherLabels[i] = 1;
  for (size_t i = 100; i < 150; ++i)
    otherLabels[i] = 0;
  for (size_t i = 150; i < 200; ++i)
    otherLabels[i] = 2;

  Perceptron<> p2(otherData, otherLabels, 3, 500);
  AdaBoost<> abText(otherData, otherLabels, 3, p2, 50, 1e-10);

  AdaBoost<> abXml, abBinary;

  SerializeObjectAll(ab, abXml, abText, abBinary);

  // Now check that the objects are the same.
  BOOST_REQUIRE_CLOSE(ab.Tolerance(), abXml.Tolerance(), 1e-5);
  BOOST_REQUIRE_CLOSE(ab.Tolerance(), abText.Tolerance(), 1e-5);
  BOOST_REQUIRE_CLOSE(ab.Tolerance(), abBinary.Tolerance(), 1e-5);

  BOOST_REQUIRE_EQUAL(ab.WeakLearners(), abXml.WeakLearners());
  BOOST_REQUIRE_EQUAL(ab.WeakLearners(), abText.WeakLearners());
  BOOST_REQUIRE_EQUAL(ab.WeakLearners(), abBinary.WeakLearners());

  for (size_t i = 0; i < ab.WeakLearners(); ++i)
  {
    CheckMatrices(ab.WeakLearner(i).Weights(),
                  abXml.WeakLearner(i).Weights(),
                  abText.WeakLearner(i).Weights(),
                  abBinary.WeakLearner(i).Weights());

    CheckMatrices(ab.WeakLearner(i).Biases(),
                  abXml.WeakLearner(i).Biases(),
                  abText.WeakLearner(i).Biases(),
                  abBinary.WeakLearner(i).Biases());
  }
}

BOOST_AUTO_TEST_CASE(ID3DecisionStumpSerializationTest)
{
  // Build an AdaBoost object.
  mat data = randu<mat>(10, 500);
  Row<size_t> labels(500);
  for (size_t i = 0; i < 250; ++i)
    labels[i] = 0;
  for (size_t i = 250; i < 500; ++i)
    labels[i] = 1;

  ID3DecisionStump p(data, labels, 2, 800);
  AdaBoost<ID3DecisionStump> ab(data, labels, 2, p, 50, 1e-10);

  // Now create another dataset to train with.
  mat otherData = randu<mat>(5, 200);
  Row<size_t> otherLabels(200);
  for (size_t i = 0; i < 100; ++i)
    otherLabels[i] = 1;
  for (size_t i = 100; i < 150; ++i)
    otherLabels[i] = 0;
  for (size_t i = 150; i < 200; ++i)
    otherLabels[i] = 2;

  ID3DecisionStump p2(otherData, otherLabels, 3, 500);
  AdaBoost<ID3DecisionStump> abText(otherData, otherLabels, 3, p2, 50, 1e-10);

  AdaBoost<ID3DecisionStump> abXml, abBinary;

  SerializeObjectAll(ab, abXml, abText, abBinary);

  // Now check that the objects are the same.
  BOOST_REQUIRE_CLOSE(ab.Tolerance(), abXml.Tolerance(), 1e-5);
  BOOST_REQUIRE_CLOSE(ab.Tolerance(), abText.Tolerance(), 1e-5);
  BOOST_REQUIRE_CLOSE(ab.Tolerance(), abBinary.Tolerance(), 1e-5);

  BOOST_REQUIRE_EQUAL(ab.WeakLearners(), abXml.WeakLearners());
  BOOST_REQUIRE_EQUAL(ab.WeakLearners(), abText.WeakLearners());
  BOOST_REQUIRE_EQUAL(ab.WeakLearners(), abBinary.WeakLearners());

  for (size_t i = 0; i < ab.WeakLearners(); ++i)
  {
    BOOST_REQUIRE_EQUAL(ab.WeakLearner(i).SplitDimension(),
                        abXml.WeakLearner(i).SplitDimension());
    BOOST_REQUIRE_EQUAL(ab.WeakLearner(i).SplitDimension(),
                        abText.WeakLearner(i).SplitDimension());
    BOOST_REQUIRE_EQUAL(ab.WeakLearner(i).SplitDimension(),
                        abBinary.WeakLearner(i).SplitDimension());
  }
}

BOOST_AUTO_TEST_SUITE_END();


#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]);

		}
	}

}


//  (C) Copyright Matt Borland 2022.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <benchmark/benchmark.h>
#include <boost/math/special_functions/logsumexp.hpp>
#include <boost/math/tools/random_vector.hpp>

using boost::math::logsumexp;
using boost::math::generate_random_vector;

template <typename Real>
void logsumexp_performance(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<Real> test_set = generate_random_vector<Real>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(logsumexp(test_set));
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(logsumexp_performance, float)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity()->UseRealTime();
BENCHMARK_TEMPLATE(logsumexp_performance, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity()->UseRealTime();
BENCHMARK_TEMPLATE(logsumexp_performance, long double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity()->UseRealTime();

BENCHMARK_MAIN();


#ifndef BOOST_GIL_IMAGE_PROCESSING_HESSIAN_HPP
#define BOOST_GIL_IMAGE_PROCESSING_HESSIAN_HPP

#include <boost/gil/extension/numeric/kernel.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/typedefs.hpp>
#include <stdexcept>

namespace boost {
namespace gil {

/// \brief Computes Hessian response
///
/// Computes Hessian response based on computed entries of Hessian matrix, e.g.
/// second order derivates in x and y, and derivatives in both x, y. d stands
/// for derivative, and x or y stand for derivative direction. For example, ddxx
/// means taking two derivatives (gradients) in horizontal direction. Weights
/// change perception of surroinding pixels. Additional filtering is strongly
/// advised.
template <typename GradientView, typename T, typename Allocator,
          typename OutputView>
inline void compute_hessian_responses(
    GradientView ddxx, GradientView dxdy, GradientView ddyy,
    const detail::kernel_2d<T, Allocator> &weights, OutputView dst) {
  if (ddxx.dimensions() != ddyy.dimensions() ||
      ddyy.dimensions() != dxdy.dimensions() ||
      dxdy.dimensions() != dst.dimensions() ||
      weights.center_x() != weights.center_y()) {
    throw std::invalid_argument("dimensions of views are not the same"
                                " or weights don't have equal width and height"
                                " or weights' dimensions are not odd");
  }
  // Use pixel type of output, as values will be written to output
  using pixel_t =
      typename std::remove_reference<decltype(std::declval<OutputView>()(
          0, 0))>::type;

  using channel_t =
      typename std::remove_reference<decltype(std::declval<pixel_t>().at(
          std::integral_constant<int, 0>{}))>::type;

  auto center = weights.center_y();
  for (auto y = center; y < dst.height() - center; ++y) {
    for (auto x = center; x < dst.width() - center; ++x) {
      auto ddxx_i = channel_t();
      auto ddyy_i = channel_t();
      auto dxdy_i = channel_t();
      for (typename OutputView::coord_t w_y = 0; w_y < weights.size(); ++w_y) {
        for (typename OutputView::coord_t w_x = 0; w_x < weights.size();
             ++w_x) {
          ddxx_i += ddxx(x + w_x - center, y + w_y - center)
                        .at(std::integral_constant<int, 0>{}) *
                    weights.at(w_x, w_y);
          ddyy_i += ddyy(x + w_x - center, y + w_y - center)
                        .at(std::integral_constant<int, 0>{}) *
                    weights.at(w_x, w_y);
          dxdy_i += dxdy(x + w_x - center, y + w_y - center)
                        .at(std::integral_constant<int, 0>{}) *
                    weights.at(w_x, w_y);
        }
      }
      auto determinant = ddxx_i * ddyy_i - dxdy_i * dxdy_i;
      dst(x, y).at(std::integral_constant<int, 0>{}) = determinant;
    }
  }
}

} // namespace gil
} // namespace boost

#endif


//==================================================================================================
/*!
  @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_FUNCTION_DIV_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DIV_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-oerator
    Function object extending divides

    Calculate the quotient of the two parameters of the same type with or without options.

    @par Semantic

    For any value @c a and @c b of type @c T,

    @code
    T r = div({option, }a, b);
    @endcode

    returns the quotient of @c a by @c b respecting to the options specified.

    By default, this functions is equivalent to divides(a, b).
    Options may be ceil, floor, fix, round, nearbyint (in the namespace booost::simd)
    and provide the same result as the calls divceil(a, b), divfloor(a, b),
    divfix(a, b), divround(a, b), divnearbyint(a, b).

    @return The quotient of the two parameters.
  **/
  T div(T const& a, T const& b);

  //@overload
  T div(Option const& o, T const& a, T const& b);

} }
#endif

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

#endif


#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include <iostream>
#include <boost/test/unit_test.hpp>

#include "Map/ProbabilityValues.h"

namespace VISFS {
namespace Map {

BOOST_AUTO_TEST_CASE(OddsConversions) {
    BOOST_CHECK_CLOSE(probabilityFromOdds(odds(kMinProbability)), kMinProbability, 1e-6);
    BOOST_CHECK_CLOSE(probabilityFromOdds(odds(kMaxProbability)), kMaxProbability, 1e-6);
    BOOST_CHECK_CLOSE(probabilityFromOdds(odds(0.5)), 0.5, 1e-6);
}

BOOST_AUTO_TEST_CASE(OddsConversionsCorrespondenceCost) {
    BOOST_CHECK_CLOSE(probabilityToCorrespondceCost(probabilityFromOdds(odds(correspondenceCostToProbability(kMinCorrespondenceCost)))), kMinCorrespondenceCost, 1e-6);
    BOOST_CHECK_CLOSE(probabilityToCorrespondceCost(probabilityFromOdds(odds(correspondenceCostToProbability(kMaxCorrespondenceCost)))), kMaxCorrespondenceCost, 1e-6);
    BOOST_CHECK_CLOSE(probabilityToCorrespondceCost(probabilityFromOdds(odds(correspondenceCostToProbability(0.5)))), 0.5, 1e-6);
}

BOOST_AUTO_TEST_CASE(ProbabilityValueToCorrespondenceCostValueConversions) {
    for (uint16_t i = 0; i < 32768; ++i) {
        BOOST_CHECK_EQUAL(probabilityValueToCorrespondenceCostValue(correspondenceCostValueToProbabilityValue(i)), i);
        BOOST_CHECK_EQUAL(correspondenceCostValueToProbabilityValue(probabilityValueToCorrespondenceCostValue(i)), i);
    }
}

BOOST_AUTO_TEST_CASE(ProbabilityValueToCorrespondenceCostValueConversionsWithUpdateMarker) {
    for (uint16_t i = 1; i < 32768; ++i) {
        BOOST_CHECK_EQUAL(probabilityValueToCorrespondenceCostValue(correspondenceCostValueToProbabilityValue(i + kUpdateMarker)), i + kUpdateMarker);
        BOOST_CHECK_EQUAL(correspondenceCostValueToProbabilityValue(probabilityValueToCorrespondenceCostValue(i + kUpdateMarker)), i + kUpdateMarker);
    }
}

BOOST_AUTO_TEST_CASE(ConversionLookUpTable) {
    BOOST_CHECK_CLOSE(valueToProbability(0), 1.0 - valueToCorrespondenceCost(0), 1e-6);
    for (uint16_t i = 1; i < 32768; ++i) {
        BOOST_CHECK_CLOSE(valueToProbability(i), valueToCorrespondenceCost(i), 1e-6);
    }
}

BOOST_AUTO_TEST_CASE(CellUpdate) {
    std::vector<uint16_t> probabilityTable = computeLookupTableToApplyOdds(odds(0.9));
    std::vector<uint16_t> correspondenceTable = computeLookupTableToApplyCorrespondenceCostOdds(odds(0.9));
    uint16_t cellPgPreUpdate = 0;
    uint16_t cellCgPreUpdate = 0;
    uint16_t cellPgPoseUpdate = probabilityTable[cellPgPreUpdate];
    uint16_t cellCgPoseUpdate = correspondenceTable[cellCgPreUpdate];
    double pPost = valueToProbability(cellPgPoseUpdate);
    double cPost = valueToCorrespondenceCost(cellCgPoseUpdate);
    BOOST_CHECK_CLOSE(pPost, 1.0 - cPost, 1e-6);
    int numEvaluations = 5000;
    for (int iProbability = 0; iProbability < numEvaluations; ++iProbability) {
        double p = (static_cast<double>(iProbability) / static_cast<double>(numEvaluations)) * (kMaxProbability - kMinProbability) + kMinProbability;
        cellPgPreUpdate = probabilityToValue(p);
        cellCgPreUpdate = correspondenceCostToValue(probabilityToCorrespondceCost(p));
        double pValue = (uClamp(p, kMinProbability, kMaxProbability) - kMinProbability) * (32766.0 / (kMaxProbability - kMinProbability));
        double cValue = (uClamp(probabilityToCorrespondceCost(p), kMinProbability, kMaxProbability) - kMinProbability)* (32766.0 / (kMaxProbability - kMinProbability));
        // BOOST_CHECK_CLOSE(cellPgPreUpdate, static_cast<uint16_t>(32768) - cellCgPreUpdate, 1);
        BOOST_CHECK_EQUAL(static_cast<int>(cellPgPreUpdate), 32768 - static_cast<int>(cellCgPreUpdate));
        cellPgPoseUpdate = probabilityTable[cellPgPreUpdate];
        cellCgPoseUpdate = correspondenceTable[cellCgPreUpdate];
        pPost = valueToProbability(cellPgPoseUpdate);
        cPost = valueToCorrespondenceCost(cellCgPoseUpdate);
        BOOST_CHECK_CLOSE(pPost, 1.0 - cPost, 5e-3);
    }
}

BOOST_AUTO_TEST_CASE(MultipleCellUpdate) {
    std::vector<uint16_t> probabilityTable = computeLookupTableToApplyOdds(odds(0.55));
    std::vector<uint16_t> correspondenceTable = computeLookupTableToApplyCorrespondenceCostOdds(odds(0.55));
    uint16_t cellPgPostUpdate = probabilityTable[0];
    uint16_t cellCgPostUpdate = correspondenceTable[0];
    double pPost = valueToProbability(cellPgPostUpdate);
    double cPost = valueToCorrespondenceCost(cellCgPostUpdate);
    BOOST_CHECK_CLOSE(pPost, 1.0 - cPost, 1e-6);
    int numEvaluations = 5000;
    for (int iProbability = 0; iProbability < numEvaluations; ++ iProbability) {
        double p = (static_cast<double>(iProbability) / static_cast<double>(numEvaluations)) * (kMaxProbability - kMinProbability) + kMinProbability;
        cellPgPostUpdate = probabilityToValue(p) + kUpdateMarker;
        cellCgPostUpdate = correspondenceCostToValue(probabilityToCorrespondceCost(p)) + kUpdateMarker;
        for (int iUpdate = 0; iUpdate < 20; ++ iUpdate) {
            cellPgPostUpdate = probabilityTable[cellPgPostUpdate - kUpdateMarker];
            cellCgPostUpdate = correspondenceTable[cellCgPostUpdate - kUpdateMarker];
        }
        pPost = valueToProbability(cellPgPostUpdate);
        cPost = valueToCorrespondenceCost(cellCgPostUpdate);
        BOOST_CHECK_CLOSE(pPost, 1.0 - cPost, 5e-5);
    }

}

BOOST_AUTO_TEST_CASE(EqualityLookupTableToApplyOdds) {
    std::vector<uint16_t> probabilityTable = computeLookupTableToApplyOdds(0.3);
    std::vector<uint16_t> correspondenceTable = computeLookupTableToApplyCorrespondenceCostOdds(0.3);

    for (int i = 0; i < 32768; ++i) {
        BOOST_CHECK_EQUAL(probabilityTable[i], correspondenceCostValueToProbabilityValue(correspondenceTable[probabilityValueToCorrespondenceCostValue(i)]));
        BOOST_CHECK_EQUAL(probabilityValueToCorrespondenceCostValue(probabilityTable[correspondenceCostValueToProbabilityValue(i)]), correspondenceTable[i]);
    }
}

}
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <vector>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"

#include "Tudat/Mathematics/Interpolators/cubicSplineInterpolator.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

// NOTE: No benchmark data from established software package has been used. Matlab implementation
// is slightly different than that implemented here, yielding error of at worst 10e-7 compared
// when using error function, which is used here as test function.
// However, a Matlab implementation of the same algorithm was found on te Matlab forum,
// which is used here as benchmark.
// In addition, the code has been used rather extensively and has been checked for basic
// characteristics such as continuity.
BOOST_AUTO_TEST_SUITE( test_cubic_spline_interpolator )

// Test implementation of cubic spline class.
BOOST_AUTO_TEST_CASE( testCubicSplineInterpolator )
{
    // Test 1: Compare with analytical function 2 + 3x + 5x^2.
    {
        // Declare and initialize independent variable values.
        std::vector< double > independentVariables;
        independentVariables.resize( 6 );
        independentVariables[ 0 ] = 1.0;
        independentVariables[ 1 ] = 3.0;
        independentVariables[ 2 ] = 5.0;
        independentVariables[ 3 ] = 7.0;
        independentVariables[ 4 ] = 9.0;
        independentVariables[ 5 ] = 11.0;

        // Declare and initialize dependent variable values.
        std::vector< double > dependentVariables;
        dependentVariables.resize( 6 );
        dependentVariables[ 0 ] = 10.0;
        dependentVariables[ 1 ] = 56.0;
        dependentVariables[ 2 ] = 142.0;
        dependentVariables[ 3 ] = 268.0;
        dependentVariables[ 4 ] = 434.0;
        dependentVariables[ 5 ] = 640.0;

        // Declare and initialize target independent variable value.
        const double targetIndependentVariableValue = 6.0;

        // Declare and initialize expected result of interpolation from analytical equation.
        const double analyticalValue = 200.0;

        // Declare cubic spline object and initialize with input data.
        interpolators::CubicSplineInterpolatorDouble cubicSplineInterpolation(
                    independentVariables, dependentVariables );

        // Declare interpolated dependent variable value and execute interpolation.
        const double interpolatedDependentVariableValue = cubicSplineInterpolation.interpolate(
                    targetIndependentVariableValue );

        // Check if test result match analytical result.
        BOOST_CHECK_SMALL(  std::fabs( analyticalValue - interpolatedDependentVariableValue )
                            / analyticalValue,
                            5.0e-3 );
    }
}

// Test exception handling implementation of cubic spline class.
BOOST_AUTO_TEST_CASE( testCubicSplineInterpolation_exception_empty_vectors )
{
    // Test 2: Interpolate with empty vectors.
    // Declare independent and dependent variable vectors.
    std::vector< double > independentVariables, dependentVariables;

    // Declare and initialize flag.
    bool areDependentAndIndependentVariablesInitialized = true;

    // Try to initialize with empty vectors.
    try
    {
        // Declare cubic spline object and initialize with input data.
        interpolators::CubicSplineInterpolatorDouble cubicSplineInterpolation(
                    independentVariables, dependentVariables );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        areDependentAndIndependentVariablesInitialized = false;
    }

    // Check value of flag.
    BOOST_CHECK( !areDependentAndIndependentVariablesInitialized );
}

// Test cubic spline interpolator by comparing to Matlab code posted at
// http://www.mathworks.com/matlabcentral/newsreader/view_thread/173708.
BOOST_AUTO_TEST_CASE( test_cubicSplineInterpolator_matlab_forum_compare )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/interpolator_test_input_data.dat","," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;
    std::vector< double > dependentVariableValues;
    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
        dependentVariableValues.push_back( inputData( i, 1 ) );
    }

    // Create cubic spline interpolator using hunting algorithm.
    CubicSplineInterpolatorDouble cubicSplineInterpolator(
                independentVariableValues, dependentVariableValues, huntingAlgorithm );

    // Load points at which interpolator is to be evaluated and data generated by Matlab.
    Eigen::MatrixXd benchmarkData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/"
                + "cubic_spline_interpolator_test_output_data.dat",
                "," );

    // Perform interpolation for required data points.
    Eigen::VectorXd outputData = Eigen::VectorXd( benchmarkData.rows( ) );
    for ( int i = 0; i < outputData.rows( ); i++ )
    {
        outputData[ i ] = cubicSplineInterpolator.interpolate( benchmarkData( i, 0 ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( benchmarkData.block( 0, 1, benchmarkData.rows( ), 1 ),
                                       outputData, 1.0e-13 );

    // Create cubic spline interpolator, now using binary search algorithm.
    cubicSplineInterpolator = CubicSplineInterpolatorDouble(
                independentVariableValues, dependentVariableValues, binarySearch );

    // Perform interpolation for required data points.
    outputData = Eigen::VectorXd( benchmarkData.rows( ) );
    for ( int i = 0; i < outputData.rows( ); i++ )
    {
        outputData[ i ] = cubicSplineInterpolator.interpolate( benchmarkData( i, 0 ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( benchmarkData.block( 0, 1, benchmarkData.rows( ), 1 ),
                                       outputData, 1.0e-13 );
}

// Test cubic spline interpolator by comparing to Matlab implementation. Note that the two
// implementations are not identical, since the Matlab implementation imposes zero first
// derivatives whereas the present implementation imposes zero second derivatives at endpoints.
BOOST_AUTO_TEST_CASE( test_cubicSplineInterpolator_matlab_compare )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/interpolator_test_input_data.dat", "," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;
    std::vector< double > dependentVariableValues;
    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
        dependentVariableValues.push_back( inputData( i, 1 ) );
    }

    // Create cubic spline interpolator.
    CubicSplineInterpolatorDouble linearInterpolator(
                independentVariableValues, dependentVariableValues );

    // Load points at which interpolator is to be evaluated and data generated by Matlab.
    Eigen::MatrixXd benchmarkData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/"
                + "cubic_spline_interpolator_approximate_test_output_data.dat", "," );

    // Perform interpolation for required data points.
    Eigen::VectorXd outputData = Eigen::VectorXd( benchmarkData.rows( ) );
    for ( int i = 0; i < outputData.rows( ); i++ )
    {
        outputData[ i ] = linearInterpolator.interpolate( benchmarkData( i, 0 ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( benchmarkData.block( 0, 1, benchmarkData.rows( ), 1 ),
                                       outputData, 1.0e-5 );
}

// Test linear interpolation outside of independent variable range
BOOST_AUTO_TEST_CASE( test_cubicSplineInterpolator_boundary_case )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/interpolator_test_input_data.dat","," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;
    std::vector< double > dependentVariableValues;

    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
        dependentVariableValues.push_back( inputData( i, 1 ) );
    }

    // Create linear interpolator using hunting algorithm.
    double valueOffset = 2.0;
    double valueBelowMinimumValue = independentVariableValues[ 0 ] - valueOffset;
    double valueAboveMaximumValue = independentVariableValues[ inputData.rows( ) - 1 ] + valueOffset;
    double interpolatedValue = TUDAT_NAN, expectedValue = TUDAT_NAN;
    bool exceptionIsCaught = false;

    for( unsigned int i = 0; i < 5; i++ )
    {
        CubicSplineInterpolatorDouble linearInterpolator(
                    independentVariableValues, dependentVariableValues, huntingAlgorithm,
                    static_cast< BoundaryInterpolationType >( i ) );

        if( static_cast< BoundaryInterpolationType >( i ) == throw_exception_at_boundary )
        {
            try
            {
                linearInterpolator.interpolate( valueBelowMinimumValue );
            }
            catch( std::runtime_error )
            {
                exceptionIsCaught = true;
            }
            BOOST_CHECK_EQUAL( exceptionIsCaught, true );

            exceptionIsCaught = false;
            try
            {
                linearInterpolator.interpolate( valueAboveMaximumValue );
            }
            catch( std::runtime_error )
            {
                exceptionIsCaught = true;
            }
            BOOST_CHECK_EQUAL( exceptionIsCaught, true );
        }
        else if( ( static_cast< BoundaryInterpolationType >( i ) == use_boundary_value ) ||
                 ( static_cast< BoundaryInterpolationType >( i ) == use_boundary_value_with_warning ) )
        {
            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, dependentVariableValues.at( 0 ), 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, dependentVariableValues.at( inputData.rows( ) - 1 ), 1.0E-15 );

        }
        else if( ( static_cast< BoundaryInterpolationType >( i ) == extrapolate_at_boundary ) ||
                 ( static_cast< BoundaryInterpolationType >( i ) == extrapolate_at_boundary_with_warning ) )
        {
            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            expectedValue = -1.0171383008483266; // computed with MATLAB
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, expectedValue, 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            expectedValue = 1.0171383008483266; // computed with MATLAB
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, expectedValue, 1.0E-15 );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//==================================================================================================
/*!
  @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


/* ----------------------------------------------------------------------------
 * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file  testStatics.cpp
 * @brief Test calculations for statics.
 * @author Frank Dellaert
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>

#include <boost/bind.hpp>
#include <cmath>

#include "gtdynamics/statics/Statics.h"
#include "gtdynamics/universal_robot/RobotModels.h"

using namespace gtdynamics;
using namespace gtsam;
constexpr double kTol = 1e-6;

namespace example {
constexpr double g = 9.8;
const Robot robot = gtdynamics::CreateRobotFromFile(
    kSdfPath + std::string("test/four_bar_linkage.sdf"));
Vector3 gravity(0, 0, -g);
}  // namespace example

TEST(Statics, GravityWrench1) {
  using namespace example;
  const Pose3 wTcom(Rot3(), Point3(1, 0, 0));
  const double mass = robot.link("l1")->mass();
  Matrix6 actualH;
  EXPECT(assert_equal((Vector(6) << 0, 0, 0, 0, 0, -100 * g).finished(),
                      GravityWrench(gravity, mass, wTcom, actualH), kTol));
  Matrix6 numericalH = numericalDerivative11<Vector6, Pose3>(
      boost::bind(&GravityWrench, gravity, mass, _1, boost::none), wTcom);
  EXPECT(assert_equal(numericalH, actualH, kTol));
}

TEST(Statics, GravityWrench2) {
  using namespace example;
  const Pose3 wTcom(Rot3::Rx(M_PI_2), Point3(1, 0, 0));
  const double mass = robot.link("l2")->mass();
  Matrix6 actualH;
  EXPECT(assert_equal((Vector(6) << 0, 0, 0, 0, -15 * g, 0).finished(),
                      GravityWrench(gravity, mass, wTcom, actualH), kTol));
  Matrix6 numericalH = numericalDerivative11<Vector6, Pose3>(
      boost::bind(&GravityWrench, gravity, mass, _1, boost::none), wTcom);
  EXPECT(assert_equal(numericalH, actualH, kTol));
}

TEST(Statics, ResultantWrench) {
  std::vector<Vector6> wrenches(2);
  wrenches[0] << 1, 2, 3, 4, 5, 6;
  wrenches[1] << 6, 5, 4, 3, 2, 1;
  std::vector<Matrix> actualH(2);
  EXPECT(assert_equal((Vector(6) << 7, 7, 7, 7, 7, 7).finished(),
                      ResultantWrench(wrenches, actualH), kTol));
  Matrix expected(6, 6);
  expected.setIdentity();
  EXPECT(assert_equal(expected, actualH[0], kTol));
  EXPECT(assert_equal(expected, actualH[1], kTol));
}

int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}


//////////////////////////////////////////////////////////////////////////////////
// random::poisson_ex::poisson_devroye::detail::q_function::using_sum.hpp       //
//                                                                              //
//                                                                              //
//  (C) Copyright 2010 Erwann Rogard                                            //
//  Use, modification and distribution are subject to the                       //
//  Boost Software License, Version 1.0. (See accompanying file                 //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_Q_FUNCTION_USING_SUM_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_Q_FUNCTION_USING_SUM_ER_2010
#include <cmath>
#include <boost/mpl/bool.hpp>
#include <boost/math/special_functions/log1p.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace q{

namespace sum_method{
    template<typename T,typename Int,typename P,typename IntT>
    T fun(const Int& i_mean,const Int& i_y,const P& p,const IntT& converter);
}
    struct using_sum{ using_sum(){} };

    template<typename T,typename Int,typename P,typename IntT>
    T fun(const Int& i_mean,const Int& i_y,const P& p,
        const IntT& converter,using_sum/*method*/)
    {
        return sum_method::fun<T>(i_mean,i_y,p,converter);
    }

namespace sum_method{

    template<typename T,typename Int,typename P,typename IntT>
    T fun(const T& inv,const Int& i_y,const P& p,const IntT& converter,
        boost::mpl::bool_<false>/*y>0*/)
    {
        using namespace boost::math;
        BOOST_ASSERT(i_y<0);
        T j, r = IntT::convert( 0 );
        Int n = -( 1 + i_y ) + 1;
        for(Int i = 0; i < n; i++){
            j = IntT::convert( i );
            r += log1p(- inv * j, p );
        }
        return r;
    }

    template<typename T,typename Int,typename P,typename IntT>
    T fun(const T& inv,const Int& i_y,const P& p,const IntT& converter,
        boost::mpl::bool_<true>/*y>0*/)
    {
        using namespace boost::math;

        BOOST_ASSERT(i_y>0);
        T j , r = IntT::convert( 0 );
        Int n = 1 + i_y;
        for(Int i = 1; i < n; i++){
            j = IntT::convert( i );
            r -= log1p( inv * j, p );
        }
        return r;
    }

    template<typename T,typename Int,typename P,typename IntT>
    T fun(const Int& i_mean,const Int& i_y,const P& p,const IntT& converter)
    {
        T r = IntT::convert( 0 );
        if(i_y!=0){
            T m = IntT::convert( i_mean );
            T inv = pow( m , -1);
            typedef boost::mpl::bool_<false> false_;
            typedef boost::mpl::bool_<true> true_;
            if(i_y<0)
                r += sum_method::fun( inv, i_y, p , converter, false_());
            if(i_y>0)
                r += sum_method::fun( inv, i_y, p , converter, true_());
        }
        return r;
    }

}// sum_method
}// q
}// devroye
}// poisson
}// random
}// boost

#endif