// #include <signal.h>
// #include <stdio.h>
// #include <stdlib.h>
//
// #include <iostream>
// #include <limits>
// #include <boost/ptr_container/ptr_vector.hpp>
// #include <vector>
// #include <cmath>
//
#include <lcm/lcm.h>
// #include <lcmtypes/bot_core.h>
#include "visualization/collections.hpp"
#include <lcmtypes/particle_pf_cloud_t.h>
// #include <bot_core/bot_core.h>

// #include <bot_param/param_client.h>
// #include <bot_param/param_util.h>

#include "particle_filter.hpp"

using namespace std;
#define VERBOSE_TXT 0

ParticleFilter::ParticleFilter(lcm_t* publish_lcm, long N_p,
  Eigen::Isometry3d init_pose, std::vector<double> initial_var,
  int rng_seed, double resample_threshold_):
  publish_lcm(publish_lcm),N_p(N_p),resample_threshold_(resample_threshold_){

  pRng = new rng(gsl_rng_default,rng_seed); // type, seed

  for (int i=0;i<N_p;i++){
    particleset.push_back(new Particle());
  }

  for (int i=0;i<N_p;i++){
    particleset[i].InitializeState(pRng, log((double) 1/N_p), init_pose, initial_var);
  }

  //Default:
  dResampleThreshold = resample_threshold_ * N_p;

  // Allocate (Specifically c arrays because of GSL)
  dRSWeights = new double[N_p];
  uRSCount  = new unsigned[N_p];
  uRSIndices = new unsigned[N_p];

  // Vis Config:
  pc_vis_ = new pronto_vis(publish_lcm);
  // obj: id name type reset
  // pts: id name type reset objcoll usergb rgb
  bool reset = true;
  pc_vis_->obj_cfg_list.push_back( obj_cfg(4002,"Particles",1,reset) );
  pc_vis_->obj_cfg_list.push_back( obj_cfg(4008,"Mean Estimate",1,reset) );
  pc_vis_->obj_cfg_list.push_back( obj_cfg(4108,"Mean Estimate [Wrap]",1,reset) );
  pc_vis_->obj_cfg_list.push_back( obj_cfg(4010,"Best Estimate",1,reset) );

}

void ParticleFilter::ReinitializeFraction(Eigen::Isometry3d reinit_pose,
    std::vector<double> reinit_var, double fraction){

  if (VERBOSE_TXT){
    std::stringstream ss;// (stringstream::in | stringstream::out);
    print_Isometry3d(reinit_pose,ss);
    std::cout << "in reinit: " << ss.str() << "\n";
  }

  // particles have unnormalized weights. so need the sum of weights in the system:
  // (value varies between  N_p * resamplethreshold   and   N_p
  double total_weight=0;
  for(int i = 0; i < N_p; ++i) {
    total_weight +=   particleset[i].GetWeight();
  }

  if (VERBOSE_TXT){
    cout << "total_weight: " << total_weight << "\n";
  }

  vector<int> pleft( N_p,0); // four ints with value 100
  for (int i=0;i < N_p;i++){
    pleft[i] = (int) i;
  }
  int size_pleft = N_p;

  // Select a random set of particles representing a fraction of the PF weight
  double running_frac =0.0;
  vector <int> chosen_particles;
  while(running_frac < fraction){
    int pleft_select = rand() % size_pleft; // number between [0, number of ints in pleft]
    int w_particle = pleft[pleft_select]; // look up which particle that corresponds to
    // remove the list pointer from the list
    pleft.erase (pleft.begin()+pleft_select);
    size_pleft--;
    chosen_particles.push_back(w_particle);

    // Reset location of particles - but keep original weight for now:
    double unnormalized_weight =particleset[w_particle].GetWeight();
    double normalized_weight  = unnormalized_weight/total_weight;
    particleset[w_particle].InitializeState(pRng, log(unnormalized_weight), reinit_pose, reinit_var);
    running_frac += (normalized_weight) ;

    if (VERBOSE_TXT){
      cout << "s " << pleft_select << " | p " << w_particle
          << " | nw " <<  normalized_weight << " | f " << running_frac << "/" << fraction << "\n";
    }
  }

  // Assign a uniform weight to each of the modified particles:
  double new_weight = (total_weight * running_frac)/chosen_particles.size();
  for(size_t i = 0; i < chosen_particles.size(); i++) {
    particleset[chosen_particles[i]].SetLogWeight(log( new_weight));
  }

  if (VERBOSE_TXT){
    double total_weight=0;
    for(int i = 0; i < N_p; ++i) {
      total_weight +=   particleset[i].GetWeight();
    }
    cout << "total_weight AFTER: " << total_weight << "\n";
    cout << "replaced " << chosen_particles.size() << " parts with weight " << new_weight << "\n";

    cout << "checking the particle state\n";
    for (int i=0; i<N_p; i++) {
      pf_state particle_state;
      particle_state =GetParticleState(i);
      Eigen::Quaterniond r_temp(particle_state.velocity.rotation());
      if ( isnan_particle (r_temp.w() )){
        std::stringstream ss6;
        print_Isometry3d(particle_state.velocity,ss6);
        std::cout << "particle_state.velocity after periodic reinit: " << ss6.str() << " ["<< i<< "\n";
        std::cout << "paused\n";
        int pause;
        std::cin >> pause;
      }
    }

    pf_state reinit_state = IntegrateWrapSafe();
    std::stringstream ss;// (stringstream::in | stringstream::out);
    print_Isometry3d(reinit_state.velocity,ss);
    std::cout << "after periodic reinit: " << ss.str() << "\n";
    Eigen::Quaterniond r_temp(reinit_state.velocity.rotation());
    if ( isnan_particle (r_temp.w() )){
      std::cout << "paused\n";
      int pause;
      std::cin >> pause;
    }
  }

}

void ParticleFilter::ReinitializeComplete(Eigen::Isometry3d reinit_pose,std::vector<double> reinit_var){
      for (int i=0;i<N_p;i++){
	particleset[i].InitializeState(pRng, log((double) 1/N_p), reinit_pose, reinit_var);
      }
}

void ParticleFilter::MoveParticles(pf_state odom_diff, std::vector<double> move_var,
	  double dtime, int mode ){
  typedef boost::ptr_vector< Particle >::iterator ParticleIterator;
  for ( ParticleIterator it = particleset.begin(); it != particleset.end(); it++ ){
    if (mode==0){
      it->MoveParticle(pRng,odom_diff,move_var,dtime);
    }else if (mode==1){
      it->MoveParticleDrift(pRng,move_var,dtime);
    }else{
      cerr << "invalid mode ("<< mode <<") given in ParticleFilter::MoveParticles\n";
      exit(-1);
    }
  }
}

void ParticleFilter::SetHeightPitchRoll(std::vector<double> height_pitch_roll){
  // Set the HPR at fixed values:
  // should this be done outsite the particle filter class?
  for(int i = 0; i < N_p ; ++i) {
    pf_state state = particleset[i].GetState();
    Eigen::Quaterniond r(state.pose.rotation());
    double yaw,pitch,roll;
    quat_to_euler(r, roll, pitch, yaw) ;
    pitch =height_pitch_roll[1];
    roll  =height_pitch_roll[2];

    Eigen::Quaterniond m = euler_to_quat(roll, pitch, yaw);
    Eigen::Isometry3d ipose;
    ipose.setIdentity();
    ipose.translation() << state.pose.translation().x(),state.pose.translation().y(), height_pitch_roll[0];
    ipose.rotate(m);

    state.pose = ipose;
    particleset[i].SetState(state);
  }
}

void ParticleFilter::applyPlaneConstraint(std::vector<double> xyzrpy,
        std::vector<bool> set_xyzrpy, Eigen::Isometry3d plane_pose){

  for(int i = 0; i < N_p ; ++i) {
    pf_state state = particleset[i].GetState();
    state.pose = plane_pose.inverse() * state.pose ;

    double current_rpy[3];
    quat_to_euler(  Eigen::Quaterniond( state.pose.rotation()) , current_rpy[0], current_rpy[1], current_rpy[2]);
    if (set_xyzrpy[3]){ current_rpy[0] = xyzrpy[3]; }
    if (set_xyzrpy[4]){ current_rpy[1] = xyzrpy[4]; }
    if (set_xyzrpy[5]){ current_rpy[2] = xyzrpy[5]; }
    Eigen::Quaterniond revised_quat = euler_to_quat( current_rpy[0], current_rpy[1], current_rpy[2]);

    Eigen::Isometry3d ipose;
    ipose.setIdentity();
    ipose.translation() << state.pose.translation();
    if (set_xyzrpy[0]){ ipose.translation().x() = xyzrpy[0]; }
    if (set_xyzrpy[1]){ ipose.translation().y() = xyzrpy[1]; }
    if (set_xyzrpy[2]){ ipose.translation().z() = xyzrpy[2]; }

    ipose.rotate(revised_quat);
    state.pose = ipose;
    state.pose = plane_pose * state.pose;
    particleset[i].SetState(state);
  }
}

void ParticleFilter::LogLikelihoodParticles(std::vector<float> loglikelihoods){
  for(int i = 0; i < N_p; ++i) {
    particleset[i].AddToLogWeight(loglikelihoods[i]);
  }

  //Normalise the weights to sensible values....
  double dMaxWeight = -std::numeric_limits<double>::infinity();
  typedef boost::ptr_vector< Particle >::iterator ParticleIterator;
  for ( ParticleIterator it = particleset.begin(); it != particleset.end(); it++ ){
      dMaxWeight = max(dMaxWeight, it->GetLogWeight());
  }
  //printf("%f is adjustment\n",dMaxWeight);
  for ( ParticleIterator it = particleset.begin(); it != particleset.end(); it++ ){
      it->SetLogWeight(it->GetLogWeight() - (dMaxWeight));
  }

  // keep a local copy:
  loglikelihoods_ = loglikelihoods;
}

// Residual Resampling
void ParticleFilter::Resample(){
  //Resampling is done in place.
  double dWeightSum = 0;
  unsigned uMultinomialCount;

  //First obtain a count of the number of children each particle has.
  //Sample from a suitable multinomial vector and add the integer replicate
  //counts afterwards.
  dWeightSum = 0;
  ////for ( ParticleIterator it = particleset.begin(); it != particleset.end(); it++ ){
  for(int i = 0; i < N_p; ++i) {
    dRSWeights[i] =   particleset[i].GetWeight();  //pParticles[i].GetWeight();
    dWeightSum += dRSWeights[i];
  }

  uMultinomialCount = N_p;
  for(int i = 0; i < N_p; ++i) {
    dRSWeights[i] = N_p*dRSWeights[i] / dWeightSum;
    uRSIndices[i] = unsigned(floor(dRSWeights[i])); //Reuse temporary storage.
    dRSWeights[i] = (dRSWeights[i] - uRSIndices[i]);
    uMultinomialCount -= uRSIndices[i];
  }
  pRng->Multinomial(uMultinomialCount,N_p,dRSWeights,uRSCount);
  for(int i = 0; i < N_p; ++i)
    uRSCount[i] += uRSIndices[i];

  ///////////////////////
  //Map count to indices to allow in-place resampling
  for (int i=0, j=0; i<N_p; ++i) {
    if (uRSCount[i]>0) {
      uRSIndices[i] = i;
      while (uRSCount[i]>1) {
	while (uRSCount[j]>0) ++j; // find next free spot
	uRSIndices[j++] = i; // assign index
	--uRSCount[i]; // decrement number of remaining offsprings
      }
    }
  }

  //Perform the replication of the chosen.
  for(int i = 0; i < N_p ; ++i) {
    if(uRSIndices[i] != (unsigned int) i)
      particleset[i].SetState(particleset[uRSIndices[i]].GetState());
    particleset[i].SetLogWeight(0);
  }

}

double ParticleFilter::ConsiderResample(){
  //Check if the ESS is below some reasonable threshold and resample if necessary.
  //A mechanism for setting this threshold is required.
  double ESS = GetESS();
  if(ESS < dResampleThreshold) {
    nResampled = 1;
    Resample();
  }else
    nResampled = 0;

  return ESS;
}

double ParticleFilter::GetESS(void) {
  long double sum = 0;
  long double sumsq = 0;

  typedef boost::ptr_vector< Particle >::iterator ParticleIterator;
  for ( ParticleIterator it = particleset.begin(); it != particleset.end(); it++ ){
    sum += expl(it->GetLogWeight());
  }

  for ( ParticleIterator it = particleset.begin(); it != particleset.end(); it++ ){
    sumsq += expl(2.0*(it->GetLogWeight()));
  }

  return expl(-log(sumsq) + 2*log(sum));
}

pf_state ParticleFilter::Integrate(){
  pf_state istate;
  long double wSum = 0;

  double pos[]={0,0,0};
  double rpy[]={0,0,0};
  for(int i =0; i < N_p; i++){
    pf_state state = particleset[i].GetState();
    double w = expl(particleset[i].GetLogWeight());

    Eigen::Vector3d t(state.pose.translation());
    Eigen::Quaterniond r(state.pose.rotation());
    double yaw,pitch,roll;
    quat_to_euler(r, roll, pitch, yaw) ;

    /*if(isnan_particle(t[0])){
      cout << t[0] << " is t[0] "<< i <<"\n";
      cout << "paused\n";
      int pause;
      cin >> pause;
    }*/

    pos[0] += w*t[0];
    pos[1] += w*t[1];
    pos[2] += w*t[2];
    rpy[0] += w*roll;
    rpy[1] += w*pitch;
    rpy[2] += w*yaw;
    wSum  += w ;
  }
  //cout << wSum << " is wSum\n";

  pos[0] /= wSum;
  pos[1] /= wSum;
  pos[2] /= wSum;
  rpy[0] /= wSum;
  rpy[1] /= wSum;
  rpy[2] /= wSum;

  Eigen::Quaterniond m = euler_to_quat(rpy[0], rpy[1], rpy[2]);
  Eigen::Isometry3d ipose;
  ipose.setIdentity();
    ipose.translation() << pos[0],pos[1],pos[2];
  ipose.rotate(m);
  istate.pose = ipose;

  return istate;
}

pf_state ParticleFilter::IntegrateVelocity(){
  pf_state istate;
  long double wSum = 0;

  double pos[]={0,0,0};
  double rpy[]={0,0,0};
  for(int i =0; i < N_p; i++){
    pf_state state = particleset[i].GetState();
    double w = expl(particleset[i].GetLogWeight());

    Eigen::Vector3d t(state.velocity.translation());
    Eigen::Quaterniond r(state.velocity.rotation());
    double yaw,pitch,roll;
    quat_to_euler(r, roll, pitch, yaw) ;

    pos[0] += w*t[0];
    pos[1] += w*t[1];
    pos[2] += w*t[2];
    rpy[0] += w*roll;
    rpy[1] += w*pitch;
    rpy[2] += w*yaw;
    wSum  += w ;
  }
  pos[0] /= wSum;
  pos[1] /= wSum;
  pos[2] /= wSum;
  rpy[0] /= wSum;
  rpy[1] /= wSum;
  rpy[2] /= wSum;

  Eigen::Quaterniond m = euler_to_quat(rpy[0], rpy[1], rpy[2]);
  Eigen::Isometry3d ipose;
  ipose.setIdentity();
  ipose.translation() << pos[0],pos[1],pos[2];
  ipose.rotate(m);
  istate.velocity = ipose;

  return istate;
}

pf_state ParticleFilter::IntegrateWrapSafe(){
  pf_state istate;
  long double wSum = 0;

  double thres = 0.05*N_p;  // proportion of headings to be withing 5d of 0 to count as ackward
  double count =0.0;
  bool wrapping = false;
  for(int i =0; i < N_p; i++){
    pf_state state = particleset[i].GetState();

    Eigen::Vector3d t(state.pose.translation());
    Eigen::Quaterniond r(state.pose.rotation());
    double yaw,pitch,roll;
    quat_to_euler(r, roll, pitch, yaw) ;

    if (yaw < -170*M_PI/180){
      count=count+1.0;
    }else if (yaw > 170*M_PI/180){
      count=count+1.0;
    }
    if (count > thres){
      wrapping=true;
    }
  }

  double pos[]={0,0,0};
  double rpy[]={0,0,0};
  for(int i =0; i < N_p; i++){
    pf_state state = particleset[i].GetState();
    double w = expl(particleset[i].GetLogWeight());

    Eigen::Vector3d t(state.pose.translation());
    Eigen::Quaterniond r(state.pose.rotation());
    double yaw,pitch,roll;
    quat_to_euler(r, roll, pitch, yaw) ;

    pos[0] += w*t[0];
    pos[1] += w*t[1];
    pos[2] += w*t[2];

    if (wrapping){
//      cerr << yaw << ": " << i<< "\n";
      if (yaw < 0){
	yaw = 2*M_PI + yaw ;
      }
    }

    rpy[0] += w*roll;
    rpy[1] += w*pitch;
    rpy[2] += w*yaw;
    wSum  += w ;
  }
  pos[0] /= wSum;
  pos[1] /= wSum;
  pos[2] /= wSum;
  rpy[0] /= wSum;
  rpy[1] /= wSum;
  rpy[2] /= wSum;

  Eigen::Quaterniond m = euler_to_quat(rpy[0], rpy[1], rpy[2]);
  Eigen::Isometry3d ipose;
  ipose.setIdentity();
    ipose.translation() << pos[0],pos[1],pos[2];
  ipose.rotate(m);
  istate.pose = ipose;

  return istate;
}

pf_state ParticleFilter::MaxWeight(){
  double w_max = numeric_limits<double>::min();
  double i_max =0;
  for(int i =0; i < N_p; i++){
    double w = expl(particleset[i].GetLogWeight());
    if (w > w_max){
      w_max = w;
      i_max =i;
    }
  }
  return particleset[i_max].GetState();
}

bot_core_pose_t getLCMPoseFromIsometry3d(Eigen::Isometry3d pose, int64_t time_stamp){
  bot_core_pose_t pose_msg;
  memset(&pose_msg, 0, sizeof(pose_msg));
  pose_msg.utime =   time_stamp;// msg->timestamp;
  pose_msg.pos[0] = pose.translation().x();
  pose_msg.pos[1] = pose.translation().y();
  pose_msg.pos[2] = pose.translation().z();
  Eigen::Quaterniond r_x(pose.rotation());
  pose_msg.orientation[0] =  r_x.w();
  pose_msg.orientation[1] =  r_x.x();
  pose_msg.orientation[2] =  r_x.y();
  pose_msg.orientation[3] =  r_x.z();
  return pose_msg;
}

void ParticleFilter::SendParticlesLCM(int64_t time_stamp,int vo_estimate_status){
  // NB: these methods compile, but haven't been tested since changing recently

  pf_state mean_state;
  mean_state = Integrate();
  mean_state = IntegrateWrapSafe();

  bot_core_pose_t pose_msg = getLCMPoseFromIsometry3d(mean_state.pose, time_stamp);
  bot_core_pose_t_publish(publish_lcm, "POSE", &pose_msg);

  pf_state mean_velocity;
  mean_velocity = IntegrateVelocity();
  bot_core_pose_t vel_msg = getLCMPoseFromIsometry3d(mean_velocity.velocity, time_stamp);
  bot_core_pose_t_publish(publish_lcm, "POSE_VELOCITY", &vel_msg);

  std::vector<Isometry3dTime> particle_poses;
  for (int i=0;i<N_p;i++){
    pf_state particle_state = particleset[i].GetState();
    Isometry3dTime particle_pose(time_stamp + i, particle_state.pose);
    particle_poses.push_back(particle_pose);
  }
  pc_vis_->pose_collection_to_lcm_from_list(4002, particle_poses);

  Isometry3dTime poseT = Isometry3dTime(time_stamp, mean_state.pose);
  pc_vis_->pose_to_lcm_from_list(4008, poseT);

  pf_state mean_state_wrap;
  mean_state_wrap = IntegrateWrapSafe();
  Isometry3dTime poseTwrap = Isometry3dTime(time_stamp, mean_state_wrap.pose);
  pc_vis_->pose_to_lcm_from_list(4108, poseTwrap);

  particle_pf_cloud_t pc;
  pc.utime = time_stamp;
  pc.nparticles = N_p;
  double likelihoods[N_p];
  double weights[N_p];
  bot_core_pose_t particles[N_p];
  for (int i=0;i<N_p;i++){
    particles[i].utime = time_stamp;
    likelihoods[i] = loglikelihoods_[i];// particleset[i].GetLogWeight();
  }
  pc.weights = weights;
  pc.likelihoods = likelihoods;
  pc.particles = particles;
  // resample thresholds:
  pc.neff = GetESS()/N_p;
  pc.resample_threshold = resample_threshold_;  // currently hard coded
  pc.vo_estimate_status = vo_estimate_status-1; // this -1 is because fovis and fovis_upate_t types are not in sync

  particle_pf_cloud_t_publish(publish_lcm, "PF_CLOUD", &pc);

  // Highest Weight Particle:
  pf_state max_particle;
  max_particle = MaxWeight();
  Isometry3dTime poseTmax = Isometry3dTime(time_stamp, max_particle.pose);
  pc_vis_->pose_to_lcm_from_list(4010, poseTmax);

}


#ifndef HOPS_ACCEPTANCERATETUNER_HPP
#define HOPS_ACCEPTANCERATETUNER_HPP

#include <hops/FileWriter/FileWriter.hpp>
#include <hops/FileWriter/FileWriterFactory.hpp>
#include <hops/FileWriter/FileWriterType.hpp>
#include <hops/MarkovChain/MarkovChain.hpp>
#include <hops/MarkovChain/MarkovChainAttribute.hpp>
#include <hops/Optimization/GaussianProcess.hpp>
#include <hops/Optimization/ThompsonSampling.hpp>

#include <Eigen/Core>

#include <chrono>
#include <cmath>
#include <memory>
#include <vector>

namespace hops {
    class AcceptanceRateTuner {
    public:
        struct param_type {
            double acceptanceRateTargetValue;
            size_t iterationsToTestStepSize;
            size_t posteriorUpdateIterations;
            size_t pureSamplingIterations;
            size_t iterationsForConvergence;
            size_t posteriorUpdateIterationsNeeded;
            size_t stepSizeGridSize;
            double stepSizeLowerBound;
            double stepSizeUpperBound;
            double smoothingLength;
            size_t randomSeed;
            bool recordData;

            param_type(double acceptanceRateTargetValue,
                       size_t iterationsToTestStepSize,
                       size_t posteriorUpdateIterations,
                       size_t pureSamplingIterations,
                       size_t iterationsForConvergence,
                       size_t stepSizeGridSize,
                       double stepSizeLowerBound,
                       double stepSizeUpperBound,
                       double smoothingLength,
                       size_t randomSeed,
                       bool recordData = false
            );
        };

        /**
         * @brief tunes markov chain acceptance rate by nested intervals. The chain is not guaranteed to have converged
         *        to the specified acceptance rate.
         * @details Clears Markov chain history.
         * @param markovChain
         * @param parameters
         * @return true if markov chain is tuned
         */
        static bool
        tune(std::vector<std::shared_ptr<MarkovChain>>& markovChain,
             std::vector<RandomNumberGenerator>& randomNumberGenerator,
             param_type &parameters);

        /**
         * @brief tunes markov chain acceptance rate by nested intervals. The chain is not guaranteed to have converged
         *        to the specified acceptance rate.
         * @details Clears Markov chain history.
         * @param markovChain
         * @param parameters
         * @return true if markov chain is tuned
         */
        static bool
        tune(double& stepSize,
             double& deltaAcceptanceRate,
             std::vector<std::shared_ptr<MarkovChain>>& markovChain,
             std::vector<RandomNumberGenerator>& randomNumberGenerator,
             param_type &parameters);

        /**
         * @brief tunes markov chain acceptance rate by nested intervals. The chain is not guaranteed to have converged
         *        to the specified acceptance rate.
         * @details Clears Markov chain history.
         * @param markovChain
         * @param parameters
         * @return true if markov chain is tuned
         */
        static bool
        tune(double& stepSize,
             double& deltaAcceptanceRate,
             std::vector<std::shared_ptr<MarkovChain>>& markovChain,
             std::vector<RandomNumberGenerator>& randomNumberGenerator,
             param_type&,
             Eigen::MatrixXd&,
             Eigen::MatrixXd&);

        AcceptanceRateTuner() = delete;
    };

    namespace internal {
        struct AcceptanceRateTarget : public ThompsonSamplingTarget<double, Eigen::VectorXd> {
            std::vector<std::shared_ptr<hops::MarkovChain>> markovChain;
            std::vector<RandomNumberGenerator>* randomNumberGenerator;
            AcceptanceRateTuner::param_type parameters;

            AcceptanceRateTarget(std::vector<std::shared_ptr<hops::MarkovChain>>& markovChain,
                                              std::vector<hops::RandomNumberGenerator>& randomNumberGenerator,
                                              const hops::AcceptanceRateTuner::param_type& parameters) :
                    markovChain(markovChain),
                    randomNumberGenerator(&randomNumberGenerator),
                    parameters(parameters) {}

            virtual std::tuple<double, double> operator()(const Eigen::VectorXd& x) override;
        };
    }
}

#endif //HOPS_ACCEPTANCERATETUNER_HPP


#include <iostream>
#include <vector>

#include <unordered_map>
#include <exception>

#include "dynet/dict.h"

#include <boost/program_options.hpp>
#include <boost/serialization/unordered_set.hpp>

#include "serialization.h"
#include "utils.h"

#include "dependency.h"
#include "conll.h"
#include "probs.h"

int main(int argc, char **argv)
{
    std::string data_path;
    std::string model_path;

    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
        ("data", po::value<std::string>(&data_path)->required(), "")
        ("model", po::value<std::string>(&model_path)->required(), "")
    ;

    po::positional_options_description pod;
    pod.add("data", 1);
    pod.add("model", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(pod).run(), vm);
    po::notify(vm);

    ConllSettings conll_settings;
    read_object(model_path + ".conll_settings.param", conll_settings);

    std::cerr << "Reading data..." << std::endl << std::flush;
    Conll conll_data(conll_settings);
    std::vector<IntSentence> data;
    conll_data.read(data_path);
    conll_data.as_int_sentence([&](const IntSentence& s) { data.push_back(s); });

    Probs probs;
    std::vector<double> total(conll_settings.pos_dict.size(), 0.0);
    for (auto const& sentence : data)
    {
        for (auto const& token : sentence)
        {
            total.at(token.pos) += 1.0;

            if (token.head == 0)
                probs.head[token.pos] += 1.0;
            else
                probs.pos[std::make_pair(sentence[token.head].pos, token.pos)] += 1.0;
        }
    }
    std::for_each(
        std::begin(probs.head),
        std::end(probs.head),
        [&](std::pair<const int, double>& p) {
            p.second /= total.at(p.first);
        }
    );
    std::for_each(
        std::begin(probs.pos),
        std::end(probs.pos),
        [&](std::pair<const std::pair<int, int>, double>& p) {
            p.second /= total.at(p.first.second);
        }
    );

    save_object(model_path + ".attachment-probs", probs);
}


#include "AStarPathFinder.h"
#include "Logging.h"
#include "Map.h"
#include "Path.h"
#include "utils/Types.h"
#include "utils/AdjacentCellIterator.h"

#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>

using namespace boost;
using namespace std;

namespace map_solver {

namespace internal {
typedef weight_t cost;

template <class Graph, class CostType, class LocMap>
class distance_heuristic : public astar_heuristic<Graph, CostType>
{
public:
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    distance_heuristic(LocMap l, Vertex goal)
        : m_location(l), m_goal(goal) {}
    CostType operator()(Vertex u)
    {
        index_t ux, uy, goalx, goaly;
        tie(ux, uy) = m_location.indexToCartesian(u);
        tie(goalx, goaly) = m_location.indexToCartesian(m_goal);

        CostType dx = goalx - ux;
        CostType dy = goaly - uy;
        return dx + dy;
    }
private:
    LocMap& m_location;
    Vertex m_goal;
};

struct found_goal {}; // exception for termination

// visitor that terminates when we find the goal
template <class Vertex>
class throwing_astar_visitor : public boost::default_astar_visitor
{
public:
    throwing_astar_visitor(Vertex goal) : m_goal(goal) {}
    template <class Graph>
    void examine_vertex(Vertex u, Graph& /*g*/) throw(found_goal) {
        LOG_TRACE << "Examining vertex: " << u;
        if(u == m_goal)
            throw found_goal();
    }
private:
    Vertex m_goal;
};
}

using namespace internal;

AStarPathFinder::AStarPathFinder(std::shared_ptr<Map> mmap)
    : IPathFinder(mmap) {

}

unique_ptr<Path> AStarPathFinder::solve() {
    // TODO: use union/find to check the path for solvability earlier

    // specify some types
    typedef adjacency_list<vecS, vecS, directedS, no_property,
                           property<edge_weight_t, cost> > mygraph_t;
    typedef property_map<mygraph_t, edge_weight_t>::type WeightMap;
    typedef mygraph_t::vertex_descriptor vertex;
    typedef mygraph_t::edge_descriptor edge_descriptor;
    // typedef mygraph_t::vertex_iterator vertex_iterator;
    // typedef std::pair<int, int> edge;

    index_t start_idx, finish_idx;
    start_idx = map()->start();
    finish_idx = map()->finish();

    if (kInvalidWeight == start_idx || kInvalidWeight == finish_idx) {
        LOG_FATAL << "The map is not correct. Cannot find a start or finish cell.";
        return nullptr;
    }

    // graph: directed, vector for storing vecs and edges
    // create graph
    const int num_nodes = map()->width() * map()->height();
    mygraph_t g(num_nodes);
    // fill edges' weights
    WeightMap weightmap = get(edge_weight, g);
    LOG_TRACE << "num_nodes: " << num_nodes;
    for (int i=0; i < num_nodes; ++i) {
        auto& current_cell = i;
        auto cell_weight = map()->weight(current_cell);
        LOG_TRACE << "node " << current_cell << " weight: " << cell_weight;

        AdjacentCells<index_t> adjacent_indexes(map()->width(), map()->height(), i);

        for_each(adjacent_indexes.begin(), adjacent_indexes.end(), [&](int adjacent_index) {
                edge_descriptor e; bool inserted;
                tie(e, inserted) = add_edge(current_cell, adjacent_index, g);
                LOG_TRACE << "e, inserted: " << e << ", " << inserted;
                weightmap[e] = cell_weight;
            });
    }

    // pick start/finish
    vertex start = start_idx;
    vertex finish = finish_idx;

    // run astar
    vector<mygraph_t::vertex_descriptor> p(num_vertices(g));
    vector<cost> d(num_vertices(g));
    list<vertex> shortest_path;
    try {
        // call astar named parameter interface
        astar_search
            (g, start,
             distance_heuristic<mygraph_t, cost, Map>
             (*map(), finish),
             predecessor_map(&p[0]).distance_map(&d[0]).
             visitor(throwing_astar_visitor<vertex>(finish)));

    } catch(found_goal fg) { // found a path to the goal
        for(vertex v = finish;; v = p[v]) {
            shortest_path.push_front(v);
            if(p[v] == v)
                break;
        }
        LOG_TRACE << "Shortest path from " << start << " to "
             << finish << ": ";
        auto spi = shortest_path.begin();
        LOG_TRACE << start;
        for(++spi; spi != shortest_path.end(); ++spi)
            LOG_TRACE  << *std::prev(spi) << " -> " << *spi;
        LOG_TRACE << '\n' << "Total travel time: " << d[finish] << '\n';

        if (d[finish] >= kWallWeight)
            throw DestinationUnreachableException("The destination can't be reached without jumping over the walls.");

        auto ret = make_unique<Path>(map()->width(), map()->height());
        ret->setWeight(d[finish]);

        for (auto&& i : shortest_path)
            ret->markCell(i);
        return ret;
    }

    LOG_WARN << "Didn't find a path from " << start << " to "
             << finish << "!" << '\n';

    throw DestinationUnreachableException("Start and finish points are not connected.");

    return nullptr;
}

} //namespace map_solver


#ifndef BART_SRC_CALCULATOR_FOURIER_FOURIER_TRANSFORM_I_HPP_
#define BART_SRC_CALCULATOR_FOURIER_FOURIER_TRANSFORM_I_HPP_

#include <complex>
#include <vector>

#include <deal.II/lac/vector.h>

#include "utility/named_type.h"
#include "utility/has_description.h"

//! \brief Calculators for calculating the Fourier transform.
namespace bart::calculator::fourier {

using Normalized = utility::NamedType<bool, struct NormalizedStruct>;

/*! \brief Interface for classes that calculate the discrete fourier transform. */
class FourierTransformI : public utility::HasDescription {
 public:
  /// Vector of complex values, used for both input and output of the DFT.
  using ComplexVector = std::vector<std::complex<double>>;
  virtual ~FourierTransformI() = default;
  /*! \brief Calculate the DFT of a provided complex vector.
   *
   * @param input input vector of complex values to take the DFT of.
   * @param normalize_return if true, the result is normalized by dividing by the total size of the vector.
   * @return vector of complex vector that holds the DFT of the input.
   */
  virtual auto CalculateDFT(const ComplexVector& input, Normalized normalize_return) -> ComplexVector = 0;
  /*! \brief Calculate the DFT of a provided dealii-format Vector.
   *
   * @param input input vector of real values to take the DFT of.
   * @param normalize_return if true, the result is normalized by dividing by the total size of the vector.
   * @return vector of complex vector that holds the DFT of the input.
   */
  virtual auto CalculateDFT(const dealii::Vector<double>& input, Normalized normalize_return) -> ComplexVector = 0;
};

} // namespace bart::calculator::fourier

#endif //BART_SRC_CALCULATOR_FOURIER_FOURIER_TRANSFORM_I_HPP_


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/cyl_bessel_y1.hpp>
#include <eve/function/diff/cyl_bessel_y1.hpp>
#include <eve/function/prev.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_y1"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_y1(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_y1(v_t(0)), v_t);
};

EVE_TEST( "Check behavior of cyl_bessel_y1 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 200.0))
        )
  <typename T>(T const& a0)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_y1 =  [](auto x) { return eve::cyl_bessel_y1(x); };
  auto std__cyl_bessel_y1 =  [](auto x)->v_t { return boost::math::cyl_neumann(v_t(1), x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_y1(eve::minf(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y1(eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y1(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y1(eve::minf(eve::as< T>())), eve::nan(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y1(eve::inf(eve::as< T>())),  T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y1(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__cyl_bessel_y1(v_t(1500)), std__cyl_bessel_y1(v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1(v_t(500)), std__cyl_bessel_y1(v_t(500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1(v_t(10)), std__cyl_bessel_y1(v_t(10))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1(v_t(5)),  std__cyl_bessel_y1(v_t(5))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1(v_t(2)),  std__cyl_bessel_y1(v_t(2))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1(v_t(1.5)),std__cyl_bessel_y1(v_t(1.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1(v_t(0.5)),std__cyl_bessel_y1(v_t(0.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1(v_t(1)),  std__cyl_bessel_y1(v_t(1))   , 2.5);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1(v_t(0)),  eve::minf(eve::as<v_t>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_y1( T(1500)), T(std__cyl_bessel_y1(v_t(1500))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1( T(500)),  T(std__cyl_bessel_y1(v_t(500)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1( T(10)) ,  T(std__cyl_bessel_y1( v_t(10)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1( T(5))  ,  T(std__cyl_bessel_y1( v_t(5))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1( T(2))  ,  T(std__cyl_bessel_y1( v_t(2))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1( T(1.5)),  T(std__cyl_bessel_y1( v_t(1.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1( T(0.5)),  T(std__cyl_bessel_y1( v_t(0.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1( T(1))  ,  T(std__cyl_bessel_y1( v_t(1))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y1( T(0))  , eve::minf(eve::as< T>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_y1(a0), map(std__cyl_bessel_y1, a0), 16.0);
};

EVE_TEST( "Check behavior of cyl_bessel_y1 on wide with negative non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 60.0))
        )
  <typename T>(T a0 )
{
  using v_t = eve::element_type_t<T>;
  auto eve__diff_bessel_y1 =  [](auto x) { return eve::diff(eve::cyl_bessel_y1)(x); };
  auto std__diff_bessel_y1 =  [](auto x)->v_t { return boost::math::cyl_neumann_prime(1, x); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_y1(a0),   map(std__diff_bessel_y1, a0)   , 1.0e-3);

};


#include "pprint.hpp" // python-like print()
#include "CLI11.hpp" // cli parameters
#include "archive.h" // serialization
#include "profiling.h" // for code profiling / timing
#include <armadillo> // matlab-like fast math

#include <vector>
#include <algorithm>
#include <cfloat>
#include <cmath>
#include <ctime> // time() / srand
#include <csignal> // for ctrl-c signal
#include <unordered_set>
#include <queue>
#include <fstream> // ifstream, ofstream
#include <string>
#include <iterator>
#include <random>

#ifdef _WIN32
  #include <process.h> // process ID
#else /* linux */
  #include <unistd.h> // process ID
#endif

// deterministic but fast generator
// should probably replace the randf()
// with this one because it's faster
// than the underyling C rand() by a lot
// and C rand() is not x-platform deterministic.
std::mt19937_64 generator(34);
std::uniform_real_distribution<float> real_dist(0.0f,1.0f);
std::uniform_int_distribution<int> int_dist(0,INT_MAX);
static inline float drandf() {
  return real_dist(generator);
}
static inline int drand() {
  return int_dist(generator);
}
static inline void dsrand(const unsigned int& newseed) {
  generator.seed(newseed);
}

/* return a seed and also use it as the new seed */
static inline unsigned int drandGetSeed() {
  unsigned int seed = generator();
  generator.seed(seed);
  return seed;
}

#ifdef NO_BOUNDS_CHECKING
// safety first :)
//#define at operator[] // stl-specific version (use .at everywhere in stl and use this to get performance when ready)
#define ARMA_NO_DEBUG 1 // armadillo-specific version (use () indexing in armadillo and then use this to get performance when ready)
#endif

// set up better cout as a python-like print()
// now we can use `print(anything,...);`
using namespace pprint;
pprint::PrettyPrinter printer;
#define print printer.print

// convenience for begin, end
using std::begin, std::end;

// set up armadillo lib math experience
// now we can do linear algebra
using namespace arma;

// set up faster rand fn
// much faster random num from 0 to 1 than using
// "high quality" mersenne twister or armadillo lib.
#define randf() float(rand())/float(RAND_MAX)

namespace arma::fill {
  // the arma version of filling with random numbers is very slow compared to C's rand fn.
  // here, we make a templated fn similar to arma's that does things much faster.
  // we aren't doing statistical physics or cryptography, so we don't care about
  // the quality of the random distribution, so we can just use C's rand fn.
  // example: fill::random(my_arma_container);
  template <typename C, typename T = typename C::value_type, typename = T>
  void random(C& vec) {
    //std::generate(begin(vec), end(vec), [] { return (T)std::rand() / RAND_MAX; });
    std::generate(begin(vec), end(vec), [] { return (T)drandf(); });
  }
}

// temp pasted

/** globals **/
namespace GLB {
  /* EVOLVE command parameters */
  // evolution
  unsigned int screen_update_interval = 500;
  unsigned int generations_limit = 1'000;
  unsigned int lod_save_interval = 1'000;
  unsigned int current_generation; // used in main() loop
  int seed {-1};
  int pop_size = 100;
  int G = 40;
  float point_mutation_rate = 0.005f;
  float vector_mutation_rate = 0.0005f;
  bool use_point_mu = true;
  bool use_col_mu = true;
  bool use_row_mu = true;
  bool local_locality_mu = false;
  int developmental_updates = 200;
  std::vector<int> hist_gens;
  // fitness landscape
  int N=20;
  int K=1;
  float speed_change = 0.001;
  /* overlap */
  // saving, loading
  std::string savefilename{""}, loadfilename{""};
  /* ANALYZE command parameters */
  std::vector<int> gen_pick {0,1,-1};
  unsigned int ntrials {500};
  unsigned int maxmutations {5};
  bool show_fitness {false};
  bool show_robustness {false};
  bool show_phenotype {false};
  bool show_deltaperfect {false}; // num mutations from perfect genotype
  /* INFO command parameters */
}

/** GRN **/
class GRN {
  public:
    //this is a dampening constant, leave it where it is
    const static inline float GRN_DAMP {0.1f};
  public:
    int num_genes;
    fmat network;
    frowvec state, projected_state;
    GRN() = delete;
    GRN(const fmat& /*genome*/);
    void updateNetwork(const int& iterations=1);
};

GRN::GRN(const fmat& matrix) : num_genes(size(matrix)[0]) {
  state.resize(num_genes);
  projected_state.resize(num_genes);
  network = matrix;
  // fill with values of 1/SIZE
  state = ones<frowvec>(num_genes) / num_genes;
  // constrain values and make into unit-vector
  state = clamp(state, 0.0001, 1.0);
  state /= accu(state);
}

void GRN::updateNetwork(const int& iterations) {
  for (int iteration=iterations-1; iteration>=0; --iteration) {
    projected_state = state * network;
    projected_state = (2.0f/(exp(-projected_state)+1.0f)) - 1.0f;
    state -= (state-projected_state) * GRN_DAMP;
  }
}

/** Agent Code **/

// recording genomic diffs
struct GenomeChange {
  public:
    int location;
    float newvalue;
};

/* We make mu and num_genes not constants
 * but runtime definable variables
 * to support loading data and rerunning.
 */
struct AgentClassConfiguration {
  public:
    int num_genes;
    float mu_point;
    float mu_vector;
};

// used for specifying in the Agent ctor
// if it should be randomly initialized or not.
enum INIT {NONE=0, RANDOM=1};

/** Agent **/
class Agent {
  public:
    static inline int num_genes {-1}; // default to invalid value
    static inline float mu {-0.0001f}; // default to invalid value
    // must be called before using the class.
    static void configure(const AgentClassConfiguration& config);
    static inline std::default_random_engine mu_generator{};
    static inline std::binomial_distribution<int> mu_point_distribution{}, mu_vector_distribution{};

  public:
    float fitness{-1};
    fmat genome; // (serialized)
    bool* phenotype {nullptr}; // continuous genome -> continuous grn -> boolean phenotype
    bool developed {false};
    unsigned int gen_of_evaluation; // (serialized) (needed for reconstructing env::treadmill)
    std::unique_ptr<GRN> grn {nullptr};
    std::shared_ptr<Agent> ancestor {nullptr};
    std::shared_ptr<Agent> progenitor {nullptr}; // only used during LOD serialization
    std::vector<GenomeChange> changeset; // (serialized) tracks mutations
    unsigned int new_mutations {0}; // usually same as changeset.size(), unless picking sparsely from LOD, then will be larger

    Agent(); // alias for Agent(INIT::NONE);
    ~Agent(); // alias for Agent(INIT::NONE);
    Agent(const INIT& /*init*/); // determines how the agent is initialized
    void inheritFrom(const std::shared_ptr<Agent> /*parent*/); // inherit from another agent asexually
    void initGRN();
    void runGRN(const int& num_updates);
    void developPhenotypeFromGRN();
    void mutatePointWise();
    void mutateColWise();
    void mutateRowWise();
};

/*
 * Parameters used by all agents.
 * gene number,
 * mutation rate,
 * and mutation distribution generator.
 */
void Agent::configure(const AgentClassConfiguration& config) {
  Agent::num_genes = config.num_genes;
  mu_point_distribution.param(std::binomial_distribution<int>::param_type(Agent::num_genes*Agent::num_genes, config.mu_point));
  mu_vector_distribution.param(std::binomial_distribution<int>::param_type(Agent::num_genes, config.mu_vector));
}

Agent::Agent() : Agent(INIT::NONE) { }

Agent::Agent(const INIT& init) {
  genome.resize(num_genes,num_genes);
  switch(init) {
    case INIT::NONE:
      break;
    case INIT::RANDOM:
      // fill with random values [0,1] then stretch to [-1,1]
      fill::random(genome);
      genome = genome*2 - 1;
      break;
  }
}

Agent::~Agent() {
  if (phenotype) delete [] phenotype;
}

void Agent::mutatePointWise() {
  // determine number of mutations
  // that should happen,
  // and return early if 0.
  // if no mutations, then
  // return early.
  int num_mutations = Agent::mu_point_distribution(Agent::mu_generator);
  if (num_mutations == 0) { return; }
  developed = false;
  int genome_size = num_genes * num_genes;
  int pos; // flattened matrix index position
  float mutation_value;
  while (num_mutations > 0) {
    pos = drand() % genome_size;
    if (GLB::local_locality_mu) {
      mutation_value = randf()*0.1f - 0.05f;
      genome(pos) += mutation_value; // local-locality mutation
      changeset.emplace_back(GenomeChange{
                             .location = pos,
                             .newvalue = genome(pos)
                             });
    } else {
      mutation_value = randf()*2-1;
      mutation_value = std::clamp(mutation_value, 0.0f, 1.0f);
      genome(pos) = mutation_value; // global-locality mutation
      changeset.emplace_back(GenomeChange{
                             .location = pos,
                             .newvalue = mutation_value
                             });
    }
    --num_mutations;
  }
}

void Agent::mutateColWise() {
  int num_mutations = Agent::mu_vector_distribution(Agent::mu_generator);
  if (num_mutations == 0) { return; }
  developed = false;
  int col_size = num_genes;
  int pos; // flattened matrix index position
  fcolvec col_offset_values(col_size);
  while (num_mutations > 0) {
    pos = drand() % col_size;
    // make random offsets in [-0.1,0.1]
    fill::random(col_offset_values);
    col_offset_values -= 0.5;
    col_offset_values *= 0.2;
    genome.col(pos) += col_offset_values;
    // record each change into changeset
    for (int i=col_size-1; i>=0; --i) {
      changeset.emplace_back(GenomeChange{
                             .location = (pos*col_size)+i,
                             .newvalue = genome(pos*col_size+i)
                             });
    }
    --num_mutations;
  }
}

void Agent::mutateRowWise() {
  int num_mutations = Agent::mu_vector_distribution(Agent::mu_generator);
  if (num_mutations == 0) { return; }
  developed = false;
  int row_size = num_genes;
  int pos; // flattened matrix index position
  frowvec row_offset_values(row_size);
  while (num_mutations > 0) {
    pos = drand() % row_size;
    // make random offsets in [-0.1,0.1]
    fill::random(row_offset_values);
    row_offset_values -= 0.5;
    row_offset_values *= 0.2;
    genome.row(pos) += row_offset_values;
    // record each change into changeset
    for (int i=row_size-1; i>=0; --i) {
      changeset.emplace_back(GenomeChange{
                             .location = (pos*row_size)+i,
                             .newvalue = genome(pos*row_size+i)
                             });
    }
    --num_mutations;
  }
}

void Agent::inheritFrom(const std::shared_ptr<Agent> parent) {
  genome = parent->genome;
  ancestor = parent;
  developed = true;
  if (GLB::use_point_mu) mutatePointWise();
  if (GLB::use_col_mu) mutateColWise();
  if (GLB::use_row_mu) mutateRowWise();
  if (developed) {
    if (not phenotype) { phenotype = new bool[GLB::N]; }
    for (int i=0; i<GLB::N; i++) phenotype[i] = parent->phenotype[i];
  }
}

auto Agent::initGRN() -> void {
  grn = std::make_unique<GRN>(genome);
}

auto Agent::runGRN(const int& num_updates) -> void {
  grn->updateNetwork(num_updates);
}

/** selection methods **/
namespace SELECTION {
  /* complete_roulette performs fitness-proportional
   * selection, replacing the entire population
   * at each generation, inheriting from the previous
   * generation.
   */
  void complete_roulette(std::vector<std::shared_ptr<Agent>>& oldpop, std::vector<std::shared_ptr<Agent>>& newpop, const fcolvec& fitnesses, const float& minW, const float& maxW) {
    int popsize = oldpop.size();
    int parent;
    // if no selection gradient, then pick randomly
    /* [[ unlikely ]] */
    if (minW == maxW) {
      for (int i=0; i<popsize; i++) {
        parent = drand() % popsize;
        newpop.emplace_back( std::make_shared<Agent>(INIT::NONE) );
        newpop.back()->inheritFrom( oldpop[parent] );
      }
    // if selection gradient, then pick fitness proportional (stochastic)
    } else /*(minW != maxW)*/ {
      fcolvec W = (fitnesses - minW) / (maxW - minW);
      for (int i=0; i<popsize; i++) {
        do {
          parent = rand() % popsize;
        } while (randf()>W(parent));
        newpop.emplace_back( std::make_shared<Agent>(INIT::NONE) );
        newpop.back()->inheritFrom( oldpop[parent] );
      }
    }
  }
}

auto Agent::developPhenotypeFromGRN() -> void {
  if (not phenotype) { phenotype = new bool[GLB::N]; }
  for (int i=0; i<GLB::N; ++i) { phenotype[i] = (grn->state[i]>0.0f); }
}

/** fitness functions **/
namespace ENV {
  namespace MAXONES {
    auto evaluate(std::shared_ptr<Agent> agent) -> float {
      return accu(clamp(agent->genome,0,1));
    }
  }
  namespace NKTREADMILL {
    /* helper function for fitness
     * evaluation.
     */
    float triangleSin(float x, int resolution){
      float Y=0.0;
      for(int i=0;i<GLB::N;i++){
        float n=(2*i)+1;
        Y+=std::pow(-1,float(i))*std::pow(n,-2.0f)*std::sin(n*x);
      }
      return (0.25f*float{M_PI})*Y;
    }

    /* initialize alphas and betas
     * tables. Set and store random
     * number seed before this.
     */
    fmat Alphas,Betas;
    unsigned int alpha_beta_seed {0};
    auto init(const unsigned int& new_seed) -> void {
      Alphas.set_size(GLB::N,1<<GLB::K);
      Betas.set_size(GLB::N,1<<GLB::K);
      dsrand(new_seed);
      alpha_beta_seed = new_seed;
      fill::random(Alphas);
      fill::random(Betas);
    }

    /* call init() before this,
     * call agent's developPhenotypeFromGRN() before this.
     */
    auto evaluate(std::shared_ptr<Agent> agent) -> float {
      float W=0.0;
      for(int n=0;n<GLB::N;n++){ // ex: 0-20
        int val=0;
        for(int k=0;k<GLB::K;k++){ // ex: 0-3
          val=(val<<1)+agent->phenotype[(n+k)%GLB::N]; // phenotype is bool data
        }
        W+=std::log((1.0f+ENV::NKTREADMILL::triangleSin(((GLB::speed_change*agent->gen_of_evaluation)*(Betas(n,val)+0.5f))+(Alphas(n,val)*float{M_PI}*2.0f),5))/2.0f);
      }
      return std::exp(W/float(GLB::N));
    }

    /* getMaxFitAgent
     * Returns max fit ideal organism.
     * Enumerates genomes to find.
     * fitness peak (for generation 0)
     */
    struct MaxFitConfiguration {
      public:
        int generation;
    };
    auto getMaxFitAgent(const MaxFitConfiguration& cfg) -> std::shared_ptr<Agent> {
      std::shared_ptr<Agent> A = std::make_shared<Agent>(INIT::NONE);
      int bestg;
      A->phenotype = new bool[GLB::N];
      A->gen_of_evaluation = cfg.generation;
      float globalPeak=0.0f, fitness=0.0f;
      for(int g=0;g<(1<<GLB::N);g++){
        for(int j=0;j<GLB::N;j++) { A->phenotype[j] = (g>>j)&1; }
        fitness = ENV::NKTREADMILL::evaluate(A);
        if(fitness>globalPeak) {
          globalPeak=fitness;
          bestg = g;
        }
      }
      // fill in fitness and phenotyp only
      std::shared_ptr<Agent> bestAgent = std::make_shared<Agent>(INIT::NONE);
      bestAgent->fitness = globalPeak;
      bestAgent->phenotype = new bool[GLB::N];
      bestAgent->gen_of_evaluation = 0;
      for(int j=0;j<GLB::N;j++) { bestAgent->phenotype[j] = (bestg>>j)&1; }
      return bestAgent;
    }
  }
}

/** LOD functions **/
namespace LOD {
  /* Finds the Most Recent Common Ancestor,
   * returns a tuple (pair) with a pointer to
   * the ancestor, and how many generations
   * back we had to crawl to find it. [0-N)
   */
  auto getMRCA(std::vector<std::shared_ptr<Agent>> population) -> std::pair< std::shared_ptr<Agent>, unsigned int> {
      std::queue<std::shared_ptr<Agent>> mrca_queue;
      std::queue<int> mrca_queue_gen_sizes;
      std::unordered_set<std::shared_ptr<Agent>> mrca_set_prev_gen;
      std::shared_ptr<Agent> mrca;
      std::shared_ptr<Agent> ancestor;
      // fill queue with initial population
      mrca_queue_gen_sizes.push(population.size());
      for (auto& member : population) {
        mrca_queue.push(member);
      }
      // begin traversing queue
      bool stop_search {false};
      int generations_back {0};
      while (true) {
        // go through n members of the queue
        // for n members who were together
        // in the same generation, because
        // the queue can hold more members
        // from the next generation, too.
        mrca_set_prev_gen.clear();
        int generation_memberi = mrca_queue_gen_sizes.front()-1; mrca_queue_gen_sizes.pop();
        for ( ; generation_memberi>=0; --generation_memberi) {
          ancestor = mrca_queue.front(); mrca_queue.pop();
          //look at grandparent, then add to set if not null
          // if found null then there is no mrca
          if (ancestor->ancestor == nullptr) { return std::make_pair(nullptr,generations_back); }
          mrca_set_prev_gen.insert(ancestor->ancestor);
        }
        // stop and return if we're at a common ancestor
        if (mrca_set_prev_gen.size() == 1) {
          mrca = *begin(mrca_set_prev_gen);
          return std::make_pair(mrca,generations_back);
        }
        // after adding all grandparents to the set,
        // go through the unique set and add them to queue
        for (auto& ancestor : mrca_set_prev_gen) {
          mrca_queue.push(ancestor);
        }
        mrca_queue_gen_sizes.push(mrca_set_prev_gen.size());
        ++generations_back;
      }
      // should never hit
      return std::make_pair(nullptr,generations_back);
    }

  /* serialize the LOD below the mrca
   * to a file (not mrca itself).
   */
  auto save(const std::string& filename, std::shared_ptr<Agent> mrca) -> void {
    std::ofstream touchFS(filename,ios::out|ios::app); touchFS.close(); // necessary
    std::fstream outFS(filename,ios::binary|ios::in|ios::out);
    outFS.seekp(0,ios::end);
    if (not outFS.is_open()) {
      std::cerr << "Error: '" << filename << "' can't be opened." << std::endl;
      exit(1);
    }
    std::shared_ptr<Agent> current, mdca; // most distant common ancestor
    // crawl LOD to find how many there will be.
    // nothing ot be done if we're already at the end
    if (mrca->ancestor == nullptr) return;
    // format:
    // header: (only if stream position is 0 - start of file)
    // generations (int) // num genes (int)
    // first genome in LOD (floats)
    // body:
    // num changes in this generation (int)
    // [for each change] // location (int) // new value (float)
    current = mrca;
    int generations_count{0};
    while (current->ancestor not_eq nullptr) {
      // make forward link
      current->ancestor->progenitor = current;
      // crawl to next one and count
      current = current->ancestor;
      ++generations_count;
    }
    mdca = current;
    Archive<std::fstream> serialize(outFS);

    // HEADER only if at pos 0 in file
    // serialize the initial genome of
    // most distant common ancestor on
    // remaining LOD.
    if (outFS.tellp() == 0) {
      serialize << generations_count;
      serialize << ENV::NKTREADMILL::alpha_beta_seed;
      serialize << GLB::N << GLB::K << GLB::G;
      serialize << GLB::speed_change; // rate of change
      serialize << Agent::num_genes;
      for (auto& e : mdca->genome) { serialize << e; }
      current = current->progenitor;
    } else {
      // update the generations count at front of file
      // (newvalue = oldvalue + morevalue)
      int previous_count;
      outFS.seekp(0);
      serialize >> previous_count; // read old
      generations_count += previous_count; // calculate new
      outFS.seekp(0);
      serialize << generations_count; // write new
      outFS.seekp(0,ios::end); // go back to appending
    }
    // proceed back up from mdca using forward links.
    // crawl the LOD and serialize each changeset
    int changeset_size;
    while (current->progenitor not_eq nullptr) {
      changeset_size = current->changeset.size();
      serialize << changeset_size;
      for (auto& change : current->changeset) {
        serialize << change.location << change.newvalue;
      }
      current = current->progenitor;
    }
    outFS << std::flush;
    outFS.close();
  }

  /* clean up (delete) the LOD after
   * a common ancestor. Typically, this
   * is the MRCA.
   */
  auto pruneAt(std::shared_ptr<Agent> mrca) -> void {
    std::shared_ptr<Agent> current = mrca;
    // get to mdca
    while (current->ancestor not_eq nullptr) {
      current->ancestor->progenitor = current;
      current = current->ancestor;
    }
    // now walk back up, clearing pointers
    while ( (current->progenitor not_eq nullptr) and (current->progenitor not_eq mrca) ) {
      current = current->progenitor;
      current->ancestor->progenitor = nullptr;
      current->ancestor = nullptr;
    }
    current->progenitor = nullptr;
    mrca->ancestor = nullptr;
  }

  /* Utility fn to find the depth of a unidirectional
   * linked list (lod). I don't think I use this anymore.
   */
  auto getDepthBelow(std::shared_ptr<Agent> mrca) -> int {
    std::shared_ptr<Agent> current = mrca;
    int depth {0};
    while (current->ancestor not_eq nullptr) {
      current = current->ancestor;
      ++depth;
    }
    return depth;
  }

  // specifier when loading lod file
  enum RECONSTRUCTION {SPARSE=0, FULL};

  /* Load lod data from disk.
   * Takes filename.
   * only loads generations specified by --pick [start,skip,end]
   * returns vector of only those lod generations.
   */
  auto load(const std::string& filename) -> std::vector<std::shared_ptr<Agent>> {
    std::ifstream inFS(filename,ios::binary|ios::in);
    if (not inFS.is_open()) {
      std::cerr << "Error: '" << filename << "' can't be opened." << std::endl;
      exit(1);
    }
    std::vector<std::shared_ptr<Agent>> lod;
    Archive<std::ifstream> deserialize(inFS);
    // load generations, gene count, first genome
    int generations, gene_count;
    deserialize >> generations;
    // if no one supplied end, then pick very end
    if (GLB::gen_pick.back() == -1) GLB::gen_pick.back() = generations;
    if (GLB::gen_pick.front() == -1) GLB::gen_pick.front() = generations;
    unsigned int space_to_reserve = (GLB::gen_pick.back()-GLB::gen_pick.front())/GLB::gen_pick[1]+1;
    lod.reserve(space_to_reserve);
    deserialize >> ENV::NKTREADMILL::alpha_beta_seed;
    deserialize >> GLB::N >> GLB::K >> GLB::G;
    deserialize >> GLB::speed_change; // rate of change
    deserialize >> gene_count;
    GLB::G = gene_count;
    Agent::configure({ .num_genes=GLB::G, .mu_point=GLB::point_mutation_rate/(GLB::G*GLB::G), .mu_vector=GLB::vector_mutation_rate/GLB::G });
    int network_size = gene_count*gene_count;
    lod.emplace_back(std::make_shared<Agent>(INIT::NONE));
    lod.back()->genome.resize(gene_count,gene_count);
    float genome_value;
    // read first genome
    for (int i=0; i<network_size; i++) {
      deserialize >> genome_value;
      lod.back()->genome(i) = genome_value;
    }
    int numchanges, location;
    float newvalue;
    // update first genome to start of picked generations, if they are different
    for (int i=1; i<GLB::gen_pick.front(); i++) {
      deserialize >> numchanges;
      lod[0]->new_mutations += numchanges;
      lod[0]->gen_of_evaluation = i;
      for (int changei=0; changei<numchanges; ++changei) {
        deserialize >> location >> newvalue;
        lod[0]->genome(location) = newvalue;
      }
    }
    // gen_pick [start,stride,end]
    for (int i=GLB::gen_pick.front()+GLB::gen_pick[1]; i<=GLB::gen_pick.back(); i+=GLB::gen_pick[1]) {
      lod.emplace_back(std::make_shared<Agent>(INIT::NONE));
      lod.back()->genome = lod[lod.size()-2]->genome;
      lod.back()->gen_of_evaluation = i;
      for (int ii=i-GLB::gen_pick[1]; ii<i; ++ii) {
        deserialize >> numchanges;
        lod.back()->new_mutations += numchanges;
        for (int changei=0; changei<numchanges; ++changei) {
          deserialize >> location >> newvalue;
          lod.back()->genome(location) = newvalue;
        }
      }
    }
    inFS.close();
    return lod;
  }
}

namespace ANALYSIS {

  auto inline showWDist(std::vector<std::shared_ptr<Agent>>& pop, const float& minval, const float& maxval, const int& bins) -> void {
    std::vector<int> dist(bins, 0);
    for (auto& agent : pop) {
      ++dist[int(floor(agent->fitness/maxval*(bins-1)))];
    }
    std::cout << std::string(bins,'=') << std::endl;
    int height = int(floor((*std::max_element(begin(pop), end(pop), [&](std::shared_ptr<Agent>& a, std::shared_ptr<Agent>& b){return a->fitness < b->fitness;}))->fitness/maxval*(bins-1)));
    while (height > 0) {
      for (int i=0; i<bins; i++) {
        if (dist[i] >= height) std::cout << '*';
        else                   std::cout << ' ';
      }
      std::cout << std::endl;
      --height;
    }
    std::cout << std::string(bins,'=') << std::endl;
  }

  struct RobustnessConfiguration {
    public:
    const std::shared_ptr<Agent>& agent;
    const unsigned int& maxmutations;
    const unsigned int& ntrials;
    const unsigned int& generation;
    const std::function<float(std::shared_ptr<Agent>)>& fitness_fn;
  };

  auto inline getRobustness(const RobustnessConfiguration& cfg) -> float {
    uvec mu_locs;
    fcolvec mu_newvals;
    fcolvec mu_oldvals;
    fmat& matrix = cfg.agent->genome;
    int genome_size = matrix.n_elem;
    std::shared_ptr<Agent> agent = cfg.agent;
    agent->gen_of_evaluation = cfg.generation;

    fcolvec avg_robustness(cfg.maxmutations); // the 'curve' of robustness
    fcolvec w(cfg.ntrials); // filled each detail level
    for (int num_mutations=1; num_mutations<=cfg.maxmutations; num_mutations++) {
      mu_locs.set_size(num_mutations);
      mu_newvals.set_size(num_mutations);
      mu_oldvals.set_size(num_mutations);
      for (int i=0; i<cfg.ntrials; i++) {
        // make mutations of matrix
        std::generate_n(begin(mu_locs), num_mutations, [&](){return drand()%genome_size;});
        fill::random(mu_newvals);
        mu_oldvals = matrix(mu_locs);
        matrix(mu_locs) = mu_newvals;
        // evaluate agent
        agent->initGRN();
        agent->runGRN(GLB::developmental_updates);
        agent->developPhenotypeFromGRN();
        w(i) = cfg.fitness_fn(agent);
        // reset matrix values
        matrix(mu_locs) = mu_oldvals;
      }
      // record avg robustness for particular mutation sample size (num_mutations-1)
      avg_robustness(num_mutations-1) = mean(w);
    }
    // return accumulation under the "robustness curve" for this individual
    return accu(avg_robustness);
  }
}

namespace RUN {
  /* evolution loop for running nk treadmill
   * with specified fitness function.
   */
  auto grn_nktreadmill_haploid_evolution(const std::function<float(std::shared_ptr<Agent>)> fitness_fn) {
    // init code
    auto seed = (GLB::seed == -1) ? getpid() : GLB::seed;
    print("rand_seed:",seed);
    srand(seed); dsrand(seed);
    Agent::configure({ .num_genes=GLB::G, .mu_point=GLB::point_mutation_rate/(GLB::G*GLB::G), .mu_vector=GLB::vector_mutation_rate/GLB::G });
    ENV::NKTREADMILL::init(seed);

    // zero out save file
    if (GLB::savefilename.size() > 0) {
      std::ofstream outFS(GLB::savefilename,ios::binary|ios::trunc|ios::out);
      if (not outFS.is_open()) {
        std::cerr << "Error: couldn't open '"+GLB::savefilename+"'" << std::endl;
        exit(1);
      }
      outFS.close();
    }

    std::vector<std::shared_ptr<Agent>> population, newpopulation;
    fcolvec fitnesses(GLB::pop_size);

    // create population
    for (int i=0; i<GLB::pop_size; i++) population.emplace_back(std::make_shared<Agent>(INIT::RANDOM));

    // loop generations
    float minW(FLT_MAX),maxW(FLT_MIN),meanW(0);
    for (GLB::current_generation=0; GLB::current_generation<GLB::generations_limit+1; GLB::current_generation++) {
      // evaluate population
      minW = FLT_MAX; maxW = FLT_MIN; meanW = 0.0f;
      for (int i=0; i<GLB::pop_size; i++) {
        if (not population[i]->developed) {
          population[i]->initGRN();
          population[i]->runGRN(GLB::developmental_updates);
          population[i]->developPhenotypeFromGRN();
        }
        population[i]->gen_of_evaluation = GLB::current_generation;

        // fitness functions
        //fitnesses(i) = ENV::MAXONES::evaluate(population[i]);
        //fitnesses(i) = ENV::NKTREADMILL::evaluate(population[i]);
        fitnesses(i) = fitness_fn(population[i]);

        population[i]->fitness = fitnesses(i);
        meanW += fitnesses(i);
        if (minW > fitnesses(i)) minW = fitnesses(i);
        if (maxW < fitnesses(i)) maxW = fitnesses(i);
      }

      // display running progress in intervals
      if (GLB::current_generation == 0) {print("time","\t","meanW","\t\t","maxW");}
      if ((GLB::current_generation % GLB::screen_update_interval)==0) {meanW /= GLB::pop_size; print(GLB::current_generation,'\t',meanW,'\t',maxW);}

      // display histogram
      if ( (GLB::hist_gens.size()>0) && (GLB::current_generation == GLB::hist_gens[0]) ) {
        ANALYSIS::showWDist(population,0,1,60);
        GLB::hist_gens.erase(begin(GLB::hist_gens), begin(GLB::hist_gens)+1);
      }

      // save and prune LOD in intervals
      if (GLB::savefilename.size() > 0) {
        if ((GLB::current_generation % GLB::lod_save_interval)==0) {
          auto [mrca, gens_back] = LOD::getMRCA(population);
          if (mrca not_eq nullptr) {
            LOD::save(GLB::savefilename, mrca);
            LOD::pruneAt(mrca);
          }
        }
      }

      // select new population
      SELECTION::complete_roulette(population, newpopulation, fitnesses, minW, maxW);
      // generation progression
      population.clear();
      population.swap(newpopulation);
    }
  }

  /* Analyzes the robustness of a lod
   * Produces 2 columns: generation robustness
   */
  auto analyze(const std::function<float(std::shared_ptr<Agent>)>& fitness_fn) -> void {
    // load() populates ENV::NKTREADMILL::alpha_beta_seed
    // load() only loads generations specified by --pick command line option [start,skip,end]
    // with default [0,1,-1] (where -1 indicates end)
    auto lod = LOD::load(GLB::loadfilename); // std::vector<std::shared_ptr<Agent>>
    ENV::NKTREADMILL::init(ENV::NKTREADMILL::alpha_beta_seed);
    std::ofstream outFS;
    if (GLB::savefilename.size() > 0) { outFS.open(GLB::savefilename,ios::out|ios::trunc); }

    // find best agent
    std::shared_ptr<Agent> best = ENV::NKTREADMILL::getMaxFitAgent({.generation=0});
    std::cout << "best phenotype: ";
    for (int i=0; i<GLB::N; i++) std::cout << best->phenotype[i];
    std::cout << endl;

    // print headers
    std::cout << "generation"; if (GLB::show_fitness) std::cout << ",fitness,norm_fitness"; if (GLB::show_robustness) std::cout << ",robustness"; if (GLB::show_phenotype) std::cout << ",phenotype";
    std::cout << std::endl;
    if (outFS.is_open()) {
      outFS << "generation"; if (GLB::show_fitness) outFS << ",fitness,norm_fitness"; if (GLB::show_robustness) outFS << ",robustness"; if (GLB::show_phenotype) outFS << ",phenotype";
      outFS << std::endl;
    }
    // loop through picked gens
    for (int i=0; i<lod.size(); ++i) {
      // reconstruct genome i
      lod[i]->initGRN();
      lod[i]->runGRN(GLB::developmental_updates);
      lod[i]->developPhenotypeFromGRN();
      std::cout << lod[i]->gen_of_evaluation;
      if (outFS.is_open()) outFS << lod[i]->gen_of_evaluation;
      // fitness inspection
      if (GLB::show_fitness) {
        // fitness functions
        //fitnesses(i) = ENV::MAXONES::evaluate(population[i]);
        //fitnesses(i) = ENV::NKTREADMILL::evaluate(population[i]);
        float w = fitness_fn(lod[i]);
        std::cout << "," << w << "," << w/best->fitness;
        if (outFS.is_open()) outFS << "," << w << "," << w/best->fitness;
      }
      // robustness inspection
      if (GLB::show_robustness) {
        float rmean = ANALYSIS::getRobustness({ .agent=lod[i], .maxmutations=GLB::maxmutations, .ntrials=GLB::ntrials, .generation=lod[i]->gen_of_evaluation, .fitness_fn=fitness_fn });
        std::cout << "," << rmean;
        if (outFS.is_open()) outFS << "," << rmean;
      }
      // phenotype inspection
      if (GLB::show_phenotype) {
        std::cout << ",";
        if (outFS.is_open()) outFS << ",";
        for (int n=0; n<GLB::N; n++) {
          std::cout << lod[i]->phenotype[n];
          if (outFS.is_open()) outFS << lod[i]->phenotype[n];
        }
      }
      // delta perfect genotype inspection
      if (GLB::show_deltaperfect) {
        std::cout << ",";
        if (outFS.is_open()) outFS << ",";
        std::shared_ptr<Agent> perfect;
        int genomic_differences {0};
        perfect = ENV::NKTREADMILL::getMaxFitAgent({.generation=lod[i]->gen_of_evaluation});
        for (int n=0; n<GLB::N; n++) {
          genomic_differences += int(lod[i]->phenotype[n] != perfect->phenotype[n]);
        }
        std::cout << genomic_differences;
        if (outFS.is_open()) outFS << genomic_differences;
      }
      // finish line
      std::cout << std::endl;
      if (outFS.is_open()) outFS << std::flush << std::endl;
    }
    if (outFS.is_open()) outFS.close();
  }

  /* Reports information about a lod file */
  auto info() -> void {
    // set selection to start and finish at end of lod
    // thereby only recreating the genome and other
    // information for 1 agent (the final one)
    GLB::gen_pick.front() = -1;
    GLB::gen_pick.back() = -1;
    // load() automatically populates ENV::NKTREADMILL::alpha_beta_seed
    auto lod = LOD::load(GLB::loadfilename); // std::vector<std::shared_ptr<Agent>>
    print("lod size:",lod[0]->gen_of_evaluation+1);
    print("lod mutations:",lod[0]->new_mutations, "(average",ceil(float(lod[0]->new_mutations)/lod[0]->gen_of_evaluation*100)/100,"per generation)");
    print("GRN genes:",GLB::G);
  }
}

int main(int argc, char* argv[]) {
  CLI::App app{"Evolvability Science Tool"}; // shrug
  app.require_subcommand(1); // allow only 1 subcommand (evolve, analyze, info)
  CLI::App* sc_evolve = app.add_subcommand("evolve","evolve a population");
  CLI::App* sc_test = app.add_subcommand("test","evolve a population on Max Ones");
  CLI::App* sc_analyze = app.add_subcommand("analyze","analyze a line of descent");
  CLI::App* sc_info = app.add_subcommand("info","show information about a line of descent");

  /* evolve subcommand */
  for (auto& sc : {sc_evolve,sc_test}) {
    sc->add_option("-p,--popsize",GLB::pop_size,"population size ["+std::to_string(GLB::pop_size)+"]")->check(CLI::Range(0,INT_MAX));
    sc->add_option("-N",GLB::N,"NK (N) 'genome' length")->check(CLI::Range(0,INT_MAX))->required();
    sc->add_option("-K",GLB::K,"NK (K) 'gene' length")->check(CLI::Range(0,INT_MAX))->required();
    sc->add_option("-G",GLB::G,"GRN (G) number of 'genes' (mat GxG)")->check(CLI::Range(0,INT_MAX))->required();
    sc->add_option("-d,--dev-update",GLB::developmental_updates,"number of developmental updates ["+std::to_string(GLB::developmental_updates)+"]");
    sc->add_option("--rate",GLB::speed_change,"rate of environmental change ["+std::to_string(GLB::speed_change)+"]");
    sc->add_option("-g,--gen",GLB::generations_limit,"number of generations ["+std::to_string(GLB::generations_limit)+"]")->check(CLI::Range(0,INT_MAX));
    sc->add_option("-s,--screen-update",GLB::screen_update_interval,"stats screen update interval ["+std::to_string(GLB::screen_update_interval)+"]")->check(CLI::Range(0,INT_MAX));
    sc->add_option("-l,--lod-update",GLB::lod_save_interval,"lod saving/prune interval ["+std::to_string(GLB::lod_save_interval)+"]")->check(CLI::Range(0,INT_MAX));
    sc->add_option("--hist-gens",GLB::hist_gens,"csv which gens to show hist")->delimiter(',');
    sc->add_option("--mup",GLB::point_mutation_rate,"genomic point-wise mutation rate ["+std::to_string(GLB::point_mutation_rate)+"]")->check(CLI::Bound(0.0,1.0));
    sc->add_option("--muv",GLB::vector_mutation_rate,"genomic vector-wise mutation rate ["+std::to_string(GLB::vector_mutation_rate)+"]")->check(CLI::Bound(0.0,1.0));
    sc->add_option("--seed",GLB::seed,"set the seed")->check(CLI::Range(0,INT_MAX));
    sc->add_flag("--use-point-mu",GLB::use_point_mu,"enable point-wise mutations ["+std::to_string(GLB::use_point_mu)+"]");
    sc->add_flag("--use-col-mu",GLB::use_col_mu,"enable vector-wise col mutations ["+std::to_string(GLB::use_col_mu)+"]");
    sc->add_flag("--use-row-mu",GLB::use_row_mu,"enable vector-wise row mutations ["+std::to_string(GLB::use_row_mu)+"]");
    sc->add_flag("--local-mu",GLB::local_locality_mu,"enable local 'point-offset' mutations ["+std::to_string(GLB::local_locality_mu)+"]");
    sc->add_option("--save",GLB::savefilename,"file to save lod to [none]");
  }

  /* analyze subcommand */
  sc_analyze->add_option("filename",GLB::loadfilename,"lod file to load from")->check(CLI::ExistingFile);
  sc_analyze->add_option("--save",GLB::savefilename,"file to save lod to [none]");
  sc_analyze->add_option("--pick",GLB::gen_pick,"gens to analyze from lod [0,1,-1] [beg,skp,end]")->delimiter(':')->expected(3);
  sc_analyze->add_option("-n,--ntrials",GLB::ntrials,"times to mutate and get fitness per resolution ["+std::to_string(GLB::ntrials)+"]")->check(CLI::PositiveNumber);
  sc_analyze->add_option("-m,--maxmutations",GLB::maxmutations,"starting from 1, up to # of concurrent mutations to investigate ["+std::to_string(GLB::maxmutations)+"]")->check(CLI::PositiveNumber);
  sc_analyze->add_option("--rate",GLB::speed_change,"rate of environmental change ["+std::to_string(GLB::speed_change)+"]");
  sc_analyze->add_flag("-w,--fitness",GLB::show_fitness,"enable inspecting the fitness");
  sc_analyze->add_flag("-p,--phenotype",GLB::show_phenotype,"enable inspecting the phenotype array");
  sc_analyze->add_flag("-r,--robustness",GLB::show_robustness,"enable inspecting the robustness");
  sc_analyze->add_flag("-d,--deltaperfect",GLB::show_deltaperfect,"enable inspecting the delta from ideal genome");

  /* info subcommand */
  sc_info->add_option("filename",GLB::loadfilename,"lod file to load from")->check(CLI::ExistingFile);

  CLI11_PARSE(app, argc, argv);

  /* available fitness functions */
  // ENV::MAXONES::evaluate
  // ENV::NKTREADMILL::evaluate

  if (sc_evolve->parsed()) {
    RUN::grn_nktreadmill_haploid_evolution(ENV::NKTREADMILL::evaluate);
  } else if (sc_test->parsed()) {
    RUN::grn_nktreadmill_haploid_evolution(ENV::MAXONES::evaluate);
  } else if (sc_analyze->parsed()) {
    RUN::analyze(ENV::NKTREADMILL::evaluate);
  } else if (sc_info->parsed()) {
    RUN::info();
  }

  return(0);
}

//int main() {
//  auto seed = getpid();
//  srand(seed); dsrand(seed);
//  Agent::configure({ .num_genes=GLB::G, .mu_point=GLB::point_mutation_rate/(GLB::G*GLB::G), .mu_vector=GLB::vector_mutation_rate/GLB::G });
//  std::vector<std::shared_ptr<Agent>> pop;
//  for (int i=0; i<100; i++) { pop.emplace_back(std::make_shared<Agent>(INIT::RANDOM)); }
//  for (int g=0; g<100; g++) {
//    auto agent = pop[g];
//    agent->initGRN();
//    std::ofstream outFS("states.csv",ios::trunc|ios::out);
//    std::ostream& out = outFS;
//    for (int i=0; i<800; i++) {
//      agent->runGRN(1);
//      agent->developPhenotypeFromGRN();
//      if (g > 97) {
//        for (int n=0; n<GLB::N-1; n++) {
//          out << agent->grn->state[n] << ",";
//          std::cout << agent->phenotype[n];
//        }
//        out << agent->grn->state[GLB::N-1] << std::endl;
//        std::cout << std::endl;
//      }
//    }
//  }
//  return(0);
//}

/* Arend's script-like main() */
//int main(int argc, char* argv[]) {
//  /* set up parameters */
//  using namespace GLB; // to easily set global variables below
//  // required
//  G=40;
//  N=20;
//  K=3;
//  // optional
//  generations_limit=10'000;
//  pop_size=100;
//  speed_change=0.001; // rate of environment change
//  developmental_updates = 200;
//  point_mutation_rate = 0.0005f;
//  local_locality_mu = false; // true = incremental genomic changes via mutation
//  savefilename="out.bin"; // non-empty saves
//
//  /* run evolution (& save data) */
//  // run evolution & write lod file with all genomes (5 million gen lod ~ 20 MB)
//  RUN::grn_nktreadmill_haploid_evolution(ENV::NKTREADMILL::evaluate);
//  /* currently available fitness functions */
//  // ENV::MAXONES::evaluate
//  // ENV::NKTREADMILL::evaluate
//
//  /* analyze subset of data (robustness, etc.) */
//  // do analysis on resulting lod file
//  loadfilename=savefilename;
//  savefilename="results.csv";
//  gen_pick={0,1'000,-1}; // [start,stride,end] -1 is "the final" if you don't know exactly
//  maxmutations=5; // try 1-mutations, 2-mutations.. n-mutations
//  ntrials=500; // how many trials to samples random n-mutations before averaging
//  // analyze robustness and write results to savefilename if non-empty string
//  RUN::analyze(ENV::NKTREADMILL::evaluate);
//}


//!
//! Contains objects and methods for querying.
//!
//! \file world/query.hpp
//! \author Alistair Reid
//! \author Darren Shen
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <Eigen/Core>
#include "datatype/world.hpp"
#include "world/interpolatorspec.hpp"
#include "world/grid.hpp"

namespace obsidian
{
  namespace world
  {
    //! Represents a sampling strategy for queries.
    //!
    enum SamplingStrategy
    {
      //! Sample with no anti-aliasing.
      noAA,

      //! Supersample at twice the resolution.
      superSamp2X,

      //! Supersample at four times the resolution.
      superSamp4X
    };

    // We could call getTransitions or getVoxels on this...
    struct Query
    {
      //! Create a new query.
      //!
      //! \param boundaries List of interpolator specifications.
      //! \param region The world specifications.
      //! \param resX The resolution in the X direction.
      //! \param resY The resolution in the X direction.
      //! \param resZ The resolution in the X direction.
      //! \param sampling The sampling strategy used.
      //!
      Query(const std::vector<InterpolatorSpec>& boundaries, WorldSpec region,
          uint resX, uint resY, uint resZ, SamplingStrategy sampling = SamplingStrategy::noAA)
        :resX(resX), resY(resY), resZ(resZ), sampling(sampling)
      {
        // auto-build a query for the region...
        // Patch into existing code
        Eigen::Vector2d startPt(region.xBounds.first, region.yBounds.first);
        Eigen::Vector2d endPt(region.xBounds.second, region.yBounds.second);
        Eigen::Vector2i resolution(resX, resY);

        positionXY = internalGrid2DX(region.xBounds, region.yBounds, resX, resY);

        edgeX  = Eigen::VectorXd::LinSpaced(resX+1, region.xBounds.first, region.xBounds.second);
        edgeY  = Eigen::VectorXd::LinSpaced(resY+1, region.yBounds.first, region.yBounds.second);
        edgeZ  = Eigen::VectorXd::LinSpaced(resZ+1, region.zBounds.first, region.zBounds.second);

        cacheInitialised = false;
        boundariesAreTimes = region.boundariesAreTimes;
        initInterpolatorWeights(boundaries);
      }

      // MT needs a scatter sample
      // note:locations should be n*2
      Query(const std::vector<InterpolatorSpec>& boundaries, WorldSpec region, Eigen::MatrixXd locations)
       : positionXY(locations), cacheInitialised(false)
      {
        boundariesAreTimes = region.boundariesAreTimes;
        initInterpolatorWeights(boundaries);
      }

      //! Initialise interpolator weights.
      //!
      //! \param boundaries List of interpolator specifications.
      //!
      void initInterpolatorWeights(const std::vector<InterpolatorSpec>& boundaries)
      {
        for (const InterpolatorSpec & boundary : boundaries)
        {
          interpolatorWeights.push_back(boundary.getWeights(positionXY).transpose());
        }
      }

      //! Default constructor for global objects
      Query(){}

      //! Get the number of points in the query.
      //!
      //! \return The number of points in the query.
      //!
      int numPoints() const
      {
        return positionXY.rows();
      }

      //! The control point resolution in the x direction (longitude).
      uint resX;

      //! The control point resolution in the y direction (latitude).
      uint resY;

      //! The control point resolution in the z direction (depth).
      uint resZ;

      //!
      Eigen::MatrixXd positionXY;

      //! A vector containing the
      Eigen::VectorXd edgeX;

      //!
      Eigen::VectorXd edgeY;

      //!
      Eigen::VectorXd edgeZ;

      //!
      SamplingStrategy sampling;

      //! Flag indicating whether the cache has been initialised
      bool cacheInitialised;

      //!
      bool boundariesAreTimes;

      //!
      std::vector<Eigen::MatrixXd> interpolatorWeights;
    };
  }
}


#ifndef CCPCA_HPP
#define CCPCA_HPP

#include "cpca.hpp"
#include <Eigen/Core>
#include <mutex>
#include <tuple>
#include <vector>

class CCPCA : public CPCA {
public:
  CCPCA(Eigen::Index const nComponents = 2, bool const standardize = true);

  Eigen::MatrixXf
  fitTransform(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
               bool const autoAlphaSelection = true, float const alpha = 0.0f,
               float const varThresRatio = 0.5f, bool parallel = true,
               unsigned int const nAlphas = 40, float const maxLogAlpha = 3.0f,
               bool const keepReports = false);
  void fit(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
           bool const autoAlphaSelection = true, float const alpha = 0.0f,
           float const varThresRatio = 0.5f, bool parallel = true,
           unsigned int const nAlphas = 40, float const maxLogAlpha = 3.0f,
           bool const keepReports = false);
  void fitWithBestAlpha(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                        float const varThresRatio = 0.5f, bool parallel = true,
                        unsigned int const nAlphas = 40,
                        float const maxLogAlpha = 3.0f,
                        bool const keepReports = false);
  void fitWithManualAlpha(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                          float const alpha = 0.0f);
  Eigen::MatrixXf transform(Eigen::MatrixXf const &X);
  float bestAlpha(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                  float const varThresRatio = 0.5f, bool parallel = true,
                  unsigned int const nAlphas = 40,
                  float const maxLogAlpha = 3.0f,
                  bool const keepReports = false);

  // first loading is the same with featContribs_
  Eigen::VectorXf getFeatContribs() { return featContribs_; };
  Eigen::VectorXf getScaledFeatContribs() {
    float absMax = featContribs_.array().abs().maxCoeff();
    return featContribs_ / absMax;
  };
  Eigen::MatrixXf getComponents() { return cpca_.getComponents(); };
  Eigen::VectorXf getComponent(Eigen::Index const index) {
    return cpca_.getComponent(index);
  };
  Eigen::VectorXf getFirstComponent() { return getComponent(0); };
  Eigen::RowVectorXf getEigenvalues() { return cpca_.getEigenvalues(); };
  float getEigenvalue(Eigen::Index const index) {
    return cpca_.getEigenvalue(index);
  };
  Eigen::MatrixXf getLoadings() { return cpca_.getLoadings(); };
  Eigen::VectorXf getLoading(Eigen::Index const index) {
    return cpca_.getLoading(index);
  };
  float getBestAlpha() { return bestAlpha_; };
  std::vector<std::tuple<float, float, float, Eigen::VectorXf, Eigen::VectorXf,
                         Eigen::VectorXf>>
  getReports() {
    return reports_;
  };

private:
  CPCA cpca_;
  float bestAlpha_;
  Eigen::MatrixXf concatMat_;
  Eigen::VectorXf featContribs_;

  // this tuple is used because pybind11 cannot handle structure well
  // the order is alpha, discrepancy score, var, projK, projR, loadings
  std::vector<std::tuple<float, float, float, Eigen::VectorXf, Eigen::VectorXf,
                         Eigen::VectorXf>>
      reports_;
  std::mutex mtx;
  std::pair<float, float> scaledVar(Eigen::VectorXf const &a,
                                    Eigen::VectorXf const &b);
  float binWidthScott(Eigen::VectorXf const &vals);
  int histIntersect(Eigen::VectorXf const &a, Eigen::VectorXf const &b);
};

#endif // CCPCA_HPP


/***************************************************************************
 * Copyright 1998-2017 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxRender.                                       *
 *                                                                         *
 * 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 <stdexcept>
#include <boost/foreach.hpp>
#include <boost/regex.hpp>

#include "slg/film/film.h"
#include "slg/kernels/kernels.h"
#include "slg/film/imagepipeline/plugins/vignetting.h"
#include "luxrays/kernels/kernels.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Vignetting plugin
//------------------------------------------------------------------------------

BOOST_CLASS_EXPORT_IMPLEMENT(slg::VignettingPlugin)

VignettingPlugin::VignettingPlugin(const float s) : scale(s) {
#if !defined(LUXRAYS_DISABLE_OPENCL)
	applyKernel = NULL;
#endif
}

VignettingPlugin::~VignettingPlugin() {
#if !defined(LUXRAYS_DISABLE_OPENCL)
	delete applyKernel;
#endif
}

ImagePipelinePlugin *VignettingPlugin::Copy() const {
	return new VignettingPlugin(scale);
}

//------------------------------------------------------------------------------
// CPU version
//------------------------------------------------------------------------------

void VignettingPlugin::Apply(Film &film, const u_int index) {
	Spectrum *pixels = (Spectrum *)film.channel_IMAGEPIPELINEs[index]->GetPixels();

	const u_int width = film.GetWidth();
	const u_int height = film.GetHeight();
	const float invWidth = 1.f / width;
	const float invHeight = 1.f / height;

	#pragma omp parallel for
	for (
			// Visual C++ 2013 supports only OpenMP 2.5
#if _OPENMP >= 200805
			unsigned
#endif
			int y = 0; y < height; ++y) {
		for (u_int x = 0; x < width; ++x) {
			const u_int index = x + y * width;

			if (*(film.channel_FRAMEBUFFER_MASK->GetPixel(index))) {
				const float nx = x * invWidth;
				const float ny = y * invHeight;
				const float xOffset = (nx - .5f) * 2.f;
				const float yOffset = (ny - .5f) * 2.f;
				const float tOffset = sqrtf(xOffset * xOffset + yOffset * yOffset);

				// Normalize to range [0.f - 1.f]
				const float invOffset = 1.f - (fabsf(tOffset) * 1.42f);
				float vWeight = Lerp(invOffset, 1.f - scale, 1.f);

				pixels[index].c[0] *= vWeight;
				pixels[index].c[1] *= vWeight;
				pixels[index].c[2] *= vWeight;
			}
		}
	}
}

//------------------------------------------------------------------------------
// OpenCL version
//------------------------------------------------------------------------------

#if !defined(LUXRAYS_DISABLE_OPENCL)
void VignettingPlugin::ApplyOCL(Film &film, const u_int index) {
	if (!applyKernel) {
		// Compile sources
		const double tStart = WallClockTime();

		cl::Program *program = ImagePipelinePlugin::CompileProgram(
				film,
				"-D LUXRAYS_OPENCL_KERNEL -D SLG_OPENCL_KERNEL",
				slg::ocl::KernelSource_plugin_vignetting_funcs,
				"VignettingPlugin");

		SLG_LOG("[VignettingPlugin] Compiling VignettingPlugin_Apply Kernel");
		applyKernel = new cl::Kernel(*program, "VignettingPlugin_Apply");

		delete program;

		// Set kernel arguments
		u_int argIndex = 0;
		applyKernel->setArg(argIndex++, film.GetWidth());
		applyKernel->setArg(argIndex++, film.GetHeight());
		applyKernel->setArg(argIndex++, *(film.ocl_IMAGEPIPELINE));
		applyKernel->setArg(argIndex++, *(film.ocl_FRAMEBUFFER_MASK));
		applyKernel->setArg(argIndex++, scale);

		const double tEnd = WallClockTime();
		SLG_LOG("[VignettingPlugin] Kernels compilation time: " << int((tEnd - tStart) * 1000.0) << "ms");
	}

	film.oclIntersectionDevice->GetOpenCLQueue().enqueueNDRangeKernel(*applyKernel,
			cl::NullRange, cl::NDRange(RoundUp(film.GetWidth() * film.GetHeight(), 256u)), cl::NDRange(256));
}
#endif


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NQS_BOSONHUBBARD_HPP
#define NQS_BOSONHUBBARD_HPP

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <memory>
#include <vector>

#include "Graph/graph.hpp"
#include "Hilbert/abstract_hilbert.hpp"
#include "Utils/exceptions.hpp"
#include "Utils/messages.hpp"
#include "abstract_operator.hpp"

namespace nqs {

// Heisenberg model on an arbitrary graph
class BoseHubbard : public AbstractOperator {
  const AbstractGraph &graph_;

  int nsites_;

  // cutoff in occupation number
  int nmax_;

  double U_;
  double V_;

  double mu_;

  // list of bonds for the interaction part
  std::vector<std::vector<int>> bonds_;

 public:
  using VectorType = AbstractOperator::VectorType;
  using VectorRefType = AbstractOperator::VectorRefType;
  using VectorConstRefType = AbstractOperator::VectorConstRefType;

  BoseHubbard(std::shared_ptr<const AbstractHilbert> hilbert, double U,
              double V = 0., double mu = 0.)
      : AbstractOperator(hilbert),
        graph_(hilbert->GetGraph()),
        nsites_(hilbert->Size()),
        U_(U),
        V_(V),
        mu_(mu) {
    nmax_ = hilbert->LocalSize() - 1;
    Init();
  }

  void Init() {
    GenerateBonds();
    InfoMessage() << "Bose Hubbard model created \n";
    InfoMessage() << "U= " << U_ << std::endl;
    InfoMessage() << "V= " << V_ << std::endl;
    InfoMessage() << "mu= " << mu_ << std::endl;
    InfoMessage() << "Nmax= " << nmax_ << std::endl;
  }

  void GenerateBonds() {
    auto adj = graph_.AdjacencyList();

    bonds_.resize(nsites_);

    for (int i = 0; i < nsites_; i++) {
      for (auto s : adj[i]) {
        if (s > i) {
          bonds_[i].push_back(s);
        }
      }
    }
  }

  void FindConn(VectorConstRefType v, std::vector<Complex> &mel,
                std::vector<std::vector<int>> &connectors,
                std::vector<std::vector<double>> &newconfs) const override {
    connectors.clear();
    connectors.resize(1);
    newconfs.clear();
    newconfs.resize(1);
    mel.resize(1);

    mel[0] = 0.;
    connectors[0].resize(0);
    newconfs[0].resize(0);

    for (int i = 0; i < nsites_; i++) {
      // chemical potential
      mel[0] -= mu_ * v(i);

      // on-site interaction
      mel[0] += 0.5 * U_ * v(i) * (v(i) - 1);

      for (auto bond : bonds_[i]) {
        // nn interaction
        mel[0] += V_ * v(i) * v(bond);
        // hopping
        if (v(i) > 0 && v(bond) < nmax_) {
          connectors.push_back(std::vector<int>({i, bond}));
          newconfs.push_back(std::vector<double>({v(i) - 1, v(bond) + 1}));
          mel.push_back(-std::sqrt(v(i)) * std::sqrt(v(bond) + 1));
        }
        if (v(bond) > 0 && v(i) < nmax_) {
          connectors.push_back(std::vector<int>({bond, i}));
          newconfs.push_back(std::vector<double>({v(bond) - 1, v(i) + 1}));
          mel.push_back(-std::sqrt(v(bond)) * std::sqrt(v(i) + 1));
        }
      }
    }
  }
};

}  // namespace nqs

#endif


#include <memory>
#include <string>

#include <Eigen/Core>
#include <aslam/common/hash-id.h>
#include <aslam/common/memory.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <maplab-common/unique-id.h>
#include <posegraph/unique-id.h>

#include "vi-map/mission-baseframe.h"
#include "vi-map/unique-id.h"
#include "vi-map/vi_map.pb.h"

class ViwlsGraph : public ::testing::Test {
 protected:
  void constructBaseframe();
  void serializeAndDeserialize();

  vi_map::MissionBaseFrame::UniquePtr initial_baseframe_;
  vi_map::MissionBaseFrame::UniquePtr baseframe_from_msg_;

  pose::Transformation T_G_M_;
  Eigen::Matrix<double, 6, 6> T_G_M_covariance_;
  vi_map::MissionBaseFrameId mission_baseframe_id_;
};

void ViwlsGraph::serializeAndDeserialize() {
  vi_map::proto::MissionBaseframe baseframe_proto, proto_from_msg;
  initial_baseframe_->serialize(&baseframe_proto);
  std::string serialized_baseframe = baseframe_proto.SerializeAsString();

  proto_from_msg.ParseFromString(serialized_baseframe);

  baseframe_from_msg_ = aligned_unique<vi_map::MissionBaseFrame>();
  baseframe_from_msg_->deserialize(initial_baseframe_->id(), proto_from_msg);
}

void ViwlsGraph::constructBaseframe() {
  T_G_M_.setRandom();
  T_G_M_covariance_.setRandom();
  mission_baseframe_id_.fromHexString("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa1");

  initial_baseframe_ = aligned_unique<vi_map::MissionBaseFrame>(
      mission_baseframe_id_, T_G_M_, T_G_M_covariance_);
}

TEST_F(ViwlsGraph, ViwlsBaseframeSerializeAndDeserializeTest) {
  constructBaseframe();
  serializeAndDeserialize();

  EXPECT_EQ(initial_baseframe_->id(), baseframe_from_msg_->id());
  EXPECT_NEAR_ASLAM_TRANSFORMATION(
      initial_baseframe_->get_T_G_M(), baseframe_from_msg_->get_T_G_M(), 1e-15);
  EXPECT_NEAR_EIGEN(
      initial_baseframe_->get_T_G_M_Covariance(),
      baseframe_from_msg_->get_T_G_M_Covariance(), 1e-15);
}

MAPLAB_UNITTEST_ENTRYPOINT


/**
 * @file fit_python.cpp
 * @date 2021-08-16
 *
 * Defines the function fitPython in KMedoidsWrapper class
 * which is used in Python bindings.
 *
 */

#include "kmedoids_pywrapper.hpp"

#include <carma>
#include <armadillo>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>

namespace py = pybind11;

/**
 * \brief Python binding for fitting a KMedoids object to the
 *
 * This is the primary function of the KMedoids module: this finds the build and swap
 * medoids for the desired data
 *
 * @param inputData Input data to find the medoids of
 * @param loss The loss function used during medoid computation
 * @param k The number of medoids to compute
 * @param logFilename The name of the outputted log file
 */
void KMedoidsWrapper::fitPython(const py::array_t<double>& inputData,
                                const std::string& loss,
                                const std::string& logFilename,
                                py::kwargs kw
)
{
    // throw an error if the number of medoids is not specified in either
    // the KMedoids object or the fitPython function
    try {
        if (KMedoids::getNMedoids() == 0) { // Check for 0 as NULL
            if (kw.size() == 0) {
                throw py::value_error("Must specify number of medoids via n_medoids in KMedoids or k in fit function.");
            }
        }
    } catch (py::value_error &e) {
        // Throw it again (pybind11 will raise ValueError)
        throw;
    }
    // if k is specified here, we set the number of medoids as k and override previous value
    if ((kw.size() != 0) && (kw.contains("k"))) {
        KMedoids::setNMedoids(py::cast<int>(kw["k"]));
    }
    KMedoids::setLogFilename(logFilename);
    KMedoids::fit(carma::arr_to_mat<double>(inputData), loss);
}

void fit_python(py::class_<KMedoidsWrapper> &cls) {
    cls.def("fit", &KMedoidsWrapper::fitPython);
}