#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/Pose2_Point2_Factor.h>
#include <vector>
#include <fstream>
#include <string>
#include <time.h>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::algorithm;

using symbol_shorthand::X;
using symbol_shorthand::L;

// Testing params
const size_t max_odom_count = 3800; //3800

noiseModel::Diagonal::shared_ptr prior_noise_model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.0001, 0.0001, 0.0001).finished());

noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0/100.0, 1.0/500.0, 1.0/500.0).finished());

noiseModel::Diagonal::shared_ptr point_noise_model = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0/1.581139, 1.0/1.581139).finished());
/* ************************************************************************* */

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

  ifstream in("../data/mh_T2_victoriaPark_01.txt"); //Type #2 only
  //ifstream in("../data/mh_T1_T2_victoriaPark_08.txt"); //Type #1 + Type #2

  size_t odom_count = 0;
  size_t landmark_count = 0;

  std::list<double> time_list;

  ISAM2Params parameters;
  MHParams mh_parameters(10, 30); //(10, 30)

  mh_parameters.isPrintPruningDetails = true;

  //parameters.optimizationParams = gtsam::ISAM2DoglegParams(0.1);  //_initialDelta = 1.0
  parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); //_wildfireThreshold = 0.001

  parameters.relinearizeThreshold = 0.01;

  parameters.relinearizeSkip = 1;
  MHISAM2* mh_isam2 = new MHISAM2(parameters, mh_parameters);

  NonlinearFactorGraph* mh_graph = new NonlinearFactorGraph();

  Values mh_init_values;
  Values mh_results;

  double x = 0.0;
  double y = 0.0;
  double rad = 0.0;

  std::vector<Pose2> prior_arr;
  prior_arr.push_back(Pose2(x, y, rad));

  MHGenericValue<Pose2> prior_value(prior_arr[0], mh_isam2->getFirstHypoLayer());

  mh_init_values.mhInsert(X(0), prior_value);

  mh_graph->add(MHPriorFactor<Pose2>(X(0), prior_arr, prior_noise_model));

  mh_isam2->update(*mh_graph, mh_init_values);
  mh_graph->resize(0);
  mh_init_values.clear();
  mh_results = mh_isam2->mhCalculateBestEstimate();

  //*
  size_t key_s = 0;
  size_t key_t = 0;

  clock_t start_time = clock();
  string str;
  while (getline(in, str) && odom_count < max_odom_count) {

    //cout << str << endl;
    vector<string> parts;
    split(parts, str, is_any_of(" "));

    if (parts[0] == "ODOMETRY") {
      key_s = stoi(parts[1]);
      key_t = stoi(parts[3]);
      int m_num = stoi(parts[5]);
      vector<double> x_arr(m_num);
      vector<double> y_arr(m_num);
      vector<double> rad_arr(m_num);
      for (int i = 0; i < m_num; ++i) {
        x_arr[i] = stod(parts[6 + 3*i]);
        y_arr[i] = stod(parts[7 + 3*i]);
        rad_arr[i] = stod(parts[8 + 3*i]);
      }

      std::vector<Pose2> odom_arr;
      for (int i = 0; i < m_num; ++i) {
        odom_arr.push_back(Pose2(x_arr[i], y_arr[i], rad_arr[i]));
      }

      if (m_num > 1) { //MH
        mh_isam2->createNextHypoLayer(odom_arr.size(), 3); //mhsiao: should get dim from factor type
      }

      MHGenericValue<Pose2> pose_value(mh_results.mhAt<Pose2>(X(key_s)), odom_arr, mh_isam2->getLastHypoLayer() );

      mh_init_values.mhInsert(X(key_t), pose_value);

      mh_graph->add(MHBetweenFactor<Pose2>(X(key_s), X(key_t), odom_arr, pose_noise_model));

      if (m_num > 1) { //MH
        mh_isam2->assocLatestHypoLayerWith(mh_graph->back());
      }

      odom_count++;
      //cout << odom_count << endl;

    } else { //LANDMARK
      //*
      key_s = stoi(parts[1]);
      int k_num = stoi(parts[2]);

      if (k_num == 1) {
        key_t = stoi(parts[3]);
        //int m_num = 1;
        double l_x = stod(parts[6]);
        double l_y = stod(parts[7]);

        std::vector<Point2> measured_arr;
        measured_arr.push_back(Point2(l_x, l_y));

        if (key_t >= landmark_count) {
          MHGenericValue<Point2> point_value(mh_results.mhAt<Pose2>(X(key_s)), measured_arr, mh_isam2->getLastHypoLayer() );

          mh_init_values.mhInsert(L(key_t), point_value);

          landmark_count++;
        } else {
          mh_isam2->accumulateCommonDim(2);
        }

        std::list<Key> key_list;
        key_list.push_back(X(key_s));
        key_list.push_back(L(key_t));
        mh_graph->add(MH_Pose2_Point2_Factor(key_list, measured_arr[0], point_noise_model));

        //landmark_count++;
      } else {
        size_t key_t1 = stoi(parts[3]);
        size_t key_t2 = stoi(parts[4]);
        //int m_num = 1;
        double l_x = stod(parts[7]);
        double l_y = stod(parts[8]);

        Point2 measured_point(l_x, l_y);

        std::list<Key> key_list;
        key_list.push_back(X(key_s));
        key_list.push_back(L(key_t1));
        key_list.push_back(L(key_t2));

        mh_isam2->createNextHypoLayer(2, 2);

        mh_graph->add(MH_Pose2_Point2_Factor(key_list, measured_point, point_noise_model));
        mh_isam2->setVariableToExpand(X(key_s), mh_isam2->getLastHypoLayer());
        mh_isam2->assocLatestHypoLayerWith(mh_graph->back());

      }
      // */
    }

    mh_isam2->update(*mh_graph, mh_init_values);
    mh_graph->resize(0);
    mh_init_values.clear();
    mh_results = mh_isam2->mhCalculateBestEstimate();

    if (parts[0] == "ODOMETRY") {
      clock_t cur_time = clock();
      time_list.push_back(cur_time - start_time);
    }

    if (time_list.size()%100 == 0) {
      string step_file_idx = std::to_string(100000 + time_list.size());

      ofstream step_outfile;
      string step_file_name = "mh_T2_step_files/MH_ISAM2_TEST_victoriaPark_S" + step_file_idx;
      step_outfile.open(step_file_name + ".txt");

      MHISAM2::HypoListIter it = mh_isam2->getLastHypoLayer()->getNodeList().begin();
      for (size_t i = 0; i < (odom_count + 1); ++i) {
        Pose2 out_pose = mh_results.mhAtHypo<Pose2>(X(i), (*it));

        Vector tt = out_pose.translation().vector();
        step_outfile << tt(0) << " " << tt(1) << " "; // [x1 y1] [x2 y2] ...
      }
      step_outfile.close();
    }

    //*
    if (odom_count%50 == 0 && key_s != key_t - 1) {
       std::cout << "odom_count: " << odom_count << std::endl;

       //MHISAM2::HypoList& curr_hypo_list = mh_isam2->getLastHypoLayer()->getNodeList();
       //std::cout << "Last layer hypos:  " << curr_hypo_list.size() << std::endl;

       std::cout << "acc_time:  " << time_list.back() << std::endl;
    }
    // */

  }

  clock_t end_time = clock();
  clock_t total_time = end_time - start_time;
  cout << "total_time: " << total_time << endl;
  cout << "# layers: " << mh_isam2->getLastHypoLayer()->getLayerIdx() << endl;
  //*

  ofstream outfile;
  string file_name = "MH_ISAM2_TEST_victoriaPark";
  outfile.open(file_name + ".txt");

  for (size_t i = 0; i < odom_count; ++i) {
    vector<Pose2> out_arr = mh_results.mhAt<Pose2>(X(i));
    for (size_t h = 0; h < out_arr.size(); ++h) {
      outfile << out_arr[h].x() << " " << out_arr[h].y() << " " << out_arr[h].theta() << endl; //WRONG FORMAT... JUST FOR SIMPLE SHOW
    }
  }

  outfile.close();
  cout << "output " << file_name << ".txt file." << endl;
  // */
  ofstream lm_outfile;
  string lm_file_name = "MH_ISAM2_TEST_victoriaPark_lm";
  lm_outfile.open(lm_file_name + ".txt");
  //outfile << "total_time:  " << total_time << endl;
  for (size_t i = 0; i < landmark_count; ++i) {
    vector<Point2> out_arr = mh_results.mhAt<Point2>(L(i));
    for (size_t h = 0; h < out_arr.size(); ++h) {
      lm_outfile << out_arr[h].x() << " " << out_arr[h].y() << endl; //WRONG FORMAT... JUST FOR SIMPLE SHOW
    }
  }

  lm_outfile.close();
  cout << "output " << lm_file_name << ".txt file." << endl;

  //*
  // Output each hypo
  ofstream outfile_hypo;
  string hypo_file_name = "MH_ISAM2_TEST_victoriaPark_hypos";
  outfile_hypo.open(hypo_file_name + ".txt");

  int hypo_count = 0;
  MHISAM2::HypoList& max_hypo_list = mh_isam2->getLastHypoLayer()->getNodeList();
  for (MHISAM2::HypoListIter it = max_hypo_list.begin(); it != max_hypo_list.end(); ++it) {
    for (size_t i = 0; i < (odom_count); ++i) {
      Pose2 out_pose = mh_results.mhAtHypo<Pose2>(X(i), (*it));

      Vector tt = out_pose.translation().vector();
      outfile_hypo << tt(0) << " " << tt(1) << " "; // [x1 y1] [x2 y2] ...
    }
    outfile_hypo << endl;
    hypo_count++;
  }
  outfile_hypo.close();
  cout << "output " << hypo_file_name << ".txt file." << endl;
  // */

  //*
  // Output each landmark hypo
  ofstream outfile_lm_hypo;
  string lm_hypo_file_name = "MH_ISAM2_TEST_victoriaPark_lm_hypos";
  outfile_lm_hypo.open(lm_hypo_file_name + ".txt");

  hypo_count = 0;
  for (MHISAM2::HypoListIter it = max_hypo_list.begin(); it != max_hypo_list.end(); ++it) {
    for (size_t i = 0; i < (landmark_count); ++i) {
      Point2 out_pt = mh_results.mhAtHypo<Point2>(L(i), (*it));

      outfile_lm_hypo << out_pt.x() << " " << out_pt.y() << " "; // [x1 y1] [x2 y2] ...
    }
    outfile_lm_hypo << endl;
    hypo_count++;
  }
  outfile_lm_hypo.close();
  cout << "output " << lm_hypo_file_name << ".txt file." << endl;
  // */

  //*
  ofstream outfile_time;
  string time_file_name = "MH_ISAM2_TEST_victoriaPark_time";
  outfile_time.open(time_file_name + ".txt");
  for (auto acc_time : time_list) {
    outfile_time << acc_time << endl;
  }
  outfile_time.close();
  cout << "output " << time_file_name << ".txt file." << endl;
  // */

  return 0;
}


/* -*- mode: c++; c-basic-offset: 2; indent-tabs-mode: nil; tab-width: 8 -*-
 *
 * $Id: ext.cpp 21225 2014-11-28 21:06:02Z phyy-nx $
 */
#include <cctbx/boost_python/flex_fwd.h>
#include <boost/python.hpp>
#include <boost/python/class.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/math/basic_statistics.h>
#include <cctbx/miller.h>
#include <cctbx/uctbx.h>

/*

This file contains an implmentation of calc_avg_I from
postrefine/mod_util.py, which averages observed intensities,
does outlier rejection and computes various statistics.

*/

using namespace boost::python;

namespace prime {
  enum Average_Mode {
    Average,  ///< normal avearaging
    Weighted, ///< weighted averaging
    Final     ///< averaging done when the postrefinement is finishing
  };

  typedef
    scitbx::af::shared<cctbx::miller::index<int> > shared_miller;

  struct average_result_store {
    // class for returning results
    shared_miller miller_index;
    scitbx::af::shared<double> I_avg;
    scitbx::af::shared<double> sigI_avg;
    scitbx::af::shared<double> r_meas_w_top;
    scitbx::af::shared<double> r_meas_w_btm;
    scitbx::af::shared<double> r_meas_top;
    scitbx::af::shared<double> r_meas_btm;
    scitbx::af::shared<int> multiplicity;
    scitbx::af::shared<double> I_avg_even;
    scitbx::af::shared<double> I_avg_odd;
    scitbx::af::shared<double> I_avg_even_h;
    scitbx::af::shared<double> I_avg_odd_h;
    scitbx::af::shared<double> I_avg_even_k;
    scitbx::af::shared<double> I_avg_odd_k;
    scitbx::af::shared<double> I_avg_even_l;
    scitbx::af::shared<double> I_avg_odd_l;
    std::string txt_obs_out;
    std::string txt_reject_out;
  };

  class averaging_engine {
    // interface for computing averages
    const int group_no_;
    const scitbx::af::shared<int> group_id_list_;
    const shared_miller miller_index_;
    const shared_miller miller_index_ori_;
    const scitbx::af::shared<double> I_;
    const scitbx::af::shared<double> sigI_;
    const scitbx::af::shared<double> G_;
    const scitbx::af::shared<double> B_;
    const scitbx::af::shared<double> p_set_;
    const scitbx::af::shared<double> rs_set_;
    const scitbx::af::shared<double> wavelength_set_;
    const scitbx::af::shared<double> sin_theta_over_lambda_sq_;
    const scitbx::af::shared<double> SE_;
    const scitbx::af::shared<std::string> pickle_filename_set_;
    public:
    Average_Mode avg_mode_;
    double sigma_max_;
    bool flag_volume_correction_;
    int n_rejection_cycle_;
    bool flag_output_verbose_;

    public: averaging_engine(
      // main constructor, used for passing in arrays. other parmaters exposed
      // as properties to python
      int group_no,
      scitbx::af::shared<int> group_id_list,
      const shared_miller& miller_index,
      const shared_miller& miller_index_ori,
      const scitbx::af::shared<double>& I,
      const scitbx::af::shared<double>& sigI,
      const scitbx::af::shared<double>& G,
      const scitbx::af::shared<double>& B,
      const scitbx::af::shared<double>& p_set,
      const scitbx::af::shared<double>& rs_set,
      const scitbx::af::shared<double>& wavelength_set,
      const scitbx::af::shared<double>& sin_theta_over_lambda_sq,
      const scitbx::af::shared<double>& SE,
      const scitbx::af::shared<std::string>& pickle_filename_set
      ):
        group_no_(group_no),
        group_id_list_(group_id_list),
        miller_index_(miller_index),
        miller_index_ori_(miller_index_ori),
        I_(I),sigI_(sigI),G_(G),B_(B),p_set_(p_set),
        rs_set_(rs_set),
        wavelength_set_(wavelength_set),
        sin_theta_over_lambda_sq_(sin_theta_over_lambda_sq),
        SE_(SE),
        pickle_filename_set_(pickle_filename_set)
    {
      avg_mode_ = Average;
      sigma_max_ = 99.0;
      flag_volume_correction_ = true;
      n_rejection_cycle_ = 1;
      flag_output_verbose_ = false;
    }

    void calc_avg_two_halves(
      const scitbx::af::shared<double>& I_full_group,
      const scitbx::af::shared<double>& SE_norm,
      const Average_Mode& avg_mode_,
      double& I_avg_even,
      double& I_avg_odd
      )
    {
      double I_even_sum = 0;
      double I_odd_sum = 0;
      double I_even_weighted_sum = 0;
      double I_odd_weighted_sum = 0;
      double SE_norm_even_sum = 0;
      double SE_norm_odd_sum = 0;
      I_avg_even = 0;
      I_avg_odd = 0;
      if (I_full_group.size() > 2) {
        for (int i = 0; i < I_full_group.size(); i++) {
          if (i % 2 == 0) {
            I_even_sum += I_full_group[i];
            I_even_weighted_sum += I_full_group[i] * SE_norm[i];
            SE_norm_even_sum += SE_norm[i];
          }
          else {
            I_odd_sum += I_full_group[i];
            I_odd_weighted_sum += I_full_group[i] * SE_norm[i];
            SE_norm_odd_sum += SE_norm[i];
          }
        }

        if (I_full_group.size() % 2 == 1) {
          I_odd_sum += I_full_group[I_full_group.size()-1];
          I_odd_weighted_sum += I_full_group[I_full_group.size()-1] * SE_norm[SE_norm.size()-1];
          SE_norm_odd_sum += SE_norm[SE_norm.size()-1];
        }

        if (avg_mode_ == Weighted || avg_mode_ == Final) {
          I_avg_even = I_even_weighted_sum/SE_norm_even_sum;
          I_avg_odd = I_odd_weighted_sum/SE_norm_odd_sum;
        }
        else {
          SCITBX_ASSERT(avg_mode_ == Average);
          int size;
          if(I_full_group.size() % 2 == 1)
            size = (I_full_group.size()+1)/2;
          else
            size = I_full_group.size()/2;
          I_avg_even = I_even_sum/size;
          I_avg_odd = I_odd_sum/size;
        }
      }
    }

    static const double CONST_SE_MIN_WEIGHT;
    static const double CONST_SE_MAX_WEIGHT;
    static const double CONST_SIG_I_FACTOR;

    public: average_result_store
    calc_avg_I() {
      /*
      Average the intensites.

      I_ is an array with N observations.  miller_indices is an array with M indices.
      M should always be <= N. Observations are arranged into groups where the members
      of the same group have the same miller index.  group_id_list is N long and indexes
      each observation to a group and its miller index. group_id_list should be sorted
      (ascending) and that sort order applied to G, B, etc.
      */

      average_result_store results;
      std::ostringstream txt_obs_out;
      std::ostringstream txt_reject_out;

      // convert to the full intensity and calculate mosaic spread (currently only logged)
      scitbx::af::shared<double> I_full;
      scitbx::af::shared<double> sigI_full;
      scitbx::af::shared<double> mosaic_radian_set;
      for(int x = 0; x < G_.size(); x++) {
        double tmp1 = G_[x] *std::exp(-2*B_[x]*sin_theta_over_lambda_sq_[x]);
        double tmp2 = I_[x];
        double tmp3 = p_set_[x];
        if (x==0)
          printf("c++ check overflow: %70.70f\n",tmp1*tmp2); // Without this printf, I_full comes out differently between
                                                               // python and cpp in the highest significant digits. No idea why.
        I_full.push_back(tmp2/(tmp1*tmp3));
        sigI_full.push_back(sigI_[x]/(G_[x] * std::exp(-2*B_[x]*sin_theta_over_lambda_sq_[x]) * p_set_[x]));
        mosaic_radian_set.push_back(2 * rs_set_[x] * wavelength_set_[x]);
      }

      if (flag_volume_correction_)
        for (int x = 0; x < I_full.size(); x++){
          I_full[x] *= (4.0/3.0) * (rs_set_[x]);
          sigI_full[x] *= (4.0/3.0) * (rs_set_[x]);
        }

      // Iterate over each group of intensites. They will match a single miller_index each
      int obs_ptr = 0; // this will track along the intensites array as each group is processed
      for (int g = 0; g < group_no_; g++) {
        SCITBX_ASSERT(group_id_list_[obs_ptr] == g); // this verifies that group_id_list is sorted

        int obs_ptr_start = obs_ptr;

        // gather intensites and errors for this group
        double max_w = CONST_SE_MAX_WEIGHT;
        double min_w = std::sqrt(CONST_SE_MIN_WEIGHT);
        scitbx::af::shared<double> I_group;
        scitbx::af::shared<double> sigI_group;
        scitbx::af::shared<double> I_full_group;
        scitbx::af::shared<double> sigI_full_group;
        scitbx::af::shared<double> SE_group;
        cctbx::miller::index<int> current_index;
        shared_miller current_index_ori;
        scitbx::af::shared<std::string>pickle_filename_set_group;
        std::ostringstream txt_reject_out_group;

        scitbx::af::shared<int> valid_ptrs;

        bool found_one = false;
        while (obs_ptr < I_.size()) {
          if (group_id_list_[obs_ptr] != g) {
            break; // found them all
          }

          if (found_one) {
            SCITBX_ASSERT(current_index == miller_index_[obs_ptr]);
          }
          else {
            found_one = true;
            current_index = miller_index_[obs_ptr];
          }
          I_group.push_back(I_[obs_ptr]);
          sigI_group.push_back(sigI_[obs_ptr]);
          I_full_group.push_back(I_full[obs_ptr]);
          sigI_full_group.push_back(sigI_full[obs_ptr]);
          SE_group.push_back(SE_[obs_ptr]);
          current_index_ori.push_back(miller_index_ori_[obs_ptr]);
          pickle_filename_set_group.push_back(pickle_filename_set_[obs_ptr]);
          valid_ptrs.push_back(obs_ptr);
          obs_ptr++;
        }
        SCITBX_ASSERT(found_one);

        // log
        char buf[512];
        sprintf(buf, "Reflection: %d,%d,%d\nmeanI    medI  sigI_est sigI_true delta_sigI   n_refl\n",current_index[0],current_index[1],current_index[2]);
        txt_obs_out << buf;
        scitbx::af::shared<double> I_full_group_copy;
        for (int i = 0; i < I_full_group.size(); i++)
          I_full_group_copy.push_back(I_full_group[i]);
        scitbx::math::basic_statistics<double> basic_stat(I_full_group_copy.const_ref());
        scitbx::math::median_functor mf;

        double median_I = mf(I_full_group_copy.ref());
        double mean_I = basic_stat.mean;
        double std_I = 0;
        //if (I_full_group.size() > 1)
        std_I = basic_stat.biased_standard_deviation; // based on what I think numpy is doing compared to basic_statistics

        sprintf(buf, "%6.2f %6.2f %8.2f %8.0f\n", mean_I, median_I, std_I, double(I_full_group.size()));
        txt_obs_out << buf;

        //reject outliers
        if (I_full_group.size() > 2) {
          for (int i_rejection = 0; i_rejection < n_rejection_cycle_; i_rejection++) {
            scitbx::af::shared<double> I_full_group_copy;
            for (int i = 0; i < I_full_group.size(); i++)
              I_full_group_copy.push_back(I_full_group[i]);
            scitbx::math::basic_statistics<double> basic_stat(I_full_group_copy.const_ref());
            scitbx::math::median_functor mf;

            scitbx::af::shared<double> I_group_filtered;
            scitbx::af::shared<double> sigI_group_filtered;
            scitbx::af::shared<double> I_full_group_filtered;
            scitbx::af::shared<double> sigI_full_group_filtered;
            scitbx::af::shared<double> SE_group_filtered;
            shared_miller current_index_ori_filtered;
            scitbx::af::shared<std::string> pickle_filename_set_group_filtered;

            scitbx::af::shared<int> valid_ptrs_filtered;

            double median_I = mf(I_full_group_copy.ref());
            double mean_I = basic_stat.mean;
            double std_I = basic_stat.biased_standard_deviation; // based on what I think numpy is doing compared to basic_statistics

            for (int i = 0; i < I_full_group.size(); i++) {
              double I_full_as_sigma = (I_full_group[i] - median_I) / std_I;
              if (std::abs(I_full_as_sigma) > sigma_max_) {
                char buf[512];
                sprintf(buf, "%s %3.0f %3.0f %3.0f %10.2f %10.2f\n", pickle_filename_set_group[i].c_str(),
                  double(current_index_ori[i][0]), double(current_index_ori[i][1]), double(current_index_ori[i][2]), I_group[i], sigI_group[i]);
                txt_reject_out_group << buf;
              }
              else {
                I_group_filtered.push_back(I_group[i]);
                sigI_group_filtered.push_back(sigI_group[i]);
                I_full_group_filtered.push_back(I_full_group[i]);
                sigI_full_group_filtered.push_back(sigI_full_group[i]);
                SE_group_filtered.push_back(SE_group[i]);
                current_index_ori_filtered.push_back(current_index_ori[i]);
                pickle_filename_set_group_filtered.push_back(pickle_filename_set_group[i]);
                valid_ptrs_filtered.push_back(valid_ptrs[i]);
              }
            }
            I_group = I_group_filtered;
            sigI_group = sigI_group_filtered;
            I_full_group = I_full_group_filtered;
            sigI_full_group = sigI_full_group_filtered;
            SE_group = SE_group_filtered;
            current_index_ori = current_index_ori_filtered;
            pickle_filename_set_group = pickle_filename_set_group_filtered;
            valid_ptrs = valid_ptrs_filtered;

            char buf[512];
            sprintf(buf, "%6.2f %6.2f %8.2f %8.0f\n", mean_I, median_I, std_I, double(I_full_group.size()));
            txt_obs_out << buf;

            if (I_full_group.size() <= 3)
              break;
          }
          if (I_full_group.size() == 0) {
            printf("miller_index (%d, %d, %d) rejected at calc_avg", current_index[0], current_index[1], current_index[2]);
            continue;
          }
        }
        // normalize the SE
        scitbx::af::shared<double> SE_norm;
        double se_max = scitbx::af::max(SE_group.ref());
        double se_min = scitbx::af::min(SE_group.ref());
        double SE_norm_val;
        if (SE_group.size() == 1 || ((se_max-se_min) < 0.1) || avg_mode_ == Average) {
          SE_norm_val = 1;
          for (int i = 0; i < SE_group.size(); i++) {
            SE_norm.push_back(SE_norm_val);
          }
        }
        else {
          double m = (max_w - min_w)/(se_min-se_max);
          double b = max_w - (m*se_min);

          for (int i = 0; i < SE_group.size(); i++) {
            SE_norm.push_back( (m*SE_group[i]) + b);
          }
        }

        double SE_norm_sum = scitbx::af::sum(SE_norm.const_ref());
        SCITBX_ASSERT(SE_norm_sum != 0);
        scitbx::af::shared<double> avg_tmp;
        for (int i = 0; i < SE_norm.size(); i++) {
          avg_tmp.push_back(SE_norm[i] * I_full_group[i]);
        }

        double I_avg = scitbx::af::sum(avg_tmp.const_ref())/SE_norm_sum;
        double sigI_avg = scitbx::af::mean(sigI_full_group.const_ref());

        //Rmeas, Rmeas_w, multiplicity
        int multiplicity = I_full_group.size();
        double r_meas_w_top = 0;
        double r_meas_w_btm = 0;
        double r_meas_top = 0;
        double r_meas_btm = 0;
        double r_meas = 0;
        double r_meas_w = 0;
        if (multiplicity > 1) {
          for (int i = 0; i < multiplicity; i++) {
            r_meas_w_top += std::pow(((I_full_group[i] - I_avg)*SE_norm[i]),2);
            r_meas_w_btm += std::pow(I_full_group[i]*SE_norm[i],2);
            r_meas_top += std::abs(((I_full_group[i] - I_avg)*SE_norm[i]));
            r_meas_btm += std::abs(I_full_group[i]*SE_norm[i]);
          }
          r_meas_w = r_meas_w_top/r_meas_w_btm;
          r_meas = r_meas_top/r_meas_btm;
        }

        //for calculation of cc1/2
        //separate the observations into two groups
        double I_avg_even = 0;
        double I_avg_odd = 0;
        double I_avg_even_h = 0;
        double I_avg_odd_h = 0;
        double I_avg_even_k = 0;
        double I_avg_odd_k = 0;
        double I_avg_even_l = 0;
        double I_avg_odd_l = 0;

        calc_avg_two_halves(I_full_group, SE_norm, avg_mode_, I_avg_even, I_avg_odd);

        //select reflections on h axis
        scitbx::af::shared<double> I_full_group_h;
        scitbx::af::shared<double> SE_norm_h;
        scitbx::af::shared<double> I_full_group_k;
        scitbx::af::shared<double> SE_norm_k;
        scitbx::af::shared<double> I_full_group_l;
        scitbx::af::shared<double> SE_norm_l;
        for (int i = 0; i < I_full_group.size(); i++) {
          if (current_index_ori[i][0] == 0) {
            I_full_group_h.push_back(I_full_group[i]);
            SE_norm_h.push_back(SE_norm[i]);
          }

          if (current_index_ori[i][1] == 0) {
            I_full_group_k.push_back(I_full_group[i]);
            SE_norm_k.push_back(SE_norm[i]);
          }

          if (current_index_ori[i][2] == 0) {
            I_full_group_l.push_back(I_full_group[i]);
            SE_norm_l.push_back(SE_norm[i]);
          }
        }

        calc_avg_two_halves(I_full_group_h, SE_norm_h, avg_mode_, I_avg_even_h, I_avg_odd_h);
        calc_avg_two_halves(I_full_group_k, SE_norm_k, avg_mode_, I_avg_even_k, I_avg_odd_k);
        calc_avg_two_halves(I_full_group_l, SE_norm_l, avg_mode_, I_avg_even_l, I_avg_odd_l);

        // save the results for this group
        results.miller_index.push_back(current_index);
        results.I_avg.push_back(I_avg);
        results.sigI_avg.push_back(sigI_avg);
        results.r_meas_w_top.push_back(r_meas_w_top);
        results.r_meas_w_btm.push_back(r_meas_w_btm);
        results.r_meas_top.push_back(r_meas_top);
        results.r_meas_btm.push_back(r_meas_btm);
        results.multiplicity.push_back(multiplicity);
        results.I_avg_even.push_back(I_avg_even);
        results.I_avg_odd.push_back(I_avg_odd);
        results.I_avg_even_h.push_back(I_avg_even_h);
        results.I_avg_odd_h.push_back(I_avg_odd_h);
        results.I_avg_even_k.push_back(I_avg_even_k);
        results.I_avg_odd_k.push_back(I_avg_odd_k);
        results.I_avg_even_l.push_back(I_avg_even_l);
        results.I_avg_odd_l.push_back(I_avg_odd_l);

        if (flag_output_verbose_) {
          txt_obs_out << "    I_o        sigI_o    G      B     Eoc      rs    lambda rocking(deg) W     I_full     sigI_full\n";
          for (int i = 0; i < I_full_group.size(); i++) {
            char buf[512];
            sprintf(buf, "%10.2f %10.2f %6.2f %6.2f %6.2f %8.5f %8.5f %8.5f %6.2f %10.2f %10.2f\n",
              I_group[i],sigI_group[i],1/G_[valid_ptrs[i]],B_[valid_ptrs[i]],p_set_[valid_ptrs[i]],rs_set_[valid_ptrs[i]],
              wavelength_set_[valid_ptrs[i]],mosaic_radian_set[valid_ptrs[i]]*180/scitbx::constants::pi,SE_norm[i],
              I_full_group[i],sigI_full[i]);
            txt_obs_out << buf;
          }
          char buf[512];
          sprintf(buf, "Merged I, sigI: %6.2f, %6.2f\n",I_avg,sigI_avg);
          txt_obs_out << buf;
          sprintf(buf, "Rmeas: %6.2f Qw: %6.2f\n",r_meas,r_meas_w);
          txt_obs_out << buf;
          sprintf(buf, "No. total observed: %4.0f No. after rejection: %4.0f\n", double(obs_ptr-obs_ptr_start), double(I_full_group.size()));
          txt_obs_out << buf;
          txt_obs_out << "List of rejected observations:\n";
          txt_obs_out << txt_reject_out_group.str();
        }
        txt_reject_out << txt_reject_out_group.str();
      }
      results.txt_obs_out = txt_obs_out.str();
      results.txt_reject_out = txt_reject_out.str();

      return results;
    }
  };

const double averaging_engine::CONST_SE_MIN_WEIGHT = 0.17;
const double averaging_engine::CONST_SE_MAX_WEIGHT = 1.0;
const double averaging_engine::CONST_SIG_I_FACTOR = 1.5;

namespace boost_python { namespace {
  void
  init_module() {
    using namespace boost::python;
    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;
    typedef averaging_engine w_t;

    class_<w_t>("averaging_engine", no_init)
      .def(init<int,scitbx::af::shared<int>,
        const shared_miller&,const shared_miller&,
        const scitbx::af::shared<double>&,
        const scitbx::af::shared<double>&,
        const scitbx::af::shared<double>&,
        const scitbx::af::shared<double>&,
        const scitbx::af::shared<double>&,
        const scitbx::af::shared<double>&,
        const scitbx::af::shared<double>&,
        const scitbx::af::shared<double>&,
        const scitbx::af::shared<double>&,
        const scitbx::af::shared<std::string>&
        >(
        (arg("group_no"),arg("group_id_list"),
        arg("miller_list"),arg("miller_list_ori"),arg("I"),arg("sigI"),
        arg("G"),arg("B"),arg("p_set"),arg("rs_set"),
        arg("wavelength_set"),arg("sin_theta_over_lambda_sq"),arg("SE"),
        arg("pickle_filename_set")
        )))
      .def("calc_avg_I", &averaging_engine::calc_avg_I)
      .add_property("avg_mode",
        make_getter(&averaging_engine::avg_mode_, rbv()),
        make_setter(&averaging_engine::avg_mode_, dcp()))
      .add_property("sigma_max",
        make_getter(&averaging_engine::sigma_max_, rbv()),
        make_setter(&averaging_engine::sigma_max_, dcp()))
      .add_property("flag_volume_correction",
        make_getter(&averaging_engine::flag_volume_correction_, rbv()),
        make_setter(&averaging_engine::flag_volume_correction_, dcp()))
      .add_property("n_rejection_cycle",
        make_getter(&averaging_engine::n_rejection_cycle_, rbv()),
        make_setter(&averaging_engine::n_rejection_cycle_, dcp()))
      .add_property("flag_output_verbose",
        make_getter(&averaging_engine::flag_output_verbose_, rbv()),
        make_setter(&averaging_engine::flag_output_verbose_, dcp()))
      ;

    class_<average_result_store>("average_result_store",init<>())
      .add_property("miller_index",
        make_getter(&average_result_store::miller_index, rbv()),
        make_setter(&average_result_store::miller_index, dcp()))
      .add_property("I_avg",
        make_getter(&average_result_store::I_avg, rbv()),
        make_setter(&average_result_store::I_avg, dcp()))
      .add_property("sigI_avg",
        make_getter(&average_result_store::sigI_avg, rbv()),
        make_setter(&average_result_store::sigI_avg, dcp()))
      .add_property("r_meas_w_top",
        make_getter(&average_result_store::r_meas_w_top, rbv()),
        make_setter(&average_result_store::r_meas_w_top, dcp()))
      .add_property("r_meas_w_btm",
        make_getter(&average_result_store::r_meas_w_btm, rbv()),
        make_setter(&average_result_store::r_meas_w_btm, dcp()))
      .add_property("r_meas_top",
        make_getter(&average_result_store::r_meas_top, rbv()),
        make_setter(&average_result_store::r_meas_top, dcp()))
      .add_property("r_meas_btm",
        make_getter(&average_result_store::r_meas_btm, rbv()),
        make_setter(&average_result_store::r_meas_btm, dcp()))
      .add_property("multiplicity",
        make_getter(&average_result_store::multiplicity, rbv()),
        make_setter(&average_result_store::multiplicity, dcp()))
      .add_property("I_avg_even",
        make_getter(&average_result_store::I_avg_even, rbv()),
        make_setter(&average_result_store::I_avg_even, dcp()))
      .add_property("I_avg_odd",
        make_getter(&average_result_store::I_avg_odd, rbv()),
        make_setter(&average_result_store::I_avg_odd, dcp()))
      .add_property("I_avg_even_h",
        make_getter(&average_result_store::I_avg_even_h, rbv()),
        make_setter(&average_result_store::I_avg_even_h, dcp()))
      .add_property("I_avg_odd_h",
        make_getter(&average_result_store::I_avg_odd_h, rbv()),
        make_setter(&average_result_store::I_avg_odd_h, dcp()))
      .add_property("I_avg_even_k",
        make_getter(&average_result_store::I_avg_even_k, rbv()),
        make_setter(&average_result_store::I_avg_even_k, dcp()))
      .add_property("I_avg_odd_k",
        make_getter(&average_result_store::I_avg_odd_k, rbv()),
        make_setter(&average_result_store::I_avg_odd_k, dcp()))
      .add_property("I_avg_even_l",
        make_getter(&average_result_store::I_avg_even_l, rbv()),
        make_setter(&average_result_store::I_avg_even_l, dcp()))
      .add_property("I_avg_odd_l",
        make_getter(&average_result_store::I_avg_odd_l, rbv()),
        make_setter(&average_result_store::I_avg_odd_l, dcp()))
      .add_property("txt_obs_out",
        make_getter(&average_result_store::txt_obs_out, rbv()),
        make_setter(&average_result_store::txt_obs_out, dcp()))
      .add_property("txt_reject_out",
        make_getter(&average_result_store::txt_reject_out, rbv()),
        make_setter(&average_result_store::txt_reject_out, dcp()))
    ;
  };

  using namespace boost::python;

  void export_average_mode()
  {
    enum_<Average_Mode>("Average_Mode")
      .value("Average", Average)
      .value("Weighted", Weighted)
      .value("Final", Final);
  }

}}} // namespace prime::boost_python::<anonymous>

BOOST_PYTHON_MODULE(prime_ext)
{
  prime::boost_python::init_module();
  prime::boost_python::export_average_mode();

}


#include <iostream>
#include <cmath>
#include <complex>
#include <Eigen/Core>

#include "celerite/celerite.h"
#include "celerite/carma.h"
#include "celerite/utils.h"

// Timer for the benchmark.
#if defined(_MSC_VER)
    //no sys/time.h in visual c++
    //http://jakascorner.com/blog/2016/04/time-measurement.html
    #include <chrono>
    double get_timestamp ()
    {
      using micro_s = std::chrono::microseconds;
      auto tnow = std::chrono::steady_clock::now();
      auto d_micro = std::chrono::duration_cast<micro_s>(tnow.time_since_epoch()).count();
      return double(d_micro) * 1.0e-6;
    }
#else
   //no std::chrono in g++ 4.8
   #include <sys/time.h>
   double get_timestamp ()
   {
     struct timeval now;
     gettimeofday (&now, NULL);
     return double(now.tv_usec) * 1.0e-6 + double(now.tv_sec);
   }
#endif

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

  size_t max_terms = 10;
  if (argc >= 2) max_terms = atoi(argv[1]);
  size_t N = pow(2, 16);
  if (argc >= 3) N = atoi(argv[2]);
  size_t niter = 5;
  if (argc >= 4) niter = atoi(argv[3]);

  double sigma = 1.0;

  // Generate some fake data.
  double jitter = 0.0;
  Eigen::VectorXd x = Eigen::VectorXd::Random(N),
                  yerr = Eigen::VectorXd::Random(N),
                  y, diag;
  yerr.array() *= 0.1;
  yerr.array() += 1.0;
  diag = yerr.array() * yerr.array();
  std::sort(x.data(), x.data() + x.size());
  y = sin(x.array());

  double carma_ll, celerite_ll, strt;
  Eigen::MatrixXd compute_times(max_terms, 3);
  compute_times.setConstant(0.0);
  for (size_t nterms = 1; nterms <= max_terms; ++nterms) {
    Eigen::VectorXd carma_arparams,
                    carma_maparams,
                    alpha_real, beta_real, alpha_complex_real, alpha_complex_imag,
                    beta_complex_real, beta_complex_imag;

    compute_times(nterms - 1, 0) = nterms;

    bool is_ok = false;
    while (!is_ok) {
      // Resample the parameters until good ones are chosen
      carma_arparams = Eigen::VectorXd::Random(nterms);
      carma_maparams = Eigen::VectorXd::Random(nterms-1);
      carma_arparams.array() += 1.0;
      carma_maparams.array() += 1.0;
      carma_arparams.array() /= 2.0 * nterms;
      carma_maparams.array() /= 2.0 * nterms;
      celerite::carma::CARMASolver carma_solver(0.0, carma_arparams, carma_maparams);
      carma_solver.get_celerite_coeffs(alpha_real, beta_real,
        alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag);

      is_ok = celerite::check_coefficients(alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag);
    }

    // Compute using the CARMA model.
    strt = get_timestamp();
    for (size_t i = 0; i < niter; ++i) {
      celerite::carma::CARMASolver carma_solver(0.0, carma_arparams, carma_maparams);
      carma_solver.setup();
      carma_ll = carma_solver.log_likelihood(x, y, yerr);
    }
    compute_times(nterms - 1, 1) = (get_timestamp() - strt) / niter;

    // Get the celerite parameters for the CARMA model.
    celerite::carma::CARMASolver carma_solver(0.0, carma_arparams, carma_maparams);
    carma_solver.get_celerite_coeffs(alpha_real, beta_real,
      alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag);

    // Compute using the celerite model.
    celerite::solver::CholeskySolver<double> solver;
    strt = get_timestamp();
    for (size_t i = 0; i < niter; ++i) {
      solver.compute(jitter, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x, diag);
      celerite_ll = -0.5*(solver.dot_solve(y) + solver.log_determinant() + x.rows() * log(2.0 * M_PI));
    }

    std::cerr << nterms << " " << carma_ll << " " << celerite_ll << std::endl;
  }

  std::cout<< compute_times << std::endl;

  return 0;
}


//-----------------------------------------------------------------------------
// Created on: 03 December 2016
//-----------------------------------------------------------------------------
// Copyright (c) 2017, Sergey Slyadnev
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder(s) nor the
//      names of all contributors may be used to endorse or promote products
//      derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//-----------------------------------------------------------------------------

// Own include
#include <asiAlgo_PlaneOnPoints.h>

// Eigen includes
#pragma warning(disable : 4701 4702)
#include <Eigen/Dense>
#pragma warning(default : 4701 4702)

#undef COUT_DEBUG
#if defined COUT_DEBUG
  #pragma message("===== warning: COUT_DEBUG is enabled")
#endif

//-----------------------------------------------------------------------------

bool compare(const std::pair<double, int>& p1, const std::pair<double, int>& p2)
{
  return p1.first > p2.first;
}

//-----------------------------------------------------------------------------

asiAlgo_PlaneOnPoints::asiAlgo_PlaneOnPoints(ActAPI_ProgressEntry progress,
                                             ActAPI_PlotterEntry  plotter)
: ActAPI_IAlgorithm(progress, plotter)
{}

//-----------------------------------------------------------------------------

bool asiAlgo_PlaneOnPoints::Build(const std::vector<gp_XYZ>& points,
                                  gp_Pln&                    result) const
{
  return this->internalBuild(points, result);
}

//-----------------------------------------------------------------------------

bool asiAlgo_PlaneOnPoints::Build(const Handle(asiAlgo_BaseCloud<double>)& points,
                                  gp_Pln&                                  result) const
{
  // Repack point cloud to a vector.
  std::vector<gp_XYZ> pointsVec;
  //
  for ( int k = 0; k < points->GetNumberOfElements(); ++k )
    pointsVec.push_back( points->GetElement(k) );

  return this->internalBuild(pointsVec, result);
}

//-----------------------------------------------------------------------------

bool asiAlgo_PlaneOnPoints::internalBuild(const std::vector<gp_XYZ>& points,
                                          gp_Pln&                    result) const
{
  const int nPts = (int) points.size();

  /* ======================
   *  Calculate mean point
   * ====================== */

  gp_XYZ mu;
  for ( size_t i = 0; i < points.size(); ++i )
  {
    mu += points[i];
  }
  mu /= nPts;

  /* =========================
   *  Build covariance matrix
   * ========================= */

  Eigen::Matrix3d C;
  for ( int j = 1; j <= 3; ++j )
  {
    for ( int k = 1; k <= 3; ++k )
    {
      C(j-1, k-1) = 0.0; // TODO: is that necessary?
    }
  }

  for ( size_t i = 0; i < points.size(); ++i )
  {
    const gp_XYZ& p      = points[i];
    gp_XYZ        p_dash = p - mu;

    for ( int j = 1; j <= 3; ++j )
    {
      for ( int k = 1; k <= 3; ++k )
      {
        C(j-1, k-1) += ( p_dash.Coord(j)*p_dash.Coord(k) );
      }
    }
  }

  for ( int j = 1; j <= 3; ++j )
  {
    for ( int k = 1; k <= 3; ++k )
    {
      C(j-1, k-1) /= nPts;
    }
  }

  Eigen::EigenSolver<Eigen::Matrix3d> EigenSolver(C);

#if defined COUT_DEBUG
  std::cout << "\tCovariance matrix: " << std::endl << C << std::endl;
  std::cout << "\tThe eigen values of C are:" << std::endl << EigenSolver.eigenvalues() << std::endl;
  std::cout << "\tThe matrix of eigenvectors, V, is:" << std::endl << EigenSolver.eigenvectors() << std::endl << std::endl;
#endif

  Eigen::Vector3cd v1 = EigenSolver.eigenvectors().col(0);
  Eigen::Vector3cd v2 = EigenSolver.eigenvectors().col(1);
  Eigen::Vector3cd v3 = EigenSolver.eigenvectors().col(2);

  gp_Vec V[3] = { gp_Vec( v1.x().real(), v1.y().real(), v1.z().real() ),
                  gp_Vec( v2.x().real(), v2.y().real(), v2.z().real() ),
                  gp_Vec( v3.x().real(), v3.y().real(), v3.z().real() ) };
  //
  std::vector< std::pair<double, int> >
    lambda { std::pair<double, int>( EigenSolver.eigenvalues()(0).real(), 0 ),
             std::pair<double, int>( EigenSolver.eigenvalues()(1).real(), 1 ),
             std::pair<double, int>( EigenSolver.eigenvalues()(2).real(), 2 ) };
  //
  std::sort(lambda.begin(), lambda.end(), compare);
  //
  gp_Ax1 ax_X(mu, V[lambda[0].second]);
  gp_Ax1 ax_Y(mu, V[lambda[1].second]);
  gp_Ax1 ax_Z(mu, V[lambda[2].second]);
  //
  gp_Vec vec_X( ax_X.Direction() );
  gp_Vec vec_Y( ax_Y.Direction() );
  gp_Vec vec_Z( ax_Z.Direction() );
  //
  if ( (vec_X ^ vec_Y).Magnitude() < gp::Resolution() ||
       (vec_X ^ vec_Z).Magnitude() < gp::Resolution() ||
       (vec_Y ^ vec_Z).Magnitude() < gp::Resolution() )
  {
    std::cout << "Warning: degenerated normal" << std::endl;
    return false; // Degenerated normal
  }

  // Check if the system is right-handed
  const double ang = ax_X.Direction().AngleWithRef( ax_Y.Direction(), ax_Z.Direction() );
  if ( ang < 0 )
  {
    gp_Ax1 tmp = ax_X;
    ax_X = ax_Y;
    ax_Y = tmp;
  }

  // Store results
  gp_Ax3 ax3( gp_Pnt(mu), ax_Z.Direction(), ax_X.Direction() );
  result.SetPosition(ax3);
  //
  return true;
}


#pragma once
#include <Eigen/Core>
#include <Eigen/Dense>
#include <igl/boundary_facets.h>
#include <tuple>

//! Creates a list of boundary edges for the given triangular mesh
Eigen::MatrixXi getBoundaryEdges(const Eigen::MatrixXd &vertices,
                                 const Eigen::MatrixXi &triangles) {
	std::ignore = vertices;

	Eigen::MatrixXi boundaryEdgeIndices;
	igl::boundary_facets(triangles, boundaryEdgeIndices);
	return boundaryEdgeIndices;
}


// Soccersim_test.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <random>
#define BOOST_TEST_MAIN
#include "boost\test\unit_test.hpp"
#include "Soccersim_lib\sim_game.h"
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <iostream>

using namespace boost::accumulators;

BOOST_AUTO_TEST_CASE(sim_game_dummy_test) {

	//for (int i = 0; i < 1; i++) {
	//	soccersim::result a = soccersim::sim_game(70, 70);
	//	std::cout << a.home_goals << "-" << a.away_goals << std::endl;
	//	//std::cout << a.home_mean << "-" << a.away_mean << std::endl;
	//}

	//for (int i = 0; i < 10; i++) {
	//	soccersim::result b = soccersim::sim_game(70 + i * 5, 70 - i * 5);
	//	std::cout << b.home_goals << "-" << b.away_goals << std::endl;
	//	std::cout << b.home_mean << "-" << b.away_mean << std::endl;
	//}

	//int w = 0;
	//int d = 0;
	//int l = 0;

	//for (int i = 0; i < 10000; i++) {
	//	soccersim::result a = soccersim::sim_game(70, 70);
	//	if (a.home_goals > a.away_goals) {
	//		w++;
	//	}
	//	else if (a.home_goals == a.away_goals) {
	//		d++;
	//	}
	//	else {
	//		l++;
	//	}
	//}

	//std::cout << "70-70" << "wins: " << w << " draws: " << d << " loses: " << l << std::endl;

	//w = 0;
	//d = 0;
	//l = 0;

	//for (int i = 0; i < 10000; i++) {
	//	soccersim::result a = soccersim::sim_game(75, 65);
	//	if (a.home_goals > a.away_goals) {
	//		w++;
	//	}
	//	else if (a.home_goals == a.away_goals) {
	//		d++;
	//	}
	//	else {
	//		l++;
	//	}
	//}

	//std::cout << "75-65" << "wins: " << w << " draws: " << d << " loses: " << l << std::endl;

	//w = 0;
	//d = 0;
	//l = 0;

	//for (int i = 0; i < 10000; i++) {
	//	soccersim::result a = soccersim::sim_game(80, 60);
	//	if (a.home_goals > a.away_goals) {
	//		w++;
	//	}
	//	else if (a.home_goals == a.away_goals) {
	//		d++;
	//	}
	//	else {
	//		l++;
	//	}
	//}

	//std::cout << "80-60" << "wins: " << w << " draws: " << d << " loses: " << l << std::endl;

	//w = 0;
	//d = 0;
	//l = 0;

	//for (int i = 0; i < 10000; i++) {
	//	soccersim::result a = soccersim::sim_game(85, 55);
	//	if (a.home_goals > a.away_goals) {
	//		w++;
	//	}
	//	else if (a.home_goals == a.away_goals) {
	//		d++;
	//	}
	//	else {
	//		l++;
	//	}
	//}

	//std::cout << "85-55" << "wins: " << w << " draws: " << d << " loses: " << l << std::endl;

	//w = 0;
	//d = 0;
	//l = 0;

	//for (int i = 0; i < 10000; i++) {
	//	soccersim::result a = soccersim::sim_game(86, 55);
	//	if (a.home_goals > a.away_goals) {
	//		w++;
	//	}
	//	else if (a.home_goals == a.away_goals) {
	//		d++;
	//	}
	//	else {
	//		l++;
	//	}
	//}

	//std::cout << "86-55" << "wins: " << w << " draws: " << d << " loses: " << l << std::endl;

	//w = 0;
	//d = 0;
	//l = 0;

	//for (int i = 0; i < 10000; i++) {
	//	soccersim::result a = soccersim::sim_game(87, 55);
	//	if (a.home_goals > a.away_goals) {
	//		w++;
	//	}
	//	else if (a.home_goals == a.away_goals) {
	//		d++;
	//	}
	//	else {
	//		l++;
	//	}
	//}

	//std::cout << "87-55" << "wins: " << w << " draws: " << d << " loses: " << l << std::endl;

	//w = 0;
	//d = 0;
	//l = 0;

	//for (int i = 0; i < 10000; i++) {
	//	soccersim::result a = soccersim::sim_game(90, 50);
	//	if (a.home_goals > a.away_goals) {
	//		w++;
	//	}
	//	else if (a.home_goals == a.away_goals) {
	//		d++;
	//	}
	//	else {
	//		l++;
	//	}
	//}

	//std::cout << "90-50" << "wins: " << w << " draws: " << d << " loses: " << l << std::endl;

	//w = 0;
	//d = 0;
	//l = 0;

	//for (int i = 0; i < 10000; i++) {
	//	soccersim::result a = soccersim::sim_game(91, 49);
	//	if (a.home_goals > a.away_goals) {
	//		w++;
	//	}
	//	else if (a.home_goals == a.away_goals) {
	//		d++;
	//	}
	//	else {
	//		l++;
	//	}
	//}

	//std::cout << "91-49" << "wins: " << w << " draws: " << d << " loses: " << l << std::endl;

}

BOOST_AUTO_TEST_CASE(sim_game_call_does_not_return_an_exception) {

	soccersim::simulator sim;
	soccersim::team team_a(34, "a");
	soccersim::team team_b(3, "b");
	BOOST_CHECK_NO_THROW(sim.sim_game(team_a, team_b));

}

BOOST_AUTO_TEST_CASE(sim_game_does_not_return_negative_goals) {

	soccersim::simulator sim;
	bool found_negative = false;
	std::default_random_engine generator;
	std::uniform_int_distribution<int> distribution(0, 99);

	for (int i = 0; i < 10000; i++) {
		soccersim::team team_a(distribution(generator), "a");
		soccersim::team team_b(distribution(generator), "b");
		soccersim::result result = sim.sim_game(team_a, team_b);
		if (result.home_goals < 0 || result.away_goals < 0) {
			found_negative = true;
		}
	}
	BOOST_CHECK_EQUAL(found_negative, false);

}

BOOST_AUTO_TEST_CASE(sim_game_does_not_return_outrageous_results) {

	soccersim::simulator sim;
	int super_losses = 0;
	for (int i = 0; i < 10000; i++) {
		soccersim::team team_a(99, "a");
		soccersim::team team_b(0, "b");
		soccersim::result result = sim.sim_game(team_a, team_b);
		if (result.home_goals < result.away_goals ) {
			super_losses++;
		}
	}
	BOOST_CHECK_LE(super_losses, 5);

}

BOOST_AUTO_TEST_CASE(sim_game_returns_reasonable_results) {

	soccersim::simulator sim;
	int home_wins = 0;
	int away_wins = 0;
	std::default_random_engine generator;
	std::uniform_int_distribution<int> distribution(0, 99);

	for (int i = 0; i < 10000; i++) {
		soccersim::team team_a(distribution(generator), "a");
		soccersim::result result = sim.sim_game(team_a, team_a);
		if (result.home_goals < result.away_goals) {
			away_wins++;
		} else if(result.home_goals > result.away_goals) {
			home_wins++;
		}
	}

	BOOST_CHECK_LE(abs(home_wins - away_wins), 500);

}

BOOST_AUTO_TEST_CASE(sim_game_does_not_tie_too_much) {

	soccersim::simulator sim;
	int draws = 0;
	std::default_random_engine generator;
	std::uniform_int_distribution<int> distribution(0, 99);

	for (int i = 0; i < 10000; i++) {
		soccersim::team team_a(distribution(generator), "a");
		soccersim::result result = sim.sim_game(team_a, team_a);
		if (result.home_goals == result.away_goals) {
			draws++;
		}
	}

	BOOST_CHECK_LE(draws, 3000);

}

BOOST_AUTO_TEST_CASE(sim_game_returns_good_mean) {

	soccersim::simulator sim;
	accumulator_set<double, stats<tag::mean > > acc;
	std::default_random_engine generator;
	std::uniform_int_distribution<int> distribution(50, 99);

	for (int i = 0; i < 10000; i++) {
		soccersim::team team_a(distribution(generator), "a");
		soccersim::team team_b(distribution(generator), "b");
		soccersim::result result = sim.sim_game(team_a, team_b);
		acc(result.away_goals + result.home_goals);
	}

	//BOOST_CHECK_GE(mean(acc), 2);
	//BOOST_CHECK_LE(mean(acc), 3);
	BOOST_CHECK_CLOSE(2.5, mean(acc), 20);

}

BOOST_AUTO_TEST_CASE(sim_game_returns_good_variance) {

	soccersim::simulator sim;
	accumulator_set<double, stats<tag::variance > > acc;
	std::default_random_engine generator;
	std::uniform_int_distribution<int> distribution(50, 99);

	for (int i = 0; i < 10000; i++) {
		soccersim::team team_a(distribution(generator), "a");
		soccersim::team team_b(distribution(generator), "b");
		soccersim::result result = sim.sim_game(team_a, team_b);
		acc(result.away_goals + result.home_goals);
	}

	BOOST_CHECK_CLOSE(2.5, variance(acc), 20);

}


/*
 * test_DIFF.cpp
 *
 *  Created on: 2013-4-18
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <hikyuu/indicator/crt/DIFF.h>
#include <hikyuu/indicator/crt/PRICELIST.h>

using namespace hku;

/**
 * @defgroup test_indicator_DIFF test_indicator_DIFF
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_DIFF ) {
    /** @arg 正常测试 */
    PriceList d;
    for (size_t i = 0; i < 10; ++i) {
        d.push_back(i);
    }

    Indicator ind = PRICELIST(d);
    Indicator diff = DIFF(ind);
    BOOST_CHECK(diff.size() == 10);
    BOOST_CHECK(diff.discard() == 1);
    BOOST_CHECK(diff[0] == Null<price_t>());
    for (size_t i = 1; i < 10; ++i) {
        BOOST_CHECK(diff[i] == d[i] - d[i-1]);
    }

    /** @arg operator */
    diff = DIFF();
    Indicator expect = DIFF(ind);
    Indicator result = diff(ind);
    BOOST_CHECK(expect.size() == result.size());
    for (size_t i = 0; i < expect.size(); ++i) {
        BOOST_CHECK(result[i] == expect[i]);
    }
}

/** @} */


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

  @copyright 2016 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_HPP_INCLUDED

namespace boost { namespace simd
{
    /*!
      @ingroup group-functions
      @defgroup group-constant Constant Functions

      These functions provides scalar and SIMD constant generators for the most usual
      constant values such as zero, numeric limits, IEEE special values.

      Constant generator functions take no arguments but require a template type parameter
      so that the correct type of the required constant is returned.
    **/

} }

#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/constant/bitincrement.hpp>
#include <boost/simd/constant/cgold.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/deginrad.hpp>
#include <boost/simd/constant/eight.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/euler.hpp>
#include <boost/simd/constant/exp_1.hpp>
#include <boost/simd/constant/expnibig.hpp>
#include <boost/simd/constant/expnibig.hpp>
#include <boost/simd/constant/fact_10.hpp>
#include <boost/simd/constant/fact_11.hpp>
#include <boost/simd/constant/fact_12.hpp>
#include <boost/simd/constant/fact_4.hpp>
#include <boost/simd/constant/fact_5.hpp>
#include <boost/simd/constant/fact_6.hpp>
#include <boost/simd/constant/fact_7.hpp>
#include <boost/simd/constant/fact_8.hpp>
#include <boost/simd/constant/fact_9.hpp>
#include <boost/simd/constant/false.hpp>
#include <boost/simd/constant/five.hpp>
#include <boost/simd/constant/four.hpp>
#include <boost/simd/constant/fouropi.hpp>
#include <boost/simd/constant/fourpi.hpp>
#include <boost/simd/constant/fourthrooteps.hpp>
#include <boost/simd/constant/gold.hpp>
#include <boost/simd/constant/greatestnoninteger.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/inv2pi.hpp>
#include <boost/simd/constant/invexp_1.hpp>
#include <boost/simd/constant/invlog10_2.hpp>
#include <boost/simd/constant/invlog_10.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/constant/invpi.hpp>
#include <boost/simd/constant/invsqrt_2pi.hpp>
#include <boost/simd/detail/constant/ldexpmask.hpp>
#include <boost/simd/detail/constant/limitexponent.hpp>
#include <boost/simd/detail/constant/log10_2hi.hpp>
#include <boost/simd/detail/constant/log10_2lo.hpp>
#include <boost/simd/detail/constant/log10_ehi.hpp>
#include <boost/simd/detail/constant/log10_elo.hpp>
#include <boost/simd/constant/log_10.hpp>
#include <boost/simd/constant/log10_pi.hpp>
#include <boost/simd/constant/log2_e.hpp>
#include <boost/simd/constant/log2_em1.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/log_2olog_10.hpp>
#include <boost/simd/constant/logeps.hpp>
#include <boost/simd/constant/logpi.hpp>
#include <boost/simd/constant/logsqrt2pi.hpp>
#include <boost/simd/detail/constant/mantissamask.hpp>
#include <boost/simd/detail/constant/mask1frexp.hpp>
#include <boost/simd/detail/constant/mask2frexp.hpp>
#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/detail/constant/maxexponentm1.hpp>
#include <boost/simd/detail/constant/maxflint.hpp>
#include <boost/simd/detail/constant/maxinit.hpp>
#include <boost/simd/detail/constant/maxleftshift.hpp>
#include <boost/simd/detail/constant/maxlog10.hpp>
#include <boost/simd/detail/constant/maxlog2.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/detail/constant/medium_pi.hpp>
#include <boost/simd/constant/meight.hpp>
#include <boost/simd/constant/mfive.hpp>
#include <boost/simd/constant/mfour.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/detail/constant/minexponent.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/detail/constant/minlog10.hpp>
#include <boost/simd/detail/constant/minlog2.hpp>
#include <boost/simd/detail/constant/minlog.hpp>
#include <boost/simd/constant/mlog10two2nmb.hpp>
#include <boost/simd/constant/mlog2two2nmb.hpp>
#include <boost/simd/constant/mlogeps2.hpp>
#include <boost/simd/constant/mlogtwo2nmb.hpp>
#include <boost/simd/constant/mnine.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/mseven.hpp>
#include <boost/simd/constant/msix.hpp>
#include <boost/simd/constant/mten.hpp>
#include <boost/simd/constant/mthree.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/nbdigits.hpp>
#include <boost/simd/constant/nbexponentbits.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/constant/nine.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/oneosqrt5.hpp>
#include <boost/simd/constant/oneosqrteps.hpp>
#include <boost/simd/constant/oneotwoeps.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/detail/constant/pio2_1.hpp>
#include <boost/simd/detail/constant/pio2_1t.hpp>
#include <boost/simd/detail/constant/pio2_2.hpp>
#include <boost/simd/detail/constant/pio2_2t.hpp>
#include <boost/simd/constant/pio2_3.hpp>
#include <boost/simd/detail/constant/pio2_3t.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/detail/constant/pio_2lo.hpp>
#include <boost/simd/constant/pio_3.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/detail/constant/pio_4lo.hpp>
#include <boost/simd/constant/pio_6.hpp>
#include <boost/simd/detail/constant/pix2_1.hpp>
#include <boost/simd/detail/constant/pix2_2.hpp>
#include <boost/simd/detail/constant/pix2_3.hpp>
#include <boost/simd/detail/constant/powlargelim.hpp>
#include <boost/simd/detail/constant/powlowlim.hpp>
#include <boost/simd/constant/quarter.hpp>
#include <boost/simd/constant/radindeg.hpp>
#include <boost/simd/detail/constant/radindegr.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/real.hpp>
#include <boost/simd/constant/seven.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/constant/six.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/detail/constant/splitfactor.hpp>
#include <boost/simd/constant/sqrt_1o_5.hpp>
#include <boost/simd/constant/sqrt_2.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/simd/constant/sqrt_2o_3.hpp>
#include <boost/simd/constant/sqrt_2opi.hpp>
#include <boost/simd/constant/sqrt_2pi.hpp>
#include <boost/simd/constant/sqrteps.hpp>
#include <boost/simd/constant/sqrtpio_2.hpp>
#include <boost/simd/constant/sqrtsmallestposval.hpp>
#include <boost/simd/constant/sqrtvalmax.hpp>
#include <boost/simd/constant/tan_3pio_8.hpp>
#include <boost/simd/constant/tanpio_8.hpp>
#include <boost/simd/constant/ten.hpp>
#include <boost/simd/constant/third.hpp>
#include <boost/simd/constant/thirdrooteps.hpp>
#include <boost/simd/constant/threeeps.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/threepio_4.hpp>
#include <boost/simd/constant/true.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/twoopi.hpp>
#include <boost/simd/constant/twopi.hpp>
#include <boost/simd/constant/twopio_3.hpp>
#include <boost/simd/constant/twoto31.hpp>
#include <boost/simd/constant/twotom10.hpp>
#include <boost/simd/constant/twotomnmbo_3.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/zero.hpp>

#endif


/****
   Copyright 2005-2007, Moshe Looks and Novamente LLC

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
****/

#include <iostream>

#include "combo/tree_generation.h"
#include "util/selection.h"
#include <boost/lexical_cast.hpp>

int main(int argc,char** argv) {
  using namespace util;
  using namespace trees;
  using namespace boost;
  using namespace std;

  int max_depth;
  int noe; //number of expressions to generate
  bool tce; //type check enabled
  NodeSelector<string> ss;

  if(argc!=4) {
    cout << "Usage :" << endl <<
      "tree_gen [bool|real|mix] max_depth number_of_expressions" << endl;
    exit(1);
  }

  max_depth = lexical_cast<int>(argv[2]);
  noe = lexical_cast<int>(argv[3]);
  tce = string(argv[1])==string("mix");

  if(string(argv[1])==string("bool") || string(argv[1])==string("mix")) {
    ss.add("and",2,100);
    ss.add("or",2,100);
    ss.add("not",1,100);
  }
  if(string(argv[1])==string("real") || string(argv[1])==string("mix")) {
    //second arg is arity, third is (relative) selection likelihood
    ss.add("+",2,100);
    ss.add("*",2,100);
    ss.add("/",2,100);

    ss.add("log",1,100);
    ss.add("exp",1,100);
    ss.add("sin",1,100);

    ss.add("1",0,100);
    ss.add("0",0,100);

    for (int i=0;i<200;++i)
      ss.add(lexical_cast<string>(randfloat()),0,1);
  }
  if(string(argv[1])==string("mix")) {
    ss.add("contin_if",3,100);
    ss.add("0<",1,100);
    ss.add("impulse",1,100);
  }
  ss.add("#1",0,100);
  ss.add("#2",0,100);
  ss.add("#3",0,100);
  ss.add("#4",0,100);
  ss.add("#5",0,100);

  vector<tree<string> > trees(noe);
  ramped_half_and_half(trees.begin(),trees.end(),ss,2,max_depth,tce);
  for (vector<tree<string> >::const_iterator tr=trees.begin();
       tr!=trees.end();++tr)
    cout << (*tr) << endl;
}


///////////////////////////////////////////////////////////////////////////////
// kernel::scalar::include.hpp                                               //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_KERNELS_SCALAR_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_KERNELS_SCALAR_INCLUDE_HPP_ER_2009

#include <boost/statistics/detail/kernel/kernels/scalar/crtp.hpp>
#include <boost/statistics/detail/kernel/kernels/scalar/gaussian.hpp>

#endif


/*
	COL 719 Assignment 1
	Author - Shreshth Tuli

	Determine if a circuit is combinational or sequential
*/

#include <iostream>
#include <vector>
#include <fstream>

#include <boost/graph/adjacency_list.hpp>

using namespace boost;

typedef boost::adjacency_list<
  boost::listS, boost::vecS,
  boost::directedS
  > Graph;

std::vector<int> adj;
int counter = 0;
bool cycle = false;

void dfs(Graph &graph, int v, int arrival[], int departure[])
{
	arrival[v-1] = counter++;
	adj.clear();

	Graph::out_edge_iterator eit, eend;
	std::tie(eit, eend) = boost::out_edges(v, graph);
	std::for_each(eit, eend, [&graph](Graph::edge_descriptor it)
	{ adj.push_back(boost::target(it, graph)); });

	std::vector<int> cp = adj;
	for (auto i = cp.begin(); i !=cp.end(); ++i){
		if(arrival[*i-1] == -1)
			dfs(graph, *i, arrival, departure);
		else if(departure[*i-1] == -1)
			cycle = true;
	}

	departure[v-1] = counter++;
}

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

	std::ifstream infile(argv[1]);

	int a, b, n;

	infile >> n;

	if(n==0){
		std::cout << "Combinational Circuit";
		return 0;
	}

	Graph g;

	while(infile >> a >> b)
		boost::add_edge(a, b, g);

	int *arrival = new int[n];
	int *departure = new int[n];

    for (int i = 0; i < n; i++){
        arrival[i] = -1;
        departure[i] = -1;
    }

	dfs(g, 1, arrival, departure);

	/*
	std::cout << std::endl;
	for (int i = 0; i < n; i++){
		std::cout << "arrival(" << i+1 << ") = " << arrival[i] << std::endl;
	}
		for (int i = 0; i < n; i++){
		std::cout << "departure(" << i+1 << ") = " << departure[i] << std::endl;
	}
	*/

	if(cycle)
		std::cout << "Sequential Circuit";
	else
		std::cout << "Combinational Circuit";

	return 0;
};


#pragma once
#include <Eigen/Sparse>

#include "mtao/eigen/type_info.hpp"

namespace mtao::eigen {
template <typename Derived>
auto mat_to_triplets(const Derived& V, typename Derived::Scalar thresh) {
    using Scalar = typename Derived::Scalar;
    std::vector<Eigen::Triplet<Scalar>> ret;

    ret.reserve(V.nonZeros());
    if constexpr (is_sparse(V)) {
        for (int k = 0; k < V.outerSize(); ++k) {
            for (typename Derived::InnerIterator it(V, k); it; ++it) {
                if (std::abs(it.value()) >= thresh) {
                    ret.emplace_back(it.row(), it.col(), it.value());
                }
            }
        }
    } else {
        constexpr bool RM = is_row_major(V);
        const int iCount = RM ? V.rows() : V.cols();
        const int jCount = RM ? V.cols() : V.rows();
        for (int i = 0; i < iCount; ++i) {
            for (int j = 0; j < jCount; ++j) {
                auto&& value = RM ? V(i, j) : V(j, i);
                if (std::abs(value) > thresh) {
                    if constexpr (RM) {
                        ret.emplace_back(i, j, value);
                    } else {
                        ret.emplace_back(j, i, value);
                    }
                }
            }
        }
    }
    return ret;
}
template <typename Derived>
auto mat_to_triplets(const Derived& V) {
    using Scalar = typename Derived::Scalar;
    std::vector<Eigen::Triplet<Scalar>> ret;

    ret.reserve(V.nonZeros());
    if constexpr (is_sparse(V)) {
        for (int k = 0; k < V.outerSize(); ++k) {
            for (typename Derived::InnerIterator it(V, k); it; ++it) {
                ret.emplace_back(it.row(), it.col(), it.value());
            }
        }
    } else {
        constexpr bool RM = is_row_major(V);
        const int iCount = RM ? V.rows() : V.cols();
        const int jCount = RM ? V.cols() : V.rows();
        for (int i = 0; i < iCount; ++i) {
            for (int j = 0; j < jCount; ++j) {
                auto&& value = RM ? V(i, j) : V(j, i);
                if (value != 0) {
                    if constexpr (RM) {
                        ret.emplace_back(i, j, value);
                    } else {
                        ret.emplace_back(j, i, value);
                    }
                }
            }
        }
    }
    return ret;
}
}  // namespace mtao::eigen


/**
 * minimisation test
 * @author Tobias Weber <tweber@ill.fr>
 * @date 27-aug-20
 * @license GPLv3, see 'LICENSE' file
 *
 * g++-10 -std=c++20 -I.. -o min min0.cpp ../libs/log.cpp -lMinuit2
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program 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, version 3 of the License.
 *
 * This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#include <string>
#include <iostream>

#define BOOST_TEST_MODULE Min Test
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include "libs/fit.h"

using t_types_real = std::tuple<double, float>;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_expr_real, t_real, t_types_real)
{
	std::string func{"(x-5.67)^2"};

	std::vector<std::string> params{{"x"}};
	std::vector<t_real> vals{{0.}};
	std::vector<t_real> errs{{0.1}};
	std::vector<bool> fixed{false};

	bool ok = tl2::minimise_expr(func, params, vals, errs, &fixed);

	BOOST_TEST(ok);
	BOOST_TEST(tl2::equals<t_real>(vals[0], 5.67, 1e-3));
}


//
//  Copyright (c) 2018-2020, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019-2020, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include "utility.hpp"

BOOST_AUTO_TEST_SUITE(test_tensor_static_rank_arithmetic_operations)

using double_extended = boost::multiprecision::cpp_bin_float_double_extended;

using test_types = zip<int,float,double_extended>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

struct fixture
{
  template<size_t N>
  using extents_t = boost::numeric::ublas::extents<N>;

  std::tuple<
    extents_t<2>, // 1
    extents_t<2>, // 2
    extents_t<3>, // 3
    extents_t<3>, // 4
    extents_t<4>  // 5
    > extents = {
      extents_t<2>{1,1},
      extents_t<2>{2,3},
      extents_t<3>{4,1,3},
      extents_t<3>{4,2,3},
      extents_t<4>{4,2,3,5}
  };
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_binary_arithmetic_operations, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e)
  {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto t  = tensor_t (e);
    auto t2 = tensor_t (e);
    auto r  = tensor_t (e);
    auto v  = value_t  {};

    std::iota(t.begin(), t.end(), v);
    std::iota(t2.begin(), t2.end(), v+2);
    r = t + t + t + t2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), 3*t(i) + t2(i) );

    r = t2 / (t+3) * (t+1) - t2; // r = ( t2/ ((t+3)*(t+1)) ) - t2

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), t2(i) / (t(i)+3)*(t(i)+1) - t2(i) );

    r = 3+t2 / (t+3) * (t+1) * t - t2; // r = 3+( t2/ ((t+3)*(t+1)*t) ) - t2

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), 3+t2(i) / (t(i)+3)*(t(i)+1)*t(i) - t2(i) );

    r = t2 - t + t2 - t;

    for(auto i = 0ul; i < r.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), 4 );

    r = t * t * t * t2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), t(i)*t(i)*t(i)*t2(i) );

    r = (t2/t2) * (t2/t2);

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), 1 );
  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_unary_arithmetic_operations, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e)
  {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto t  = tensor_t (e);
    auto t2 = tensor_t (e);
    auto v  = value_t  {};

    std::iota(t.begin(), t.end(), v);
    std::iota(t2.begin(), t2.end(), v+2);

    tensor_t r1 = t + 2 + t + 2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r1(i), 2*t(i) + 4 );

    tensor_t r2 = 2 + t + 2 + t;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r2(i), 2*t(i) + 4 );

    tensor_t r3 = (t-2) + (t-2);

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r3(i), 2*t(i) - 4 );

    tensor_t r4 = (t*2) * (3*t);

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r4(i), 2*3*t(i)*t(i) );

    tensor_t r5 = (t2*2) / (2*t2) * t2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r5(i), (t2(i)*2) / (2*t2(i)) * t2(i) );

    tensor_t r6 = (t2/2+1) / (2/t2+1) / t2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r6(i), (t2(i)/2+1) / (2/t2(i)+1) / t2(i) );

  };

  for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_assign_arithmetic_operations, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e)
  {
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto t  = tensor_t (e);
    auto t2 = tensor_t (e);
    auto r  = tensor_t (e);
    auto v  = value_t  {};

    std::iota(t.begin(), t.end(), v);
    std::iota(t2.begin(), t2.end(), v+2);

    r  = t + 2;
    r += t;
    r += 2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), 2*t(i) + 4 );

    r  = 2 + t;
    r += t;
    r += 2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), 2*t(i) + 4 );

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), 2*t(i) + 4 );

    r = (t-2);
    r += t;
    r -= 2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), 2*t(i) - 4 );

    r  = (t*2);
    r *= 3;
    r *= t;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), 2*3*t(i)*t(i) );

    r  = (t2*2);
    r /= 2;
    r /= t2;
    r *= t2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), (t2(i)*2) / (2*t2(i)) * t2(i) );

    r  = (t2/2+1);
    r /= (2/t2+1);
    r /= t2;

    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( r(i), (t2(i)/2+1) / (2/t2(i)+1) / t2(i) );

    tensor_t q = -r;
    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( q(i), -r(i) );

    tensor_t p = +r;
    for(auto i = 0ul; i < t.size(); ++i)
      BOOST_CHECK_EQUAL ( p(i), r(i) );
  };

  for_each_in_tuple(extents,check);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef INCLUDE_EXPRTK_ARMADILLO_HPP
#define INCLUDE_EXPRTK_ARMADILLO_HPP

#include <exprtk.hpp>
#include <armadillo>

#ifdef exprtk_enable_debugging
#define exprtk_debug(params) printf params
#else
#define exprtk_debug(params) (void)0
#endif

namespace exprtk
{
   namespace armadillo
   {
      using namespace arma;

      template <typename T>
      struct conv : public exprtk::igeneric_function<T>
      {
         typedef typename exprtk::igeneric_function<T> igfun_t;
         typedef typename igfun_t::parameter_list_t    parameter_list_t;
         typedef typename igfun_t::generic_type        generic_type;
         typedef typename generic_type::scalar_view    scalar_t;
         typedef typename generic_type::vector_view    vector_t;

         conv() : exprtk::igeneric_function<T>("VVV") {}

         inline T operator() (parameter_list_t parameters)
         {
            vector_t signal1(parameters[0]);
            vector_t signal2(parameters[1]);
            vector_t signalConv(parameters[2]);

            Col<T> A(signal1.begin(), signal1.size(), false, true);
            Col<T> B(signal2.begin(), signal2.size(), false, true);

            Col<T> C = arma::conv(A, B);

            memcpy(signalConv.begin(), C.memptr(), std::min((size_t)C.size(), signalConv.size()) * sizeof(T));
            return 1;
         }
      };

      template <typename T>
      struct package
      {
         conv<T> conv_f;
         bool register_package(exprtk::symbol_table<T>& symtab)
         {
            #define exprtk_register_function(FunctionName,FunctionType)                  \
            if (!symtab.add_function(FunctionName,FunctionType))                         \
            {                                                                            \
               exprtk_debug((                                                            \
               "armadillo::register_package - Failed to add function: %s\n",             \
               FunctionName));                                                           \
               return false;                                                             \
            }                                                                            \

            exprtk_register_function("conv", conv_f)
            #undef exprtk_register_function

            return true;
         }
      };
   } // namespace exprtk::armadillo
} // namespace exprtk

#ifdef exprtk_debug
#undef exprtk_debug
#endif

#endif


/**
 * @file TriangleElement.cpp
 * @author Adam Li (adamli@umich.edu)
 * @date 2021-11-12
 *
 * @brief Implements TriangleElement from meshGenerator.py.
 *
 */

#include "TriangleElement.hpp"
#include <cstring>
#include <cstdlib>
#include <Eigen/Dense>
#include "TriangularMesh.hpp"

using sel_map::core::ElemArray;
using sel_map::core::ElemArrayBase;

#include "core/EigenMathUtil.hpp"
namespace EigenMathUtil = sel_map::core::EigenMathUtil;

//Avoid rewriting the whole thing everytime
using sel_map::mesh::TriangularMesh;
using sel_map::mesh::TriangleElement;
using sel_map::mesh::Element;

// TriangleElement::TriangleElement(const std::reference_wrapper<TriangularMesh>& parentMesh, unsigned int index, double elementLength, unsigned int pointLimit, float heightPartition,
//                     double heightSafetyCheck, unsigned int classes)
//                       : Element<TriangularMesh>(parentMesh, index, elementLength, pointLimit, heightPartition, heightSafetyCheck, classes)
//                     //   , lambdas()
TriangleElement::TriangleElement(const std::reference_wrapper<TriangularMesh>& parentMesh, unsigned int index)
                      : Element<TriangularMesh>(parentMesh, index)
{
    // Initializer
    active = false;
}

TriangleElement::~TriangleElement()
{
    // Destructor
}

void TriangleElement::reset()
{
    active = false;
    dirichletParamsForTerrainClass.setZero();
}

/**
 * @brief Computes the Barycentric coordinates of all points within the element from Cartesian points.
 */
// void TriangleElement::cartesianToBarycentric()
// {
//     // Auto keywords in eigen are really dangerous, oops
//     // These are all effectively Map<vector2d, unaligned> types
//     auto vertices = parentMesh.get().vertexVector(simplex(), Eigen::all);
//     auto v1 = vertices(0, Eigen::seqN(Eigen::fix<0>, Eigen::fix<2>));
//     auto v2 = vertices(0, Eigen::seqN(Eigen::fix<0>, Eigen::fix<2>));
//     auto v3 = vertices(0, Eigen::seqN(Eigen::fix<0>, Eigen::fix<2>));
//     // These two variables should never be touched again! they're just intermediates
//     auto area_iexp1 = v2 - v1;
//     auto area_iexp2 = v3 - v1;
//     double area = EigenMathUtil::crossProduct2D(area_iexp1, area_iexp2);

//     // This is effectively Map<ArrayXd, aligned> type
//     auto eigenPointsArray = points.getEigenArray();
//     auto eigenLambdaArray = lambdas.reserve(points.length(), true).getEigenArray();

//     // The following is simplified to keep eveything in scalar math
//     // alpha = abs(np.cross(p2-self.points[:,0:2], p3-self.points[:,0:2]) / area)
//     // beta  = abs(np.cross(p1-self.points[:,0:2], p3-self.points[:,0:2]) / area)
//     // gamma = abs(np.cross(p1-self.points[:,0:2], p2-self.points[:,0:2]) / area)
//     // self.lambdas = np.array([alpha, beta, gamma])

//     double v1_x_v2 = EigenMathUtil::crossProduct2D(v1, v2);
//     double v1_x_v3 = EigenMathUtil::crossProduct2D(v1, v3);
//     double v2_x_v3 = EigenMathUtil::crossProduct2D(v2, v3);

//     // update alpha
//     eigenLambdaArray.col(0) = ((v2_x_v3
//                                  + (v2(1) - v3(1)) * eigenPointsArray.col(0)
//                                  + (v3(0) - v2(0)) * eigenPointsArray.col(1)
//                               ) / area).abs();

//     // update beta
//     eigenLambdaArray.col(1) = ((v1_x_v3
//                                  + (v1(1) - v3(1)) * eigenPointsArray.col(0)
//                                  + (v3(0) - v1(0)) * eigenPointsArray.col(1)
//                               ) / area).abs();

//     // update gamma
//     eigenLambdaArray.col(2) = ((v1_x_v2
//                                  + (v1(1) - v2(1)) * eigenPointsArray.col(0)
//                                  + (v2(0) - v1(0)) * eigenPointsArray.col(1)
//                               ) / area).abs();
// }

void TriangleElement::computeElementNormal()
{
    // auto vertices = parentMesh.get().vertexVector(simplex(), Eigen::all);
    // auto a = vertices(2, Eigen::seqN(Eigen::fix<0>(), Eigen::fix<3>()));
    // auto b = vertices(1, Eigen::seqN(Eigen::fix<0>(), Eigen::fix<3>()));
    // auto c = vertices(0, Eigen::seqN(Eigen::fix<0>(), Eigen::fix<3>()));

    // auto v1 = a - b;
    // auto v2 = c - b;
    // normal = v1.matrix().cross(v2.matrix());
}

Eigen::Block<Eigen::Array<unsigned int, -1, 3, 1>, 1, 3, true> TriangleElement::simplex()
{
    return parentMesh.get().simplices.row(index);
}