#include "urrp.h"
#include <boost/math/special_functions/digamma.hpp>
using namespace std;

inline double square(double x)
{
  return x * x;
}

inline double digamma(double x)
{
  if(x==0.0)
  {
    return 0.0;
  } else {
    return boost::math::digamma(x);
  }
}

void URRP::init_model()
{
  for(vector<rating*>::iterator it=trainratings.begin(); it != trainratings.end(); it++)
  {
    int user = (*it)->user;
    int item = (*it)->item;
    int rating = (*it)->value;
    vector< pair<int,int> >* words = &((*it)->words);

    //init attitude and topic
    int attitude = rand() % K;
    (*it)->attitude = attitude;
    (*muk)[user][attitude]++;
    (*ckvs)[attitude][item][rating]++;
    (*mu)[user]++;
    (*ckv)[attitude][item]++;

    int topic = rand() % K;
    for(vector< pair<int,int> >::iterator it2=words->begin(); it2 != words->end(); it2++)
    {
      (*it2).second = topic;
      (*nuk)[user][topic]++;
      (*nkw)[topic][(*it2).first]++;
      (*nu)[user]++;
      (*nk)[topic]++;
      topic = rand() % K;
    }
  }
}

void URRP::sample_attitudes(bool with_topic)
{
  double* p = new double[K];
  for(vector<rating*>::iterator it=trainratings.begin(); it != trainratings.end(); it++)
  {
    int user = (*it)->user;
    int item = (*it)->item;
    int rating = (*it)->value;

    //delete related attitude and topic for this rating;
    int current_attitude = (*it)->attitude;
    (*muk)[user][current_attitude]--;
    (*ckvs)[current_attitude][item][rating]--;
    (*mu)[user]--;
    (*ckv)[current_attitude][item]--;

    int k=0;
    //#pragma omp parallel for
    for(k=0; k<K; k++)
    {
      if(with_topic)
        p[k] = ((*nuk)[user][k] + (*muk)[user][k] + alpha[k]) / ((*nu)[user] + (*mu)[user] + sum_alpha) * (((*ckvs)[k][item][rating] + lambda[rating]) / ((*ckv)[k][item] + sum_lambda));
      else
        p[k] = ((*muk)[user][k] + alpha[k]) / ((*mu)[user] + sum_alpha) * (((*ckvs)[k][item][rating] + lambda[rating]) / ((*ckv)[k][item] + sum_lambda));
    }

    for(k=1; k<K; k++)
      p[k] += p[k-1];

    double randdouble = (rand() * 1.0 / RAND_MAX) * p[K-1];
    for(k=0; k<K; k++)
    {
      if(randdouble <= p[k])
      {
        break;
      }
    }
    (*it)->attitude = k;
    (*muk)[user][k]++;
    (*ckvs)[k][item][rating]++;
    (*mu)[user]++;
    (*ckv)[k][item]++;
  }
  delete[] p;
}

void URRP::sample_topics(bool with_attitude)
{
  double* p = new double[K];
  for(vector<rating*>::iterator it=trainratings.begin(); it != trainratings.end(); it++)
  {
    int user = (*it)->user;
    vector< pair<int,int> >* words = &((*it)->words);

    for(vector< pair<int,int> >::iterator it2=words->begin(); it2 != words->end(); it2++)
    {
      int w = (*it2).first;
      int current_topic = (*it2).second;
      (*nuk)[user][current_topic]--;
      (*nkw)[current_topic][w]--;
      (*nu)[user]--;
      (*nk)[current_topic]--;
      int k=0;
      for(k=0; k<K; k++)
      {
        if(with_attitude)
          p[k] = (((*nuk)[user][k] + (*muk)[user][k] + alpha[k]) / ((*nu)[user] + (*mu)[user] + sum_alpha) * (((*nkw)[k][w] + beta[w]) / ((*nk)[k] + sum_beta)));
        else
          p[k] = (((*nuk)[user][k] + alpha[k]) / ((*nu)[user] + sum_alpha) * (((*nkw)[k][w] + beta[w]) / ((*nk)[k] + sum_beta)));
      }
      for(k=1; k<K; k++)
        p[k] += p[k-1];

      double randdouble = (rand() * 1.0 / RAND_MAX) * p[K-1];
      for(k=0; k<K; k++)
      {
        if(randdouble <= p[k])
        {
          break;
        }
      }
      (*it2).second = k;
      (*nuk)[user][k]++;
      (*nkw)[k][w]++;
      (*nu)[user]++;
      (*nk)[k]++;
    }
  }
  delete[] p;
}

void URRP::readout_topic_theta(bool with_attitude)
{
  int u,k;
#pragma omp parallel for collapse(2)
  for(u=0; u<nUsers; u++)
    for(k=0; k<K; k++)
      if(with_attitude)
        (*theta)[u][k] = ((*nuk)[u][k] + (*muk)[u][k] + alpha[k]) / ((*nu)[u] + (*mu)[u] + sum_alpha);
      else
        (*theta)[u][k] = ((*nuk)[u][k] + alpha[k]) / ((*nu)[u] + sum_alpha);
}

void URRP::readout_attitude_theta(bool with_topic)
{
  int u,k;
#pragma omp parallel for collapse(2)
  for(u=0; u<nUsers; u++)
    for(k=0; k<K; k++)
      if(with_topic)
        (*theta)[u][k] = ((*nuk)[u][k] + (*muk)[u][k] + alpha[k]) / ((*nu)[u] + (*mu)[u] + sum_alpha);
      else
        (*theta)[u][k] = ((*muk)[u][k] + alpha[k]) / ((*mu)[u] + sum_alpha);
}

void URRP::readout_phi()
{
  int k,w;
#pragma omp parallel for collapse(2)
  for(k=0; k<K; k++)
    for(w=0; w<nWords; w++)
      (*phi)[k][w] = ((*nkw)[k][w] + beta[w]) / ((*nk)[k] + sum_beta);
}

void URRP::readout_xi()
{
  int v,k,s;
#pragma omp parallel for collapse(3)
  for(k=0; k<K; k++)
    for(v=0; v<nItems; v++)
      for(s=0; s<S; s++)
        (*xi)[k][v][s] = ((*ckvs)[k][v][s] + lambda[s]) / ((*ckv)[k][v] + sum_lambda);
}

void URRP::update_alpha_by_topic()
{
  int k,u;
  double ak, numerator, denominator;
  //update alpha
  for (k = 0; k < K; k++) {
    ak = alpha[k];
    numerator = 0, denominator = 0;
    #pragma omp parallel for reduction (+:numerator,denominator)
    for (u = 0; u < nUsers; u++) {
      numerator += digamma((*nuk)[u][k] + ak) - digamma(ak);
      denominator += digamma((*nu)[u] + sum_alpha) - digamma(sum_alpha);
      //numerator += digamma((*nuk)[u][k] + (*muk)[u][k] + ak) - digamma(ak);
      //denominator += digamma(get_nu(u) + get_mu(u) + sum_alpha) - digamma(sum_alpha);
    }
    if (numerator != 0)
      alpha[k] = ak * (numerator / denominator);
  }
  sum_alpha = 0;
  for (k = 0; k < K; k++)
  {
    sum_alpha += alpha[k];
  }
}

void URRP::update_alpha(bool with_topic)
{
  int k,u;
  double ak, numerator, denominator;
  //update alpha
  for (k = 0; k < K; k++) {
    ak = alpha[k];
    numerator = 0, denominator = 0;
    #pragma omp parallel for reduction (+:numerator,denominator)
    for (u = 0; u < nUsers; u++) {
      if (with_topic)
      {
        numerator += digamma((*nuk)[u][k] + (*muk)[u][k] + ak) - digamma(ak);
        denominator += digamma((*nu)[u] + (*mu)[u] + sum_alpha) - digamma(sum_alpha);
      }
      else
      {
        numerator += digamma((*muk)[u][k] + ak) - digamma(ak);
        denominator += digamma((*mu)[u] + sum_alpha) - digamma(sum_alpha);
      }
    }
    if (numerator != 0)
      alpha[k] = ak * (numerator / denominator);
  }
  sum_alpha = 0;
  for (k = 0; k < K; k++)
  {
    sum_alpha += alpha[k];
  }
}

void URRP::update_beta()
{
  int k,w;
  double betaw, numerator, denominator;
  //update beta
  for (w = 0; w < nWords; w++) {
    betaw = beta[w];
    numerator = 0, denominator = 0;
    //#pragma omp parallel for reduction (+:numerator,denominator)
    for (k = 0; k < K; k++) {
      numerator += digamma((*nkw)[k][w] + betaw) - digamma(betaw);
      denominator += digamma((*nk)[k] + sum_beta) - digamma(sum_beta);
    }
    if (numerator != 0)
      beta[w] = betaw * (numerator / denominator);
  }
  double tmp_sum_beta = 0;
  //#pragma omp parallel for reduction (+:tmp_sum_beta)
  for (w = 0; w < nWords; w++)
  {
    tmp_sum_beta += beta[w];
  }
  sum_beta = tmp_sum_beta;

}

void URRP::update_lambda()
{
  int v,k,s;
  double lambdas, numerator, denominator;
  //update lambda
  for (s = 0; s < S; s++) {
    lambdas = lambda[s];
    numerator = 0, denominator = 0;
    #pragma omp parallel for collapse(2) reduction (+:numerator,denominator)
    for(k=0; k<K; k++)
      for(v=0; v<nItems; v++)
      {
        numerator += digamma((*ckvs)[k][v][s] + lambdas) - digamma(lambdas);
        denominator += digamma((*ckv)[k][v] + sum_lambda) - digamma(sum_lambda);
      }
    if (numerator != 0)
      lambda[s] = lambdas * (numerator / denominator);
  }
  sum_lambda = 0;
  for (s = 0; s < S; s++)
  {
    sum_lambda += lambda[s];
  }
}

void URRP::evaluate(int iter)
{
  double train_err = 0.0;
  double validate_err = 0.0;
  double test_err = 0.0;
  double test_ste = 0.0;
  size_t i;

  #pragma omp parallel for reduction (+:validate_err)
  for(i=0; i<trainratings.size(); i++)
  {
    train_err += square(predict_with_expect(trainratings[i]) - trainratings[i]->value - 1);
  }

  #pragma omp parallel for reduction (+:validate_err)
  for(i=0; i<validratings.size(); i++)
  {
    validate_err += square(predict_with_expect(validratings[i]) - validratings[i]->value - 1);
  }

  #pragma omp parallel for reduction (+:test_err,test_ste)
  for(i=0; i<testratings.size(); i++)
  {
    double err = square(predict_with_expect(testratings[i]) - testratings[i]->value - 1);
    test_err += err;
    test_ste += err*err;
  }
  train_err /= trainratings.size();
  validate_err /= validratings.size();
  test_err /= testratings.size();
  test_ste /= testratings.size();
  test_ste = sqrt((test_ste-test_err*test_err)/testratings.size());

  if(test_err < current_best)
  {
    current_best = test_err;
    current_best_ste = test_ste;
  }

  double delta = validate_err - prev_mse;
  printf("\nRecommend: Iter %d, Train MSE: %.4lf, Validation MSE: %.4lf, Test MSE: %.4lf (%.2lf), validate_mse_delta: %.4lf\n", iter, train_err, validate_err, test_err, test_ste, delta);
  printf("\nCurrent best MSE:\t%.4lf (%.2lf)\n", current_best, current_best_ste);
  prev_mse = validate_err;
  fflush(stdout);
}

/// Train a model
void URRP::train()
{
  init_model();
  readout_attitude_theta(false);
  readout_xi();
  evaluate(0);
  // learn topic distribution by lda
  for(int i=1; i<=burn_in; i++)
  {
    sample_topics(false);
    if (i % 10 == 0)
    {
      printf("LDA stage: iter %d\n", i);
      fflush(stdout);
    }
  }
  readout_topic_theta(false);
  readout_phi();
  topic_words();
  for (int iter = 0; iter <= max_iter; iter++) {
    // sample topic and attitude for all words and ratings
    sample_attitudes(false);
    sample_topics(false);
    // update hyper-parameters
    update_alpha(true);
    update_beta();
    update_lambda();
    // get statistics after burn-in
    if (iter % sample_lag == 0)
    {
      readout_attitude_theta(false);
      readout_phi();
      readout_xi();
      evaluate(iter);
    }
  }
  topic_words();
}

bool word_prob_com(pair<int, double> p1, pair<int, double> p2)
{
  return p1.second > p2.second;
}

void URRP::topic_words()
{
  map<int, string> * id2word = &(corp->id2word);
  for(int k=0; k<K; k++)
  {
    vector< pair<int, double> > topic_words;
    for(int w=0; w<nWords; w++)
    {
      topic_words.push_back(make_pair(w, (*phi)[k][w]));
    }
    sort(topic_words.begin(), topic_words.end(), word_prob_com);
    printf("\nTopic %d:", k+1);
    for(int i=0; i<10; i++)
    {
      printf(" %s(%.4lf)", (*id2word)[topic_words[i].first].c_str(), (*phi)[k][topic_words[i].first]);
    }
  }
  printf("\n");
  fflush(stdout);
}

// Predict a particular rating given the current parameter values
double URRP::predict_with_expect(rating* vi)
{
  int user = vi->user;
  int item = vi->item;
  double pred = 0.0;
  double ps = 0.0;
  for(int s=0; s<S; s++)
  {
    ps = 0.0;
    for(int k=0; k<K; k++)
    {
      ps += (*theta)[user][k] * (*xi)[k][item][s];
    }
    pred += ps * (s+1);
  }
  return pred;
}

// Predict a particular rating given the current parameter values
double URRP::predict_with_most_prob(rating* vi)
{
  int user = vi->user;
  int item = vi->item;
  double pred = 0.0;
  double most_prob = 0.0;
  double ps = 0.0;
  for(int s=0; s<S; s++)
  {
    ps = 0.0;
    for(int k=0; k<K; k++)
    {
      ps += (*theta)[user][k] * (*xi)[k][item][s];
    }
    if(ps > most_prob)
    {
      pred = s+1;
      most_prob = ps;
    }
  }
  return pred;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE PRESOLVE

#include <boost/test/unit_test.hpp>

#include "api/umo.hpp"

#include <vector>

using namespace umo;
using namespace std;

BOOST_AUTO_TEST_CASE(Model1) {
    Model model;
    FloatExpression fdec1 = model.floatVar(-10.0, 10.0);
    FloatExpression fdec2 = model.floatVar(0.0, 10.0);
    FloatExpression fdec3 = model.floatVar(0.0, 10.0);
    maximize(fdec1 + 3.0 * fdec2 + 2 * fdec3);
    constraint(fdec1 + fdec2 <= 4.0 && fdec1 - fdec3 >= fdec2 - 1);
    model.check();
}

BOOST_AUTO_TEST_CASE(Model2) {
    Model model;
    FloatExpression fdec1 = model.floatVar(-10.0, 10.0);
    FloatExpression fdec2 = model.floatVar(0.0, 10.0);
    FloatExpression fdec3 = model.floatVar(1.0, 10.0);
    maximize(fdec1 + 3.0 * fdec2 + 2 / fdec3);
    constraint(fdec1 + fdec2 <= 4.0 && fdec1 / fdec3 >= 1.5 * fdec2);
    model.check();
}

BOOST_AUTO_TEST_CASE(Model3) {
    Model model;
    FloatExpression fdec1 = model.floatVar(-10.0, 10.0);
    FloatExpression fdec2 = model.floatVar(0.0, 10.0);
    FloatExpression fdec3 = model.floatVar(1.0, 10.0);
    maximize(fdec1 - 3.0 * fdec2 + 2 / exp(fdec3));
    constraint(fdec1 + fdec2 <= 4.0 || fdec1 / fdec3 >= 1.5 * fdec2);
    constraint(cos(fdec1) >= -1.2);
    constraint(sin(fdec2) <= 2.0);
    model.check();
}

BOOST_AUTO_TEST_CASE(Knapsack1) {
    Model model;
    FloatExpression weight = model.constant(0.0);
    FloatExpression value = model.constant(0.0);
    vector<BoolExpression> expressions;
    for (int i = 0; i < 100; ++i) {
        BoolExpression expr = model.boolVar();
        weight += i * expr;
        value += i * i * expr;
    }
    maximize(value);
    constraint(weight <= 1000.0);
    model.check();
}


/** @file gridder_benchmark.cpp
 *  @brief Test Gridder module performance
 */

#include <armadillo>
#include <benchmark/benchmark.h>
#include <load_data.h>
#include <load_json_config.h>
#include <stp.h>

std::string data_path(_PIPELINE_DATAPATH);
std::string input_npz("simdata_nstep10.npz");

void load_data(arma::mat& uv_in_pixels, arma::cx_mat& residual_vis, arma::mat& snr_weights, int image_size, double cell_size)
{
    //Load simulated data from input_npz
    arma::mat input_uvw = load_npy_double_array<double>(data_path + input_npz, "uvw_lambda");
    arma::cx_mat input_vis = load_npy_complex_array<double>(data_path + input_npz, "vis");
    snr_weights = load_npy_double_array<double>(data_path + input_npz, "snr_weights");
    arma::mat skymodel = load_npy_double_array<double>(data_path + input_npz, "skymodel");

    // Generate model visibilities from the skymodel and UVW-baselines
    arma::cx_mat input_model = stp::generate_visibilities_from_local_skymodel(skymodel, input_uvw);

    // Subtract model-generated visibilities from incoming data
    residual_vis = input_vis - input_model;

    // Size of a UV-grid pixel, in multiples of wavelength (lambda):
    double grid_pixel_width_lambda = (1.0 / (arc_sec_to_rad(cell_size) * double(image_size)));
    uv_in_pixels = (input_uvw / grid_pixel_width_lambda);
    // Remove W column
    uv_in_pixels.shed_col(2);
}

static void gridder_exact_benchmark(benchmark::State& state)
{
    int image_size = pow(2, state.range(0));
    double cell_size = 0.5;
    bool kernel_exact = true;
    int oversampling = 1;
    bool shift_uv = true;
    bool halfplane_gridding = true;

    arma::mat uv_in_pixels;
    arma::cx_mat residual_vis;
    arma::mat snr_weights;
    load_data(uv_in_pixels, residual_vis, snr_weights, image_size, cell_size);

    stp::PSWF kernel_func(state.range(1));

    for (auto _ : state) {
        benchmark::DoNotOptimize(stp::convolve_to_grid<true>(kernel_func, state.range(1), image_size, uv_in_pixels, residual_vis, snr_weights, kernel_exact,
            oversampling, shift_uv, halfplane_gridding));
    }
}

static void gridder_oversampling_benchmark(benchmark::State& state)
{
    int image_size = state.range(0);
    int kernel_support = state.range(1);
    double cell_size = 0.5;
    bool kernel_exact = false;
    int oversampling = 8;
    bool shift_uv = true;
    bool halfplane_gridding = true;

    arma::mat uv_in_pixels;
    arma::cx_mat residual_vis;
    arma::mat snr_weights;
    load_data(uv_in_pixels, residual_vis, snr_weights, image_size, cell_size);

    stp::PSWF kernel_func(kernel_support);

    for (auto _ : state) {
        benchmark::DoNotOptimize(stp::convolve_to_grid<true>(kernel_func, kernel_support, image_size, uv_in_pixels, residual_vis, snr_weights, kernel_exact,
            oversampling, shift_uv, halfplane_gridding));
    }
}

BENCHMARK(gridder_oversampling_benchmark)
    ->RangeMultiplier(2)
    ->Ranges({ { 1 << 10, 1 << 16 }, { 3, 3 } })
    ->Ranges({ { 1 << 10, 1 << 16 }, { 5, 5 } })
    ->Ranges({ { 1 << 10, 1 << 16 }, { 7, 7 } })
    ->Unit(benchmark::kMillisecond);

BENCHMARK(gridder_exact_benchmark)
    ->RangeMultiplier(2)
    ->Ranges({ { 1 << 10, 1 << 16 }, { 3, 3 } })
    ->Ranges({ { 1 << 10, 1 << 16 }, { 5, 5 } })
    ->Ranges({ { 1 << 10, 1 << 16 }, { 7, 7 } })
    ->Unit(benchmark::kMillisecond);

BENCHMARK_MAIN();


#include "policy.hpp"
#include "helpers.hpp"
#include <ctime>
#include <sys/time.h>
#include <boost/math/distributions.hpp>
#include <stdlib.h>

template <class Policy>
double timePolicy(Policy& pol, int arms, int horizon, bool gaussian, RandUnif& urandom, RandNorm& nrandom)
{

  int trials = 1;
  static double mean_rewards[] = {0.6, 0.1, 0.7, 0.45, 0.43, 0.32, 0.8, 0.93, 0.4, 0.5};
  clock_t timing = clock();
  for (int tr= 1; tr <= trials; ++tr)
  {
    Table rewards(arms, vector<double>(horizon, 0));
    for (int a = 0; a < arms; ++a)
    {
      double mu = mean_rewards[a];
      for (int t = 0; t < horizon; ++t)
      {
        rewards[a][t] = gaussian ? (mu + nrandom()) : (urandom() < mu ? 1 : 0);
      }
    }
    for (int t = 1; t <= horizon; ++t)
    {
      int arm = pol.choosearm(t);

      pol.totalsampled[arm] += 1;
      double outcome = rewards[arm][pol.sample_index[arm]];
      pol.totalrewards[arm] += outcome;
      pol.sample_index[arm]++;
      pol.totalscore += outcome;
    }
  }
  timing = clock() - timing;
  return ((double)timing)/CLOCKS_PER_SEC;
}

int main(int argc, char** argv)
{
  timeval tv;
  gettimeofday(&tv,NULL);

  boost::mt19937 seed(tv.tv_sec);
  boost::uniform_real<> dist(0.0,1.0);
  boost::normal_distribution<> normdist(0.0,1.0);
  boost::uniform_int<> idist(1,1000000);
  RandomSampleHelper bh(tv.tv_sec);
  RandUnif urandom(seed,dist);
  RandNorm nrandom(seed,normdist);
  int horizon = 1000;
  int arms = 10;

  vector<double> uniforms(horizon, 0);
  IDSPolicy idsBer(horizon, uniforms,  arms, false, 1000);
  IDSPolicy idsGauss(horizon, uniforms, arms, true, 1000);
  InterestingPolicy nitpBer(horizon, arms, 1, 0, false);
  InterestingPolicy nitpBer3(horizon, arms, 1, 0, false, 3);
  InterestingPolicy nitpGauss(horizon, arms, 1, 0, true);

  ThompsonPolicy tompBer(bh, horizon, arms, false);
  ThompsonPolicy tompGauss(bh, horizon, arms, true);
  BayesUCBPolicy bucbBer(horizon, arms, false);
  BayesUCBPolicy bucbGauss(horizon, arms, true);

  cout << "TOM:" << timePolicy(tompBer, arms, horizon, false, urandom, nrandom) << endl;
  cout << "BUCB:" << timePolicy(bucbBer, arms, horizon, false, urandom, nrandom) << endl;
  cout << "IDS:" << timePolicy(idsBer, arms, horizon, false, urandom, nrandom) << endl;
  cout << "OURS-1:" << timePolicy(nitpBer, arms, horizon, false, urandom, nrandom) << endl;
  cout << "OURS-3:" <<  timePolicy(nitpBer3, arms, horizon, false, urandom, nrandom) << endl;

  cout << endl << "Gaussian!" << endl;
  cout << "TOM:" << timePolicy(tompGauss, arms, horizon, true, urandom, nrandom) << endl;
  cout << "BUCB:" << timePolicy(bucbGauss, arms, horizon, true, urandom, nrandom) << endl;
  cout << "IDS:" << timePolicy(idsGauss, arms, horizon, true, urandom, nrandom) << endl;
  cout << "OURS-1:" << timePolicy(nitpGauss, arms, horizon, true, urandom, nrandom) << endl;
}


#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
//include the bie header files
#include "material.hh"
#include "precomputed_kernel.hh"
#include "bimat_interface.hh"
#include "infinite_boundary.hh"
//include the fem header files
#include "mesh_Generated_multi_faults.hpp"
#include "bcdof.hpp"
#include "bcdof_ptr.hpp"
#include "cal_ke.hpp"
#include "cal_fe_global_const_ke.hpp"
#include "mapglobal.hpp"
#include "Slip_Weakening_reg.hpp"
#include "Slip_Weakening.hpp"
#include "cal_slip_sliprate.hpp"
#include "time_advance.hpp"
#include "BIE_correct.hpp"
#include "igl/list_to_matrix.h"
#include <omp.h>
using namespace Eigen;
using namespace std;

typedef Eigen::Matrix<int, -1, -1,RowMajor> MatrixXi_rm;

void read_matrix(std::string fileName, Eigen::MatrixXd &outputMat);

double time_fem=0;
double time_bie;
int main() {
     time_fem = 0.0;
    // Domain Size
    double x_min = -30e3;
    double x_max = 30e3;
    double y_min = -0.5e3;
    double y_max = 0.5e3;
    int dim = 2.0;
    double dx = 3.125;
    double dy = 3.125;
    int nx_el = (x_max-x_min)/dx;
    int ny = (y_max-y_min)/dy;
    MatrixXd Node = MatrixXd::Zero((nx_el+1)*(ny+1),2);
    MatrixXi_rm Element(nx_el*ny,4); Element.setZero();
    ArrayXi BIE_top_surf_nodes = ArrayXi::Zero((nx_el+1),1);
    ArrayXi BIE_bot_surf_nodes = ArrayXi::Zero((nx_el+1),1);
    int num_faults = 5;
    // Mesh
    // Position of the fault : y_pos, x_left, x_right
    MatrixXd fault_pos(num_faults,3);
    fault_pos << 0.0,  -30.0e3 ,30.0e3,
                 0.3e3, 3.0e3, 30.0e3,
                 0.3e3, -30.0e3, -3.0e3,
                 -0.3e3, 3.0e3, 30.0e3,
                 -0.3e3, -30.0e3, -3.0e3;

//    MatrixXd fault_pos;
//    read_matrix("fault_pos.txt", fault_pos);
//    int num_faults = fault_pos.rows();
    cout<<"test_mat="<<"\n"<<fault_pos<<"\n"<<"rows="<<fault_pos.rows()<<"\n";
    std::vector<std::vector<int>> fault_nodes(num_faults*2);
    mesh_Generated_multi_faults(x_min,x_max,y_min,y_max,dx,dy,nx_el,ny,Node, Element,BIE_top_surf_nodes, BIE_bot_surf_nodes,fault_pos, fault_nodes);

    int n_nodes = Node.rows();
    int n_el = Element.rows();
    int Ndofn = 2;
    int Nnel = Element.cols();
    // Material
    double density = 2670.0;
    double v_s =3.464e3;
    double v_p = 6.0e3;
    double G= pow(v_s,2)*density;
    double Lambda = pow(v_p,2)*density-2.0*G;
    double E  = G*(3.0*Lambda+2.0*G)/(Lambda+G);
    double nu = Lambda/(2.0*(Lambda+G));
    // Time
    double alpha = 0.4;
    double dt = alpha*dx/v_p;
    dt = 0.0002;
    // Reyleigh Damping
    double beta =0.1;
    double q = beta*dt;
    double time_run = 6.0;
    int numt = time_run/dt;
    //numt = 1;
    //numt = 200;
    //numt = 3;
    //numt =4;
    VectorXd time = dt*VectorXd::LinSpaced(numt,1,numt);
    // Slip weakening friction parameters
    double Dc = 0.2;
    double mu_d= 0.2;
    double mu_s = 0.6;
    // Intialization
    // disp velocity current and next time step (new)
    VectorXd u_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd u_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd a_n = VectorXd::Zero(n_nodes*Ndofn,1);
    // slip and slip-rate
    std::vector<Eigen::Array<double, -1, 1> > delt_u_n(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > delt_v_n(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > T_c(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > T_0(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > tau_s(num_faults);

    // Vector containing number of elements on each faults (nx)
    std::vector<int> nx_faults(num_faults);
    for (int i=0;i<num_faults;i++)
    {
        nx_faults[i] =fault_nodes[2*i].size();
    }

    for (int i=0;i<num_faults;i++)
    {
        delt_u_n[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        delt_v_n[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        T_c[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        T_0[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
       // tau_s[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        tau_s[i] = 0.6*ArrayXd::Ones((nx_faults[i]),1)*50.0e6;
    }
    VectorXd F_ext_global = VectorXd::Zero(n_nodes*Ndofn,1);
    // Setting intial stress on the fault
    for (int j=0; j<num_faults;j++)
    {
        for (int i=0; i<nx_faults[j]; i++)
        {
            T_0[j](2*i+1) = -50.0e6;
            T_c[j](2*i+1) = T_0[j](2*i+1);
        }
        if (j==0)
        {
            VectorXd x = VectorXd::LinSpaced(nx_faults[j], fault_pos(j,1), fault_pos(j,2));
            for (int i=0 ; i<nx_faults[j]; i++)
            {
                if ((x(i)<=(fault_pos(j,1)+fault_pos(j,2))/2+0.2e3)&&(x(i)>=(fault_pos(j,1)+fault_pos(j,2))/2-0.2e3))
                {
                    T_0[j](2*i) = 31.0e6;
                    T_c[j](2*i) = T_0[j](2*i);
                }
                else
                {
                    T_0[j](2*i) = 20.0e6;
                    T_c[j](2*i) = T_0[j](2*i);
                }
            }
        }
        else
        {
            for (int i=0 ; i<nx_faults[j]; i++)
            {
                T_0[j](2*i) = 20.0e6;
            }
        }
    }
    // Get the index degree of freedome for each element
    VectorXi index_el = VectorXi::Zero(Ndofn*Nnel,1);
    MatrixXi index_store = MatrixXi::Zero(Nnel*Ndofn,n_el);
    for (int i=0;i<n_el;i++)
    {
        bcdof(Element.row(i),dim,index_el);
        index_store.col(i) = index_el;
    }
    VectorXi BIE_top_surf_index = VectorXi::Zero(Ndofn*(BIE_top_surf_nodes.size()),1);
    VectorXi BIE_bot_surf_index = VectorXi::Zero(Ndofn*(BIE_bot_surf_nodes.size()),1);

    std::vector<Eigen::ArrayXi> Fault_surf_index(num_faults*2);
    for (int i=0; i <num_faults*2; i++)
    {
        Fault_surf_index[i] = ArrayXi::Zero(Ndofn*(fault_nodes[i].size()),1);
    }

    // Getting the faults DOF index
    for (int i=0; i<num_faults*2;i++)
    {
        bcdof_ptr(fault_nodes[i], dim, Fault_surf_index[i].data());
    }
    bcdof(BIE_top_surf_nodes,dim,BIE_top_surf_index);
    bcdof(BIE_bot_surf_nodes,dim,BIE_bot_surf_index);
    // Calculating the Global Mass Vector (lumped mass)
    // Element mass
    double M=density*dx*dy*1.0;
    VectorXd M_el_vec = M/4*VectorXd::Ones(Nnel*Ndofn,1);
    VectorXd M_global_vec=VectorXd::Zero(n_nodes*Ndofn,1);
    for (int i=0 ; i<n_el;i++)
    {
        index_el = index_store.col(i);
        mapglobal(index_el,M_global_vec,M_el_vec);
    }
    // Element matrix
    MatrixXd ke = MatrixXd::Zero(8,8);
    MatrixXd coord = MatrixXd::Zero(4,2);
    VectorXi Element_0= Element.row(0);
    coord.row(0) = Node.row(Element_0(0));
    coord.row(1) = Node.row(Element_0(1));
    coord.row(2) = Node.row(Element_0(2));
    coord.row(3) = Node.row(Element_0(3));
    cal_ke (coord,E,nu,ke);
    // BIE part initiation
    // Setting up the material property for the BIE code
    Material BIE_top_mat = Material(E,nu,density);
    Material BIE_bot_mat = Material(E,nu,density);
    double length = x_max-x_min;
    // infinte bc BIE call infinite_boundary.cc
    PrecomputedKernel h11("kernels/nu_.25_h11.dat");
    PrecomputedKernel h12("kernels/nu_.25_k12.dat");
    PrecomputedKernel h22("kernels/nu_.25_h22.dat");
    InfiniteBoundary BIE_inf_top(length,nx_el+1,1.0,&BIE_top_mat,&h11,&h12,&h22);
    InfiniteBoundary BIE_inf_bot(length,nx_el+1,-1.0,&BIE_bot_mat,&h11,&h12,&h22);
    // BIE setting time step
    BIE_inf_top.setTimeStep(dt);
    BIE_inf_bot.setTimeStep(dt);
    // BIE initialization
    BIE_inf_top.init();
    BIE_inf_bot.init();
    printf("ready to start\n");
    // Output
    ofstream file;
    file.open("results/num_nodes_fault.bin",ios::binary);
    file.write((char*)(nx_faults.data()),nx_faults.size()*sizeof(int));
    file.close();

    for (int i=0;i<num_faults;i++)
    {
        std::string slip = "results/slip_"+std::to_string(i)+".bin";
        file.open(slip);
        file.close();
        std::string slip_rate = "results/slip_rate_"+std::to_string(i)+".bin";
        file.open(slip_rate);
        file.close();
        std::string shear = "results/shear_"+std::to_string(i)+".bin";
        file.open(shear);
        file.close();
    }
    double start = omp_get_wtime();

    // Main time loop
    for (int j=0;j<numt;j++)
    {
        // Compute the global internal force
        VectorXd fe_global= VectorXd::Zero(n_nodes*Ndofn,1);
        cal_fe_global_const_ke(n_nodes, n_el, index_store, q, u_n, v_n, Ndofn, ke, fe_global);
        // Friction subroutine
        VectorXd F_total = F_ext_global-fe_global;
        for (int i=0 ; i<num_faults; i++)
        {
            VectorXd F_fault = VectorXd::Zero(Ndofn*(nx_faults[i]),1);
            Slip_Weakening(M_global_vec, Fault_surf_index[2*i], Fault_surf_index[2*i+1], fe_global, dt, dx, dy, nx_faults[i]-1, delt_v_n[i], delt_u_n[i], T_0[i], tau_s[i], mu_s, mu_d, Dc, Ndofn, M, F_fault, T_c[i]);
            mapglobal(Fault_surf_index[2*i],F_total,-F_fault);
            mapglobal(Fault_surf_index[2*i+1],F_total,F_fault);
        }

        // Central Difference Time integration
        time_advance(u_n, v_n, F_total, M_global_vec, dt);
        // Get the slip and slip rate

        for (int i=0; i<num_faults; i++)
        {
            cal_slip_slip_rate(u_n, v_n,  Fault_surf_index[2*i], Fault_surf_index[2*i+1] , Ndofn, nx_faults[i]-1, delt_u_n[i], delt_v_n[i]);
        }
        // Correct the BIE surf nodes solutions from FEM with the BIE solution
        BIE_correct(BIE_top_surf_index, BIE_bot_surf_index, fe_global, Ndofn, nx_el, dx, BIE_inf_top, BIE_inf_bot, u_n, v_n);

//        VectorXd delt_u_n_out = delt_u_n[0];
//        VectorXd delt_v_n_out = delt_v_n[0];
//        VectorXd T_c_out = T_c[0];
//        file.open("results/slip_main.bin",ios::binary | ios::app);
//        file.write((char*)(delt_u_n[0].data()),delt_u_n[0].size()*sizeof(double));
//        file.close();
//        file.open("results/slip_rate_main.bin",ios::binary | ios::app);
//        file.write((char*)(delt_v_n[0].data()),delt_v_n[0].size()*sizeof(double));
//        file.close();
//        file.open("results/shear_main.bin",ios::binary | ios::app);
//        file.write((char*)(T_c[0].data()),T_c[0].size()*sizeof(double));
//        file.close();
        //
        if (j%4==3)
        {
        for (int i=0;i<num_faults;i++)
        {
            std::string slip = "results/slip_"+std::to_string(i)+".bin";
            file.open(slip,ios::binary | ios::app);
            file.write((char*)(delt_u_n[i].data()),delt_u_n[i].size()*sizeof(double));
            file.close();

            std::string slip_rate = "results/slip_rate_"+std::to_string(i)+".bin";
            file.open(slip_rate,ios::binary | ios::app);
            file.write((char*)(delt_v_n[i].data()),delt_v_n[i].size()*sizeof(double));
            file.close();

            std::string shear = "results/shear_"+std::to_string(i)+".bin";
            file.open(shear,ios::binary | ios::app);
            file.write((char*)(T_c[i].data()),T_c[i].size()*sizeof(double));
            file.close();
        }
        }
        printf("Simulation time = %f\n",time(j));
      //  double end_t = omp_get_wtime();
       // std::cout<<"time_cpu_t="<<end_t-start<<std::endl;
    //   std::cout<<"time_fem="<< time_fem<<std::endl;
    //   std::cout<<"time_bie="<< time_bie<<std::endl;
    }
    double end = omp_get_wtime();
    std::cout<<"time_cpu="<<end-start<<std::endl;
    return 0;
}

void read_matrix(std::string fileName, Eigen::MatrixXd &outputMat) {
    fstream cin;
    cin.open(fileName.c_str());
    if (cin.fail())
    {
        std::cerr << "Failed to open file: " << fileName << std::endl;
        std::cin.get(); }
    string s;
    vector <vector <double> > matrix;
    while (getline(cin, s)) {
        stringstream input(s);
        double temp;
        vector <double> currentLine;
        while (input >> temp)
            currentLine.push_back(temp);
        matrix.push_back(currentLine);
    }
    if (!igl::list_to_matrix(matrix, outputMat))
    { std::cerr << "list tom matrix error" << std::endl; std::cin.get();
        //return false;
    }
}


//
// Created by kerail on 11.07.16.
//

#include "geom/SO3.hpp"

#include <boost/function.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include <iostream>

typedef boost::multiprecision::cpp_dec_float_100 float_100;
typedef Eigen::Matrix<float_100, 3, 1> Vector3f100;
typedef Eigen::Matrix<float_100, 3, 3> Matrix3f100;
typedef Eigen::Quaternion<float_100> Quaternionf100;
typedef SO3<float_100> SO3f100;

inline void so3_test_matrix(const SO3d &s, const Eigen::Vector3d &v) {
  Eigen::Vector3d v1 = s.getMatrix() * v;
}

inline void so3_test_quaternion(const SO3d &s, const Eigen::Vector3d &v) {
  Eigen::Vector3d v1 = s.getQuaternion().toRotationMatrix() * v;
}

inline void so3_test_rotation(const SO3d &s, const Eigen::Vector3d &v) {
  Eigen::Vector3d v1 = s.rotateVector(v);
}

double test(size_t testCount, const boost::function<void(const SO3d &, const Eigen::Vector3d &)> &f) {
  clock_t startClock = clock();
  for (size_t i = 0; i < testCount; ++i) {
    SO3d so3((Eigen::Vector3d) Eigen::Vector3d::Random());
    Eigen::Vector3d testVec = Eigen::Vector3d::Random();

    so3_test_matrix(so3, testVec);
  }

  clock_t endClock = clock();
  return (endClock - startClock) / double(CLOCKS_PER_SEC);
}

int main() {
  const size_t testCount = 1e8;

  std::cout << "SO3 matrix time: " << test(testCount, &so3_test_matrix) << std::endl;
  std::cout << "SO3 quaternion matrix time: " << test(testCount, &so3_test_quaternion) << std::endl;
  std::cout << "SO3 direct rotation time: " << test(testCount, &so3_test_rotation) << std::endl;
}


#include <iostream>
#include <queue>
#include <cmath>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <ctime>
#include <climits>
#include <Eigen/Dense>
#include "laneDetector.hpp"

const int threshold=45;

class point{
    public: int x,y;
    point(int x_,int y_){
        x=x_;y=y_;
    }

    point(){
        x=0;y=0;
    }
};

class Point_XY_c{
public:
    double x, y, z;
    Point_XY_c() {
        x = y = z = 0;
    }
    Point_XY_c(double x_, double y_, double z_) {
        x = x_;
        y = y_;
        z = z_;
    }
    void operator=(const Point_XY_c& p_) {
        this->x = p_.x;
        this->y = p_.y;
        this->z = p_.z;
    }

    Point_XY_c operator+(const Point_XY_c& right) {
        return Point_XY_c(x + right.x, y + right.y, z + right.z);
    }

    Point_XY_c operator-(const Point_XY_c& right) {
        return Point_XY_c(x - right.x, y -right.y, z - right.z);;
    }
    Point_XY_c operator*(const double& right) {
        return Point_XY_c(x * right, y *right, z * right);
    }
    Point_XY_c operator/(const double& right) {
        return Point_XY_c(x / right, y /right, z / right);
    }
};

struct ind{
    int x; int y;
    struct ind* next;
};

struct ind *front,*rear;

cv::Mat game_binary(cv::Mat img);
void enqueue(int x,int y);
void dequeue();
void qfront(int* x,int *y);
int qempty();
cv::Mat qblobdetect(cv::Mat& bin,int** A,int* mark);
cv::Mat game_binary(cv::Mat img);
cv::Mat link_blobs(cv::Mat qblob,int** A,int mark,point* min_x,point* max_x,point* min_y,point* max_y,int* connected_index);

cv::Mat LaneDetector::SeperateLanes(cv::Mat &image){

    image=game_binary(image);

    int mark=0;

    int**A=new int*[image.rows];

    for(int i=0;i<image.rows;i++)A[i] = new int[image.cols];

    for(int i=0;i<image.rows;i++){
        for(int j=0;j<image.cols;j++)A[i][j]=-1;
    }

    cv::Mat qblob = qblobdetect(image, A, &mark);

    point* min_x = new point[mark+1];
    point* max_x = new point[mark+1];
    point* min_y = new point[mark+1];
    point* max_y = new point[mark+1];

    for(int i=0;i<=mark;i++){
        min_x[i].x=65535;
        max_x[i].x=0;
        min_y[i].y=65535;
        max_y[i].y=0;
    }

    int* connected_index = new int[mark+1];

    for(int i=0;i<=mark;i++) connected_index[i]=i;

    cv::Mat linked_image = link_blobs(qblob,A,mark,min_x,max_x,min_y,max_y,connected_index);

    return linked_image;
}

void enqueue(int x,int y){
    struct ind* n;
    n=(struct ind*)malloc(sizeof(struct ind));
    n->x=x;
    n->y=y;
    n->next=NULL;
    if(front==NULL){front=n;rear=front;}
    else {rear->next=n;rear=n;}
}

void dequeue(){
	struct ind* t;
	t=front;
	front=t->next;
	free(t);
}

void qfront(int* x,int *y){
	if(front!=NULL){
	*x=front->x;
	*y=front->y;}
	return;
}

int qempty(){
	if(front==NULL)return 1;
	return 0;}

    float dist(point p1,point p2){
        return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y));
    }

cv::Mat qblobdetect(cv::Mat& bin,int** A,int* mark){
    int i,j,x,y,wd=bin.cols,ht=bin.rows;

    std::queue<std::pair<int, int> > Q;

    for(i=0;i<ht;i++){
        for(j=0;j<wd;j++){
            if(bin.at<uchar>(i,j)==255){
                if(A[i][j]==-1){
                    *mark=*mark+1;
                    enqueue(i,j);

                    while(qempty()==0){
                        qfront(&x,&y);
                        int k,l;
                        dequeue();
                        for(k=x-1;k<=x+1;k++){
                            for(l=y-1;l<=y+1;l++){
                                if((k<ht)&&(l<wd)&&(k>=0)&&(l>=0)&&(A[k][l]==-1) && bin.at<uchar>(k,l)==255){
                                    enqueue(k,l);
                                    A[k][l]=*mark;}
                            }}
                        A[x][y]=*mark;
                    }
                    //std::cout<<*mark<<" "<<x<<","<<y<<std::endl;
                }
            }
        }}
    //cout<<"A->"<<A[10][10]<<endl;

    cv::Mat qblob(bin.rows,bin.cols,CV_8UC3);
    //IplImage* qblobdetect=cvCreateImage(cvGetSize(bin),IPL_DEPTH_8U,3);
    //int *a;double *c_x,*c_y;

    //a=(int*)malloc((*mark+1)*sizeof(int));
    for(i=0;i<ht;i++){
        for(j=0;j<wd;j++){
            if(A[i][j]==-1)qblob.at<cv::Vec3b>(i,j)[0]=qblob.at<cv::Vec3b>(i,j)[1]=qblob.at<cv::Vec3b>(i,j)[2]=255;
            else{
                qblob.at<cv::Vec3b>(i,j)[0]=255*sin(A[i][j]);
                qblob.at<cv::Vec3b>(i,j)[1]=255*cos(A[i][j]);
                qblob.at<cv::Vec3b>(i,j)[2]=255*(sin(A[i][j])+cos(A[i][j]))/1.5;
            }
        }}

    //centroid(bin,A,*mark,count_x,count_y);
    return qblob;
}

cv::Mat game_binary(cv::Mat img){
    cv::Mat bin(img.rows,img.cols,CV_8UC1,cvScalarAll(0));
    // TODO : img.at<cv::Vec3b>(i, j)[0] = sdfsa
    int i,j;
    for(i=0;i<img.rows;i++){
        for(j=0;j<img.cols;j++){
            if( img.at<cv::Vec3b>(i, j)[0]>=235 || img.at<cv::Vec3b>(i, j)[1] >= 235|| img.at<cv::Vec3b>(i, j)[2] >= 235)
                bin.at<uchar>(i, j) = 255;
            else bin.at<uchar>(i, j) = 0;
        }

    }
    return bin;
}

cv::Mat link_blobs(cv::Mat qblob,int** A,int mark,point* min_x,point* max_x,point* min_y,point* max_y,int* connected_index){
    for(int i=0;i<qblob.rows;i++){
        for(int j=0;j<qblob.cols;j++){ int r=A[i][j];
            if(r!=-1){
                if(i>max_x[r].x){max_x[r].x=i;max_x[r].y=j;}
                if(i<min_x[r].x){min_x[r].x=i;min_x[r].y=j;}
                if(j>max_y[r].y){max_y[r].x=i;max_y[r].y=j;}
                if(j<min_y[r].y){min_y[r].x=i;min_y[r].y=j;}
            }
        }
    }
    for(int i=1;i<=mark;i++){
        for(int j=i+1;j<=mark;j++){
            if(dist(max_x[i],min_x[j])<threshold || dist(min_x[i],max_x[j])<threshold || dist(min_y[i],max_y[j])<threshold ||dist(max_y[i],min_y[j])<threshold){
                connected_index[j]=connected_index[i];
                std::cout<<" if condition running"<<i<<" "<<j<<std::endl;
            }
            else if(dist(max_x[i],max_x[j])<threshold || dist(min_x[i],min_x[j])<threshold || dist(min_y[i],min_y[j])<threshold ||dist(max_y[i],max_y[j])<threshold){
                connected_index[j]=connected_index[i];
                std::cout<<"else if condition running"<<i<<" "<<j<<std::endl;
            }
        }
    }
    for(int i=0;i<=mark;i++)std::cout<<connected_index[i]<<" "<<min_x[i].x<<" "<<max_x[i].x<<" "<<min_y[i].y<<" "<<max_y[i].y<<std::endl;
    cv::Mat linked(qblob.rows,qblob.cols,CV_8UC3);
    for(int i=0;i<qblob.rows;i++){
        for(int j=0;j<qblob.cols;j++){
            if(A[i][j]==-1)qblob.at<cv::Vec3b>(i,j)[0]=qblob.at<cv::Vec3b>(i,j)[1]=qblob.at<cv::Vec3b>(i,j)[2]=255;
            else{
                linked.at<cv::Vec3b>(i,j)[0]=255*sin(connected_index[A[i][j]]);
                linked.at<cv::Vec3b>(i,j)[1]=255*cos(connected_index[A[i][j]]);
                linked.at<cv::Vec3b>(i,j)[2]=255*(sin(connected_index[A[i][j]])+cos(connected_index[A[i][j]]))/1.5;
            }
        }
    }
    return linked;

}


#pragma once

#include <igl/predicates/predicates.h>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <array>

namespace wmtk {

// convert triangle soup to manifold components for TriMesh.
bool separate_to_manifold(
    const std::vector<Eigen::Vector3d>& vertices,
    const std::vector<std::array<size_t, 3>>& faces,
    std::vector<Eigen::Vector3d>& out_v,
    std::vector<std::array<size_t, 3>>& out_f,
    std::vector<size_t>& modified_vertices);

namespace manifold_internal {

using Vertices = Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>;
using Facets = Eigen::Matrix<uint64_t, Eigen::Dynamic, 3, Eigen::RowMajor>;

void resolve_nonmanifoldness(
    Vertices& V,
    Facets& F,
    std::vector<size_t>& modified_vertices);

} // namespace manifold_internal

} // namespace wmtk


#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <iostream>
#include <chrono>
#include <vector>
#include <tuple>
#include <cmath>
#include <thread>

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <err.h>

/**
 * SOCKET FUNCTIONS description (teleop)
 **/
int create_socket(void);
void close_socket(int fd);
void set_ip_port(struct sockaddr_in & addr, const char * ip, int port);
void make_bind(int fd, struct sockaddr_in & addr);
int recv_message(int fd, struct sockaddr_in & dest_addr, void * buf, size_t len);

const float PI = 3.14159265;
const int QUADRANTS = 4;
const std::string IP = "0.0.0.0";
const int PORT = 36677;

namespace gazebo
{
    class Person : public ModelPlugin
    {

    private:

        int state, current_wp;
        int turn_dir, linear_dir;
        bool auto_movement, linear_movement;
        float lv_dt = 0.002; // discrete lineal velocity
        float av_dt = 0.003; // discrete angular velocity

        // variables to use in MoveToWaypoint method
        bool orientation_reached = false;
        bool direction_chosen = false;

        physics::ModelPtr model;
        event::ConnectionPtr updateConnection;

        // waypoints where (px, py, next_waypoint)
        std::vector <std::tuple<float, float, int>> wp;

        // quadrants vector to know the correct direction of turn
        std::vector <std::tuple<float, float>> quadrants;

        // The server will receive velocity commands from client to move the model
        int sockfd;
        struct sockaddr_in addr;
        std::thread server_thread;

    private:

        float GetDistanceEuclidean(float rx, float ry)
        {
            auto pose = this->model->WorldPose();

            return sqrt(pow(pose.Pos().X() - rx, 2) + pow(pose.Pos().Y() - ry, 2));
        }

        float GetDistanceEuclidean(std::tuple <float, float, int> & waypoint)
        {
            int wx = std::get<0>(waypoint);
            int wy = std::get<1>(waypoint);

            return GetDistanceEuclidean(wx, wy);
        }

        int GetNearestWaypoint(std::vector < std::tuple<float, float, int> > & waypoints)
        {
            float current_dist;
            int nearest_index = 0;
            float min_dist = GetDistanceEuclidean(waypoints[0]);

            for (size_t i = 0; i < waypoints.size(); i++) {
                current_dist = GetDistanceEuclidean(waypoints[i]);
                if (current_dist < min_dist) {
                    min_dist = current_dist;
                    nearest_index = i;
                }
            }

            return nearest_index;
        }

        float GetAngle(float rx, float ry)
        {
            // Returns the Angle in Radians

            auto pose = this->model->WorldPose();
            float angle = atan2(abs(rx - pose.Pos().X()), abs(ry - pose.Pos().Y()));

            if (ry > pose.Pos().Y()) {
                angle = PI - angle;
            }
            if (rx < pose.Pos().X()) {
                angle *= -1;
            }
            return angle;
        }

        int GetBestTurnDirection(float desired_yaw, float actual_yaw)
        {
            auto get_quadrant = [](int yaw, std::vector<std::tuple<float, float>> & quadrants) {
                // iterate over all quadrants (4)
                for (std::size_t i = 0; i < QUADRANTS; i++) {
                    if (yaw >= std::get<0>(quadrants[i]) && yaw < std::get<1>(quadrants[i])) {
                        return i;
                    }
                }
            };

            // Search the corresponding quadrants
            int actual_quadrant = get_quadrant(actual_yaw, this->quadrants);
            int desired_quadrant = get_quadrant(desired_yaw, this->quadrants);

            // both angles share the same quadrant
            if (actual_quadrant == desired_quadrant) {
                return (desired_yaw > actual_yaw) ? 1 : -1;
            }

            /**
             * Calculate distance between angles in different quadrants
             * n1 -> quadrants of separation between the yaw angles for distance 1
             * n2 -> quadrants of separation between the yaw angles for distance 2
             **/
            int n1, n2, dist1, dist2;

            n1 = (desired_quadrant > actual_quadrant) ?
                  desired_quadrant - actual_quadrant :
                  QUADRANTS - abs((desired_quadrant - actual_quadrant) % QUADRANTS);
            n2 = QUADRANTS - n1;

            // Calculating Distance 1
            dist1 = std::get<1>(quadrants[actual_quadrant]) - actual_yaw;
            for (int i = 0; i < (n1 - 1); i++) {
                dist1 += PI/2;
            }
            dist1 += desired_yaw - std::get<0>(quadrants[desired_quadrant]);

            // Calculating Distance 2
            dist2 = actual_yaw - std::get<0>(quadrants[actual_quadrant]);
            for (int i = 0; i < (n2 - 1); i++) {
                dist2 += PI/2;
            }
            dist2 += std::get<1>(quadrants[desired_quadrant]) - desired_yaw;

            // Return best direction
            return (dist1 <= dist2) ? 1 : -1;
        }

        bool MoveToWaypoint(std::tuple<float, float, int> & waypoint)
        {
            float rx = std::get<0>(waypoint);
            float ry = std::get<1>(waypoint);
            float angle = GetAngle(rx, ry);
            auto pose = this->model->WorldPose();

            /**
             * Procedure for moving to the next waypoint
             **/
            // First, it will determine the orientation of turn
            if (!direction_chosen) {
                direction_chosen = true;
                turn_dir = GetBestTurnDirection(angle, pose.Rot().Yaw());
            }
            // Second, It will turn until Yaw desired is reached
            if (!orientation_reached) {
                pose.Rot() = ignition::math::Quaterniond(0, 0, pose.Rot().Yaw() + turn_dir*av_dt);
                if (abs(angle - pose.Rot().Yaw()) < 0.005) {
                    orientation_reached = true;
                }
            }
            // Third, it will move to position desired
            else {
                pose.Pos().X() += -lv_dt * (0*cos(pose.Rot().Yaw()) - 1*sin(pose.Rot().Yaw()));
                pose.Pos().Y() += -lv_dt * (0*sin(pose.Rot().Yaw()) + 1*cos(pose.Rot().Yaw()));
            }
            this->model->SetWorldPose(pose);

            if (orientation_reached && GetDistanceEuclidean(rx, ry) < 0.1) {
                // Reset static boolean values;
                orientation_reached = false;
                direction_chosen = false;
                return true;
            }
            return false;
        }

        void ServerThreadLoop(void)
        {
            char msg[3];

            while (true) {
                recv_message(this->sockfd, this->addr, &msg, sizeof msg);

                if (msg[0] == 'U') {
                    auto_movement = false;

                    if (msg[1] == 'V') {
                        linear_movement = true;
                        if (msg[2] == 'F') {
                            linear_dir = 1;
                        }
                        else if (msg[2] == 'B') {
                            linear_dir = -1;
                        }
                    }
                    else if (msg[1] == 'A') {
                        linear_movement = false;
                        if (msg[2] == 'R') {
                            turn_dir = -1;
                        }
                        else if (msg[2] == 'L') {
                            turn_dir = 1;
                        }
                    }
                    else if (msg[1] == 'S') {
                        linear_dir = 0;
                        turn_dir = 0;
                    }
                }
                else if (msg[0] == 'A') {
                    current_wp = GetNearestWaypoint(this->wp);
                    auto_movement = true;
                    orientation_reached = false;
                    direction_chosen = false;
                }
            }
        }

    public:

        virtual ~Person()
        {
            this->ModelPlugin::~ModelPlugin();
            close_socket(this->sockfd);
        }

        void Load(physics::ModelPtr _parent, sdf::ElementPtr)
        {
            this->model = _parent;
            this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                boost::bind(&Person::OnUpdate, this, _1));

            std::cout << "Initial Position Person [" << this->model->WorldPose() << "]\n";

            // Setting WayPoints
            current_wp = 0;
            wp = {
                std::make_tuple(4, 6, 1),
                std::make_tuple(5, 3, 2),
                std::make_tuple(5, -14.5, 3),
                std::make_tuple(-5, -14.5, 4),
                std::make_tuple(-5, -25, 5),
                std::make_tuple(5, -25, 6),
                std::make_tuple(5, -14.5, 7),
                std::make_tuple(-5, -14.5, 8),
                std::make_tuple(-5, -1, 9),
                std::make_tuple(-4, 2, 10),
                std::make_tuple(-4, 5, 11),
                std::make_tuple(-2.5, 13, 12),
                std::make_tuple(3, 13, 13),
                std::make_tuple(4, 10, 0),
            };

            quadrants = {
                std::make_tuple(0.0, PI/2),
                std::make_tuple(PI/2, PI),
                std::make_tuple(-PI, -PI/2),
                std::make_tuple(-PI/2, 0.0)
            };

            auto_movement = false;
            linear_movement = true;
            linear_dir = 0;

            // Initializing Server
            this->sockfd = create_socket();
            set_ip_port(this->addr, IP.c_str(), PORT);
            make_bind(this->sockfd, this->addr);
            server_thread = std::thread(&Person::ServerThreadLoop, this);
        }

        void OnUpdate(const common::UpdateInfo &)
        {
            if (auto_movement) {
                if (MoveToWaypoint(wp[current_wp])) {
                    current_wp = std::get<2>(wp[current_wp]);
                }
            }
            else {
                auto pose = this->model->WorldPose();
                if (linear_movement) {
                    pose.Pos().X() += -(linear_dir)*lv_dt * (0*cos(pose.Rot().Yaw()) - 1*sin(pose.Rot().Yaw()));
                    pose.Pos().Y() += -(linear_dir)*lv_dt * (0*sin(pose.Rot().Yaw()) + 1*cos(pose.Rot().Yaw()));
                }
                else {
                    pose.Rot() = ignition::math::Quaterniond(0, 0, pose.Rot().Yaw() + turn_dir*av_dt);
                }
                this->model->SetWorldPose(pose);
            }
        }
    };
    GZ_REGISTER_MODEL_PLUGIN(Person)
}

/**
 * SOCKETS FUNCTIONS Definitions
 **/

int create_socket(void)
{
    int fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (fd == -1) {
        err(1, "socket failed");
    }
    return fd;
}

void close_socket(int fd)
{
    if (close(fd) == -1) {
        err(1, "close failed");
    }
}

void set_ip_port(struct sockaddr_in & addr, const char * ip, int port)
{
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr(ip);
    addr.sin_port = htons(port);
}

void make_bind(int fd, struct sockaddr_in & addr)
{
    if (bind(fd, (struct sockaddr *)&addr, sizeof addr) == -1) {
        err(1, "bind failed");
    }
}

int recv_message(int fd, struct sockaddr_in & dest_addr, void * buf, size_t len)
{
    socklen_t socklen = sizeof dest_addr;
    if (recvfrom(fd, buf, len, 0, (struct sockaddr *)&dest_addr, &socklen) == -1) {
        warn("recvfrom failed");
        return 1;
    }
    return 0;
}


/**
 * @file state.hpp
 * @author Licheng Wen (wenlc@zju.edu.cn)
 * @brief State base class
 * @version 0.1
 * @date 2021-01-14
 *
 * @copyright Copyright (c) 2021
 *
 */
#pragma once

#include <boost/functional/hash.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "constants.hpp"

struct Location {
  Location(double x, double y) : x(x), y(y) {}
  double x;
  double y;

  bool operator<(const Location& other) const {
    return std::tie(x, y) < std::tie(other.x, other.y);
  }

  bool operator==(const Location& other) const {
    return std::tie(x, y) == std::tie(other.x, other.y);
  }

  friend std::ostream& operator<<(std::ostream& os, const Location& c) {
    return os << "(" << c.x << "," << c.y << ")";
  }
};

namespace std {
template <>
struct hash<Location> {
  size_t operator()(const Location& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    return seed;
  }
};
}  // namespace std

struct State {
  State(double x, double y, double yaw, int time = 0)
      : time(time), x(x), y(y), yaw(yaw) {
    rot.resize(2, 2);
    rot(0, 0) = cos(-this->yaw);
    rot(0, 1) = -sin(-this->yaw);
    rot(1, 0) = sin(-this->yaw);
    rot(1, 1) = cos(-this->yaw);
  }

  State() = default;

  bool operator==(const State& s) const {
    return std::tie(time, x, y, yaw) == std::tie(s.time, s.x, s.y, s.yaw);
  }

  friend std::ostream& operator<<(std::ostream& os, const State& s) {
    return os << "(" << s.x << "," << s.y << ":" << s.yaw << ")@" << s.time;
  }

  int time;
  double x;
  double y;
  double yaw;
  boost::numeric::ublas::matrix<double> rot;
};

namespace std {
template <>
struct hash<State> {
  size_t operator()(const State& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    boost::hash_combine(seed, s.yaw);
    return seed;
  }
};
}  // namespace std


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/hyperbolic/include/functions/asinh.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/functions/rec.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/oneosqrteps.hpp>
#include <boost/math/special_functions/asinh.hpp>

NT2_TEST_CASE_TPL ( asinh,  NT2_REAL_TYPES)
{
  using nt2::asinh;
  using nt2::tag::asinh_;
  typedef typename nt2::meta::call<asinh_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(asinh(nt2::Inf<T>()), nt2::Inf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(asinh(nt2::Minf<T>()), nt2::Minf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(asinh(nt2::Nan<T>()), nt2::Nan<r_t>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(asinh(nt2::Zero<T>()), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(asinh(nt2::Valmax<T>()), boost::math::asinh(nt2::Valmax<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(asinh(nt2::rec(nt2::Sqrteps<T>())*2),  boost::math::asinh(nt2::rec(nt2::Sqrteps<T>())*2), 0.5);
  NT2_TEST_ULP_EQUAL(asinh(nt2::Eps<T>()), nt2::Eps<T>(), 0.5);
 for(T i=T(0.1); i <= T(1.1); i+= T(0.5))
 {
   T ri =  nt2::rec(i);
   NT2_TEST_ULP_EQUAL(asinh(i), boost::math::asinh(i), 0.5);
   NT2_TEST_ULP_EQUAL(asinh(ri), boost::math::asinh(ri), 0.5);
 }
}


/*
 * open_ggcm_test.cpp
 *
 *  Created on: Sep 23, 2009
 *      Author: dberrios
 */

#include <boost/lexical_cast.hpp>
#include <iostream>
#include <ccmc/ENLIL.h>
#include <ccmc/Interpolator.h>
#include <string>

using namespace std;

/**
 * enlil <inputfile> variable r theta(longitude) phi(latitude)
 */
int main (int argc, char * argv[])
{
	if (argc != 6)
	{
		cout << "enlil <inputfile> variable r theta(latitude) phi(longitude)" << endl;
		exit(1);
	}

	string filename = argv[1];
	string variable = argv[2];
	float r = boost::lexical_cast<float>(argv[3]);
	float lat = boost::lexical_cast<float>(argv[4]);
	float lon = boost::lexical_cast<float>(argv[5]);
	ccmc::ENLIL enlil;
	enlil.open(filename);
	enlil.loadVariable(variable);

	ccmc::Interpolator * interpolator = enlil.createNewInterpolator();
	float value = interpolator->interpolate(variable, r, lat, lon);
	delete interpolator;

	string units = enlil.getNativeUnit(variable);

	cout << "interpolated value: " << value << " " << units << endl;
	enlil.close();

}


//  Copyright John Maddock 2006, 2007.
//  Copyright Paul A. Bristow 2006, 2007.

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

#ifndef BOOST_STATS_NORMAL_HPP
#define BOOST_STATS_NORMAL_HPP

// http://en.wikipedia.org/wiki/Normal_distribution
// http://www.itl.nist.gov/div898/handbook/eda/section3/eda3661.htm
// Also:
// Weisstein, Eric W. "Normal Distribution."
// From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/NormalDistribution.html

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/erf.hpp> // for erf/erfc.
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>

#include <utility>

namespace boost{ namespace math{

template <class RealType = double, class Policy = policies::policy<> >
class normal_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   normal_distribution(RealType l_mean = 0, RealType sd = 1)
      : m_mean(l_mean), m_sd(sd)
   { // Default is a 'standard' normal distribution N01.
     static const char* function = "boost::math::normal_distribution<%1%>::normal_distribution";

     RealType result;
     detail::check_scale(function, sd, &result, Policy());
     detail::check_location(function, l_mean, &result, Policy());
   }

   RealType mean()const
   { // alias for location.
      return m_mean;
   }

   RealType standard_deviation()const
   { // alias for scale.
      return m_sd;
   }

   // Synonyms, provided to allow generic use of find_location and find_scale.
   RealType location()const
   { // location.
      return m_mean;
   }
   RealType scale()const
   { // scale.
      return m_sd;
   }

private:
   //
   // Data members:
   //
   RealType m_mean;  // distribution mean or location.
   RealType m_sd;    // distribution standard deviation or scale.
}; // class normal_distribution

typedef normal_distribution<double> normal;

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const normal_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
  if (std::numeric_limits<RealType>::has_infinity)
  {
     return std::pair<RealType, RealType>(-std::numeric_limits<RealType>::infinity(), std::numeric_limits<RealType>::infinity()); // - to + infinity.
  }
  else
  { // Can only use max_value.
    using boost::math::tools::max_value;
    return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>()); // - to + max value.
  }
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const normal_distribution<RealType, Policy>& /*dist*/)
{ // This is range values for random variable x where cdf rises from 0 to 1, and outside it, the pdf is zero.
  if (std::numeric_limits<RealType>::has_infinity)
  {
     return std::pair<RealType, RealType>(-std::numeric_limits<RealType>::infinity(), std::numeric_limits<RealType>::infinity()); // - to + infinity.
  }
  else
  { // Can only use max_value.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(-max_value<RealType>(),  max_value<RealType>()); // - to + max value.
  }
}

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

template <class RealType, class Policy>
inline RealType pdf(const normal_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   RealType sd = dist.standard_deviation();
   RealType mean = dist.mean();

   static const char* function = "boost::math::pdf(const normal_distribution<%1%>&, %1%)";

   RealType result = 0;
   if(false == detail::check_scale(function, sd, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_location(function, mean, &result, Policy()))
   {
      return result;
   }
   if((boost::math::isinf)(x))
   {
     return 0; // pdf + and - infinity is zero.
   }
   // Below produces MSVC 4127 warnings, so the above used instead.
   //if(std::numeric_limits<RealType>::has_infinity && abs(x) == std::numeric_limits<RealType>::infinity())
   //{ // pdf + and - infinity is zero.
   //  return 0;
   //}
   if(false == detail::check_x(function, x, &result, Policy()))
   {
      return result;
   }

   RealType exponent = x - mean;
   exponent *= -exponent;
   exponent /= 2 * sd * sd;

   result = exp(exponent);
   result /= sd * sqrt(2 * constants::pi<RealType>());

   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const normal_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   RealType sd = dist.standard_deviation();
   RealType mean = dist.mean();
   static const char* function = "boost::math::cdf(const normal_distribution<%1%>&, %1%)";
   RealType result = 0;
   if(false == detail::check_scale(function, sd, &result, Policy()))
   {
      return result;
   }
   if(false == detail::check_location(function, mean, &result, Policy()))
   {
      return result;
   }
   if((boost::math::isinf)(x))
   {
     if(x < 0) return 0; // -infinity
     return 1; // + infinity
   }
   // These produce MSVC 4127 warnings, so the above used instead.
   //if(std::numeric_limits<RealType>::has_infinity && x == std::numeric_limits<RealType>::infinity())
   //{ // cdf +infinity is unity.
   //  return 1;
   //}
   //if(std::numeric_limits<RealType>::has_infinity && x == -std::numeric_limits<RealType>::infinity())
   //{ // cdf -infinity is zero.
   //  return 0;
   //}
   if(false == detail::check_x(function, x, &result, Policy()))
   {
     return result;
   }
   RealType diff = (x - mean) / (sd * constants::root_two<RealType>());
   result = boost::math::erfc(-diff, Policy()) / 2;
   return result;
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const normal_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   RealType sd = dist.standard_deviation();
   RealType mean = dist.mean();
   static const char* function = "boost::math::quantile(const normal_distribution<%1%>&, %1%)";

   RealType result = 0;
   if(false == detail::check_scale(function, sd, &result, Policy()))
      return result;
   if(false == detail::check_location(function, mean, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, p, &result, Policy()))
      return result;

   result= boost::math::erfc_inv(2 * p, Policy());
   result = -result;
   result *= sd * constants::root_two<RealType>();
   result += mean;
   return result;
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<normal_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   RealType sd = c.dist.standard_deviation();
   RealType mean = c.dist.mean();
   RealType x = c.param;
   static const char* function = "boost::math::cdf(const complement(normal_distribution<%1%>&), %1%)";

   RealType result = 0;
   if(false == detail::check_scale(function, sd, &result, Policy()))
      return result;
   if(false == detail::check_location(function, mean, &result, Policy()))
      return result;
   if((boost::math::isinf)(x))
   {
     if(x < 0) return 1; // cdf complement -infinity is unity.
     return 0; // cdf complement +infinity is zero
   }
   // These produce MSVC 4127 warnings, so the above used instead.
   //if(std::numeric_limits<RealType>::has_infinity && x == std::numeric_limits<RealType>::infinity())
   //{ // cdf complement +infinity is zero.
   //  return 0;
   //}
   //if(std::numeric_limits<RealType>::has_infinity && x == -std::numeric_limits<RealType>::infinity())
   //{ // cdf complement -infinity is unity.
   //  return 1;
   //}
   if(false == detail::check_x(function, x, &result, Policy()))
      return result;

   RealType diff = (x - mean) / (sd * constants::root_two<RealType>());
   result = boost::math::erfc(diff, Policy()) / 2;
   return result;
} // cdf complement

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<normal_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   RealType sd = c.dist.standard_deviation();
   RealType mean = c.dist.mean();
   static const char* function = "boost::math::quantile(const complement(normal_distribution<%1%>&), %1%)";
   RealType result = 0;
   if(false == detail::check_scale(function, sd, &result, Policy()))
      return result;
   if(false == detail::check_location(function, mean, &result, Policy()))
      return result;
   RealType q = c.param;
   if(false == detail::check_probability(function, q, &result, Policy()))
      return result;
   result = boost::math::erfc_inv(2 * q, Policy());
   result *= sd * constants::root_two<RealType>();
   result += mean;
   return result;
} // quantile

template <class RealType, class Policy>
inline RealType mean(const normal_distribution<RealType, Policy>& dist)
{
   return dist.mean();
}

template <class RealType, class Policy>
inline RealType standard_deviation(const normal_distribution<RealType, Policy>& dist)
{
   return dist.standard_deviation();
}

template <class RealType, class Policy>
inline RealType mode(const normal_distribution<RealType, Policy>& dist)
{
   return dist.mean();
}

template <class RealType, class Policy>
inline RealType median(const normal_distribution<RealType, Policy>& dist)
{
   return dist.mean();
}

template <class RealType, class Policy>
inline RealType skewness(const normal_distribution<RealType, Policy>& /*dist*/)
{
   return 0;
}

template <class RealType, class Policy>
inline RealType kurtosis(const normal_distribution<RealType, Policy>& /*dist*/)
{
   return 3;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const normal_distribution<RealType, Policy>& /*dist*/)
{
   return 0;
}

template <class RealType, class Policy>
inline RealType entropy(const normal_distribution<RealType, Policy> & dist)
{
   using std::log;
   RealType arg = constants::two_pi<RealType>()*constants::e<RealType>()*dist.standard_deviation()*dist.standard_deviation();
   return log(arg)/2;
}

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_NORMAL_HPP


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

#include <boost/math/concepts/real_concept.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/special_functions/ulp.hpp>
#include <boost/math/special_functions/relative_difference.hpp>
#include <iostream>
#include <iomanip>

template <class T>
void test_value(const T& val, const char* name)
{
   using namespace boost::math;
   using std::fabs;
   T next = float_next(val);
   T prev = float_prior(val);

   if((boost::math::isinf)(next))
   {
      BOOST_CHECK_EQUAL(relative_difference(val, next), tools::max_value<T>());
      return;
   }
   if((boost::math::isinf)(prev))
   {
      BOOST_CHECK_EQUAL(relative_difference(val, prev), tools::max_value<T>());
      return;
   }

   BOOST_CHECK_EQUAL(relative_difference(val, next), relative_difference(next, val));
   BOOST_CHECK_EQUAL(epsilon_difference(val, next), epsilon_difference(next, val));
   BOOST_CHECK_LE(relative_difference(val, next), boost::math::tools::epsilon<T>());
   BOOST_CHECK_LE(epsilon_difference(val, next), T(1));
   if((fabs(val) > tools::min_value<T>()) || (fabs(next) > tools::min_value<T>()))
   {
      BOOST_CHECK_GT(relative_difference(val, next), T(0));
      BOOST_CHECK_GT(epsilon_difference(val, next), T(0));
   }
   else
   {
      BOOST_CHECK_EQUAL(relative_difference(val, next), T(0));
      BOOST_CHECK_EQUAL(epsilon_difference(val, next), T(0));
   }

   BOOST_CHECK_EQUAL(relative_difference(val, prev), relative_difference(prev, val));
   BOOST_CHECK_EQUAL(epsilon_difference(val, prev), epsilon_difference(prev, val));
   if((fabs(val) > tools::min_value<T>()) || (fabs(prev) > tools::min_value<T>()))
   {
      BOOST_CHECK_GT(relative_difference(val, prev), T(0));
      BOOST_CHECK_GT(epsilon_difference(val, prev), T(0));
   }
   else
   {
      BOOST_CHECK_EQUAL(relative_difference(val, prev), T(0));
      BOOST_CHECK_EQUAL(epsilon_difference(val, prev), T(0));
   }
}

template <class T>
void test_values(const T& val, const char* name)
{
   static const T a = static_cast<T>(1.3456724e22);
   static const T b = static_cast<T>(1.3456724e-22);
   static const T z = 0;
   static const T one = 1;
   static const T two = 2;

   std::cout << "Testing type " << name << std::endl;

   T den = (std::numeric_limits<T>::min)() / 4;
   if(den != 0)
   {
      std::cout << "Denormals are active\n";
   }
   else
   {
      std::cout << "Denormals are flushed to zero.\n";
   }

   test_value(a, name);
   test_value(-a, name);
   test_value(b, name);
   test_value(-b, name);
   test_value(boost::math::tools::epsilon<T>(), name);
   test_value(-boost::math::tools::epsilon<T>(), name);
   test_value(boost::math::tools::min_value<T>(), name);
   test_value(-boost::math::tools::min_value<T>(), name);
   if (std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_denorm == std::denorm_present) && ((std::numeric_limits<T>::min)() / 2 != 0))
   {
      test_value(z, name);
      test_value(-z, name);
   }
   test_value(one, name);
   test_value(-one, name);
   test_value(two, name);
   test_value(-two, name);

   static const int primes[] = {
      11,     13,     17,     19,     23,     29,
      31,     37,     41,     43,     47,     53,     59,     61,     67,     71,
      73,     79,     83,     89,     97,    101,    103,    107,    109,    113,
      127,    131,    137,    139,    149,    151,    157,    163,    167,    173,
      179,    181,    191,    193,    197,    199,    211,    223,    227,    229,
      233,    239,    241,    251,    257,    263,    269,    271,    277,    281,
      283,    293,    307,    311,    313,    317,    331,    337,    347,    349,
      353,    359,    367,    373,    379,    383,    389,    397,    401,    409,
      419,    421,    431,    433,    439,    443,    449,    457,    461,    463,
   };

   for(unsigned i = 0; i < sizeof(primes) / sizeof(primes[0]); ++i)
   {
      for(unsigned j = 0; j < sizeof(primes) / sizeof(primes[0]); ++j)
      {
         test_value(T(primes[i]) / T(primes[j]), name);
         test_value(-T(primes[i]) / T(primes[j]), name);
      }
   }
   using namespace boost::math;
   BOOST_CHECK_EQUAL(relative_difference(tools::min_value<T>(), -tools::min_value<T>()), tools::max_value<T>());
   BOOST_CHECK_EQUAL(epsilon_difference(tools::min_value<T>(), -tools::min_value<T>()), tools::max_value<T>());
   if(std::numeric_limits<T>::has_infinity)
   {
      BOOST_CHECK_EQUAL(relative_difference(std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity()), T(0));
      BOOST_CHECK_EQUAL(epsilon_difference(std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity()), T(0));
      BOOST_CHECK_EQUAL(relative_difference(std::numeric_limits<T>::infinity(), tools::max_value<T>()), tools::max_value<T>());
      BOOST_CHECK_EQUAL(epsilon_difference(std::numeric_limits<T>::infinity(), tools::max_value<T>()), tools::max_value<T>());
      BOOST_CHECK_EQUAL(relative_difference(std::numeric_limits<T>::infinity(), tools::max_value<T>() / 2), tools::max_value<T>());
      BOOST_CHECK_EQUAL(epsilon_difference(std::numeric_limits<T>::infinity(), tools::max_value<T>() / 2), tools::max_value<T>());
      BOOST_CHECK_EQUAL(relative_difference(tools::max_value<T>(), std::numeric_limits<T>::infinity()), tools::max_value<T>());
      BOOST_CHECK_EQUAL(epsilon_difference(tools::max_value<T>(), std::numeric_limits<T>::infinity()), tools::max_value<T>());
      BOOST_CHECK_EQUAL(relative_difference(tools::max_value<T>() / 2, std::numeric_limits<T>::infinity()), tools::max_value<T>());
      BOOST_CHECK_EQUAL(epsilon_difference(tools::max_value<T>() / 2, std::numeric_limits<T>::infinity()), tools::max_value<T>());
   }
   if(std::numeric_limits<T>::has_quiet_NaN)
   {
      BOOST_CHECK_EQUAL(relative_difference(std::numeric_limits<T>::quiet_NaN(), std::numeric_limits<T>::quiet_NaN()), tools::max_value<T>());
      BOOST_CHECK_EQUAL(epsilon_difference(std::numeric_limits<T>::quiet_NaN(), std::numeric_limits<T>::quiet_NaN()), tools::max_value<T>());
      BOOST_CHECK_EQUAL(relative_difference(std::numeric_limits<T>::quiet_NaN(), T(2)), tools::max_value<T>());
      BOOST_CHECK_EQUAL(epsilon_difference(std::numeric_limits<T>::quiet_NaN(), T(2)), tools::max_value<T>());
      BOOST_CHECK_EQUAL(relative_difference(T(2), std::numeric_limits<T>::quiet_NaN()), tools::max_value<T>());
      BOOST_CHECK_EQUAL(epsilon_difference(T(2), std::numeric_limits<T>::quiet_NaN()), tools::max_value<T>());
   }

}

BOOST_AUTO_TEST_CASE( test_main )
{
   test_values(1.0f, "float");
   test_values(1.0, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_values(1.0L, "long double");
   test_values(boost::math::concepts::real_concept(0), "real_concept");
#endif
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_MAXLEFTSHIFT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_MAXLEFTSHIFT_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/meta/int_c.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_maxleftshift Maxleftshift
 *
 * \par Description
 * Constant Maxleftshift
 * It is senseless to shift a value of more bits than the number of bits - 1
 * of the value type: this is that number.
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/maxleftshift.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::maxleftshift_(A0)>::type
 *     Maxleftshift();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Maxleftshift
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Maxleftshift of functor Maxleftshift
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    struct Maxleftshift : ext::pure_constant_<Maxleftshift>
    {
      template<class Target, class Dummy=void>
      struct  apply
            : meta::int_c < typename dispatch::meta::
                                     as_integer < typename Target::type
                                                , signed
                                                >::type
                          , sizeof(typename Target::type)*CHAR_BIT-1
                          > {};
    };
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Maxleftshift, Maxleftshift)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif


#include <stan/math/torsten/test/unit/test_fixture_onecpt.hpp>
#include <stan/math/torsten/test/unit/test_functors.hpp>
#include <stan/math/torsten/pmx_onecpt_model.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <boost/mp11.hpp>

TYPED_TEST_SUITE_P(test_onecpt);

TYPED_TEST_P(test_onecpt, multiple_bolus) {
  this -> compare_overload(1.e-2);
}

TYPED_TEST_P(test_onecpt, multiple_bolus_ss) {
  this -> time[0] = 0.0;
  this -> time[1] = 0.0;
  for(int i = 2; i < 10; i++) this -> time[i] = this -> time[i - 1] + 5;
  this -> amt[0] = 1200;
  this -> addl[0] = 10;
  this -> ss[0] = 1;

  this -> compare_overload(1.e-2);
}

TYPED_TEST_P(test_onecpt, multiple_infusion) {
  this -> cmt[0] = 2;
  this -> rate[0] = 350;
  this -> addl[0] = 2;

  this -> compare_overload(1.e-2);
}

TYPED_TEST_P(test_onecpt, multiple_infusion_ss) {
  this -> time[0] = 0.0;
  this -> time[1] = 0.0;
  for(int i = 2; i < 10; i++) this -> time[i] = this -> time[i - 1] + 5;
  this -> amt[0] = 1200;
  this -> rate[0] = 190;        // amt/rate = even time will cause test failure
  this -> addl[0] = 10;
  this -> ss[0] = 1;

  this -> compare_param_overload(1.e-3);
}

REGISTER_TYPED_TEST_SUITE_P(test_onecpt,
                            multiple_bolus,
                            multiple_bolus_ss,
                            multiple_infusion,
                            multiple_infusion_ss);

using ode_test_types = boost::mp11::mp_product<
  std::tuple,
  ::testing::Types<pmx_solve_rk45_functor, pmx_solve_bdf_functor>, // solver 1
  ::testing::Types<pmx_solve_linode_functor>, // solver 2
  ::testing::Types<double>,                   // TIME
  ::testing::Types<double, stan::math::var_value<double>>,  // AMT
  ::testing::Types<double, stan::math::var_value<double>> , // RATE
  ::testing::Types<double> ,                                // II
  ::testing::Types<double, stan::math::var_value<double>> , // PARAM
  ::testing::Types<double> ,                                // BIOVAR
  ::testing::Types<double> ,                                // TLAG
  ::testing::Types<torsten::PMXOneCptODE>                   // ODE
    >;

INSTANTIATE_TYPED_TEST_SUITE_P(PMX, test_onecpt, ode_test_types);


#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>


/** @file
 *
 * This file is part of Boost, not part of Chaste per se.
 *
 * We use the <autogenerated> tag to ignore it from our Doxygen checker.
 *
 * This file is provided to users of Boost up to 1.63 inclusive and gives forward compatibility with
 * Boost 1.64-1.65 and beyond.
 * (An optimisation in exponential distributions was applied to boost in version 1.64,
 * which also affects normal distribution tails.)
 *
 * Minimal changes were made to the file to include it here, simply giving the functions a unique name
 * by appending _v165 so that later boosts don't have an incompatibility if this file is lying around.
 *
 */

/* boost random/detail/int_float_pair.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Copyright Steven Watanabe 2010-2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 */

#ifndef BOOST_165_RANDOM_DETAIL_INT_FLOAT_PAIR_HPP
#define BOOST_165_RANDOM_DETAIL_INT_FLOAT_PAIR_HPP

#include <boost/integer.hpp>
#include <boost/integer/integer_mask.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/random/detail/integer_log2.hpp>
#include <boost/random/detail/signed_unsigned_tools.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/make_unsigned.hpp>
#include <utility>

namespace boost
{
namespace random
{
    namespace detail
    {

        template <class Engine>
        inline typename boost::make_unsigned<typename Engine::result_type>::type
        generate_one_digit_v165(Engine& eng, std::size_t bits)
        {
            typedef typename Engine::result_type base_result;
            typedef typename boost::make_unsigned<base_result>::type base_unsigned;

            base_unsigned range = detail::subtract<base_result>()((eng.max)(), (eng.min)());
            base_unsigned y0_mask = (base_unsigned(2) << (bits - 1)) - 1;
            base_unsigned y0 = (range + 1) & ~y0_mask;
            base_unsigned u;
            do
            {
                u = detail::subtract<base_result>()(eng(), (eng.min)());
            } while (y0 != 0 && u > base_unsigned(y0 - 1));
            return u & y0_mask;
        }

        template <class RealType, std::size_t w, class Engine>
        std::pair<RealType, int> generate_int_float_pair_v165(Engine& eng, boost::mpl::true_)
        {
            typedef typename Engine::result_type base_result;
            typedef typename boost::make_unsigned<base_result>::type base_unsigned;

            base_unsigned range = detail::subtract<base_result>()((eng.max)(), (eng.min)());

            std::size_t m = (range == (std::numeric_limits<base_unsigned>::max)()) ? std::numeric_limits<base_unsigned>::digits : detail::integer_log2(range + 1);

            int bucket = 0;
            // process as many full digits as possible into the int part
            for (std::size_t i = 0; i < w / m; ++i)
            {
                base_unsigned u = generate_one_digit_v165(eng, m);
                bucket = (bucket << m) | u;
            }
            RealType r;

            const std::size_t digits = std::numeric_limits<RealType>::digits;
            {
                base_unsigned u = generate_one_digit_v165(eng, m);
                base_unsigned mask = (base_unsigned(1) << (w % m)) - 1;
                bucket = (bucket << (w % m)) | (mask & u);
                const RealType mult = RealType(1) / RealType(base_unsigned(1) << (m - w % m));
                // zero out unused bits
                if (m - w % m > digits)
                {
                    u &= ~(base_unsigned(1) << (m - digits));
                }
                r = RealType(u >> (w % m)) * mult;
            }
            for (std::size_t i = m - w % m; i + m < digits; ++i)
            {
                base_unsigned u = generate_one_digit_v165(eng, m);
                r += u;
                r *= RealType(0.5) / RealType(base_unsigned(1) << (m - 1));
            }
            if (m - w % m < digits)
            {
                const std::size_t remaining = (digits - m + w % m) % m;
                base_unsigned u = generate_one_digit_v165(eng, m);
                r += u & ((base_unsigned(2) << (remaining - 1)) - 1);
                const RealType mult = RealType(0.5) / RealType(base_unsigned(1) << (remaining - 1));
                r *= mult;
            }
            return std::make_pair(r, bucket);
        }

        template <class RealType, std::size_t w, class Engine>
        inline std::pair<RealType, int> generate_int_float_pair_v165(Engine& eng, boost::mpl::false_)
        {
            int bucket = uniform_int_distribution<>(0, (1 << w) - 1)(eng);
            RealType r = uniform_01<RealType>()(eng);
            return std::make_pair(r, bucket);
        }

        template <class RealType, std::size_t w, class Engine>
        inline std::pair<RealType, int> generate_int_float_pair_v165(Engine& eng)
        {
            typedef typename Engine::result_type base_result;
            return generate_int_float_pair_v165<RealType, w>(eng,
                                                             boost::is_integral<base_result>());
        }

    } // namespace detail
} // namespace random
} // namespace boost

#endif // BOOST_165_RANDOM_DETAIL_INT_FLOAT_PAIR_HPP