#include <iostream>
#include <tr1/memory>

#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "filelib.h"
#include "dict.h"
#include "sampler.h"
#include "ccrp.h"

using namespace std;
using namespace std::tr1;
namespace po = boost::program_options;

Dict d; // global dictionary

string Join(char joiner, const vector<int>& phrase) {
  ostringstream os;
  for (int i = 0; i < phrase.size(); ++i) {
    if (i > 0) os << joiner;
    os << d.Convert(phrase[i]);
  }
  return os.str();
}

ostream& operator<<(ostream& os, const vector<int>& phrase) {
  for (int i = 0; i < phrase.size(); ++i)
    os << (i == 0 ? "" : " ") << d.Convert(phrase[i]);
  return os;
}

struct UnigramLM {
  explicit UnigramLM(const string& fname) {
    ifstream in(fname.c_str());
    assert(in);
  }

  double logprob(int word) const {
    assert(word < freqs_.size());
    return freqs_[word];
  }

  vector<double> freqs_;
};

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("samples,s",po::value<unsigned>()->default_value(1000),"Number of samples")
        ("input,i",po::value<string>(),"Read file from")
        ("random_seed,S",po::value<uint32_t>(), "Random seed")
        ("write_cdec_grammar,g", po::value<string>(), "Write cdec grammar to this file")
        ("write_cdec_weights,w", po::value<string>(), "Write cdec weights to this file")
        ("poisson_length,p", "Use a Poisson distribution as the length of a phrase in the base distribuion")
        ("no_hyperparameter_inference,N", "Disable hyperparameter inference");
  po::options_description clo("Command line options");
  clo.add_options()
        ("config", po::value<string>(), "Configuration file")
        ("help,h", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help") || (conf->count("input") == 0)) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

void ReadCorpus(const string& filename, vector<vector<int> >* c, set<int>* vocab) {
  c->clear();
  istream* in;
  if (filename == "-")
    in = &cin;
  else
    in = new ifstream(filename.c_str());
  assert(*in);
  string line;
  while(*in) {
    getline(*in, line);
    if (line.empty() && !*in) break;
    c->push_back(vector<int>());
    vector<int>& v = c->back();
    d.ConvertWhitespaceDelimitedLine(line, &v);
    for (int i = 0; i < v.size(); ++i) vocab->insert(v[i]);
  }
  if (in != &cin) delete in;
}

double log_poisson(unsigned x, const double& lambda) {
  assert(lambda > 0.0);
  return log(lambda) * x - lgamma(x + 1) - lambda;
}

struct UniphraseLM {
  UniphraseLM(const vector<vector<int> >& corpus,
              const set<int>& vocab,
              const po::variables_map& conf) :
    phrases_(1,1,1,1),
    gen_(1,1,1,1),
    corpus_(corpus),
    uniform_word_(1.0 / vocab.size()),
    gen_p0_(0.5),
    p_end_(0.5),
    use_poisson_(conf.count("poisson_length") > 0) {}

  double p0(const vector<int>& phrase) const {
    static vector<double> p0s(10000, 0.0);
    assert(phrase.size() < 10000);
    double& p = p0s[phrase.size()];
    if (p) return p;
    p = exp(log_p0(phrase));
    if (!p) {
      cerr << "0 prob phrase: " << phrase << "\nAssigning std::numeric_limits<double>::min()\n";
      p = std::numeric_limits<double>::min();
    }
    return p;
  }

  double log_p0(const vector<int>& phrase) const {
    double len_logprob;
    if (use_poisson_)
      len_logprob = log_poisson(phrase.size(), 1.0);
    else
      len_logprob = log(1 - p_end_) * (phrase.size() -1) + log(p_end_);
    return log(uniform_word_) * phrase.size() + len_logprob;
  }

  double llh() const {
    double llh = gen_.log_crp_prob();
    llh += gen_.num_tables(false) * log(gen_p0_) +
           gen_.num_tables(true) * log(1 - gen_p0_);
    double llhr = phrases_.log_crp_prob();
    for (CCRP<vector<int> >::const_iterator it = phrases_.begin(); it != phrases_.end(); ++it) {
      llhr += phrases_.num_tables(it->first) * log_p0(it->first);
      //llhr += log_p0(it->first);
      if (!isfinite(llh)) {
        cerr << it->first << endl;
        cerr << log_p0(it->first) << endl;
        abort();
      }
    }
    return llh + llhr;
  }

  void Sample(unsigned int samples, bool hyp_inf, MT19937* rng) {
    cerr << "Initializing...\n";
    z_.resize(corpus_.size());
    int tc = 0;
    for (int i = 0; i < corpus_.size(); ++i) {
      const vector<int>& line = corpus_[i];
      const int ls = line.size();
      const int last_pos = ls - 1;
      vector<bool>& z = z_[i];
      z.resize(ls);
      int prev = 0;
      for (int j = 0; j < ls; ++j) {
        z[j] = rng->next() < 0.5;
        if (j == last_pos) z[j] = true;  // break phrase at the end of the sentence
        if (z[j]) {
          const vector<int> p(line.begin() + prev, line.begin() + j + 1);
          phrases_.increment(p, p0(p), rng);
          //cerr << p << ": " << p0(p) << endl;
          prev = j + 1;
          gen_.increment(false, gen_p0_, rng);
          ++tc; // remove
        }
      }
      ++tc;
      gen_.increment(true, 1.0 - gen_p0_, rng); // end of utterance
    }
    cerr << "TC: " << tc << endl;
    cerr << "Initial LLH: " << llh() << endl;
    cerr << "Sampling...\n";
    cerr << gen_ << endl;
    for (int s = 1; s < samples; ++s) {
      cerr << '.';
      if (s % 10 == 0) {
        cerr << " [" << s;
        if (hyp_inf) ResampleHyperparameters(rng);
        cerr << " LLH=" << llh() << "]\n";
        vector<int> z(z_[0].size(), 0);
        //for (int j = 0; j < z.size(); ++j) z[j] = z_[0][j];
        //SegCorpus::Write(corpus_[0], z, d);
      }
      for (int i = 0; i < corpus_.size(); ++i) {
        const vector<int>& line = corpus_[i];
        const int ls = line.size();
        const int last_pos = ls - 1;
        vector<bool>& z = z_[i];
        int prev = 0;
        for (int j = 0; j < last_pos; ++j) { // don't resample last position
          int next = j+1;  while(!z[next]) { ++next; }
          const vector<int> p1p2(line.begin() + prev, line.begin() + next + 1);
          const vector<int> p1(line.begin() + prev, line.begin() + j + 1);
          const vector<int> p2(line.begin() + j + 1, line.begin() + next + 1);

          if (z[j]) {
            phrases_.decrement(p1, rng);
            phrases_.decrement(p2, rng);
            gen_.decrement(false, rng);
            gen_.decrement(false, rng);
          } else {
            phrases_.decrement(p1p2, rng);
            gen_.decrement(false, rng);
          }

          const double d1 = phrases_.prob(p1p2, p0(p1p2)) * gen_.prob(false, gen_p0_);
          double d2 = phrases_.prob(p1, p0(p1)) * gen_.prob(false, gen_p0_);
          phrases_.increment(p1, p0(p1), rng);
          gen_.increment(false, gen_p0_, rng);
          d2 *= phrases_.prob(p2, p0(p2)) * gen_.prob(false, gen_p0_);
          phrases_.decrement(p1, rng);
          gen_.decrement(false, rng);
          z[j] = rng->SelectSample(d1, d2);

          if (z[j]) {
            phrases_.increment(p1, p0(p1), rng);
            phrases_.increment(p2, p0(p2), rng);
            gen_.increment(false, gen_p0_, rng);
            gen_.increment(false, gen_p0_, rng);
            prev = j + 1;
          } else {
            phrases_.increment(p1p2, p0(p1p2), rng);
            gen_.increment(false, gen_p0_, rng);
          }
        }
      }
    }
//    cerr << endl << endl << gen_ << endl << phrases_ << endl;
    cerr << gen_.prob(false, gen_p0_) << " " << gen_.prob(true, 1 - gen_p0_) << endl;
  }

  void WriteCdecGrammarForCurrentSample(ostream* os) const {
    CCRP<vector<int> >::const_iterator it = phrases_.begin();
    for (; it != phrases_.end(); ++it) {
      (*os) << "[X] ||| " << Join(' ', it->first) << " ||| "
                          << Join('_', it->first) << " ||| C=1 P="
                          << log(phrases_.prob(it->first, p0(it->first))) << endl;
    }
  }

  double OOVUnigramLogProb() const {
    vector<int> x(1,99999999);
    return log(phrases_.prob(x, p0(x)));
  }

  void ResampleHyperparameters(MT19937* rng) {
    phrases_.resample_hyperparameters(rng);
    gen_.resample_hyperparameters(rng);
    cerr << " d=" << phrases_.discount() << ",c=" << phrases_.concentration();
  }

  CCRP<vector<int> > phrases_;
  CCRP<bool> gen_;
  vector<vector<bool> > z_;   // z_[i] is there a phrase boundary after the ith word
  const vector<vector<int> >& corpus_;
  const double uniform_word_;
  const double gen_p0_;
  const double p_end_; // in base length distribution, p of the end of a phrase
  const bool use_poisson_;
};

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  shared_ptr<MT19937> prng;
  if (conf.count("random_seed"))
    prng.reset(new MT19937(conf["random_seed"].as<uint32_t>()));
  else
    prng.reset(new MT19937);
  MT19937& rng = *prng;

  vector<vector<int> > corpus;
  set<int> vocab;
  ReadCorpus(conf["input"].as<string>(), &corpus, &vocab);
  cerr << "Corpus size: " << corpus.size() << " sentences\n";
  cerr << "Vocabulary size: " << vocab.size() << " types\n";

  UniphraseLM ulm(corpus, vocab, conf);
  ulm.Sample(conf["samples"].as<unsigned>(), conf.count("no_hyperparameter_inference") == 0, &rng);
  cerr << "OOV unigram prob: " << ulm.OOVUnigramLogProb() << endl;

  for (int i = 0; i < corpus.size(); ++i)
//    SegCorpus::Write(corpus[i], shmmlm.z_[i], d);
 ;
  if (conf.count("write_cdec_grammar")) {
    string fname = conf["write_cdec_grammar"].as<string>();
    cerr << "Writing model to " << fname << " ...\n";
    WriteFile wf(fname);
    ulm.WriteCdecGrammarForCurrentSample(wf.stream());
  }

  if (conf.count("write_cdec_weights")) {
    string fname = conf["write_cdec_weights"].as<string>();
    cerr << "Writing weights to " << fname << " .\n";
    WriteFile wf(fname);
    ostream& os = *wf.stream();
    os << "# make C smaller to use more phrases\nP 1\nPassThrough " << ulm.OOVUnigramLogProb() << "\nC -3\n";
  }

  return 0;
}


#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <lightsfm/vector2d.hpp>
#include <lightsfm/angle.hpp>
#include <lightpomcp/Random.hpp>
#include <upo_msgs/PersonPoseArrayUPO.h>
#include <nav_msgs/Odometry.h>
#include <vector>
#include <tf/transform_listener.h>
#include <teresa_wsbs/Info.h>
#include <animated_marker_msgs/AnimatedMarkerArray.h>
#include <fstream>
#include <plab/PLabInfo.h>

#define foreach BOOST_FOREACH

std::vector<double> distance_to_target;
std::vector<utils::Vector2d> target_force;
std::vector<utils::Vector2d> target_trajectory;
utils::Vector2d robot_position;
utils::Vector2d target_position;
utils::Angle target_yaw;
std::vector<utils::Vector2d> people_position;
std::vector<utils::Angle> people_yaw;
std::vector<double> confort;
std::vector<unsigned> target_detected;
std::ofstream out_stream;
double lambda;
double robot_area_radius;
int confort_particles;
bool available_groundtruth=true;
std::string bag_name;

// return true if x is in inside the intimate distance(*factor) of person p with yaw
bool intimateDistance(const utils::Vector2d& x, const utils::Vector2d& p, const utils::Angle& yaw, double factor=1.0)
{
	utils::Vector2d n = (p - x).normalized();
	utils::Vector2d e(yaw.cos(),yaw.sin());
	double cos_phi = -n.dot(e);
	double w = lambda + (1-lambda)*(1+cos_phi)*0.5;
	w *= factor;
	double dis = (p-x).norm();
	return dis < w;
}

double calculateConfort()
{
	utils::Vector2d x;
	double squared_robot_area_radius = robot_area_radius*robot_area_radius;
	double score=0;
	for (int i = 0; i< confort_particles; i++) {
		// Select a random particle in the robot area
		do {
			x.set(utils::RANDOM()*robot_area_radius, utils::RANDOM()*robot_area_radius);
		} while (x[0]*x[0] + x[1]*x[1] > squared_robot_area_radius);
		unsigned quadrant = utils::RANDOM(4);
		if (quadrant==2 || quadrant==3) {
			x.setX(-x[0]);
		}
		if (quadrant==1 || quadrant==2) {
			x.setY(-x[1]);
		}
		x+=robot_position;
		bool tooCloseToSomebody=false;
		for (unsigned j=0;j<people_position.size();j++) {
			if (intimateDistance(x,people_position[j],people_yaw[j])) {
				tooCloseToSomebody=true;
				break;
			}
		}
		if (tooCloseToSomebody) {
			continue;
		}
		if (intimateDistance(x,target_position,target_yaw,3.0)) {
			score += 1.0;
		} else if ((x - target_position).norm() < 3.0) {
			score += 0.5;
		}
	}
	score /= (double)confort_particles;
	return score;

}

bool calculateMetrics(int trajectory_index, double time)
{
	bool valid_trajectory = distance_to_target.size()>0 && target_detected.size()>0 && confort.size()>0;
	if (!valid_trajectory) {
		distance_to_target.clear();
		target_force.clear();
		target_detected.clear();
		confort.clear();
		target_trajectory.clear();

		return false;
	}

	out_stream<<bag_name<<"\t"<<trajectory_index<<"\t"<<time;
	if (distance_to_target.size()>0) {
		double d=0;
		for (unsigned i=0;i<distance_to_target.size();i++) {
			d+=distance_to_target[i];
		}
		d /= (double) distance_to_target.size();
		ROS_INFO("Average distance to target: %f m",d);
		out_stream<<"\t"<<d;
		distance_to_target.clear();
	} else {
		out_stream<<"\tInf";
	}
	if (target_force.size()>1 && target_trajectory.size()>1) {
		double work=0;
		for (unsigned i=0;i<std::min(target_trajectory.size()-1,target_force.size()-1);i++) {
			utils::Vector2d d = target_trajectory[i+1] - target_trajectory[i];
			//work += target_force[i].norm() * d.norm() * d.angleTo(target_force[i]).cos();
			work += target_force[i].dot(target_trajectory[i+1] - target_trajectory[i]);
		}
		out_stream<<"\t"<<work;
		ROS_INFO("Target Work: %f",work);
		target_force.clear();
		target_trajectory.clear();
	} else {
		out_stream<<"\tInf";
	}

	if (confort.size()>0) {
		double average_confort=0;
		for (unsigned i=0;i<confort.size();i++) {
			average_confort+=confort[i];
		}
		average_confort /= (double) confort.size();
		out_stream<<"\t"<<average_confort;
		ROS_INFO("Average confort: %f",average_confort);
		confort.clear();
	} else {
		out_stream<<"\t0";
	}
	if (target_detected.size()>0) {
		double average_target_detected=0;
		for (unsigned i=0;i<target_detected.size();i++) {
			average_target_detected+=target_detected[i];
		}
		average_target_detected /= (double) target_detected.size();
		out_stream<<"\t"<<average_target_detected;
		ROS_INFO("Target detected: %f",average_target_detected);
		target_detected.clear();

	} else {
		out_stream<<"\t0";
	}
	out_stream<<"\n";
	return true;
}

void readPeople(const upo_msgs::PersonPoseArrayUPO::ConstPtr& people_ptr)
{
	people_position.resize(people_ptr->personPoses.size());
	people_yaw.resize(people_ptr->personPoses.size());
	for (unsigned i=0; i< people_ptr->personPoses.size(); i++) {
		people_position[i].set(people_ptr->personPoses[i].position.x,people_ptr->personPoses[i].position.y);
		people_yaw[i] = utils::Angle::fromRadian(tf::getYaw(people_ptr->personPoses[i].orientation));
	}
}

void readGroundtruth(animated_marker_msgs::AnimatedMarkerArray::ConstPtr& marker_array_ptr)
{
	people_position.resize(marker_array_ptr->markers.size());
	people_yaw.resize(marker_array_ptr->markers.size());
	for (unsigned i = 0; i<marker_array_ptr->markers.size(); i++) {
		people_position[i].set(marker_array_ptr->markers[i].pose.position.x,marker_array_ptr->markers[i].pose.position.y);
		people_yaw[i] = utils::Angle::fromRadian(tf::getYaw(marker_array_ptr->markers[i].pose.orientation));
		people_yaw[i] += utils::Angle::fromRadian(M_PI*0.5);
		if (marker_array_ptr->markers[i].color.r > 0.5) {
			target_position = people_position[i];
			target_yaw = people_yaw[i];
		}
	}
	distance_to_target.push_back((robot_position-target_position).norm());
	confort.push_back(calculateConfort());
}

void readTarget(animated_marker_msgs::AnimatedMarkerArray::ConstPtr& marker_array_ptr)
{
	target_position.set(marker_array_ptr->markers[0].pose.position.x,marker_array_ptr->markers[0].pose.position.y);
	target_yaw = utils::Angle::fromRadian(tf::getYaw(marker_array_ptr->markers[0].pose.orientation));
	target_yaw += utils::Angle::fromRadian(M_PI*0.5);
	if (!available_groundtruth) {
		target_trajectory.push_back(target_position);
	}
	distance_to_target.push_back((robot_position-target_position).norm());
	confort.push_back(calculateConfort());
}

void readOdom(const nav_msgs::Odometry::ConstPtr& odom)
{
	robot_position.set(odom->pose.pose.position.x,odom->pose.pose.position.y);
}

void readInfo(const teresa_wsbs::Info::ConstPtr& info)
{
	if (info->status!=5 && info->status!=6) {
		return;
	}

	if (info->status == 6) {
		target_detected.push_back(0);
	} else {
		if (available_groundtruth) {
			utils::Vector2d target0(info->target_pose.x, info->target_pose.y);
			if ((target_position - target0).norm() < 0.3) {
				target_detected.push_back(1);
			} else {
				target_detected.push_back(0);
			}
		} else {
			target_detected.push_back(1);
		}
	}

	if (!available_groundtruth) {
		utils::Vector2d f(info->target_group_force.x, info->target_group_force.y);
		target_force.push_back(f);
	}

}

void readTargetForcesGroundtruth(plab::PLabInfo::ConstPtr& info)
{
	utils::Vector2d pos(info->target_pose.x,info->target_pose.y);
	//utils::Vector2d f(info->target_group_force.x,info->target_group_force.y);
	utils::Vector2d f(info->target_global_force.x,info->target_global_force.y);
	target_trajectory.push_back(pos);
	target_force.push_back(f);
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "wsbs_stats");
	ros::NodeHandle n;
	ros::NodeHandle pn("~");
	std::string  people_topic;
	std::string out_file;
	bool trajectory_running=false;
	unsigned trajectory_counter=0;
	bool use_clicked_points=false;

	double initial_time;
	pn.param<std::string>("bag_name",bag_name,"/home/ignacio/baseline.bag"); // Bag name
	pn.param<std::string>("people_topic",people_topic,"/people/navigation");
	pn.param<std::string>("out_file",out_file,"/home/ignacio/results.txt");
	pn.param<double>("robot_area_radius",robot_area_radius,0.5);
	pn.param<int>("confort_particles",confort_particles,1000);
	pn.param<bool>("use_clicked_points",use_clicked_points,true);  // false
	pn.param<bool>("available_groundtruth",available_groundtruth,false); // true
	pn.param<double>("lambda",lambda,0.59);
	if (argc!=1) {
		bag_name = argv[1];
	}
	std::vector<std::string> topics;

	topics.push_back("/odom");
	topics.push_back("/wsbs/controller/info");
	topics.push_back("/clicked_point");

	if (available_groundtruth) {
		topics.push_back("/plab/markers/people");
		topics.push_back("/plab/info");
	} else {
		topics.push_back(people_topic);
		topics.push_back("/wsbs/markers/target");
	}

  	out_stream.open (out_file, std::ios::app);
	//out_stream<<"File name trajectory index\tTime\tAverage Distance to Target\tTarget Work\tConfortTarget detected\n";
	rosbag::Bag bag;
	ROS_INFO("Opening bag %s...",bag_name.c_str());
	bag.open(bag_name,rosbag::bagmode::Read);
	ROS_INFO("Processing bag %s...",bag_name.c_str());

	rosbag::View view(bag, rosbag::TopicQuery(topics));
	ros::Time time;
	foreach(rosbag::MessageInstance const m, view) {
		time = m.getTime();
		if (m.getTopic().compare(people_topic)==0) {
			upo_msgs::PersonPoseArrayUPO::ConstPtr people_ptr = m.instantiate<upo_msgs::PersonPoseArrayUPO>();
			readPeople(people_ptr);
		} else if (m.getTopic().compare("/odom")==0) {
			nav_msgs::Odometry::ConstPtr odom_ptr = m.instantiate<nav_msgs::Odometry>();
			readOdom(odom_ptr);
		} else if (m.getTopic().compare("/wsbs/controller/info")==0) {
			//std::cout<<"A"<<std::endl;
			teresa_wsbs::Info::ConstPtr info_ptr = m.instantiate<teresa_wsbs::Info>();
			//std::cout<<"B"<<std::endl;
			readInfo(info_ptr);
			//std::cout<<"C"<<std::endl;
		} else if (m.getTopic().compare("/wsbs/markers/target")==0) {
			animated_marker_msgs::AnimatedMarkerArray::ConstPtr marker_array_ptr =
						m.instantiate<animated_marker_msgs::AnimatedMarkerArray>();
			readTarget(marker_array_ptr);
		} else if ((use_clicked_points && m.getTopic().compare("/clicked_point")==0) || (!use_clicked_points && !trajectory_running)) {
			trajectory_running = !trajectory_running;
			if (trajectory_running) {
				trajectory_counter++;
				initial_time = m.getTime().toSec();
				ROS_INFO("Trajectory N. %d begins at time %f s",trajectory_counter,m.getTime().toSec());
			} else {
				ROS_INFO("Trajectory N. %d finishes at time %f s",trajectory_counter,m.getTime().toSec());
				bool valid =calculateMetrics(trajectory_counter, m.getTime().toSec()-initial_time);
				if (!valid) {
					ROS_INFO("Invalid trajectory");
					trajectory_counter--;
				}
			}
		} else if (m.getTopic().compare("/plab/markers/people")==0) {
				animated_marker_msgs::AnimatedMarkerArray::ConstPtr marker_array_ptr =
						m.instantiate<animated_marker_msgs::AnimatedMarkerArray>();
				readGroundtruth(marker_array_ptr);
		} else if (m.getTopic().compare("/plab/info")==0) {
				plab::PLabInfo::ConstPtr info = m.instantiate<plab::PLabInfo>();
				m.instantiate<plab::PLabInfo>();
				readTargetForcesGroundtruth(info);
		}
	}
	if (!use_clicked_points) {
		ROS_INFO("Trajectory N. %d finishes at time %f s",trajectory_counter,time.toSec());
		bool valid =calculateMetrics(trajectory_counter, time.toSec()-initial_time);
		if (!valid) {
			ROS_INFO("Invalid trajectory");
			trajectory_counter--;
		}
	}
	out_stream.close();
	return 0;
}


#include <sstream>
#include <string>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv/cv.h>
#include <vector>
#include <cmath>
#include <cv_bridge/cv_bridge.h>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <ros/package.h>
//#include <pcl/io/pcd_io.h>
//#include <pcl/point_types.h>
#include "pcl_ros/point_cloud.h"
#include <object_assembly_msgs/Point2D.h>
#include <object_assembly_msgs/Points2D.h>
#include <object_assembly_msgs/PointCloudArray.h>
#include <object_assembly_msgs/FetchPCLsGivenLocations.h>
#include <object_assembly_msgs/FetchPCLs.h>
#include <object_assembly_msgs/FetchSurfaceParams.h>
#include <object_assembly_msgs/FetchForegroundMask.h>
//#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include "dbscan.h"

using namespace cv;

struct CropInfo {
    double top_margin;
    double left_margin;
    double width;
    double height;
};

class BackgroundRemovalNode {

public:

    BackgroundRemovalNode(float max_depth, float surf_thresh, bool recalculate_surface_params, std::string clustering_type, CropInfo crop_info, bool crop_for_table_detection, bool show_table_mask);

    void CameraInfoCallback(const sensor_msgs::CameraInfo &cinfo);

    void callback(const sensor_msgs::ImageConstPtr& depth_image, const sensor_msgs::PointCloud2ConstPtr& pointcloud);

    bool fetch_pcls_given_locations_service(
            object_assembly_msgs::FetchPCLsGivenLocations::Request &req,
            object_assembly_msgs::FetchPCLsGivenLocations::Response &res);

    bool fetch_pcls_service(
            object_assembly_msgs::FetchPCLs::Request &req,
            object_assembly_msgs::FetchPCLs::Response &res);

    bool fetch_surface_params_service(
            object_assembly_msgs::FetchSurfaceParams::Request &req,
            object_assembly_msgs::FetchSurfaceParams::Response &res);

    bool fetch_foreground_mask(
            object_assembly_msgs::FetchForegroundMask::Request &req,
            object_assembly_msgs::FetchForegroundMask::Response &res);

    void process_inputs();

    void response();

    void GetTableImage();

    int GetFlatSurfaceParams();

    int ImageSegmentation();

    int ImageSegmentation(object_assembly_msgs::Points2D points);

    void createOutputPCLs();

    void calculate_cropped_range();

    bool in_cropped_frame(int index);

private:
    float SURF_THRESH_;// = 0.97;
    float MAX_DEPTH_;// = 2000;
    int NUM_OBJECTS_ = 4;
    int KMEANS_NUM_ATTEMPTS_ = 3;
    std::string clustering_type_;
    //Pointers to the topic contents
    sensor_msgs::ImageConstPtr depth_image_ptr_;
    sensor_msgs::PointCloud2ConstPtr pointcloud_ptr_;
    //Depth image and PCL stored when needed
    Mat depth_image_, table_mask_;
    pcl::PointCloud<pcl::PointXYZ> input_pointcloud_;
    //Output PCLs (cropped around objects)
    //object_assembly_msgs::PointCloudArray output_pointclouds_;
    std::vector<pcl::PointCloud<pcl::PointXYZ>> output_pointclouds_;
    int height_;
    int width_;
    Mat table_image_;
    Mat labels_;
    std::vector<int> foreground_ind_;
    float surface_params_[3];
    bool topics_received_ = false;
    bool surface_params_found_ = true;
    CropInfo crop_info_;
    bool crop_image_;
    int min_cropped_x_;
    int min_cropped_y_;
    int max_cropped_x_;
    int max_cropped_y_;
    bool recalculate_surface_params_;
    bool crop_for_table_detection_;
    bool show_table_mask_;

    ros::NodeHandle node_handle_;
    ros::Publisher pointclouds_publisher_;
    ros::Publisher resized_depth_publisher_;
    ros::Publisher newcamerainfo_publisher_;

    sensor_msgs::CameraInfo cinfo_;
};


#include "planner_modules_pr2/module_param_cache.h"
#include <boost/foreach.hpp>
#define forEach BOOST_FOREACH
#include <sstream>

namespace planner_modules_pr2
{
using namespace std;
using namespace modules;
inline void writeKeyDouble(std::stringstream& ss, const string& name, double value, double precision = 0.01)
{
	ss << name;
	if (value < 0)
	{
		ss << "N";
	}
	ss << abs(lrint(1.0 / precision * value));
}

std::string createPoseParamString(const geometry_msgs::Pose& pose, double precPose, double precQuat)
{
	std::stringstream ss;
	ss.precision(0);
	ss << std::fixed;
	// the actual values don't matter as long as they are unique for this pose
	// cannot use doubles here, as param keys are not allowed to contain '.' or '-'
	writeKeyDouble(ss, "x", pose.position.x, precPose);
	writeKeyDouble(ss, "y", pose.position.y, precPose);
	writeKeyDouble(ss, "z", pose.position.z, precPose);
	writeKeyDouble(ss, "qx", pose.orientation.x, precQuat);
	writeKeyDouble(ss, "qy", pose.orientation.y, precQuat);
	writeKeyDouble(ss, "qz", pose.orientation.z, precQuat);
	writeKeyDouble(ss, "qw", pose.orientation.w, precQuat);

	return ss.str();
}

std::string createPoseParamString(const geometry_msgs::Pose2D& pose, double torso_position, double precPose, double precTheta)
{
	std::stringstream ss;
	ss.precision(0);
	ss << std::fixed;
	// the actual values don't matter as long as they are unique for this pose
	// cannot use doubles here, as param keys are not allowed to contain '.' or '-'
	writeKeyDouble(ss, "x", pose.x, precPose);
	writeKeyDouble(ss, "y", pose.y, precPose);
	writeKeyDouble(ss, "th", pose.theta, precTheta);
	writeKeyDouble(ss, "tp", torso_position, precPose);

	return ss.str();
}

bool splitNamedId(const string & namedId, string & name, int & id)
{
	string::size_type foundIdx = namedId.find_first_of("0123456789");
	if (foundIdx == string::npos)
		return false;

	name = namedId.substr(0, foundIdx);

	std::stringstream ss(namedId.substr(foundIdx));
	ss >> id;
	return true;
}

}


/**
 * \file main.cpp
 *
 *  \date Aug 4, 2013
 *  \author Michał Orynicz
 */

#include <boost/program_options.hpp>

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/stereo.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/xfeatures2d/nonfree.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <iostream>

#include "ConvenienceFunctions.hpp"
#include "StorageFunctions.hpp"

const int USER_TRIGGERED_EXIT = 0;

using std::cerr;
using std::endl;

void parseCommandline(const int& argc,
                      char** argv,
                      std::string& input,
                      int& leftCapture,
                      int& rightCapture)
{

  boost::program_options::options_description desc;

  desc.add_options()("help,h", "this help message")(
    "left,l",
    boost::program_options::value<int>(),
    "number of /dev/videoX device used as left camera")(
    "right,r",
    boost::program_options::value<int>(),
    "number of /dev/videoX device used as left camera")(
    "input,i",
    boost::program_options::value<std::string>(),
    "input file with calibration parameters");
  boost::program_options::variables_map vm;
  boost::program_options::store(
    boost::program_options::parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help"))
  {
    std::cout << desc << std::endl;
    throw USER_TRIGGERED_EXIT;
  }

  if (vm.count("input"))
  {
    input = vm["input"].as<std::string>();
  }

  if (vm.count("left"))
  {
    leftCapture = vm["left"].as<int>();
  }
  if (vm.count("right"))
  {
    rightCapture = vm["right"].as<int>();
  }
}

struct CameraCalibrationParams
{
  cv::Mat cameraMatrix;
  cv::Mat distCoefficients;
};

int main(int argc, char** argv)
{
  cv::VideoCapture capL, capR;

  std::string path;
  int leftDevice, rightDevice;

  cv::Mat projMatL, projMatR, rectL, rectR, disparityToDepthMap, lCM, rCM, lDC,
    rDC, R, T, E, F, lR, rR, lP, rP, Q;

  cv::Mat lMap1, lMap2, rMap1, rMap2;
  double sideLength;
  cv::Size chessboardSize, imageSize;

  parseCommandline(argc, argv, path, leftDevice, rightDevice);

  capL.open(leftDevice);
  capR.open(rightDevice);

  cv::waitKey(100);

  char c = ' ';

  loadCalibParameters(path,
                      lCM,
                      rCM,
                      lDC,
                      rDC,
                      R,
                      T,
                      E,
                      F,
                      chessboardSize,
                      imageSize,
                      sideLength);

  //    cv::Size imageSize(capL.get(CV_CAP_PROP_FRAME_WIDTH),
  //            capL.get(CV_CAP_PROP_FRAME_HEIGHT));

  std::cerr << imageSize << std::endl;

  cv::Mat disp(imageSize.height, imageSize.width * 2, CV_8UC3);
  std::cerr << disp.size() << std::endl;

  cv::Rect leftRoi(cv::Point(0, 0), imageSize);
  cv::Rect rightRoi(cv::Point(imageSize.width, 0), imageSize);

  cv::namedWindow("left", cv::WINDOW_NORMAL);
  cv::namedWindow("right", cv::WINDOW_NORMAL);
  cv::namedWindow("main", cv::WINDOW_NORMAL);
  cv::namedWindow("matches", cv::WINDOW_NORMAL);
  cv::namedWindow("disparity", cv::WINDOW_NORMAL);

  cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
  cv::Ptr<cv::xfeatures2d::FREAK> descriptor = cv::xfeatures2d::FREAK::create();

  std::vector<cv::KeyPoint> keyPointsL, keyPointsR;
  std::vector<cv::Point2f> keyPointsLS, keyPointsRS;
  std::vector<cv::KeyPoint> goodKeyPointsL, goodKeyPointsR;

  std::vector<cv::DMatch> matches, goodMatches;
  cv::Mat descriptionL, descriptionR;

  cv::BFMatcher matcher(cv::NORM_L2, true);
  cv::Mat homogenousPoints;
  cv::Ptr<cv::stereo::StereoBinarySGBM> sbm =
    cv::stereo::StereoBinarySGBM::create(0, 3, 16);

  cv::stereoRectify(lCM, lDC, rCM, rDC, imageSize, R, T, lR, rR, lP, rP, Q, 0);
  cv::initUndistortRectifyMap(
    lCM, lDC, lR, lP, imageSize, CV_16SC2, lMap1, lMap2);
  cv::initUndistortRectifyMap(
    rCM, rDC, rR, rP, imageSize, CV_16SC2, rMap1, rMap2);

  capL.set(cv::CAP_PROP_FRAME_WIDTH, imageSize.width);
  capL.set(cv::CAP_PROP_FRAME_HEIGHT, imageSize.height);
  capR.set(cv::CAP_PROP_FRAME_WIDTH, imageSize.width);
  capR.set(cv::CAP_PROP_FRAME_HEIGHT, imageSize.height);

  std::cerr << capL.get(cv::CAP_PROP_FRAME_WIDTH) << std::endl;
  std::cerr << capL.get(cv::CAP_PROP_FRAME_HEIGHT) << std::endl;
  std::cerr << capR.get(cv::CAP_PROP_FRAME_WIDTH) << std::endl;
  std::cerr << capR.get(cv::CAP_PROP_FRAME_HEIGHT) << std::endl;

  do
  {
    cv::Mat mL = disp(leftRoi);
    cv::Mat mR = disp(rightRoi);
    cv::Mat tL, tR;
    cv::Mat matched;

    capL.grab();
    capR.grab();
    capL.retrieve(tL);
    capR.retrieve(tR);
    tL.copyTo(mL);
    tR.copyTo(mR);

    {
      cv::Mat gL, gR, uL, uR;
      cv::cvtColor(tL, gL, cv::COLOR_RGB2GRAY);
      cv::remap(gL,
                uL,
                lMap1,
                lMap2,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
      cv::cvtColor(tR, gR, cv::COLOR_RGB2GRAY);
      cv::remap(gR,
                uR,
                rMap1,
                rMap2,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
      keyPointsL.clear();
      keyPointsR.clear();
      goodMatches.clear();
      detector->detect(uL, keyPointsL);
      descriptor->compute(uL, keyPointsL, descriptionL);

      detector->detect(uR, keyPointsR);
      descriptor->compute(uR, keyPointsR, descriptionR);
      // TODO need to add filtering function like RANSAC or STH for the matches
      matcher.match(descriptionL, descriptionR, matches);

      {
        double max_dist = 0, min_dist = 1000;
        for (size_t i = 0; i < matches.size(); ++i)
        {
          double dist = matches[i].distance;
          if (dist > max_dist)
            max_dist = dist;
          if (dist < min_dist)
            min_dist = dist;
        }

        for (size_t i = 0; i < matches.size(); ++i)
        {
          if (matches[i].distance < min_dist + 0.3 * (max_dist - min_dist))
            goodMatches.push_back(matches[i]);
        }
      }

      keyPointsLS.resize(matches.size());
      keyPointsRS.resize(matches.size());

      for (unsigned int i = 0; i < matches.size(); ++i)
      {
        keyPointsLS[i] = keyPointsL[matches[i].queryIdx].pt;
        keyPointsRS[i] = keyPointsR[matches[i].trainIdx].pt;
      }

      cv::triangulatePoints(lP, rP, keyPointsLS, keyPointsRS, homogenousPoints);

      if (!keyPointsL.empty() && !keyPointsR.empty() && !goodMatches.empty())
      {
        cv::drawMatches(uL, keyPointsL, uR, keyPointsR, goodMatches, matched);
        cv::imshow("matches", matched);
      }

      {
        cv::Mat disparity;
        cv::Mat shown;

        sbm->compute(uL, uR, disparity);
        if (!disparity.empty())
        {
          disparity.convertTo(shown, CV_8U, 255 / (32 * 16.));
          imshow("disparity", shown);
        }

        cv::Mat xyz;
        cv::reprojectImageTo3D(disparity, xyz, Q, true);
      }
      cv::imshow("left", uL);
      cv::imshow("right", uR);
    }
    cv::imshow("main", disp);
    c = cv::waitKey(1);
  } while ('q' != c);

  return 0;
}


#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <cmath>
#include <fstream>
#include <typeinfo>
#include <typeindex>
#include <unordered_map>
#include <functional>

#include <boost/program_options.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

#include <logging.hpp>
#include <constants.hpp>
#include <dielectric_function_v2.hpp>
#include <matrix_serialization.hpp>

namespace po = boost::program_options;

auto init_options() -> po::options_description
{
	po::options_description desc;

	desc.add_options()
		( "help", "Produce the help message." )
		( "type", po::value<std::string>()->required()
		, "Type of an element of the matrix. It may be one of: "
		  "float, double, cfloat, cdouble." );
	desc.add(tcm::init_constants_options<double>());

	return desc;
}

auto element_type(std::string input) -> std::type_index
{
	using namespace std::string_literals;
	static std::unordered_map<std::string, std::type_index> const types =
		{ { "float"s,   std::type_index(typeid(float))                }
		, { "double"s,  std::type_index(typeid(double))               }
		, { "cfloat"s,  std::type_index(typeid(std::complex<float>))  }
		, { "cdouble"s, std::type_index(typeid(std::complex<double>)) }
		};

	boost::to_lower(input);
	try {
		return types.at(input);
	} catch(std::out_of_range & e) {
		std::cerr << "Invalid element type `" + input + "`!\n";
		throw;
	}
}

template<class _Help, class _Run>
auto process_command_line( int argc, char** argv
                         , _Help&& help
						 , _Run&& run ) -> void
{
	auto const description = init_options();
	po::variables_map vm;

	po::store(po::command_line_parser(argc, argv).options(description).run()
	         , vm );

	if(vm.count("help")) {
		help(description);
		return;
	}

	po::notify(vm);

	run(element_type(vm["type"].as<std::string>()), vm);
}

template<class _T, class _IStream>
auto read_positions(_IStream & input) -> std::vector<std::array<_T, 3>>
{
	std::vector<std::array<_T, 3>> positions;

	auto i = std::istream_iterator<_T>{input};
	while(i != std::istream_iterator<_T>{}) {
		const auto x = *i++;
		const auto y = *i++;
		const auto z = *i++;
		positions.push_back({x, y, z});
	}

	return positions;
}

template<class _T, class _IStream, class _OStream>
auto make_potential( _IStream & input, _OStream & output
                   , po::variables_map const& vm ) -> void
{
	using R = tcm::utils::Base<_T>;
	auto const positions = read_positions<R>(input);
	auto const constants = tcm::load_constants<R, double, std::map<std::string, R>>(vm);
	boost::log::sources::severity_logger<tcm::severity_level> lg;
	auto const V = tcm::coulomb::make<_T>( positions
	                                     , constants
	                                     , lg );

	boost::archive::binary_oarchive output_archive{output};
	output_archive << V;
}

auto run( std::type_index type
        , po::variables_map const& vm ) -> void
{
	using make_function_t = std::function<void()>;
	static std::unordered_map<std::type_index, make_function_t> const
	dispatch = {
		{ std::type_index(typeid(float))
		, [&vm](){make_potential<float>(std::cin, std::cout, vm);} },
		{ std::type_index(typeid(double))
		, [&vm](){make_potential<double>(std::cin, std::cout, vm);} },
		{ std::type_index(typeid(std::complex<float>))
		, [&vm](){make_potential<std::complex<float>>(std::cin, std::cout, vm);} },
		{ std::type_index(typeid(std::complex<double>))
		, [&vm](){make_potential<std::complex<double>>(std::cin, std::cout, vm);} }
	};

	tcm::setup_console_logging();
	dispatch.at(type)();
}

int main(int argc, char** argv)
{
	process_command_line
		( argc, argv
		, [](auto desc) { std::cout << desc << '\n'; }
		, &run
		);

	return 0;
}


/**
 * \ file TimeVaryingSecondOrderFilter.cpp
 */

#include <ATK/Core/InPointerFilter.h>

#include <ATK/EQ/TimeVaryingSecondOrderFilter.h>
#include <ATK/EQ/TimeVaryingIIRFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/scoped_array.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingBandPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.31610222820014583));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingBandPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8408964152537104));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingBandPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingBandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.22326595903140814));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingBandPassPeakCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingBandPassPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_gain(2);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 1.0074946766389419));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingBandPassPeakCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingBandPassPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_gain(2);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 1.4142135623730931));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingBandPassPeakCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingBandPassPeakCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(1);
  filter.set_gain(2);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 1.0018333926095173));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingAllPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingAllPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(.1);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 1));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingAllPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingAllPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(.1);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 1));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingAllPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingAllPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_Q(.1);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 1));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingLowPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.09991943746806305));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingLowPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8408964152537146));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingLowPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.04984673793807906));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingLowPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingLowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.49246840910199763));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingHighPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 1000;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.8697981291708585));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingHighPassCoefficients_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 1000;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(10000, 1.0344582218093583));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingHighPassCoefficients_500_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(500);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingHighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 1000;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(500, 0.5091584471108357));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingLowShelvingCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingLowShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.9999157109896207));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);
  filter.set_input_port(1, &generator2, 0);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingLowShelvingCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingLowShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.7951544465306409));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingLowShelvingCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingLowShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 100;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.9599245087100254));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingHighShelvingCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingHighShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 1000;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.7953107582465426));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingHighShelvingCoefficients_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingHighShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 1000;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.7071168098138222));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( TimeVaryingIIRFilter_TimeVaryingHighShelvingCoefficients_500_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(500);

  ATK::TimeVaryingIIRFilter<ATK::TimeVaryingHighShelvingCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_gain(.5);
  filter.set_min_frequency(100);
  filter.set_max_frequency(12000);
  filter.set_number_of_steps(12000 - 100 + 1);

  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    data[i] = 1000;
  }

  ATK::InPointerFilter<float> generator2(data.get(), 1, 2*PROCESSSIZE, false);
  generator2.set_output_sampling_rate(1024*64);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(500, 0.960256352408842));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator2, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


#include <boost/test/unit_test.hpp>
#include <iostream>
#include "objects/numerics/integer.hpp"
#include "globals/globals.hpp"
#include "globals/parents.hpp"

using obj::Object;

BOOST_AUTO_TEST_SUITE( numbers_suite )

BOOST_AUTO_TEST_CASE( mod_int ) {
  using obj::Integer;
  Integer i(0);

  BOOST_TEST(i.parents().contains(parents::integerp));
}

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Copyright 2009 Trustees of Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek, Michael Hansen
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP
#define BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP

#include <boost/pending/indirect_cmp.hpp>
#include <boost/graph/relax.hpp>
#include <boost/pending/relaxed_heap.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/iteration_macros.hpp>

namespace boost {

  // No init version
  template <typename Graph, typename DijkstraVisitor,
            typename PredecessorMap, typename DistanceMap,
            typename WeightMap, typename VertexIndexMap,
            typename DistanceCompare, typename DistanceWeightCombine,
            typename DistanceInfinity, typename DistanceZero>
  void dijkstra_shortest_paths_no_color_map_no_init
    (const Graph& graph,
     typename graph_traits<Graph>::vertex_descriptor start_vertex,
     PredecessorMap predecessor_map,
     DistanceMap distance_map,
     WeightMap weight_map,
     VertexIndexMap index_map,
     DistanceCompare distance_compare,
     DistanceWeightCombine distance_weight_combine,
     DistanceInfinity distance_infinity,
     DistanceZero distance_zero,
     DijkstraVisitor visitor)
  {
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename property_traits<DistanceMap>::value_type Distance;
    typedef typename property_traits<WeightMap>::value_type Weight;

    typedef indirect_cmp<DistanceMap, DistanceCompare> DistanceIndirectCompare;
    DistanceIndirectCompare
      distance_indirect_compare(distance_map, distance_compare);

    // Choose vertex queue type
#if BOOST_GRAPH_DIJKSTRA_USE_RELAXED_HEAP
    typedef relaxed_heap<Vertex, DistanceIndirectCompare, VertexIndexMap>
      VertexQueue;
    VertexQueue vertex_queue(num_vertices(graph),
                             distance_indirect_compare,
                             index_map);
#else
    // Default - use d-ary heap (d = 4)
    typedef
      detail::vertex_property_map_generator<Graph, VertexIndexMap, std::size_t>
      IndexInHeapMapHelper;
    typedef typename IndexInHeapMapHelper::type IndexInHeapMap;
    typedef
      d_ary_heap_indirect<Vertex, 4, IndexInHeapMap, DistanceMap, DistanceCompare>
      VertexQueue;

    boost::scoped_array<std::size_t> index_in_heap_map_holder;
    IndexInHeapMap index_in_heap =
      IndexInHeapMapHelper::build(graph, index_map,
                                  index_in_heap_map_holder);
    VertexQueue vertex_queue(distance_map, index_in_heap, distance_compare);
#endif

    // Add vertex to the queue
    vertex_queue.push(start_vertex);

    // Starting vertex will always be the first discovered vertex
    visitor.discover_vertex(start_vertex, graph);

    while (!vertex_queue.empty()) {
      Vertex min_vertex = vertex_queue.top();
      vertex_queue.pop();

      visitor.examine_vertex(min_vertex, graph);

      // Check if any other vertices can be reached
      Distance min_vertex_distance = get(distance_map, min_vertex);

      if (!distance_compare(min_vertex_distance, distance_infinity)) {
        // This is the minimum vertex, so all other vertices are unreachable
        return;
      }

      // Examine neighbors of min_vertex
      typedef typename graph_traits<Graph>::edge_descriptor Edge;
      BGL_FORALL_OUTEDGES_T(min_vertex, current_edge, graph, Graph) {
        visitor.examine_edge(current_edge, graph);

        // Check if the edge has a negative weight
        if (distance_compare(get(weight_map, current_edge), distance_zero)) {
          boost::throw_exception(negative_edge());
        }

        // Extract the neighboring vertex and get its distance
        Vertex neighbor_vertex = target(current_edge, graph);
        Distance neighbor_vertex_distance = get(distance_map, neighbor_vertex);
        bool is_neighbor_undiscovered =
          !distance_compare(neighbor_vertex_distance, distance_infinity);

        // Attempt to relax the edge
        bool was_edge_relaxed = relax(current_edge, graph, weight_map,
          predecessor_map, distance_map,
          distance_weight_combine, distance_compare);

        if (was_edge_relaxed) {
          vertex_queue.update(neighbor_vertex);
          visitor.edge_relaxed(current_edge, graph);
        } else {
          visitor.edge_not_relaxed(current_edge, graph);
        }

        if (is_neighbor_undiscovered) {
          visitor.discover_vertex(neighbor_vertex, graph);
          vertex_queue.push(neighbor_vertex);
        }
      } // end out edge iteration

      visitor.finish_vertex(min_vertex, graph);
    } // end while queue not empty
  }

  // Full init version
  template <typename Graph, typename DijkstraVisitor,
            typename PredecessorMap, typename DistanceMap,
            typename WeightMap, typename VertexIndexMap,
            typename DistanceCompare, typename DistanceWeightCombine,
            typename DistanceInfinity, typename DistanceZero>
  void dijkstra_shortest_paths_no_color_map
    (const Graph& graph,
     typename graph_traits<Graph>::vertex_descriptor start_vertex,
     PredecessorMap predecessor_map,
     DistanceMap distance_map,
     WeightMap weight_map,
     VertexIndexMap index_map,
     DistanceCompare distance_compare,
     DistanceWeightCombine distance_weight_combine,
     DistanceInfinity distance_infinity,
     DistanceZero distance_zero,
     DijkstraVisitor visitor)
  {
    // Initialize vertices
    BGL_FORALL_VERTICES_T(current_vertex, graph, Graph) {
      visitor.initialize_vertex(current_vertex, graph);

      // Default all distances to infinity
      put(distance_map, current_vertex, distance_infinity);

      // Default all vertex predecessors to the vertex itself
      put(predecessor_map, current_vertex, current_vertex);
    }

    // Set distance for start_vertex to zero
    put(distance_map, start_vertex, distance_zero);

    // Pass everything on to the no_init version
    dijkstra_shortest_paths_no_color_map_no_init(graph,
      start_vertex, predecessor_map, distance_map, weight_map,
      index_map, distance_compare, distance_weight_combine,
      distance_infinity, distance_zero, visitor);
  }

  namespace detail {

    // Handle defaults for PredecessorMap, DistanceCompare,
    // DistanceWeightCombine, DistanceInfinity and DistanceZero
    template <typename Graph, typename DistanceMap, typename WeightMap,
              typename VertexIndexMap, typename Params>
    inline void
    dijkstra_no_color_map_dispatch2
      (const Graph& graph,
       typename graph_traits<Graph>::vertex_descriptor start_vertex,
       DistanceMap distance_map, WeightMap weight_map,
       VertexIndexMap index_map, const Params& params)
    {
      // Default for predecessor map
      dummy_property_map predecessor_map;

      typedef typename property_traits<DistanceMap>::value_type DistanceType;
      dijkstra_shortest_paths_no_color_map
        (graph, start_vertex,
         choose_param(get_param(params, vertex_predecessor), predecessor_map),
         distance_map, weight_map, index_map,
         choose_param(get_param(params, distance_compare_t()),
                      std::less<DistanceType>()),
         choose_param(get_param(params, distance_combine_t()),
                      closed_plus<DistanceType>()),
         choose_param(get_param(params, distance_inf_t()),
                      (std::numeric_limits<DistanceType>::max)()),
         choose_param(get_param(params, distance_zero_t()),
                      DistanceType()),
         choose_param(get_param(params, graph_visitor),
                      make_dijkstra_visitor(null_visitor())));
    }

    template <typename Graph, typename DistanceMap, typename WeightMap,
              typename IndexMap, typename Params>
    inline void
    dijkstra_no_color_map_dispatch1
      (const Graph& graph,
       typename graph_traits<Graph>::vertex_descriptor start_vertex,
       DistanceMap distance_map, WeightMap weight_map,
       IndexMap index_map, const Params& params)
    {
      // Default for distance map
      typedef typename property_traits<WeightMap>::value_type DistanceType;
      typename std::vector<DistanceType>::size_type
        vertex_count = is_default_param(distance_map) ? num_vertices(graph) : 1;

      std::vector<DistanceType> default_distance_map(vertex_count);

      detail::dijkstra_no_color_map_dispatch2
        (graph, start_vertex, choose_param(distance_map,
         make_iterator_property_map(default_distance_map.begin(), index_map,
                                    default_distance_map[0])),
         weight_map, index_map, params);
    }
  } // namespace detail

  // Named parameter version
  template <typename Graph, typename Param, typename Tag, typename Rest>
  inline void
  dijkstra_shortest_paths_no_color_map
    (const Graph& graph,
     typename graph_traits<Graph>::vertex_descriptor start_vertex,
     const bgl_named_params<Param, Tag, Rest>& params)
  {
    // Default for edge weight and vertex index map is to ask for them
    // from the graph. Default for the visitor is null_visitor.
    detail::dijkstra_no_color_map_dispatch1
      (graph, start_vertex,
       get_param(params, vertex_distance),
       choose_const_pmap(get_param(params, edge_weight), graph, edge_weight),
       choose_const_pmap(get_param(params, vertex_index), graph, vertex_index),
       params);
  }

} // namespace boost

#endif // BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP


//
// boost/radix/codec_traits/segment.hpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_RADIX_CODECTRAITS_SEGMENT_HPP
#define BOOST_RADIX_CODECTRAITS_SEGMENT_HPP

#include <boost/radix/common.hpp>

#include <boost/radix/detail/bits.hpp>
#include <boost/radix/detail/segment_buffer.hpp>

#include <boost/array.hpp>

#ifdef BOOST_HAS_PRAGMA_ONCE
#  pragma once
#endif

namespace boost { namespace radix { namespace codec_traits {

template <typename Codec>
struct required_bits {
  BOOST_STATIC_CONSTANT(
      std::size_t,
      value = bits::from_alphabet_size<Codec::alphabet_size>::value);
};

template <typename Codec>
struct packed_segment_size {
  BOOST_STATIC_CONSTANT(
      std::size_t,
      value = bits::to_packed_segment_size<required_bits<Codec>::value>::value);
};

template <typename Codec>
struct unpacked_segment_size {
  BOOST_STATIC_CONSTANT(
      std::size_t,
      value =
          bits::to_unpacked_segment_size<required_bits<Codec>::value>::value);
};

}}} // namespace boost::radix::codec_traits

#endif // BOOST_RADIX_CODECTRAITS_SEGMENT_HPP


#pragma once

#include "util.hxx"
#include "vec3f.hxx"
#include "spectral.hxx"
#include "box.hxx"
#include "span.hxx"
#include "distribution_mixture_models.hxx"
#include "path_guiding_quadtree.hxx"
#include "path_guiding_tree.hxx"
#include "json_fwd.hxx"

//#include <random>
#include <cmath>
#include <fstream>
#include <tuple>

//#include <boost/pool/object_pool.hpp>
#include <boost/filesystem/path.hpp>

#include <tbb/spin_mutex.h>
#include <tbb/mutex.h>
#include <tbb/atomic.h>
#include <tbb/task_group.h>
#include <tbb/task_arena.h>

struct SurfaceInteraction;
struct RenderingParameters;

#define WRITE_DEBUG_OUT
//#define PATH_GUIDING_WRITE_SAMPLES

#if (defined PATH_GUIDING_WRITE_SAMPLES & !defined NDEBUG & defined HAVE_JSON)
#   define PATH_GUIDING_WRITE_SAMPLES_ACTUALLY_ENABLED 1
#endif

namespace guiding
{

inline static constexpr int CACHE_LINE_SIZE = 64;
boost::filesystem::path GetDebugFilePrefix();

using Color::Spectral3f;
struct CellData;

struct IncidentRadiance
{
    Double3 pos;
    Float3 reverse_incident_dir;
    float weight;
    bool is_original = true;
};

using LeafStatistics = Accumulators::OnlineCovariance<double, 3, int64_t>;

// Should be quite lightweight.
static_assert(sizeof(tbb::spin_mutex) <= 16);

namespace MoVmfRadianceDistribution
{

class RadianceDistributionLearned;

class RadianceDistributionSampled
{
  friend class RadianceDistributionLearned;
  // Normalized to the total incident flux. So radiance_distribution(w) * incident_flux_density is the actual radiance from direction w.
  vmf_fitting::VonMisesFischerMixture<> normalized_distribution;
  double incident_flux_density{ 1. };
  double incident_flux_confidence_bounds{ 0. };

public:
  RadianceDistributionSampled()
  {
    vmf_fitting::InitializeForUnitSphere(normalized_distribution);
  }

  double Pdf(const Eigen::Vector3d &dir) const
  {
    return vmf_fitting::Pdf(normalized_distribution, dir.cast<float>());
  }

  std::pair<Double3, double> Sample(Sampler &sampler) const
  {
    auto r1 = sampler.Uniform01();
    auto r2 = sampler.Uniform01();
    auto r3 = sampler.Uniform01();
    Eigen::Vector3d w = vmf_fitting::Sample(normalized_distribution, { r1,r2,r3 }).template cast<double>();
    // Float32 can be lower precision than I have otherwise. So renormalize the direction.
    const double n2 = w.squaredNorm();
    if (std::abs(n2 - 1.) >= 1.e-11)
    {
      w /= std::sqrt(n2);
    }
    const double pdf = Pdf(w);
    return { w, pdf };
  }

  double Evaluate(const Eigen::Vector3d &dir) const
  {
    return Pdf(dir) * incident_flux_density;
  }

  std::pair<double, double> EvaluateErr(const Eigen::Vector3d &dir) const
  {
    const double pdf = Pdf(dir);
    return { pdf*incident_flux_density, pdf*incident_flux_confidence_bounds };
  }

  std::pair<double, double> EvaluateFluxErr() const
  {
    return std::make_pair(incident_flux_density, incident_flux_confidence_bounds);
  }

  Float3 ComputeStochasticFilteredDirection(const IncidentRadiance & rec, RandGen &sampler) const;

  rapidjson::Value ToJSON(rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> &a) const;
};

class RadianceDistributionLearned
{
  vmf_fitting::VonMisesFischerMixture<> normalized_distribution;
  vmf_fitting::incremental::Data<> fitdata;
  Accumulators::OnlineVariance<double, int64_t> incident_flux_density_accum;

public:
  class Parameters
  {
    friend class RadianceDistributionLearned;
    vmf_fitting::incremental::Params<> params;
    vmf_fitting::VonMisesFischerMixture<> prior_mode;
  public:
    Parameters(const CellData &cd);
  };

  RadianceDistributionLearned()
  {
    vmf_fitting::InitializeForUnitSphere(normalized_distribution);
  }

  void InitialFit(Span<const IncidentRadiance> buffer, const Parameters &params)
  {
    IncrementalFit(buffer, params);
  }

  void IncrementalFit(Span<const IncidentRadiance> buffer, const Parameters &params)
  {
    for (const auto &in : buffer)
    {
      if (in.is_original)
        incident_flux_density_accum += in.weight;

      if (in.weight <= 0.)
        continue;

      float weight = in.weight;
      const auto xs = Span<const Eigen::Vector3f>(&in.reverse_incident_dir, 1);
      const auto ws = Span<const float>(&weight, 1);
      vmf_fitting::incremental::Fit(normalized_distribution, fitdata, params.params, xs, ws);
    }
  }

  RadianceDistributionSampled IterationUpdateAndBake(const RadianceDistributionSampled &previous_radiance_dist)
  {
    return Bake();
  }

  RadianceDistributionSampled Bake() const
  {
    RadianceDistributionSampled dst;
    dst.normalized_distribution = this->normalized_distribution;
    dst.incident_flux_density = this->incident_flux_density_accum.Mean();
    dst.incident_flux_confidence_bounds = this->incident_flux_density_accum.Count() >= 10 ?
      std::sqrt(this->incident_flux_density_accum.Var() / static_cast<double>(this->incident_flux_density_accum.Count())) : LargeNumber;
    assert(std::isfinite(dst.incident_flux_confidence_bounds));
    return dst;
  }

  rapidjson::Value ToJSON(rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> &a) const;
};

} // namespace MoVmfRadianceDistribution

namespace quadtree_radiance_distribution
{

class RadianceDistributionLearned;

Eigen::Vector2f MapSphereToTree(const Eigen::Vector3f &dir);
Eigen::Vector3f MapTreeToSphere(const Eigen::Vector2f &uv);

inline constexpr double JacobianInv = 1./(4.*Pi);
inline constexpr double safe_error_number = 0.01*static_cast<double>(LargeFloat)*util::Pow(0.25, quadtree::detail::MAX_DEPTH);

class RadianceDistributionSampled
{
  friend class RadianceDistributionLearned;
  quadtree::Tree tree;
  Eigen::ArrayXd node_means;
  Eigen::ArrayXd node_stddev;
  Eigen::ArrayXf node_sample_probs;
  Eigen::ArrayXf node_sample_prior;
  // double incident_flux_density{ 0. };
  // double incident_flux_confidence_bounds{ 0. };

  std::pair<int, float> FindNodeAndInvSolidAngle(const Eigen::Vector2f &uv) const
  {
    float area_inv = JacobianInv;
    quadtree::detail::DecentHelper d{uv, tree};
    for (;!d.IsLeaf();)
    {
      d.TraverseInplace();
      area_inv *= 4.f;
    }
    const int idx = d.GetNode().idx;
    return {idx, area_inv};
  }

  static auto AssertFinite(const std::pair<double, double> &x)
  {
    assert(std::isfinite(x.first));
    assert(std::isfinite(x.second));
    return x;
  }

  static auto AssertFinite(double x)
  {
    assert(std::isfinite(x));
    return x;
  }

public:
  RadianceDistributionSampled() :
    tree{}, node_means(tree.NumNodes()), node_stddev(tree.NumNodes()), node_sample_probs(tree.NumNodes())
  {
    node_means.setOnes();
    node_stddev.setZero();
    node_sample_probs.setOnes();
    node_sample_prior.setOnes();
  }

  double Pdf(const Eigen::Vector3d &dir) const
  {
    const auto uv = MapSphereToTree(dir.cast<float>());
    const double ret = quadtree::Pdf(tree, AsSpan(node_sample_probs), uv)*JacobianInv;
    assert(std::isfinite(ret));
    return ret;
  }

  std::pair<Double3, double> Sample(Sampler &sampler) const
  {
    auto [uv, pdf] = quadtree::Sample(tree, AsSpan(node_sample_probs), sampler);
    pdf *= JacobianInv;

    assert (uv.allFinite());
    assert (std::isfinite(pdf) && pdf > 0.f);
    auto w = MapTreeToSphere(uv).cast<double>().eval();
    // Float32 can be lower precision than I have otherwise. So renormalize the direction.
    const double n2 = w.squaredNorm();
    if (std::abs(n2 - 1.) >= 1.e-11)
    {
      w /= std::sqrt(n2);
    }
    assert(w.allFinite());
    return std::make_pair(w, pdf);
  }

  double Evaluate(const Eigen::Vector3d &dir) const
  {
    const auto uv = MapSphereToTree(dir.cast<float>());
    auto [idx, area_inv] = FindNodeAndInvSolidAngle(uv);
    return AssertFinite(area_inv*node_means[idx]);
  }

  std::pair<double, double> EvaluateErr(const Eigen::Vector3d &dir) const
  {
    const auto uv = MapSphereToTree(dir.cast<float>());
    auto [idx, area_inv] = FindNodeAndInvSolidAngle(uv);
    return AssertFinite({ area_inv*node_means[idx], area_inv*node_stddev[idx] });
  }

  std::pair<double, double> EvaluateFluxErr() const
  {
    return AssertFinite({ node_means[tree.GetRoot().idx], node_stddev[tree.GetRoot().idx] });
  }

  Float3 ComputeStochasticFilteredDirection(const IncidentRadiance & rec, RandGen &sampler) const;

  rapidjson::Value ToJSON(rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> &a) const;
};

class RadianceDistributionLearned
{
  quadtree::Tree tree;
  Accumulators::SoaOnlineVariance<double, long> node_weights;

  inline Eigen::ArrayXd CalcRelativeCounts() const
  {
    auto ret = node_weights.Counts().cast<double>().eval();
    auto total_sample_count = ret[tree.GetRoot().idx];
    assert(total_sample_count > 0);
    ret *= 1./total_sample_count;
    return ret;
  }

  static constexpr double min_sample_count = 2;
  static constexpr double good_sample_count = 100;

public:
  class Parameters
  {
    friend class RadianceDistributionLearned;
  public:
    Parameters(const CellData &cd, int round) : round{round} {}
    int round;
  };

  RadianceDistributionLearned()
    : tree{}, node_weights(1)
  {
  }

  void IncrementalFit(Span<const IncidentRadiance> buffer, const Parameters &params, RadianceDistributionSampled &sampled_distrib);
  void InitialFit(Span<const IncidentRadiance> buffer, const Parameters &params);

  RadianceDistributionSampled IterationUpdateAndBake(const RadianceDistributionSampled &previous_radiance_dist);
  RadianceDistributionSampled Bake() const;

  rapidjson::Value ToJSON(rapidjson::MemoryPoolAllocator<rapidjson::CrtAllocator> &a) const;
};

} // namespace quadtree_radiance_distribution

using quadtree_radiance_distribution::RadianceDistributionLearned;
using quadtree_radiance_distribution::RadianceDistributionSampled;

// using MoVmfRadianceDistribution::RadianceDistributionLearned;
// using MoVmfRadianceDistribution::RadianceDistributionSampled;

// About 1100 Bytes.
struct CellData
{
    CellData() = default;

    CellData(const CellData &) = delete;
    CellData& operator=(const CellData &) = delete;

    CellData(CellData &&) = default;
    CellData& operator=(CellData &&) = default;

    alignas (CACHE_LINE_SIZE) struct CurrentEstimate {
      RadianceDistributionSampled radiance_distribution;
      Box cell_bbox{};
      Eigen::Matrix3d points_cov_frame{Eigen::zero}; // U*sqrt(Lambda), where U is composed of Eigenvectors, and Lambda composed of Eigenvalues.
      Eigen::Vector3d points_mean{Eigen::zero};
      Eigen::Vector3d points_stddev{Eigen::zero};
    } current_estimate;

    alignas (CACHE_LINE_SIZE) struct Learned {
      RadianceDistributionLearned radiance_distribution;
      LeafStatistics leaf_stats;
    } learned;

    long last_num_samples = 0;
    long max_num_samples = 0;
    int index = -1;
};

#ifdef PATH_GUIDING_WRITE_SAMPLES_ACTUALLY_ENABLED
class CellDebug
{
public:
  CellDebug() = default;
  ~CellDebug();

  CellDebug(const CellDebug &) = delete;
  CellDebug& operator=(const CellDebug &) = delete;

  CellDebug(CellDebug &&) = delete;
  CellDebug& operator=(CellDebug &&) = delete;

  void Open(std::string filename_);
  void Write(const Double3 &pos, const Float3 &dir, float weight);
  void Close();
  const std::string_view GetFilename() const { return filename; }

  vmf_fitting::incremental::Params<> params{};
private:
  std::ofstream file;
  std::string filename;
};
#endif

class CellIterator : kdtree::LeafIterator
{
    Span<const CellData> celldata;
  public:
    CellIterator(const kdtree::Tree &tree_, Span<const CellData> celldata_, const Ray &ray_, double tnear_init, double tfar_init)  noexcept
      : kdtree::LeafIterator{tree_, ray_, tnear_init, tfar_init}, celldata{celldata_}
    {}

    const CellData::CurrentEstimate& operator*() const noexcept
    {
      return celldata[kdtree::LeafIterator::Payload()].current_estimate;
    }

    using kdtree::LeafIterator::Interval;
    using kdtree::LeafIterator::operator bool;
    using kdtree::LeafIterator::operator++;
};

class PathGuiding
{
    public:
        using RadianceEstimate = CellData::CurrentEstimate;

        struct ThreadLocal
        {
            ToyVector<IncidentRadiance> samples;
        };

        PathGuiding(const Box &region, double cellwidth, const RenderingParameters &params, tbb::task_arena &the_task_arena, const char* name);

        void BeginRound(Span<ThreadLocal*> thread_locals);

        void AddSample(
          ThreadLocal& tl, const Double3 &pos,
          Sampler &sampler, const Double3 &reverse_incident_dir,
          const Spectral3 &radiance);

        const RadianceEstimate& FindRadianceEstimate(const Double3 &p) const;

        void FinalizeRound(Span<ThreadLocal*> thread_locals);

        void PrepareAdaptedStructures();

        CellIterator MakeCellIterator(const Ray &ray, double tnear_init, double tfar_init) const
        {
          return CellIterator{recording_tree, AsSpan(cell_data), ray, tnear_init, tfar_init};
        }

    private:
        void WriteDebugData();
        void AdaptIncremental();
        void AdaptInitial(Span<ThreadLocal*> thread_locals);
        void FitTheSamples(Span<ThreadLocal*> thread_locals);
        ToyVector<int> ComputeCellIndices(Span<const IncidentRadiance> samples) const;
        ToyVector<ToyVector<IncidentRadiance>> SortSamplesIntoCells(Span<const int> cell_indices, Span<const IncidentRadiance> samples) const;
        void GenerateStochasticFilteredSamplesInplace(Span<int> cell_indices, Span<IncidentRadiance> samples) const;

        static IncidentRadiance ComputeStochasticFilterPosition(const IncidentRadiance & rec, const CellData &cd, RandGen &sampler);

        void FitTheSamples(CellData &cell, Span<IncidentRadiance> buffer) const;

        void Enqueue(int cell_idx, ToyVector<IncidentRadiance> &sample_buffer);
        CellData& LookupCellData(const Double3 &p);

        Box region;
        kdtree::Tree recording_tree;
        ToyVector<CellData, AlignedAllocator<CellData, CACHE_LINE_SIZE>> cell_data;
        std::string name;
#ifdef PATH_GUIDING_WRITE_SAMPLES_ACTUALLY_ENABLED
        std::unique_ptr<CellDebug[]> cell_data_debug;
#endif
        int param_num_initial_samples;
        int param_em_every;
        double param_prior_strength;
        int64_t previous_max_samples_per_cell = 0;
        int64_t previous_total_samples = 0;

        tbb::task_arena *the_task_arena;
        tbb::task_group the_task_group;

        int round = 0;
        int sub_round = 0;
};

template<class Iter1_, class Iter2_>
class CombinedIntervalsIterator
{
  using Iter1 = Iter1_; //guiding::kdtree::LeafIterator;
  using Iter2 = Iter2_; //SegmentIterator;
  Iter1 leaf_iter;
  Iter2 boundary_iter;
  double tnear, tfar;
  double li_tnear, li_tfar;
  double bi_tnear, bi_tfar;

public:
  CombinedIntervalsIterator(Iter1 leaf_iter, Iter2 boundary_iter)
    : leaf_iter{leaf_iter}, boundary_iter{boundary_iter}
  {
    std::tie(li_tnear, li_tfar) = leaf_iter.Interval();
    std::tie(bi_tnear, bi_tfar) = boundary_iter.Interval();
    tnear = std::max(li_tnear, bi_tnear);
    tfar = std::min(li_tfar, bi_tfar);
  }

  operator bool() const noexcept
  {
    return leaf_iter && boundary_iter;
  }

  void operator++()
  {
    // If the interval ends of the two iterators coincide, then here an interval of length zero
    // will be produced, at the place of the boundary.
    tnear = tfar;
    if (li_tfar <= bi_tfar)
    {
      ++leaf_iter;
      if (leaf_iter)
      {
        std::tie(li_tnear, li_tfar) = leaf_iter.Interval();
      }
    }
    else
    {
      ++boundary_iter;
      if (boundary_iter)
      {
        std::tie(bi_tnear, bi_tfar) = boundary_iter.Interval();
      }
    }
    tfar = std::min(li_tfar, bi_tfar);
  }

  auto Interval() const noexcept
  {
    return std::make_pair(tnear, tfar);
  }

  decltype(*leaf_iter) DereferenceFirst() const
  {
    return *leaf_iter;
  }

  decltype(*boundary_iter) DereferenceSecond() const
  {
    return *boundary_iter;
  }
};

} // namespace guiding