#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <quadrotor_msgs/SO3Command.h>
#include <quadrotor_simulator/Quadrotor.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <uav_utils/geometry_utils.h>

typedef struct _Control
{
  double rpm[4];
} Control;

typedef struct _Command
{
  float force[3];
  float qx, qy, qz, qw;
  float kR[3];
  float kOm[3];
  float corrections[3];
  float current_yaw;
  bool  use_external_yaw;
} Command;

typedef struct _Disturbance
{
  Eigen::Vector3d f;
  Eigen::Vector3d m;
} Disturbance;

static Command     command;
static Disturbance disturbance;

void stateToOdomMsg(const QuadrotorSimulator::Quadrotor::State& state,
                    nav_msgs::Odometry&                         odom);
void quadToImuMsg(const QuadrotorSimulator::Quadrotor& quad,
                  sensor_msgs::Imu&                    imu);

static Control
getControl(const QuadrotorSimulator::Quadrotor& quad, const Command& cmd)
{
  const double _kf = quad.getPropellerThrustCoefficient();
  const double _km = quad.getPropellerMomentCoefficient();
  const double kf  = _kf - cmd.corrections[0];
  const double km  = _km / _kf * kf;

  const double          d       = quad.getArmLength();
  const Eigen::Matrix3f J       = quad.getInertia().cast<float>();
  const float           I[3][3] = { { J(0, 0), J(0, 1), J(0, 2) },
                          { J(1, 0), J(1, 1), J(1, 2) },
                          { J(2, 0), J(2, 1), J(2, 2) } };
  const QuadrotorSimulator::Quadrotor::State state = quad.getState();

  // Rotation, may use external yaw
  Eigen::Vector3d _ypr = uav_utils::R_to_ypr(state.R);
  Eigen::Vector3d ypr  = _ypr;
  if (cmd.use_external_yaw)
    ypr[0] = cmd.current_yaw;
  Eigen::Matrix3d R;
  R = Eigen::AngleAxisd(ypr[0], Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(ypr[1], Eigen::Vector3d::UnitY()) *
      Eigen::AngleAxisd(ypr[2], Eigen::Vector3d::UnitX());
  float R11 = R(0, 0);
  float R12 = R(0, 1);
  float R13 = R(0, 2);
  float R21 = R(1, 0);
  float R22 = R(1, 1);
  float R23 = R(1, 2);
  float R31 = R(2, 0);
  float R32 = R(2, 1);
  float R33 = R(2, 2);
  /*
    float R11 = state.R(0,0);
    float R12 = state.R(0,1);
    float R13 = state.R(0,2);
    float R21 = state.R(1,0);
    float R22 = state.R(1,1);
    float R23 = state.R(1,2);
    float R31 = state.R(2,0);
    float R32 = state.R(2,1);
    float R33 = state.R(2,2);
  */
  float Om1 = state.omega(0);
  float Om2 = state.omega(1);
  float Om3 = state.omega(2);

  float Rd11 =
    cmd.qw * cmd.qw + cmd.qx * cmd.qx - cmd.qy * cmd.qy - cmd.qz * cmd.qz;
  float Rd12 = 2 * (cmd.qx * cmd.qy - cmd.qw * cmd.qz);
  float Rd13 = 2 * (cmd.qx * cmd.qz + cmd.qw * cmd.qy);
  float Rd21 = 2 * (cmd.qx * cmd.qy + cmd.qw * cmd.qz);
  float Rd22 =
    cmd.qw * cmd.qw - cmd.qx * cmd.qx + cmd.qy * cmd.qy - cmd.qz * cmd.qz;
  float Rd23 = 2 * (cmd.qy * cmd.qz - cmd.qw * cmd.qx);
  float Rd31 = 2 * (cmd.qx * cmd.qz - cmd.qw * cmd.qy);
  float Rd32 = 2 * (cmd.qy * cmd.qz + cmd.qw * cmd.qx);
  float Rd33 =
    cmd.qw * cmd.qw - cmd.qx * cmd.qx - cmd.qy * cmd.qy + cmd.qz * cmd.qz;

  float Psi = 0.5f * (3.0f - (Rd11 * R11 + Rd21 * R21 + Rd31 * R31 +
                              Rd12 * R12 + Rd22 * R22 + Rd32 * R32 +
                              Rd13 * R13 + Rd23 * R23 + Rd33 * R33));

  float force = 0;
  if (Psi < 1.0f) // Position control stability guaranteed only when Psi < 1
    force = cmd.force[0] * R13 + cmd.force[1] * R23 + cmd.force[2] * R33;

  float eR1 = 0.5f * (R12 * Rd13 - R13 * Rd12 + R22 * Rd23 - R23 * Rd22 +
                      R32 * Rd33 - R33 * Rd32);
  float eR2 = 0.5f * (R13 * Rd11 - R11 * Rd13 - R21 * Rd23 + R23 * Rd21 -
                      R31 * Rd33 + R33 * Rd31);
  float eR3 = 0.5f * (R11 * Rd12 - R12 * Rd11 + R21 * Rd22 - R22 * Rd21 +
                      R31 * Rd32 - R32 * Rd31);

  float eOm1 = Om1;
  float eOm2 = Om2;
  float eOm3 = Om3;

  float in1 = Om2 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3) -
              Om3 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3);
  float in2 = Om3 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3) -
              Om1 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3);
  float in3 = Om1 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3) -
              Om2 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3);
  /*
    // Robust Control --------------------------------------------
    float c2       = 0.6;
    float epsilonR = 0.04;
    float deltaR   = 0.1;
    float eA1 = eOm1 + c2 * 1.0/I[0][0] * eR1;
    float eA2 = eOm2 + c2 * 1.0/I[1][1] * eR2;
    float eA3 = eOm3 + c2 * 1.0/I[2][2] * eR3;
    float neA = sqrt(eA1*eA1 + eA2*eA2 + eA3*eA3);
    float muR1 = -deltaR*deltaR * eA1 / (deltaR * neA + epsilonR);
    float muR2 = -deltaR*deltaR * eA2 / (deltaR * neA + epsilonR);
    float muR3 = -deltaR*deltaR * eA3 / (deltaR * neA + epsilonR);
    // Robust Control --------------------------------------------
  */
  float M1 = -cmd.kR[0] * eR1 - cmd.kOm[0] * eOm1 + in1; // - I[0][0]*muR1;
  float M2 = -cmd.kR[1] * eR2 - cmd.kOm[1] * eOm2 + in2; // - I[1][1]*muR2;
  float M3 = -cmd.kR[2] * eR3 - cmd.kOm[2] * eOm3 + in3; // - I[2][2]*muR3;

  float w_sq[4];
  w_sq[0] = force / (4 * kf) - M2 / (2 * d * kf) + M3 / (4 * km);
  w_sq[1] = force / (4 * kf) + M2 / (2 * d * kf) + M3 / (4 * km);
  w_sq[2] = force / (4 * kf) + M1 / (2 * d * kf) - M3 / (4 * km);
  w_sq[3] = force / (4 * kf) - M1 / (2 * d * kf) - M3 / (4 * km);

  Control control;
  for (int i = 0; i < 4; i++)
  {
    if (w_sq[i] < 0)
      w_sq[i] = 0;

    control.rpm[i] = sqrtf(w_sq[i]);
  }
  return control;
}

static void
cmd_callback(const quadrotor_msgs::SO3Command::ConstPtr& cmd)
{
  command.force[0]         = cmd->force.x;
  command.force[1]         = cmd->force.y;
  command.force[2]         = cmd->force.z;
  command.qx               = cmd->orientation.x;
  command.qy               = cmd->orientation.y;
  command.qz               = cmd->orientation.z;
  command.qw               = cmd->orientation.w;
  command.kR[0]            = cmd->kR[0];
  command.kR[1]            = cmd->kR[1];
  command.kR[2]            = cmd->kR[2];
  command.kOm[0]           = cmd->kOm[0];
  command.kOm[1]           = cmd->kOm[1];
  command.kOm[2]           = cmd->kOm[2];
  command.corrections[0]   = cmd->aux.kf_correction;
  command.corrections[1]   = cmd->aux.angle_corrections[0];
  command.corrections[2]   = cmd->aux.angle_corrections[1];
  command.current_yaw      = cmd->aux.current_yaw;
  command.use_external_yaw = cmd->aux.use_external_yaw;
}

static void
force_disturbance_callback(const geometry_msgs::Vector3::ConstPtr& f)
{
  disturbance.f(0) = f->x;
  disturbance.f(1) = f->y;
  disturbance.f(2) = f->z;
}

static void
moment_disturbance_callback(const geometry_msgs::Vector3::ConstPtr& m)
{
  disturbance.m(0) = m->x;
  disturbance.m(1) = m->y;
  disturbance.m(2) = m->z;
}

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "quadrotor_simulator_so3");

  ros::NodeHandle n("~");

  ros::Publisher  odom_pub = n.advertise<nav_msgs::Odometry>("odom", 100);
  ros::Publisher  imu_pub  = n.advertise<sensor_msgs::Imu>("imu", 10);
  ros::Subscriber cmd_sub =
    n.subscribe("cmd", 100, &cmd_callback, ros::TransportHints().tcpNoDelay());
  ros::Subscriber f_sub =
    n.subscribe("force_disturbance", 100, &force_disturbance_callback,
                ros::TransportHints().tcpNoDelay());
  ros::Subscriber m_sub =
    n.subscribe("moment_disturbance", 100, &moment_disturbance_callback,
                ros::TransportHints().tcpNoDelay());

  QuadrotorSimulator::Quadrotor quad;
  double                        _init_x, _init_y, _init_z;
  n.param("simulator/init_state_x", _init_x, 0.0);
  n.param("simulator/init_state_y", _init_y, 0.0);
  n.param("simulator/init_state_z", _init_z, 1.0);

  Eigen::Vector3d position = Eigen::Vector3d(_init_x, _init_y, _init_z);
  quad.setStatePos(position);

  double simulation_rate;
  n.param("rate/simulation", simulation_rate, 1000.0);
  ROS_ASSERT(simulation_rate > 0);

  double odom_rate;
  n.param("rate/odom", odom_rate, 100.0);
  const ros::Duration odom_pub_duration(1 / odom_rate);

  std::string quad_name;
  n.param("quadrotor_name", quad_name, std::string("quadrotor"));

  QuadrotorSimulator::Quadrotor::State state = quad.getState();

  ros::Rate    r(simulation_rate);
  const double dt = 1 / simulation_rate;

  Control control;

  nav_msgs::Odometry odom_msg;
  odom_msg.header.frame_id = "/world";
  odom_msg.child_frame_id  = "/" + quad_name;

  sensor_msgs::Imu imu;
  imu.header.frame_id = "/simulator";

  /*
  command.force[0] = 0;
  command.force[1] = 0;
  command.force[2] = quad.getMass()*quad.getGravity() + 0.1;
  command.qx = 0;
  command.qy = 0;
  command.qz = 0;
  command.qw = 1;
  command.kR[0] = 2;
  command.kR[1] = 2;
  command.kR[2] = 2;
  command.kOm[0] = 0.15;
  command.kOm[1] = 0.15;
  command.kOm[2] = 0.15;
  */

  ros::Time next_odom_pub_time = ros::Time::now();
  while (n.ok())
  {
    ros::spinOnce();

    auto last = control;
    control   = getControl(quad, command);
    for (int i = 0; i < 4; ++i)
    {
      //! @bug might have nan when the input is legal
      if (std::isnan(control.rpm[i]))
        control.rpm[i] = last.rpm[i];
    }
    quad.setInput(control.rpm[0], control.rpm[1], control.rpm[2],
                  control.rpm[3]);
    quad.setExternalForce(disturbance.f);
    quad.setExternalMoment(disturbance.m);
    quad.step(dt);

    ros::Time tnow = ros::Time::now();

    if (tnow >= next_odom_pub_time)
    {
      next_odom_pub_time += odom_pub_duration;
      odom_msg.header.stamp = tnow;
      state                 = quad.getState();
      stateToOdomMsg(state, odom_msg);
      quadToImuMsg(quad, imu);
      odom_pub.publish(odom_msg);
      imu_pub.publish(imu);
    }

    r.sleep();
  }

  return 0;
}

void
stateToOdomMsg(const QuadrotorSimulator::Quadrotor::State& state,
               nav_msgs::Odometry&                         odom)
{
  odom.pose.pose.position.x = state.x(0);
  odom.pose.pose.position.y = state.x(1);
  odom.pose.pose.position.z = state.x(2);

  Eigen::Quaterniond q(state.R);
  odom.pose.pose.orientation.x = q.x();
  odom.pose.pose.orientation.y = q.y();
  odom.pose.pose.orientation.z = q.z();
  odom.pose.pose.orientation.w = q.w();

  odom.twist.twist.linear.x = state.v(0);
  odom.twist.twist.linear.y = state.v(1);
  odom.twist.twist.linear.z = state.v(2);

  odom.twist.twist.angular.x = state.omega(0);
  odom.twist.twist.angular.y = state.omega(1);
  odom.twist.twist.angular.z = state.omega(2);
}

void
quadToImuMsg(const QuadrotorSimulator::Quadrotor& quad, sensor_msgs::Imu& imu)

{
  QuadrotorSimulator::Quadrotor::State state = quad.getState();
  Eigen::Quaterniond                   q(state.R);
  imu.orientation.x = q.x();
  imu.orientation.y = q.y();
  imu.orientation.z = q.z();
  imu.orientation.w = q.w();

  imu.angular_velocity.x = state.omega(0);
  imu.angular_velocity.y = state.omega(1);
  imu.angular_velocity.z = state.omega(2);

  imu.linear_acceleration.x = quad.getAcc()[0];
  imu.linear_acceleration.y = quad.getAcc()[1];
  imu.linear_acceleration.z = quad.getAcc()[2];
}


/////////////////////////////////////////////////////////
// (c) 2021 Tetsuro Nagai
/////////////////////////////////////////////////////////

#include <iostream>
#include <fstream>
#include <random>
#include <cstdio>
#include <cmath>
#include <ctime>
#include <iomanip>
#include <cstdlib>
#include<vector>
#include<algorithm>
#include<numeric>
#include<sstream>
#include <boost/program_options.hpp>
#include <boost/format.hpp>
#include <prettyprint.hpp>

#define DEBUG (false)
namespace po=boost::program_options;

int L;
int N;

std::vector<int> sites;
std::vector<int> parent;

// The functions init, find, unite, connect, pos2index
// were taken or adopted from https://github.com/kaityo256/mc/tree/master/percolation/
// (c) 2012-2019 H. Watanabe

void init(int size){
    L=size;
    N = L * L * L;
    parent.resize(N);
    sites.resize(N);
    for(int i = 0; i< N; ++i ){
        parent[i] = i ;
    }
}

int find(int i){
    if (i != parent[i]){
        parent[i] = find(parent[i]); // tree is flattened; https://algo-logic.info/union-find-tree/
    }
    return parent[i] ;
}

void unite(int i, int j){  //can be optimized more if necessary
    i = find(i);
    j = find(j);
    parent[j] = i ;
}

void connect(int i, int j){
    if(sites[i] == 0){
        return ;
    }
    if(sites[j] == 0){
        return ;
    }
    unite(i,j);
}

int pos2index(int ix, int iy, int iz){
    ix =(ix + L) % L;
    iy =(iy + L) % L;
    iz =(iz + L) % L;
    return ix *L*L + iy *L + iz ;
}

double crossing_probability_z(void){
    for(int ix1 = 0; ix1 < L ; ix1++){
    for(int iy1 = 0; iy1 < L ; iy1++){
        int i = pos2index(ix1, iy1, 0);
        int ci = find(i);
        for(int ix2 = 0 ; ix2 < L; ++ix2){
        for(int iy2 = 0 ; iy2 < L; ++iy2){
            int j = (pos2index(ix2, iy2, L-1));
            int cj = find(j);
            if(ci == cj){
                return 1.0;
            }
        }
        }
    }
    }
    return 0.0;
}

double crossing_probability_y(void){
    for(int ix1 = 0; ix1 < L ; ix1++){
    for(int iy1 = 0; iy1 < L ; iy1++){
        int i = pos2index(ix1, 0, iy1);
        int ci = find(i);
        for(int ix2 = 0 ; ix2 < L; ++ix2){
        for(int iy2 = 0 ; iy2 < L; ++iy2){
            int j = (pos2index(ix2, L-1, iy2));
            int cj = find(j);
            if(ci == cj){
                return 1.0;
            }
        }
        }
    }
    }
    return 0.0;
}

double crossing_probability_x(void){
    for(int ix1 = 0; ix1 < L ; ix1++){
    for(int iy1 = 0; iy1 < L ; iy1++){
        int i = pos2index(0, ix1,  iy1);
        int ci = find(i);
        for(int ix2 = 0 ; ix2 < L; ++ix2){
        for(int iy2 = 0 ; iy2 < L; ++iy2){
            int j = (pos2index(L-1, ix2, iy2));
            int cj = find(j);
            if(ci == cj){
                return 1.0;
            }
        }
        }
    }
    }
    return 0.0;
}

int count_max_cluster(void) {
      std::vector<int> size(N, 0);
      for (int i = 0; i < N; i++) {
        int ci = find(i);
        size[ci]++;
      }
      int max = *std::max_element(size.begin(), size.end());
      return max;
}

int count_max_cluster_id(void) {
      std::vector<int> size(N, 0);
      for (int i = 0; i < N; i++) {
        int ci = find(i);
        size[ci]++;
      }
      auto max_el = std::max_element(size.begin(), size.end());
      int max_id = std::distance(size.begin(), max_el);
      return max_id;
}

int count_active_sites(void) {
    int sum = std::accumulate(sites.begin(), sites.end(), 0);
  return sum;
}

int count_trees(void) {
    int sum = 0;
    for (int i = 0; i < N; i++) {
        if(sites[i] == 1 and parent[i] == i){
            sum++ ;
        }
    }
    return sum;
}

void put_string_of_time(std::string &starting_date){
    std::time_t t = std::time(nullptr);
    std::stringstream ss ;
    ss<< std::put_time(std::localtime(&t), "%c %Z");
    starting_date+=ss.str();
}

int main(int argc, char** argv){
    // getting and printing time
    std::string starting_date;
    put_string_of_time(starting_date);
    std::cout << "Execution start at " << starting_date << std::endl;

    // making command line options
    std::string  fname_input;
    double  G0 ;
    std::string  fout_prefix;

    po::options_description opt("This program analyze percolation cluster");
    opt.add_options()
      ("help,h" ,                                          "show help")
      ("finp"   ,       po::value<std::string>(),          "file name of input")
      ("G0"     ,po::value<double>()->default_value(5.0),  "threshold of G, blow which sites to be connected")
      ("fout_prefix",   po::value<std::string>(),          "fout_prefix, a number of files will be made with this prefix  ");

     // analyze argc and argv and results are stored in vm
    try{
      po::variables_map vm;
      store(parse_command_line(argc, argv, opt), vm);
      notify(vm);

      if(vm.count("help")){
        std::cout << opt << std::endl; // show help
        exit(1);
      }
      else if(!vm.count("finp")){
        std::cerr << "finp is mandatory " << std::endl;
        std::cerr << "exit!!" << std::endl;
        exit(1);
      }
      else if(!vm.count("fout_prefix")){
        std::cerr << vm.count("fout_prefix") << std::endl;
        std::cerr << "fout_prefix is mandatory " << std::endl;
        std::cerr << "exit!!" << std::endl;
        exit(1);
      }
      else
      {
        fname_input = vm["finp"].as<std::string>();
        G0 = vm["G0"].as<double>();
        fout_prefix = vm["fout_prefix"].as<std::string>();
        std::cout << "**** Input parameters ****"  << std::endl;
        std::cout << "input file name: " << fname_input << std::endl;
        std::cout << "fout_prefix: " << fout_prefix << std::endl;
        std::cout << "G0: " << G0 << std::endl;
        std::cout << "*************************\n"  << std::endl;
      }
    }
    catch (boost::bad_any_cast &e) {
        std::cout <<"something wrong and buggy happened!!"  << std::endl;
        std::cout <<"exit!!"  << std::endl;
        exit(1);
    }
    catch (std::exception  &e) {
        std::cout << e.what() << std::endl;
        std::cout <<"exit!!"  << std::endl;
        exit(1);
    }

    int nixyz[3];

    //open fval files
    std::string buf;
    std::ifstream ifs_fmap(fname_input.c_str());
    if(!ifs_fmap){ std::cerr << "can not open " << fname_input << std::endl; exit(1);}

    // first line is number of grids in each direction
    std::getline(ifs_fmap, buf);
    sscanf(buf.c_str(), "%d %d %d", &nixyz[0], &nixyz[1], &nixyz[2]);

    if(nixyz[0] != nixyz[1] or nixyz[0] != nixyz[2]){
        std::cerr << "ERROR: currently, n_x = n_y = n_z should hold. exit!" << std::endl;
        return  EXIT_FAILURE;
    }
    init(nixyz[0]);

    //data follows
    while(std::getline(ifs_fmap, buf)){
        if(DEBUG) std::cout << buf <<std::endl;

        int  ix, iy, iz ;
        double tmp;
        sscanf(buf.c_str(), "%d %d %d %lf", &ix, &iy, &iz, &tmp);

        if( ix  < 0 or nixyz[0] <= ix ){
          std::cout << "error in ix " << ix << std::endl;
        }
        if( iy  < 0 or nixyz[1] <= iy ){
          std::cout << "error in iy " << iy << std::endl;
        }
        if( iz  < 0 or nixyz[2] <= iz ){
          std::cout << "error in iz " << iz << std::endl;
        }

        if(tmp > G0){
            sites[pos2index(ix,iy,iz)] = 0 ;
        }
        else{
            sites[pos2index(ix,iy,iz)] = 1 ;
        }

    }
    ifs_fmap.close();

    const std::string fname_log_name = fout_prefix+".log";

    std::ofstream ofs_log(fname_log_name.c_str());
    if(!ofs_log){
      std::cerr << "can not open: " << fname_log_name << std::endl;
      exit(1);
    }
      ofs_log << "Excecution starts at " << starting_date << "\n" <<std::endl;

    ofs_log <<  "executed command should be like: \n";
    for(int i = 0; argv[i] != NULL; i++){
      ofs_log << boost::format("%s ")% argv[i];
    }

    for (int ix = 0; ix < L; ++ix){
        for (int iy = 0; iy < L; ++iy){
            for (int iz = 0; iz < L; ++iz){
                int i = pos2index(ix,iy,iz);
                if(ix+1 < L){
                    connect(i, pos2index(ix+1, iy, iz));
                }
                if(iy+1 < L){
                    connect(i, pos2index(ix, iy+1, iz));
                }
                if(iz+1 < L){
                    connect(i, pos2index(ix, iy, iz+1));
                }
            }
        }
    }

    std::cout << "G0 corss_x cross_y cross_z max_cluster #activesite #tree #sites maxid " <<std::endl;
    std::cout << G0 << " " ;
    std::cout << crossing_probability_x() << " " ;
    std::cout << crossing_probability_y() << " " ;
    std::cout << crossing_probability_z() << " " ;
    std::cout << count_max_cluster() << " " ;
    std::cout << count_active_sites() << " " ;
    std::cout << count_trees() << " " ;
    std::cout << N << " " ;
    std::cout << count_max_cluster_id() << " " ;
    std::cout << std::endl;

    const std::string fname_cluster_id = fout_prefix+"_cluster_id.dat";
    std::ofstream ofs_cluster(fname_cluster_id.c_str());
    if(!ofs_cluster){
      std::cerr << "can not open: " << fname_log_name << std::endl;
      exit(1);
    }
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 1999
 */

// @sect3{Include files}

// 同样，前几个include文件已经知道了，所以我们不会对它们进行评论。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

// 这个是新的。我们想从磁盘上读取一个三角图，做这个的类在下面的文件中声明。

#include <deal.II/grid/grid_in.h>

// 我们将使用一个圆形域，而描述其边界的对象来自这个文件。

#include <deal.II/grid/manifold_lib.h>

// 这是C++ ...

#include <fstream>
#include <iostream>

// 最后，这在以前的教程程序中已经讨论过了。

using namespace dealii;
// @sect3{The Step5 class template}

// 主类大部分和前面的例子一样。最明显的变化是删除了 make_grid 函数，因为现在创建网格是在 run 函数中完成的，其余功能都在 setup_system 中。除此以外，一切都和以前一样。

template <int dim>
class Step5
{
public:
  Step5();
  void run();

private:
  void setup_system();
  void assemble_system();
  void solve();
  void output_results(const unsigned int cycle) const;

  Triangulation<dim> triangulation;
  FE_Q<dim>          fe;
  DoFHandler<dim>    dof_handler;

  SparsityPattern      sparsity_pattern;
  SparseMatrix<double> system_matrix;

  Vector<double> solution;
  Vector<double> system_rhs;
};
// @sect3{Working with nonconstant coefficients}

// 在  step-4  中，我们展示了如何使用非恒定边界值和右手边。 在这个例子中，我们想在椭圆算子中使用一个可变系数来代替。由于我们有一个只取决于空间中的点的函数，我们可以做得更简单一些，使用一个普通的函数而不是继承自Function。

// 这是对单点的系数函数的实现。如果与原点的距离小于0.5，我们让它返回20，否则返回1。

template <int dim>
double coefficient(const Point<dim> &p)
{
  if (p.square() < 0.5 * 0.5)
    return 20;
  else
    return 1;
}
// @sect3{The Step5 class implementation}
// @sect4{Step5::Step5}

// 这个函数和以前一样。

template <int dim>
Step5<dim>::Step5()
  : fe(1)
  , dof_handler(triangulation)
{}

//  @sect4{Step5::setup_system}

// 这是前面例子中的函数 make_grid ，减去了生成网格的部分。其他一切都没有变化。

template <int dim>
void Step5<dim>::setup_system()
{
  dof_handler.distribute_dofs(fe);

  std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
            << std::endl;

  DynamicSparsityPattern dsp(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dsp);
  sparsity_pattern.copy_from(dsp);

  system_matrix.reinit(sparsity_pattern);

  solution.reinit(dof_handler.n_dofs());
  system_rhs.reinit(dof_handler.n_dofs());
}

//  @sect4{Step5::assemble_system}

// 和前面的例子一样，这个函数在功能上没有太大变化，但仍有一些优化，我们将展示这些优化。对此，需要注意的是，如果使用高效的求解器（如预设条件的CG方法），组装矩阵和右手边会花费相当的时间，你应该考虑在某些地方使用一到两个优化。

// 该函数的前几部分与之前完全没有变化。

template <int dim>
void Step5<dim>::assemble_system()
{
  QGauss<dim> quadrature_formula(fe.degree + 1);

  FEValues<dim> fe_values(fe,
                          quadrature_formula,
                          update_values | update_gradients |
                            update_quadrature_points | update_JxW_values);

  const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

  FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<double>     cell_rhs(dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

// 接下来是对所有单元的典型循环，以计算局部贡献，然后将它们转移到全局矩阵和向量中。与 step-4 相比，这部分的唯一变化是我们将使用上面定义的 coefficient() 函数来计算每个正交点的系数值。

  for (const auto &cell : dof_handler.active_cell_iterators())
    {
      cell_matrix = 0.;
      cell_rhs    = 0.;

      fe_values.reinit(cell);

      for (const unsigned int q_index : fe_values.quadrature_point_indices())
        {
          const double current_coefficient =
            coefficient(fe_values.quadrature_point(q_index));
          for (const unsigned int i : fe_values.dof_indices())
            {
              for (const unsigned int j : fe_values.dof_indices())
                cell_matrix(i, j) +=
                  (current_coefficient *              // a(x_q)
                   fe_values.shape_grad(i, q_index) * // grad phi_i(x_q)
                   fe_values.shape_grad(j, q_index) * // grad phi_j(x_q)
                   fe_values.JxW(q_index));           // dx

              cell_rhs(i) += (fe_values.shape_value(i, q_index) * // phi_i(x_q)
                              1.0 *                               // f(x_q)
                              fe_values.JxW(q_index));            // dx
            }
        }

      cell->get_dof_indices(local_dof_indices);
      for (const unsigned int i : fe_values.dof_indices())
        {
          for (const unsigned int j : fe_values.dof_indices())
            system_matrix.add(local_dof_indices[i],
                              local_dof_indices[j],
                              cell_matrix(i, j));

          system_rhs(local_dof_indices[i]) += cell_rhs(i);
        }
    }

// 有了这样构建的矩阵，我们再次使用零边界值。

  std::map<types::global_dof_index, double> boundary_values;
  VectorTools::interpolate_boundary_values(dof_handler,
                                           0,
                                           Functions::ZeroFunction<dim>(),
                                           boundary_values);
  MatrixTools::apply_boundary_values(boundary_values,
                                     system_matrix,
                                     solution,
                                     system_rhs);
}
// @sect4{Step5::solve}

// 求解过程看起来又和前面的例子差不多。然而，我们现在将使用一个预设条件的共轭梯度算法。做出这种改变并不难。事实上，我们唯一需要改变的是，我们需要一个作为预处理程序的对象。我们将使用SSOR（对称连续过度放松），放松系数为1.2。为此， SparseMatrix 类有一个函数可以做一个SSOR步骤，我们需要把这个函数的地址和它应该作用的矩阵（也就是要反转的矩阵）以及松弛因子打包成一个对象。 PreconditionSSOR 类为我们做了这个。(  PreconditionSSOR  类需要一个模板参数，表示它应该工作的矩阵类型。默认值是 SparseMatrix@<double@> ，这正是我们在这里需要的，所以我们只需坚持使用默认值，不在角括号中指定任何东西。)

// 请注意，在目前的情况下，SSOR的表现并不比其他大多数预处理程序好多少（尽管比没有预处理好）。在下一个教程程序  step-6  的结果部分，将对不同的预处理进行简要比较。

// 有了这个，函数的其余部分就很简单了：我们现在使用我们声明的预处理程序，而不是之前创建的 PreconditionIdentity 对象，CG求解器将为我们完成其余的工作。

template <int dim>
void Step5<dim>::solve()
{
  SolverControl            solver_control(1000, 1e-12);
  SolverCG<Vector<double>> solver(solver_control);

  PreconditionSSOR<SparseMatrix<double>> preconditioner;
  preconditioner.initialize(system_matrix, 1.2);

  solver.solve(system_matrix, solution, system_rhs, preconditioner);

  std::cout << "   " << solver_control.last_step()
            << " CG iterations needed to obtain convergence." << std::endl;
}
// @sect4{Step5::output_results and setting output flags}

// 将输出写入文件的方法与上一个教程中的基本相同。唯一不同的是，我们现在需要为每个细化周期构建一个不同的文件名。

// 这个函数以VTU格式写入输出，这是VTK格式的一个变种，因为它压缩了数据，所以需要更少的磁盘空间。当然，如果你希望使用一个不理解VTK或VTU的可视化程序，DataOut类还支持许多其他格式。

template <int dim>
void Step5<dim>::output_results(const unsigned int cycle) const
{
  DataOut<dim> data_out;

  data_out.attach_dof_handler(dof_handler);
  data_out.add_data_vector(solution, "solution");

  data_out.build_patches();

  std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
  data_out.write_vtu(output);
}

//  @sect4{Step5::run}

// 在这个程序中，倒数第二件事是对 run() 函数的定义。与之前的程序不同，我们将在一个网格序列上进行计算，在每次迭代后都会进行全局细化。因此，该函数由6个周期的循环组成。在每个循环中，我们首先打印循环编号，然后决定如何处理网格。如果这不是第一个周期，我们就简单地对现有的网格进行一次全局精炼。然而，在运行这些循环之前，我们必须先生成一个网格。

// 在前面的例子中，我们已经使用了 GridGenerator 类中的一些函数。在这里，我们想从一个存储单元的文件中读取网格，这个文件可能来自其他人，也可能是一个网格生成工具的产物。

// 为了从文件中读取网格，我们生成一个数据类型为GridIn的对象，并将三角剖分与之相关联（也就是说，当我们要求它读取文件时，我们告诉它要填充我们的三角剖分对象）。然后我们打开相应的文件，用文件中的数据初始化三角剖分。

template <int dim>
void Step5<dim>::run()
{
  GridIn<dim> grid_in;
  grid_in.attach_triangulation(triangulation);
  std::ifstream input_file("circle-grid.inp");

// 我们现在想读取该文件。但是，输入文件只针对二维三角图，而这个函数是一个任意维度的模板。由于这只是一个演示程序，我们不会为不同的维度使用不同的输入文件，而是在不在二维的情况下迅速杀死整个程序。当然，由于下面的主函数假定我们是在二维空间工作，我们可以跳过这个检查，在这个版本的程序中，不会有任何不良影响。

// 事实证明，90%以上的编程错误都是无效的函数参数，如无效的数组大小等，所以我们在整个deal.II中大量使用断言来捕捉此类错误。为此， Assert 宏是一个很好的选择，因为它确保作为第一个参数的条件是有效的，如果不是，就抛出一个异常（它的第二个参数），通常会终止程序，并给出错误发生的位置和原因的信息。关于 @p Assert 宏的具体作用，可以在 @ref Exceptions "异常文档模块 "中找到更详细的讨论）。这通常会大大减少发现编程错误的时间，我们发现断言是快速编程的宝贵手段。

// 另一方面，如果你想做大的计算，所有这些检查（目前库中有超过10000个）不应该使程序太慢。为此， Assert 宏只在调试模式下使用，如果在优化模式下则扩展为零。因此，当你在小问题上测试你的程序并进行调试时，断言会告诉你问题出在哪里。一旦你的程序稳定了，你可以关闭调试，程序将在没有断言的情况下以最大速度运行你的实际计算。更准确地说：通过在优化模式下编译你的程序，关闭库中的所有检查（这些检查可以防止你用错误的参数调用函数，从数组中走出来，等等），通常可以使程序的运行速度提高四倍左右。即使优化后的程序性能更高，我们仍然建议在调试模式下开发，因为它允许库自动发现许多常见的编程错误。对于那些想尝试的人来说。从调试模式切换到优化模式的方法是用make release命令重新编译你的程序。现在 make 程序的输出应该向你表明，该程序现在是以优化模式编译的，以后也会被链接到已经为优化模式编译的库。为了切换回调试模式，只需用  make debug  命令重新编译。

  Assert(dim == 2, ExcInternalError());

// ExcInternalError是一个全局定义的异常，每当出现严重的错误时就会抛出。通常，人们希望使用更具体的异常，特别是在这种情况下，如果 dim 不等于2，人们当然会尝试做其他事情，例如使用库函数创建一个网格。终止程序通常不是一个好主意，断言实际上只应该用于不应该发生的特殊情况，但由于程序员、用户或其他人的愚蠢而可能发生。上面的情况并不是对Assert的巧妙使用，但是再次强调：这是一个教程，也许值得展示一下什么是不应该做的，毕竟。

// 所以，如果我们通过了断言，我们就知道dim==2，现在我们就可以真正地读取网格。它的格式是UCD（非结构化单元数据）（尽管惯例是使用UCD文件的后缀 inp ）。

  grid_in.read_ucd(input_file);

// 如果你想使用其他输入格式，你必须使用其他 grid_in.read_xxx 函数之一。(参见  GridIn  类的文档，以了解目前支持哪些输入格式)。

// 文件中的网格描述了一个圆。因此，我们必须使用一个流形对象，告诉三角计算在细化网格时将边界上的新点放在哪里。与 step-1 不同的是，由于GridIn不知道域的边界是圆形的（与 GridGenerator::hyper_shell) 不同的是，我们必须在创建三角网格后明确地将流形附加到边界上，以便在细化网格时获得正确的结果。

  const SphericalManifold<dim> boundary;
  triangulation.set_all_manifold_ids_on_boundary(0);
  triangulation.set_manifold(0, boundary);

  for (unsigned int cycle = 0; cycle < 6; ++cycle)
    {
      std::cout << "Cycle " << cycle << ':' << std::endl;

      if (cycle != 0)
        triangulation.refine_global(1);

// 现在我们有了一个确定的网格，我们写一些输出，做所有我们在前面的例子中已经看到的事情。

      std::cout << "   Number of active cells: "  //
                << triangulation.n_active_cells() //
                << std::endl                      //
                << "   Total number of cells: "   //
                << triangulation.n_cells()        //
                << std::endl;

      setup_system();
      assemble_system();
      solve();
      output_results(cycle);
    }
}
// @sect3{The main function}

// 主函数看起来和前面的例子中的函数差不多，所以我们就不进一步评论了。

int main()
{
  Step5<2> laplace_problem_2d;
  laplace_problem_2d.run();
  return 0;
}


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/core/ignore_unused.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/numeric/conversion/cast.hpp>
using namespace boost;

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

void case1()
{
    cout << numeric_limits<short>::min();               //-32768
    cout << numeric_limits<short>::max();               //32767

    cout << numeric_limits<unsigned short>::min();      //0
    cout << numeric_limits<unsigned short>::max();      //65535

    cout << numeric_limits<float>::min();               //1.17549e-38
    cout << numeric_limits<float>::max();               //3.40282e+38

    cout << endl;

    using namespace boost::numeric;

    cout << bounds<short>::lowest();                        //-32768
    cout << bounds<short>::highest();                        //32767;
    cout << bounds<short>::smallest();                  //1

    cout << bounds<float>::lowest();                        //-3.40282e+38
    cout << bounds<float>::highest();                       //3.40282e+38
    cout << bounds<float>::smallest();                  //1.17549e-38

    assert(bounds<short>::lowest()==numeric_limits<short>::min());
    assert(bounds<float>::lowest()==-numeric_limits<float>::max());

    assert(bounds<float>::lowest()==numeric_limits<float>::lowest());

    cout << endl;
}

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

void case2()
{
    using namespace boost::numeric;

    short   s = bounds<short>::highest();
    int     i = numeric_cast<int>(s);
    assert(i == s);

    try
    {
        char c = numeric_cast<char>(s);

        ignore_unused(c);
    }
    catch (std::bad_cast& e)
    {
        cout << e.what() << endl;
    }

}

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

int main()
{
    std::cout << "hello numeric" << std::endl;

    case1();
    case2();
    //case3();
}


/**
 * ODE solver - JMU REU 2021
 *
 * @author Mike Lam
 */

// standard headers
#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <string>
#include <cmath>
#include <functional>
using namespace std;

// Boost ODEint headers (individual files compile faster than the catch-all)
#include <boost/numeric/odeint.hpp>
//#include <boost/numeric/odeint/integrate/integrate_const.hpp>
//#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
using namespace boost::numeric::odeint;

// floating-point precision control (control via compiler parameter -Dreal_t)
//typedef double real_t;      // 64 bits
//typedef float real_t;       // 32 bits

// solver state
typedef std::vector<real_t> state_t;

// parameters (most read from command line)
real_t b, c, x0;
double t0 = 0.0;
double tn, dt;

// output stream
ofstream out;

// ODE system
void ode(const state_t &x, state_t &dxdt, const real_t /*t*/)
{
    dxdt[0] = b*x[0] + c;
}

// debug output helper
void observe(const state_t &x, const real_t t)
{
    // known closed-form solution
    real_t sol = (x0+c/b) * exp(b*t) - c/b;

    out << setw(8) << t << " " << setw(12) << x[0]
         //<< " " << setw(12) << sol
         << " " << setw(12) << fabs(sol-x[0]) << endl;
}

int main(int argc, const char* argv[])
{
    // check parameters
    if (argc != 6) {
        cout << "Usage: " << argv[0] << " <b> <c> <x0> <tn> <dt>" << endl;
        return EXIT_FAILURE;
    }

    // parse parameters
    b  = stod(argv[1], NULL);
    c  = stod(argv[2], NULL);
    x0 = stod(argv[3], NULL);
    tn = stod(argv[4], NULL);
    dt = stod(argv[5], NULL);
    // TODO: add stepper and t0?

    // initialize state
    state_t x(1);
    x[0] = x0;

    // show floating-point width
    //cout << "sizeof(real_t)=" << sizeof(real_t) << endl;

    // stepper
    runge_kutta4<state_t> stp;

    // integrate w/ debug output
    out.open("out.dat");
    /*size_t steps =*/ integrate_const(stp, ode, x, t0, tn, dt, observe);
    out.close();

    // show final output
    //cout << "steps=" << steps << " x=" << x[0] << endl;

    return EXIT_SUCCESS;
}


/**
 *  This file is part of dvo.
 *
 *  Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
 *  For more information see <http://vision.in.tum.de/data/software/dvo>.
 *
 *  dvo is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  dvo is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with dvo.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <dvo/core/least_squares.h>

#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <dvo/core/math_sse.h>

static const dvo::core::NumType normalizer = 1.0 / (255.0 * 255.0);
static const dvo::core::NumType normalizer_inverse = 255.0 * 255.0;

// ------ Normal Equations Cholesky ------

dvo::core::NormalEquationsLeastSquares::~NormalEquationsLeastSquares() { }

void dvo::core::NormalEquationsLeastSquares::initialize(const size_t maxnum_constraints)
{
  A.setZero();
  A_opt.setZero();
  b.setZero();
  error = 0;
  this->num_constraints = 0;
  this->maxnum_constraints = maxnum_constraints;
}

void dvo::core::NormalEquationsLeastSquares::update(const dvo::core::Vector6& J, const NumType& res, const NumType& weight)
{
  NumType factor = weight;//weight * normalizer; // what happens without the normalizer? nothing!
  A_opt.rankUpdate(J, factor);
  //MathSse<Sse::Enabled, NumType>::addOuterProduct(A, J, factor);
  //A += J * J.transpose() * factor;
  MathSse<Sse::Enabled, NumType>::add(b, J, -res * factor); // not much difference :(
  //b -= J * res * factor;

  //error += res * res * factor;
  num_constraints += 1;
}

void dvo::core::NormalEquationsLeastSquares::update(const Eigen::Matrix<NumType, 2, 6>& J, const Eigen::Matrix<NumType, 2, 1>& res, const Eigen::Matrix<NumType, 2, 2>& weight)
{
  A_opt.rankUpdate(J, weight);
  b -= J.transpose() * weight * res;

  num_constraints += 1;
}

void dvo::core::NormalEquationsLeastSquares::combine(const dvo::core::NormalEquationsLeastSquares& other)
{
  A_opt += other.A_opt;
  b += other.b;
  //error += other.error;
  num_constraints += other.num_constraints;
}

void dvo::core::NormalEquationsLeastSquares::finish()
{
  A_opt.toEigen(A);
  //A /= (NumType) num_constraints;
  //b /= (NumType) num_constraints;
  //error /= (NumType) num_constraints;
}

void dvo::core::NormalEquationsLeastSquares::solve(dvo::core::Vector6& x)
{
  x = A.ldlt().solve(b);
}

// ------ Normal Equations EVD ------

dvo::core::EvdLeastSquares::~EvdLeastSquares() { }

void dvo::core::EvdLeastSquares::solve(dvo::core::Vector6& x)
{
  // eigen value decomposition seems to be equivalent to SVD for our matrix A
  Eigen::SelfAdjointEigenSolver<dvo::core::Matrix6x6> eigensolver(A);
  dvo::core::Vector6 eigenvalues = eigensolver.eigenvalues();
  dvo::core::Matrix6x6 eigenvectors = eigensolver.eigenvectors();

  bool singular = false;

  for(int i = 0; i < 6; ++i)
  {
    if(eigenvalues(i) < 0.05)
    {
      singular = true;
      throw std::exception();
    }
    else
    {
      eigenvalues(i) = 1.0 / eigenvalues(i);
    }
  }

  x = eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose() * b;
}

// ------ SVD ------

dvo::core::SvdLeastSquares::~SvdLeastSquares() { }

void dvo::core::SvdLeastSquares::initialize(const size_t maxnum_constraints)
{
  J.resize(maxnum_constraints, Eigen::NoChange);
  residuals.resize(maxnum_constraints, Eigen::NoChange);

  current = 0;
}

void dvo::core::SvdLeastSquares::update(const dvo::core::Vector6& J, const NumType& res, const NumType& weight)
{
  this->J.row(current) = J;
  this->residuals(current) = res;

  current += 1;
}

void dvo::core::SvdLeastSquares::finish()
{
  J.conservativeResize(current, Eigen::NoChange);
  residuals.conservativeResize(current);
}

void dvo::core::SvdLeastSquares::solve(dvo::core::Vector6& x)
{
  x = J.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(residuals);
}

// ------ Precomputed Normal Equations Cholesky ------

dvo::core::PrecomputedNormalEquationsLeastSquares::~PrecomputedNormalEquationsLeastSquares() { }

void dvo::core::PrecomputedNormalEquationsLeastSquares::initialize(const size_t maxnum_constraints)
{
  hessian_.setZero();
  jacobian_cache_.resize(Eigen::NoChange, maxnum_constraints);
  mask_ = cv::Mat1b::zeros(maxnum_constraints, 1);

  this->num_constraints = 0;
  this->maxnum_constraints = maxnum_constraints;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::reset()
{
  A = hessian_;
  hessian_error_.setZero();
  b.setZero();
  error = 0;

  mask_ptr_ = mask_.ptr();
  num_constraints = 0;
}
void dvo::core::PrecomputedNormalEquationsLeastSquares::next()
{
  ++mask_ptr_;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::addConstraint(const size_t& idx, const dvo::core::Vector6& J)
{
  //hessian_cache_.block<6, 6>(idx * 6, 0) = J * J.transpose() * normalizer;
  //hessian_cache_[idx] = J * J.transpose() * normalizer;

  //hessian_ += J * J.transpose() * normalizer;
  MathSse<Sse::Enabled, NumType>::addOuterProduct(hessian_, J, normalizer);

  jacobian_cache_.col(idx) = J * normalizer;
  mask_.at<uchar>(idx) = 1;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::ignoreConstraint(const size_t& idx)
{
  if((*mask_ptr_) == 0) return;

  const Vector6& J = jacobian_cache_.col(idx);

  /**
   *  J is already multiplied with normalizer, so it is:
   *
   *    J' * J'.transpose() * normalizer * normalizer
   *
   *  multiplying with normalizer_inverse gives:
   *
   *    J' * J'.transpose() * normalizer * normalizer * normalizer_inverse
   *
   *  resulting in:
   *
   *    J' * J'.transpose() * normalizer
   */
  hessian_error_ -= J * J.transpose() * normalizer_inverse;
}

bool dvo::core::PrecomputedNormalEquationsLeastSquares::setResidualForConstraint(const size_t& idx, const NumType& res, const NumType& weight)
{
  if(*mask_ptr_ == 0) return false;

  //A += *hessian_cache_it_;
  b -= jacobian_cache_.col(idx) * res * weight;

  error += res * res * weight * normalizer;
  this->num_constraints +=1;

  return true;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::finish()
{
  A += hessian_error_;
  A /= (double) num_constraints;
  b /= (double) num_constraints;
  error /= (double) num_constraints;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::solve(dvo::core::Vector6 & x)
{
  x = A.ldlt().solve(b);
}


// Copyright (c) 2015-2020 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef maths_hpp
#define maths_hpp

#include <vector>
#include <cstddef>
#include <cmath>
#include <numeric>
#include <algorithm>
#include <type_traits>
#include <iterator>
#include <functional>
#include <limits>
#include <utility>
#include <stdexcept>
#include <cassert>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/distributions/geometric.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/normal.hpp>

#include "fmath.hpp"

namespace octopus { namespace maths {

namespace constants
{
    template <typename T = double>
    constexpr T ln10Div10 = T {0.230258509299404568401799145468436420760110148862877297603};
}

template <typename RealType>
bool is_subnormal(const RealType x) noexcept
{
    return std::fpclassify(x) == FP_SUBNORMAL;
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType round(const RealType val, const unsigned precision = 2)
{
    const auto factor = std::pow(RealType {10.0}, precision);
    return std::round(val * factor) / factor;
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
T round_sf(const T x, const int n)
{
    // https://stackoverflow.com/a/13094362/2970186
    if (x == 0.0) return 0;
    auto factor = std::pow(10.0, n - std::ceil(std::log10(std::abs(x))));
    return std::round(x * factor) / factor;
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
bool almost_equal(const T lhs, T rhs, const int ulp = 1)
{
    return lhs == rhs || std::abs(lhs - rhs) < std::numeric_limits<T>::epsilon() * std::abs(lhs + rhs) * ulp;
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
bool almost_zero(const T x, const int ulp = 1)
{
    return almost_equal(x, T {0}, ulp);
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
bool almost_one(const T x, const int ulp = 1)
{
    return almost_equal(x, T {1}, ulp);
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
int count_leading_zeros(const T x)
{
    if (x == 0.0) return 0;
    return -std::ceil(std::log10(std::abs(x - std::numeric_limits<T>::epsilon())));
}

template <typename IntegerType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>>
constexpr IntegerType ipow(const IntegerType base, const IntegerType exponent) noexcept
{
    if (exponent == 0) return 1;
    if (exponent == 1) return base;
    const auto y = ipow(base, exponent / 2);
    return exponent % 2 == 0 ? y * y : base * y * y;
}

template <typename IntegerType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>>
bool is_safe_ipow(const IntegerType base, const IntegerType exponent) noexcept
{
    return exponent * std::log(base) <= std::log(std::numeric_limits<IntegerType>::max());
}

template <typename RealType>
constexpr RealType exp_maclaurin(const RealType x)
{
    return (6 + x * (6 + x * (3 + x))) * 0.16666666;
}

template <typename RealType>
constexpr RealType mercator(const RealType x)
{
    return x - x * x / 2 + x * x * x / 3;
}

struct IdFunction
{
    template <typename T>
    const T& operator()(const T& x) const noexcept { return x; }
};

template <typename RealType = double, typename InputIt, typename UnaryOperation>
auto mean(InputIt first, InputIt last, UnaryOperation unary_op)
{
    return std::accumulate(first, last, RealType {0},
                           [&] (const auto curr, const auto& x) {
                               return curr + unary_op(x);
                           }) / std::distance(first, last);
}

template <typename InputIt>
auto mean(InputIt first, InputIt last)
{
    return mean(first, last, IdFunction {});
}

template <typename Container>
auto mean(const Container& values)
{
    return mean(std::cbegin(values), std::cend(values));
}

template <typename Container, typename UnaryOperation>
auto mean(const Container& values, UnaryOperation unary_op)
{
    return mean(std::cbegin(values), std::cend(values), unary_op);
}

namespace detail {

template <typename T = double, typename ForwardIt>
T median_unsorted(ForwardIt first, ForwardIt last)
{
    const auto n = std::distance(first, last);
    assert(n > 0);
    if (n == 1) return *first;
    if (n == 2) return static_cast<T>(*first + *std::next(first)) / 2;
    const auto middle = std::next(first, n / 2);
    std::nth_element(first, middle, last);
    if (n % 2 == 1) {
        return *middle;
    } else {
        auto prev_middle_itr = std::max_element(first, middle);
        return static_cast<T>(*prev_middle_itr + *middle) / 2;
    }
}

template <typename T = double, typename ForwardIt>
T median_sorted(ForwardIt first, ForwardIt last)
{
    const auto n = std::distance(first, last);
    assert(n > 0);
    if (n == 1) return *first;
    const auto middle = std::next(first, n / 2);
    if (n % 2 == 1) {
        return *middle;
    } else {
        return static_cast<T>(*std::prev(middle) + *middle) / 2;
    }
}

template <typename T = double, typename ForwardIt>
T median_const(ForwardIt first, ForwardIt last)
{
    if (std::is_sorted(first, last)) {
        return median_sorted<T>(first, last);
    } else {
        std::vector<typename std::iterator_traits<ForwardIt>::value_type> tmp {first, last};
        return median_unsorted<T>(std::begin(tmp), std::end(tmp));
    }
}

} // namespace detail

template <typename T = double, typename ForwardIt>
T median(ForwardIt first, ForwardIt last)
{
    return detail::median_unsorted<T>(first, last);
}

template <typename T = double, typename Range>
T median(Range& values)
{
    return median<T>(std::begin(values), std::end(values));
}

template <typename T = double, typename Range>
T median(const Range& values)
{
    return detail::median_const<T>(std::cbegin(values), std::cend(values));
}

template <typename ForwardIterator, typename UnaryOperation>
auto stdev(ForwardIterator first, ForwardIterator last, UnaryOperation unary_op)
{
    const auto m = mean(first, last, unary_op);
    const auto n = std::distance(first, last);
    const auto sum_square = [&] (auto total, const auto& x) { return total + std::pow(unary_op(x) - m, 2); };
    const auto ss = std::accumulate(first, last, 0.0, sum_square);
    return std::sqrt(ss / n);
}

template <typename ForwardIterator>
auto stdev(ForwardIterator first, ForwardIterator last)
{
    return stdev(first, last, IdFunction {});
}

template <typename Container>
auto stdev(const Container& values)
{
    return stdev(std::cbegin(values), std::cend(values));
}

template <typename Container, typename UnaryOperation>
auto stdev(const Container& values, UnaryOperation unary_op)
{
    return stdev(std::cbegin(values), std::cend(values), unary_op);
}

template <typename RealType = double, typename InputIt>
RealType rmq(InputIt first, InputIt last)
{
    if (first == last) return 0.0;
    return std::sqrt((std::inner_product(first, last, first, RealType {0}))
                     / static_cast<RealType>(std::distance(first, last)));
}

template <typename RealType = double, typename Container>
RealType rmq(const Container& values)
{
    return rmq<RealType>(std::cbegin(values), std::cend(values));
}

inline float fast_exp(const float x) noexcept
{
    return fmath::exp(x);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_exp(const RealType x) noexcept
{
    return std::exp(x);
}

inline float fast_log(const float x) noexcept
{
    return fmath::log(x);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log(const RealType x) noexcept
{
    return std::log(x);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const RealType a, const RealType b)
{
    const auto r = std::minmax(a, b);
    return r.second + std::log(RealType {1} + std::exp(r.first - r.second));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const RealType a, const RealType b, const RealType c)
{
    const auto max = std::max({a, b, c});
    return max + std::log(std::exp(a - max) + std::exp(b - max) + std::exp(c - max));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(std::initializer_list<RealType> il)
{
    const auto max = std::max(il);
    return max + std::log(std::accumulate(std::cbegin(il), std::cend(il), RealType {0},
                                          [max] (const auto curr, const auto x) {
                                              return curr + std::exp(x - max);
                                          }));
}

template <typename ForwardIt,
          typename = std::enable_if_t<!std::is_floating_point<ForwardIt>::value>>
inline auto log_sum_exp(ForwardIt first, ForwardIt last)
{
    assert(first != last);
    using RealType = typename std::iterator_traits<ForwardIt>::value_type;
    const auto max = *std::max_element(first, last);
    return max + std::log(std::accumulate(first, last, RealType {0},
                                          [max] (const auto curr, const auto x) {
                                              return curr + std::exp(x - max);
                                          }));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const std::array<RealType, 1>& logs)
{
    return logs[0];
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const std::array<RealType, 2>& logs)
{
    return log_sum_exp(logs[0], logs[1]);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const std::array<RealType, 3>& logs)
{
    return log_sum_exp(logs[0], logs[1], logs[2]);
}

template <typename Container>
inline auto log_sum_exp(const Container& values)
{
    return log_sum_exp(std::cbegin(values), std::cend(values));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const RealType a, const RealType b) noexcept
{
    const auto r = std::minmax(a, b);
    return r.second + fast_log(RealType {1} + fast_exp(r.first - r.second));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const RealType a, const RealType b, const RealType c) noexcept
{
    const auto max = std::max({a, b, c});
    return max + fast_log(fast_exp(a - max) + fast_exp(b - max) + fast_exp(c - max));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(std::initializer_list<RealType> il) noexcept
{
    const auto max = std::max(il);
    return max + fast_log(std::accumulate(std::cbegin(il), std::cend(il), RealType {0},
                                          [max] (const auto curr, const auto x) noexcept {
                                              return curr + fast_exp(x - max);
                                          }));
}

template <typename ForwardIt,
          typename = std::enable_if_t<!std::is_floating_point<ForwardIt>::value>>
inline auto fast_log_sum_exp(ForwardIt first, ForwardIt last) noexcept
{
    assert(first != last);
    using RealType = typename std::iterator_traits<ForwardIt>::value_type;
    const auto max = *std::max_element(first, last);
    return max + fast_log(std::accumulate(first, last, RealType {0},
                                          [max] (const auto curr, const auto x) noexcept {
                                              return curr + fast_exp(x - max);
                                          }));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const std::array<RealType, 1>& logs) noexcept
{
    return logs[0];
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const std::array<RealType, 2>& logs) noexcept
{
    return fast_log_sum_exp(logs[0], logs[1]);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const std::array<RealType, 3>& logs) noexcept
{
    return fast_log_sum_exp(logs[0], logs[1], logs[2]);
}

template <typename Container>
inline auto fast_log_sum_exp(const Container& values) noexcept
{
    return fast_log_sum_exp(std::cbegin(values), std::cend(values));
}

template <typename T, typename IntegerType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>>
T factorial(const IntegerType x)
{
    return boost::math::factorial<double>(x);
}

template <typename RealType, typename IntegerType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>>
RealType log_factorial(IntegerType x)
{
    return std::lgamma(x + 1);
}

template <typename InputIt, typename Log>
auto entropy(InputIt first, InputIt last, Log&& log)
{
    using RealType = typename std::iterator_traits<InputIt>::value_type;
    static_assert(std::is_floating_point<RealType>::value,
                  "entropy is only defined for floating point values");
    const auto add_entropy = [&] (auto total, auto p) { return total + (p > 0 ? p * log(p) : 0); };
    return -std::accumulate(first, last, RealType {0}, add_entropy);
}

template <typename Range, typename Log>
auto entropy(const Range& values, Log&& log)
{
    return entropy(std::cbegin(values), std::cend(values), std::forward<Log>(log));
}

template <typename InputIt>
auto entropy(InputIt first, InputIt last)
{
    return entropy(first, last, [] (auto x) { return std::log(x); });
}

template <typename Range>
auto entropy(const Range& values)
{
    return entropy(std::cbegin(values), std::cend(values));
}

template <typename InputIt>
auto entropy2(InputIt first, InputIt last)
{
    return entropy(first, last, [] (auto x) { return std::log2(x); });
}

template <typename Range>
auto entropy2(const Range& values)
{
    return entropy2(std::cbegin(values), std::cend(values));
}

template <typename InputIt>
auto entropy10(InputIt first, InputIt last)
{
    return entropy(first, last, [] (auto x) { return std::log10(x); });
}

template <typename Range>
auto entropy10(const Range& values)
{
    return entropy10(std::cbegin(values), std::cend(values));
}

template <typename RealType, typename IntegerType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_binomial_coefficient(const IntegerType n, const IntegerType k)
{
    return log_factorial<RealType>(n) - (log_factorial<RealType>(k) + log_factorial<RealType>(n - k));
}

template <typename IntegerType, typename RealType = double,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_fisher_exact_test(const IntegerType a, const IntegerType b, const IntegerType c, const IntegerType d)
{
    return log_binomial_coefficient<RealType>(a + b, b) + log_binomial_coefficient<RealType>(c + d, d)
                - log_binomial_coefficient<RealType>(a + b + c + d, b + d);
}

template <typename IntegerType, typename RealType = double,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType fisher_exact_test(const IntegerType a, const IntegerType b, const IntegerType c, const IntegerType d)
{
    return std::exp(log_fisher_exact_test<IntegerType, RealType>(a, b, c, d));
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType geometric_pdf(const IntegerType k, const RealType p)
{
    boost::math::geometric_distribution<RealType> dist {p};
    return boost::math::pdf(dist, k);
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType binomial_pdf(const IntegerType k, const IntegerType n, const RealType p)
{
    boost::math::binomial_distribution<RealType> dist {static_cast<RealType>(n), p};
    return boost::math::pdf(dist, k);
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_poisson_pmf(const IntegerType k, const RealType mu)
{
    if (k > 0) {
        return k * std::log(mu) - std::lgamma(k) - mu;
    } else {
        return -mu;
    }
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType poisson_cdf(const IntegerType k, const RealType mu)
{
    return almost_zero(mu) ? 1.0 : boost::math::gamma_q(k + 1, mu);
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType poisson_sf(const IntegerType k, const RealType mu)
{
    return almost_zero(mu) ? 0.0 : boost::math::gamma_p(k + 1, mu);
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_poisson_cdf(const IntegerType k, const RealType mu)
{
    return std::log(poisson_cdf(k, mu));
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_poisson_sf(const IntegerType k, const RealType mu)
{
    return std::log(poisson_sf(k, mu));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType normal_cdf(const RealType x, const RealType mu, const RealType sigma)
{
    boost::math::normal_distribution<RealType> dist {mu, sigma};
    return boost::math::cdf(dist, x);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType normal_sf(const RealType x, const RealType mu, const RealType sigma)
{
    boost::math::normal_distribution<RealType> dist {mu, sigma};
    return boost::math::cdf(boost::math::complement(dist, x));
}

template <typename ForwardIt>
auto log_beta(const ForwardIt first, const ForwardIt last)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_beta is only defined for floating point types.");
    return std::accumulate(first, last, T {0}, [] (const auto curr, const auto x) { return curr + std::lgamma(x); })
           - std::lgamma(std::accumulate(first, last, T {0}));
}

template <typename Container>
auto log_beta(const Container& values)
{
    return log_beta(std::cbegin(values), std::cend(values));
}

template <typename ForwardIt1, typename ForwardIt2>
auto log_dirichlet(ForwardIt1 firstalpha, ForwardIt1 lastalpha, ForwardIt2 firstpi)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt1>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_dirichlet is only defined for floating point types.");
    return std::inner_product(firstalpha, lastalpha, firstpi, T {0}, std::plus<> {},
                              [] (const auto a, const auto p) { return (a - 1) * std::log(p); })
            - log_beta(firstalpha, lastalpha);
}

template <typename Container1, typename Container2>
auto log_dirichlet(const Container1& alpha, const Container2& pi)
{
    return log_dirichlet(std::cbegin(alpha), std::cend(alpha), std::cbegin(pi));
}

template <typename ForwardIt>
auto dirichlet_expectation(ForwardIt first_alpha, ForwardIt last_alpha)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_dirichlet is only defined for floating point types.");
    const auto K = static_cast<std::size_t>(std::distance(first_alpha, last_alpha));
    const auto a0 = std::accumulate(first_alpha, last_alpha, T {0});
    std::vector<T> result(K);
    std::transform(first_alpha, last_alpha, std::begin(result), [a0] (auto a) { return a / a0; });
    return result;
}

template <typename Range>
auto dirichlet_expectation(const Range& values)
{
    return dirichlet_expectation(std::cbegin(values), std::cend(values));
}

template <typename ForwardIt>
auto dirichlet_expectation(const unsigned i, ForwardIt first_alpha, ForwardIt last_alpha)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_dirichlet is only defined for floating point types.");
    assert(i < static_cast<unsigned>(std::distance(first_alpha, last_alpha)));
    return *std::next(first_alpha, i) / std::accumulate(first_alpha, last_alpha, T {0});
}

template <typename Range>
auto dirichlet_expectation(const unsigned i, const Range& values)
{
    return dirichlet_expectation(i, std::cbegin(values), std::cend(values));
}

template <typename ForwardIt>
auto dirichlet_entropy(ForwardIt first_alpha, ForwardIt last_alpha)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_dirichlet is only defined for floating point types.");
    const auto K = static_cast<T>(std::distance(first_alpha, last_alpha));
    const auto a0 = std::accumulate(first_alpha, last_alpha, T {0});
    using boost::math::digamma;
    return log_beta(first_alpha, last_alpha) + (a0 - K) * digamma(a0)
           - std::accumulate(first_alpha, last_alpha, T {0}, [] (auto curr, auto a) { return curr + (a - 1) * digamma(a); });
}

template <typename Range>
auto dirichlet_entropy(const Range& values)
{
    return dirichlet_entropy(std::cbegin(values), std::cend(values));
}

template <typename RealType, typename IntegerType>
inline RealType log_multinomial_coefficient(std::initializer_list<IntegerType> il)
{
    using std::begin; using std::end; using std::cbegin; using std::cend; using std::accumulate;
    std::vector<RealType> denoms(il.size());
    std::transform(cbegin(il), cend(il), begin(denoms), log_factorial<RealType, IntegerType>);
    return log_factorial<RealType>(accumulate(cbegin(il), cend(il), 0))
            - accumulate(cbegin(denoms), cend(denoms), RealType {0});
}

template <typename RealType, typename Iterator>
inline RealType log_multinomial_coefficient(Iterator first, Iterator last)
{
    using IntegerType = typename Iterator::value_type;
    std::vector<RealType> denoms(std::distance(first, last));
    std::transform(first, last, std::begin(denoms), log_factorial<RealType, IntegerType>);
    return log_factorial<RealType, IntegerType>(std::accumulate(first, last, IntegerType {0}))
            - std::accumulate(std::cbegin(denoms), std::cend(denoms), RealType {0});
}

template <typename RealType, typename Container>
inline RealType log_multinomial_coefficient(const Container& values)
{
    return log_multinomial_coefficient<RealType>(std::cbegin(values), std::cend(values));
}

template <typename RealType, typename IntegerType>
inline IntegerType multinomial_coefficient(std::initializer_list<IntegerType> il)
{
    return static_cast<IntegerType>(std::exp(log_multinomial_coefficient<RealType, IntegerType>(std::move(il))));
}

template <typename IntegerType, typename RealType, typename Iterator>
inline IntegerType multinomial_coefficient(Iterator first, Iterator last)
{
    return static_cast<IntegerType>(std::exp(log_multinomial_coefficient<RealType>(first, last)));
}

template <typename IntegerType, typename RealType, typename Container>
inline IntegerType multinomial_coefficient(const Container& values)
{
    return multinomial_coefficient<IntegerType, RealType>(std::cbegin(values), std::cend(values));
}

template <typename RealType, typename ForwardIt1, typename ForwardIt2>
inline RealType multinomial_pdf(ForwardIt1 first_z, ForwardIt1 last_z, ForwardIt2 first_p)
{
    auto r = std::inner_product(first_z, last_z, first_p, RealType {0}, std::multiplies<> {},
                                [] (auto z_i, auto p_i) { return std::pow(p_i, z_i); });
    return multinomial_coefficient<RealType>(first_z, last_z) * r;
}

template <typename IntegerType, typename RealType>
inline RealType multinomial_pdf(const std::vector<IntegerType>& z, const std::vector<RealType>& p)
{
    assert(z.size() == p.size());
    return multinomial_pdf<RealType>(std::cbegin(z), std::cend(z), std::cbegin(p));
}

template <typename RealType, typename ForwardIt1, typename ForwardIt2>
inline RealType log_multinomial_pdf(ForwardIt1 first_z, ForwardIt1 last_z, ForwardIt2 first_p)
{
    auto r = std::inner_product(first_z, last_z, first_p, RealType {0}, std::plus<> {},
                                [] (auto z_i, auto p_i) { return z_i > 0 ? z_i * std::log(p_i) : 0.0; });
    return log_multinomial_coefficient<RealType>(first_z, last_z) + r;
}

template <typename IntegerType, typename RealType>
inline RealType log_multinomial_pdf(const std::vector<IntegerType>& z, const std::vector<RealType>& p)
{
    assert(z.size() == p.size());
    return log_multinomial_pdf<RealType>(std::cbegin(z), std::cend(z), std::cbegin(p));
}

// Returns approximate y such that digamma(y) = x
template <typename RealType>
inline RealType digamma_inv(const RealType x, const RealType epsilon = 10e-8)
{
    RealType l {1};
    auto y = std::exp(x);
    while (l > epsilon) {
        y += l * boost::math::sign(x - boost::math::digamma<RealType>(y));
        l /= 2;
    }
    return y;
}

namespace detail {

template <typename T, typename RealType>
T ifactorial(RealType x, std::true_type)
{
    return factorial<T, unsigned>(x);
}

template <typename T, typename RealType>
T ifactorial(RealType x, std::false_type)
{
    return factorial<T>(x);
}

template <typename T, typename RealType>
T ifactorial(RealType x)
{
    return ifactorial<T>(x, std::is_floating_point<RealType> {});
}

} // namespace detail

template <typename RealType>
RealType dirichlet_multinomial(const RealType z1, const RealType z2, const RealType a1, const RealType a2)
{
    auto z_0 = z1 + z2;
    auto a_0 = a1 + a2;
    using detail::ifactorial;
    auto z_m = ifactorial<RealType>(z1) * ifactorial<RealType>(z2);
    return (ifactorial<RealType>(z_0) / z_m) *
            (std::tgamma(a_0) / std::tgamma(z_0 + a_0)) *
            (std::tgamma(z1 + a1) * std::tgamma(z2 + a2)) / (std::tgamma(a1) + std::tgamma(a2));
}

template <typename RealType>
RealType dirichlet_multinomial(const RealType z1, const RealType z2, const RealType z3,
                               const RealType a1, const RealType a2, const RealType a3)
{
    auto z_0 = z1 + z2 + z3;
    auto a_0 = a1 + a2 + a3;
    using detail::ifactorial;
    auto z_m = ifactorial<RealType>(z1) * ifactorial<RealType>(z2) * ifactorial<RealType>(z3);
    return (ifactorial<RealType>(z_0) / z_m) *
            (std::tgamma(a_0) / std::tgamma(z_0 + a_0)) *
            (std::tgamma(z1 + a1) * std::tgamma(z2 + a2) *
            std::tgamma(z3 + a3)) / (std::tgamma(a1) + std::tgamma(a2) + std::tgamma(a3));
}

template <typename RealType>
RealType dirichlet_multinomial(const std::vector<RealType>& z, const std::vector<RealType>& a)
{
    auto z_0 = std::accumulate(std::cbegin(z), std::cend(z), RealType {0});
    auto a_0 = std::accumulate(std::cbegin(a), std::cend(a), RealType {0});
    RealType z_m {1};
    using detail::ifactorial;
    for (auto z_i : z) {
        z_m *= ifactorial<RealType>(z_i);
    }
    RealType g {1};
    for (std::size_t i {0}; i < z.size(); ++i) {
        g *= std::tgamma(z[i] + a[i]) / std::tgamma(a[i]);
    }
    return (ifactorial<RealType>(z_0) / z_m) * (std::tgamma(a_0) / std::tgamma(z_0 + a_0)) * g;
}

template <typename RealType>
RealType beta_binomial(const RealType k, const RealType n, const RealType alpha, const RealType beta)
{
    return dirichlet_multinomial<RealType>(k, n - k, alpha, beta);
}

namespace detail {

template <typename RealType>
bool is_mldp_converged(std::vector<RealType>& lhs, const std::vector<RealType>& rhs,
                       const RealType epsilon)
{
    std::transform(std::cbegin(lhs), std::cend(lhs), std::cbegin(rhs), std::begin(lhs),
                   [] (const auto a, const auto b) { return std::abs(a - b); });
    return std::all_of(std::cbegin(lhs), std::cend(lhs),
                       [epsilon] (const auto x) { return x < epsilon; });
}

} // namespace detail

template <typename RealType>
std::vector<RealType>
dirichlet_mle(std::vector<RealType> pi, const RealType precision,
              const unsigned max_iterations = 100, const RealType epsilon = 0.0001)
{
    std::transform(std::cbegin(pi), std::cend(pi), std::begin(pi),
                   [] (const auto p) { return std::log(p); });
    const auto l = pi.size();
    const RealType u {RealType {1} / l};
    std::vector<RealType> result(l, u), curr_result(l, u), means(l, u);
    for (unsigned n {0}; n < max_iterations; ++n) {
        RealType v {0};
        for (std::size_t j {0}; j < l; ++j) {
            v += means[j] * (pi[j] - boost::math::digamma<RealType>(precision * means[j]));
        }
        for (std::size_t k {0}; k < l; ++k) {
            curr_result[k] = digamma_inv<RealType>(pi[k] - v);
            means[k] = curr_result[k] / std::accumulate(std::cbegin(curr_result), std::cend(curr_result), RealType {0});
        }
        if (detail::is_mldp_converged(result, curr_result, epsilon)) {
            return curr_result;
        }
        result = curr_result;
    }
    return result;
}

template <typename NumericType = float, typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
NumericType probability_true_to_phred(const RealType p)
{
    return NumericType{-10} * std::log10(std::max(RealType {1} - p, std::numeric_limits<RealType>::epsilon()));
}

template <typename NumericType = float, typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
NumericType probability_true_to_phred(const RealType p, const unsigned precision)
{
    return round(static_cast<NumericType>(RealType {-10} * std::log10(std::max(RealType {1} - p, std::numeric_limits<RealType>::epsilon()))), precision);
}

template <typename RealType = double, typename NumericType>
RealType phred_to_probability(const NumericType phred)
{
    return RealType {1} - std::pow(RealType {10}, RealType {-1} * static_cast<RealType>(phred) / RealType {10});
}

template <typename MapType>
typename MapType::key_type sum_keys(const MapType& map)
{
    return std::accumulate(std::cbegin(map), std::cend(map), typename MapType::key_type {},
                           [] (const auto previous, const auto& p) { return previous + p.first; });
}

template <typename ResultType, typename MapType, typename UnaryOperation>
ResultType sum_keys(const MapType& map, UnaryOperation op)
{
    return std::accumulate(std::cbegin(map), std::cend(map), ResultType {0},
                           [op] (const auto previous, const auto& p) { return previous + op(p.first); });
}

template <typename MapType>
typename MapType::mapped_type sum_values(const MapType& map)
{
    return std::accumulate(std::cbegin(map), std::cend(map), typename MapType::mapped_type {},
                           [] (const auto previous, const auto& p) { return previous + p.second; });
}

template <typename ResultType, typename MapType, typename UnaryOperation>
ResultType sum_values(const MapType& map, UnaryOperation op)
{
    return std::accumulate(std::cbegin(map), std::cend(map), ResultType {0},
                           [op] (const auto previous, const auto& p) { return previous + op(p.second); });
}

template <typename Map>
std::size_t sum_sizes(const Map& map)
{
    return std::accumulate(std::cbegin(map), std::cend(map), std::size_t {0},
                           [] (const auto& p, const auto& v) { return p + v.second.size(); });
}

template <typename InputIt1, typename InputIt2, typename InputIt3, typename T,
          typename BinaryOperation1, typename BinaryOperation2>
T inner_product(InputIt1 first1, InputIt1 last1,
                InputIt2 first2, InputIt3 first3, T value,
                BinaryOperation1 op1, BinaryOperation2 op2)
{
    while (first1 != last1) {
        value = op1(value, op2(*first1, *first2, *first3));
        ++first1;
        ++first2;
        ++first3;
    }
    return value;
}

template <typename InputIt1, typename InputIt2, typename InputIt3, typename InputIt4,
          typename T, typename BinaryOperation1, typename BinaryOperation2>
T inner_product(InputIt1 first1, InputIt1 last1,
                InputIt2 first2, InputIt3 first3,
                InputIt4 first4, T value,
                BinaryOperation1 op1, BinaryOperation2 op2)
{
    while (first1 != last1) {
        value = op1(value, op2(*first1, *first2, *first3, *first4));
        ++first1;
        ++first2;
        ++first3;
        ++first4;
    }
    return value;
}

template <typename RealType>
RealType beta_cdf(const RealType a, const RealType b, const RealType x)
{
    const boost::math::beta_distribution<> beta_dist {a, b};
    return boost::math::cdf(beta_dist, x);
}

template <typename RealType>
RealType beta_sf(const RealType a, const RealType b, const RealType x)
{
    const boost::math::beta_distribution<> beta_dist {a, b};
    return boost::math::cdf(boost::math::complement(beta_dist, x));
}

template <typename RealType>
RealType beta_tail_probability(const RealType a, const RealType b, const RealType x)
{
    return beta_cdf(a, b, x) + beta_sf(a, b, RealType {1} - x);
}

namespace detail {

template <typename RealType>
std::pair<RealType, RealType>
uniform_hdi(const RealType mass)
{
    const auto x = RealType {0.5} - mass / 2;
    return std::make_pair(x, x + mass);
}

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi_symmetric(const RealType a, const RealType mass)
{
    const auto x = boost::math::ibeta_inv(a, a, (RealType {1} - mass) / 2);
    return std::make_pair(x, RealType {1} - x);
}

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi_unbounded_rhs(const RealType a, const RealType mass)
{
    // Reverse J shaped
    return std::make_pair(boost::math::ibeta_inv(a, RealType {1}, RealType {1} - mass), RealType {1});
}

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi_unbounded_lhs(const RealType b, const RealType mass)
{
    // J shaped
    return std::make_pair(RealType {0}, boost::math::ibeta_inv(RealType {1}, b, mass));
}

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi_skewed(const RealType a, const RealType b, const RealType mass)
{
    const auto c = (RealType {1} - mass) / 2;
    return std::make_pair(boost::math::ibeta_inv(a, b, c), boost::math::ibeta_inv(a, b, c + mass));
}

} // namespace detail

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi(RealType a, RealType b, const RealType mass)
{
    static_assert(std::is_floating_point<RealType>::value, "beta_hdi only works for floating point types");
    if (mass < RealType {0} || mass > RealType {1}) {
        throw std::domain_error {"beta_hdi: given mass not in range [0, 1]"};
    }
    if (a <= RealType {0} || b <= RealType {0}) {
        throw std::domain_error {"beta_hdi: given non-positive parameter"};
    }
    if (mass == RealType {0}) {
        const auto mean = a / (a + b);
        return std::make_pair(mean, mean);
    }
    if (mass == RealType {1}) {
        return std::make_pair(RealType {0}, RealType {1});
    }
    if (a == b) {
        if (a == RealType {1}) {
            return detail::uniform_hdi(mass);
        } else {
            return detail::beta_hdi_symmetric(a, mass);
        }
    }
    if (a == RealType {1}) {
        return detail::beta_hdi_unbounded_lhs(b, mass);
    }
    if (b == RealType {1}) {
        return detail::beta_hdi_unbounded_rhs(a, mass);
    }
    return detail::beta_hdi_skewed(a, b, mass);
}

template <typename RealType>
RealType dirichlet_variance(const std::vector<RealType>& alphas, const std::size_t k)
{
    const auto a_0 = std::accumulate(std::cbegin(alphas), std::cend(alphas), RealType {});
    return (alphas[k] * (a_0 - alphas[k])) / (a_0 * a_0 * (a_0 + 1));
}

template <typename RealType>
RealType dirichlet_marginal_cdf(const std::vector<RealType>& alphas, const std::size_t k, const RealType x)
{
    const auto a_0 = std::accumulate(std::cbegin(alphas), std::cend(alphas), RealType {});
    return beta_cdf(alphas[k], a_0 - alphas[k], x);
}

template <typename RealType>
RealType dirichlet_marginal_sf(const std::vector<RealType>& alphas, const std::size_t k, const RealType x)
{
    const auto a_0 = std::accumulate(std::cbegin(alphas), std::cend(alphas), RealType {});
    return beta_sf(alphas[k], a_0 - alphas[k], x);
}

template <typename Range>
void log_each(Range& values)
{
    for (auto& v : values) v = std::log(v);
}

template <typename Range>
void exp_each(Range& values)
{
    for (auto& v : values) v = std::exp(v);
}

template <typename ForwardIterator>
auto normalise(const ForwardIterator first, const ForwardIterator last)
{
    using T = typename std::iterator_traits<ForwardIterator>::value_type;
    const auto norm = std::accumulate(first, last, T {});
    if (norm > 0) std::for_each(first, last, [norm] (auto& value) { value /= norm; });
    return norm;
}

template <typename Range>
auto normalise(Range& values)
{
    return normalise(std::begin(values), std::end(values));
}

template <typename ForwardIterator>
auto normalise_logs(const ForwardIterator first, const ForwardIterator last)
{
    const auto norm = log_sum_exp(first, last);
    std::for_each(first, last, [norm] (auto& value) { value -= norm; });
    return norm;
}

template <typename Range>
auto normalise_logs(Range& logs)
{
    return normalise_logs(std::begin(logs), std::end(logs));
}

template <typename ForwardIterator>
auto normalise_exp(const ForwardIterator first, const ForwardIterator last)
{
    const auto norm = log_sum_exp(first, last);
    std::transform(first, last, first, [norm] (auto& value) { return std::exp(value - norm); });
    return norm;
}

template <typename Range>
auto normalise_exp(Range& logs)
{
    return normalise_exp(std::begin(logs), std::end(logs));
}

} // namespace maths
} // namespace octopus

#endif


#include <iostream>
#include <ctime>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include "boost/program_options.hpp"
#include "boost/random.hpp"

using namespace std;
namespace po = boost::program_options;
using namespace boost::numeric::ublas;

typedef matrix<double, column_major> uBlasMat;
typedef vector<double, column_major> uBlasVec;
typedef boost::minstd_rand base_generator_type;
typedef boost::variate_generator<base_generator_type&, boost::uniform_real<> > rng;

uBlasMat initMatrix(uBlasMat &mat, rng &rnd) {

  for (unsigned i = 0; i < mat.size1(); ++i) {
    for (unsigned j = 0; j < mat.size2(); ++j) {
      mat (i, j) = rnd();
    }
  }

  return mat;
}

uBlasVec initVector(uBlasVec &vec, rng &rnd) {

  for (unsigned i = 0; i < mat.size(); ++i) {
      vec (i) = rnd();
  }

  return vec;
}

// m: numRows, n: numCols
inline double simpleDenseTest_UBlas(int m, int n, int num_trials) {
  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);

  uBlasMat A(m, n); initMatrix(A, uni);
  uBlasMat B(m, n); initMatrix(B, uni);
  uBlasMat C(m, n); initMatrix(C, uni);
  uBlasMat D(m, n); initMatrix(D, uni);
  uBlasMat E(m, n); initMatrix(E, uni);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    uBlasMat res = element_prod((element_div((A + B), C) - D), E);

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;

}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double gemmSanityTest_UBlas(int m, int n, int k, int num_trials) {

  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);
  uBlasMat A(m, n); initMatrix(A, uni);
  uBlasMat C(n, k); initMatrix(C, uni);
  uBlasMat E(m, k); initMatrix(E, uni);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    E += prod(A, C);

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;
}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double gemmDenseTest_UBlas(int m, int n, int k, int num_trials) {

  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);
  uBlasMat A(m, n); initMatrix(A, uni);
  uBlasMat B(m, n); initMatrix(B, uni);
  uBlasMat C(n, k); initMatrix(C, uni);
  uBlasMat D(n, k); initMatrix(D, uni);
  uBlasMat E(m, k); initMatrix(E, uni);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    E += prod((A + B), (C - D));

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;
}

inline double mulDenseTest_UBlas(int a, int b, int c, int d, int num_trials) {

  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);
  uBlasMat A(a, a); initMatrix(A, uni);
  uBlasMat B(a, b); initMatrix(B, uni);
  uBlasMat C(b, c); initMatrix(C, uni);
  uBlasMat D(c, d); initMatrix(D, uni);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    uBlasMat res = uBlasMat(prod(uBlasMat(prod(uBlasMat(prod(A, B)), C)), D));

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;
}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double denseVectorTest_UBlas(int l, int num_trials) {

  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);
  uBlasVec a(l); initVector(a, uni);
  uBlasVec b(l); initVector(b, uni);
  uBlasVec c(l); initVector(c, uni);
  uBlasVec d(l); initVector(d, uni);
  uBlasVec res(l);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    res = a + b + c + d;

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;

  return duration / num_trials;
}

void runUBlasTests(int num_trials, int l, int m, int n, int k, int a, int b, int c, int d,
    bool skip_vec, bool skip_simple, bool skip_gemm, bool skip_mult) {

  if (!skip_vec)
      cout << "UBlas Vectors Test:\t" << denseVectorTest_UBlas(l, num_trials) << endl;
  if (!skip_simple)
    cout << "UBlas Simple Test:\t" << simpleDenseTest_UBlas(m, n, num_trials) << endl;
  if (!skip_gemm) {
    cout << "UBlas gemmSanity Test:\t" << gemmDenseTest_UBlas(m, n, k, num_trials) << endl;
    cout << "UBlas gemm Test:\t" << gemmDenseTest_UBlas(m, n, k, num_trials) << endl;
  }

  if (!skip_mult)
    cout << "UBlas mulDense Test:\t" << mulDenseTest_UBlas(a, b, c, d, num_trials) << endl;

}

int main(int argc, char *argv[]) {

    int l, m, n, k, a, b, c, d, trials;
    bool skip_vec, skip_simple, skip_gemm, skip_mult;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("l", po::value<int>(&l)->default_value(1048576),
                    "length of vectors in vector addition test")
        ("m", po::value<int>(&m)->default_value(1024),
            "numRows of matrices in Simple Test, and gemm Test")
        ("n", po::value<int>(&n)->default_value(1024),
            "numCols of matrices in Simple Test, and gemm Test")
        ("k", po::value<int>(&k)->default_value(1024),
            "numCols of B in gemm Test")
        ("trials", po::value<int>(&trials)->default_value(10), "number of trials")
        ("a", po::value<int>(&a)->default_value(1024),
            "size matrix A in mulDense Test")
        ("b", po::value<int>(&b)->default_value(512),
            "size matrix B in mulDense Test")
        ("c", po::value<int>(&c)->default_value(256),
            "size matrix C in mulDense Test")
        ("d", po::value<int>(&d)->default_value(128),
            "size matrix D in mulDense Test")
        ("skip-vec", po::value<bool>(&skip_vec)->default_value(false),
            "skip vectors Test")
        ("skip-simple", po::value<bool>(&skip_simple)->default_value(false),
            "skip simple Test")
        ("skip-gemm", po::value<bool>(&skip_gemm)->default_value(false),
            "skip gemm Tests")
        ("skip-mult", po::value<bool>(&skip_mult)->default_value(false),
            "skip mulDense Test")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    runUBlasTests(trials, l, m, n, k, a, b, c, d, skip_vec, skip_simple, skip_gemm, skip_mult);

    return 0;
}


#include <iostream>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/algorithm/minmax.hpp>
#include <vector>
#include <fstream>
#include <string>
#include <sstream>
#include <algorithm>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "dbscan.h"

using namespace std;
namespace clustering {
    DBSCAN::ClusterData DBSCAN::gen_cluster_data( size_t features_num, size_t elements_num ,double* data)//load data for clustering
{
    DBSCAN::ClusterData cl_d( elements_num, features_num );

    for ( size_t i = 0; i < elements_num; ++i )
    {
        for ( size_t j = 0; j < features_num; ++j )
        {
            cl_d( i, j ) = data[i*features_num+j];
        }
    }

    return cl_d;
}

DBSCAN::FeaturesWeights DBSCAN::std_weights( size_t s )
{
    // num cols
    DBSCAN::FeaturesWeights ws( s );

    for ( size_t i = 0; i < s; ++i ) {
        ws( i ) = 1.0;
    }

    return ws;
}

DBSCAN::DBSCAN()
{
}

static int num_threads_or_default( int nt )
{
    if ( !nt ) {
        return 0;//omp_get_max_threads();
    }
    return nt;
}

void DBSCAN::init( double eps, size_t min_elems, int num_threads )
{
    m_eps = eps;
    m_min_elems = min_elems;
    m_num_threads = num_threads_or_default( num_threads );
}

DBSCAN::DBSCAN( double eps, size_t min_elems, int num_threads )
    : m_eps( eps )
    , m_min_elems( min_elems )
    , m_num_threads( num_threads_or_default( num_threads ) )
    , m_dmin( 0.0 )
    , m_dmax( 0.0 )
{
    reset();
}

DBSCAN::~DBSCAN()
{
}

void DBSCAN::reset()
{
    m_labels.clear();
}

void DBSCAN::prepare_labels( size_t s )
{
    m_labels.resize( s );

    for ( auto& l : m_labels ) {
        l = -1;
    }
}

const DBSCAN::DistanceMatrix DBSCAN::calc_dist_matrix( const DBSCAN::ClusterData& C, const DBSCAN::FeaturesWeights& W )
{
    DBSCAN::ClusterData cl_d = C;

 //   omp_set_dynamic( 0 );
 //   omp_set_num_threads( m_num_threads );
//#pragma omp parallel for
    for ( size_t i = 0; i < cl_d.size2(); ++i ) {
        ublas::matrix_column< DBSCAN::ClusterData > col( cl_d, i );

        const auto r = minmax_element( col.begin(), col.end() );

        double data_min = *r.first;
        double data_range = *r.second - *r.first;

        if ( data_range == 0.0 ) {
            data_range = 1.0;
        }

        const double scale = 1 / data_range;
        const double min = -1.0 * data_min * scale;

        col *= scale;
        col.plus_assign( ublas::scalar_vector< typename ublas::matrix_column< DBSCAN::ClusterData >::value_type >( col.size(), min ) );
    }

    // rows x rows
    DBSCAN::DistanceMatrix d_m( cl_d.size1(), cl_d.size1() );
    ublas::vector< double > d_max( cl_d.size1() );
    ublas::vector< double > d_min( cl_d.size1() );

 //   omp_set_dynamic( 0 );
 //   omp_set_num_threads( m_num_threads );
//#pragma omp parallel for
    for ( size_t i = 0; i < cl_d.size1(); ++i ) {
        for ( size_t j = i; j < cl_d.size1(); ++j ) {
            d_m( i, j ) = 0.0;

            if ( i != j ) {
                ublas::matrix_row< DBSCAN::ClusterData > U( cl_d, i );
                ublas::matrix_row< DBSCAN::ClusterData > V( cl_d, j );

                int k = 0;
                for ( const auto e : ( U - V ) ) {
                    d_m( i, j ) += fabs( e ) * W[k++];
                }

                d_m( j, i ) = d_m( i, j );
            }
        }

        const auto cur_row = ublas::matrix_row< DBSCAN::DistanceMatrix >( d_m, i );
        const auto mm = minmax_element( cur_row.begin(), cur_row.end() );

        d_max( i ) = *mm.second;
        d_min( i ) = *mm.first;
    }

    m_dmin = *( min_element( d_min.begin(), d_min.end() ) );
    m_dmax = *( max_element( d_max.begin(), d_max.end() ) );

    m_eps = ( m_dmax - m_dmin ) * m_eps + m_dmin;

    return d_m;
}

DBSCAN::Neighbors DBSCAN::find_neighbors( const DBSCAN::DistanceMatrix& D, uint32_t pid )
{
    Neighbors ne;

    for ( uint32_t j = 0; j < D.size1(); ++j ) {
        if ( D( pid, j ) <= m_eps ) {
            ne.push_back( j );
        }
    }
    return ne;
}

void DBSCAN::dbscan( const DBSCAN::DistanceMatrix& dm )
{
    std::vector< uint8_t > visited( dm.size1() );

    uint32_t cluster_id = 0;

    for ( uint32_t pid = 0; pid < dm.size1(); ++pid ) {
        if ( !visited[pid] ) {
            visited[pid] = 1;

            Neighbors ne = find_neighbors( dm, pid );

            if ( ne.size() >= m_min_elems ) {
                m_labels[pid] = cluster_id;

                for ( uint32_t i = 0; i < ne.size(); ++i ) {
                    uint32_t nPid = ne[i];

                    if ( !visited[nPid] ) {
                        visited[nPid] = 1;

                        Neighbors ne1 = find_neighbors( dm, nPid );

                        if ( ne1.size() >= m_min_elems ) {
                            for ( const auto& n1 : ne1 ) {
                                ne.push_back( n1 );
                            }
                        }
                    }

                    if ( m_labels[nPid] == -1 ) {
                        m_labels[nPid] = cluster_id;
                    }
                }

                ++cluster_id;
            }
        }
    }
}

void DBSCAN::fit( const DBSCAN::ClusterData& C )
{
    const DBSCAN::FeaturesWeights W = DBSCAN::std_weights( C.size2() );
    wfit( C, W );
}
void DBSCAN::fit_precomputed( const DBSCAN::DistanceMatrix& D )
{
    prepare_labels( D.size1() );
    dbscan( D );
}

void DBSCAN::wfit( const DBSCAN::ClusterData& C, const DBSCAN::FeaturesWeights& W )
{
    prepare_labels( C.size1() );
    const DBSCAN::DistanceMatrix D = calc_dist_matrix( C, W );
    dbscan( D );
}

const DBSCAN::Labels& DBSCAN::get_labels() const
{
    return m_labels;
}

    //save clusters
int DBSCAN::save_labels(std::string name, std::vector <std::string> ndata, std::vector <std::string> uid,std::vector <int>* ind, double * data)
{
    std::vector <std::vector <int>> final;
    std::vector <int> tmp;

    int mm=0;
    for (int j=0;j<m_labels.size();j++)
    {
        if((mm)<m_labels[j])
            mm=m_labels[j];
        ind->push_back(m_labels[j]);
    }

    //assign a cluster to each unassigned one I need Y to claculate the centres and then

    for (int j=0;j<=mm;j++)
    {
        final.push_back(tmp);
    }

    std::ofstream myfile (name);

  //  std::vector <double> cen;
  //  std::vector <int> ss;

/*    for(int i=0;i<=2*mm+1;i++)
    {
        cen.push_back(0);
        ss.push_back(0);
    }
    for (int i=0;i<ind->size();i++)
    {
        if(ind->at(i)>=0)
        {
            cen[ind->at(i)*2+1]=data[i*2+1];
            cen[ind->at(i)*2]=data[i*2];
            ss[ind->at(i)*2+1]++;
            ss[ind->at(i)*2]++;
        }
    }

    for (int i=0;i<cen.size();i++)
        cen[i]/=ss[i];

    ss.clear();*/
    for (int j=0;j<ind->size();j++)
    {
        if(ind->at(j)>=0)
            final[ind->at(j)].push_back(j);
      /*  else
        {
            double mind=1000000;
            int mini=-1;
            for (int i=0;i<=mm;i++)
            {
                double dist=sqrt((data[j*2+1]-cen[i*2+1])*(data[j*2+1]-cen[i*2+1])+(data[j*2]-cen[i*2])*(data[j*2]-cen[i*2]));
                if(dist<mind)
                {
                    mind=dist;
                    mini=i;
                }
            }
            ind->at(j)=mini;
            if(mini>=0)
                final[ind->at(j)].push_back(j);
        }*/
        myfile<<ind->at(j)<<", ";
    }
    myfile.close();

 //   cen.clear();
    for (int j=0;j<final.size();j++)
    {
        tmp.clear();
        copy((final[j]).begin(),final[j].end(),back_inserter(tmp));
        stringstream ss;
        ss << j;
        string str = ss.str();
        std::string name1=name+str;//static_cast<ostringstream*>( &(ostringstream() << j) )->str();
        std::ofstream myfile (name1);
        for (int k=0;k<tmp.size();k++)
        {
            myfile<<'>'<< uid[tmp[k]]<<"\n";
            myfile<<  ndata[tmp[k]]<<"\n";
        }
        myfile.close();
    }
    final.clear();
    tmp.clear();
    return mm;
}

std::ostream& operator<<( std::ostream& o, DBSCAN& d )
{
    o << "[ ";
    for ( const auto& l : d.get_labels() ) {
        o << " " << l;
    }
    o << " ] " << std::endl;

    return o;
}
}