#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
using namespace std;

#include <unistd.h>
#include <sys/wait.h>

#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;

#include "opencv2/core/core.hpp"
#include "opencv2/core/operations.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/video/tracking.hpp>

#include "../../../core/image.h"
#include "../../../tracker/stereo/stereo_tracker_base.h"
#include "../../../tracker/stereo/stereo_tracker_sim.h"
#include "../../../tracker/stereo/stereo_tracker_libviso.h"
#include "../../../tracker/stereo/stereo_tracker_bfm.h"
#include "../../../tracker/stereo/stereo_tracker_refiner.h"
#include "../../../tracker/stereo/tracker_refiner_libviso.h"
#include "../../../tracker/refiner/feature_refiner_base.cc"
#include "../../../tracker/refiner/feature_refiner_klt.h"
#include "../../../tracker/detector/feature_detector_harris_cv.h"
#include "../../../tracker/detector/feature_detector_gftt_cv.h"
#include "../../../tracker/detector/feature_detector_base.h"
#include "../../extern/libviso2/src/viso_stereo.h"
#include "../../extern/libviso2/src/matrix.h"
#include "../../math_helper.h"
#include "../../eval_helper.h"
#include "../../format_helper.h"
#include "../../helper_libviso.h"
#include "../../cv_plotter.h"
#include "../../matches.h"

using namespace core;
using namespace vo;
using namespace track;

// Lucas Kanade tracking params
// bumblebee dataset
//#define STEREO_BOX_W		 13		// 13
//#define STEREO_BOX_H		 5		// 5 or 3
//#define TRACK_BOX_W		 21 // 21
//#define TRACK_BOX_H		 21 // 21
//#define LK_PYRAMID_LEVEL	 3
//#define BORDER_FILTER		 5
//#define BORDER_MASK_SIZE	 10

//#define STEREO_BOX_W		 21		// 13
//#define STEREO_BOX_H		 21		// 5 or 3
//#define TRACK_BOX_W		 21 // 21
//#define TRACK_BOX_H		 21 // 21
//#define LK_PYRAMID_LEVEL	 3
//#define BORDER_FILTER		 5
//#define BORDER_MASK_SIZE	 5

// detector params
#define QUALITY_LVL		 1	  // 0.15
#define MIN_DISTANCE		 4	  // 10
#define BLOCK_SIZE		 3	  // 3
#define USE_HARRIS		 true	  // false
#define HARRIS_K         0.04	  // 0.04

// libviso dataset
//#define STEREO_BOX_W		 11		 // 13
//#define STEREO_BOX_H		 5		// 5 or 3
//#define TRACK_BOX_W		 11 // 21
//#define TRACK_BOX_H		 11 // 21
//#define LK_PYRAMID_LEVEL	 3  // 5
//#define BORDER_FILTER		 5
//#define BORDER_MASK_SIZE	 60

//TODO hardcoded - in future add movement checking through optical flow
// feature tracking params
#define SBA_FRAME_STEP		      2  // 10

// plotting settings
#define SCALE_FACTOR             0.5	       // 0.5, zoom with 2.0
#define CENTER_X		            300
#define CENTER_Y		            200
#define WINDOW_WIDTH		         900
#define WINDOW_HEIGHT		      700
//#define WINDOW_CAMERA_WIDTH	   1000
//#define WINDOW_CAMERA_HEIGHT	   800

void visualOdometry(vector<string>& imagelist, string& cparams_file, string& source_folder, string& output_folder)
{
   vector<KeyPoint> features_full;
   vector<KeyPoint> features_left;
   vector<KeyPoint> features_right;

   deque<vector<uchar>> track_status;
   deque<Matches> all_features;
   deque<vector<KeyPoint>> features_libviso_left;
   deque<vector<KeyPoint>> features_libviso_right;

   deque<Mat> extr_params; // pose mat
   deque<Mat> Rt_params;

   double cam_params[5];
   FormatHelper::readCameraParams(cparams_file, cam_params);
   VisualOdometryStereo::parameters param;
   param.calib.f = cam_params[0];	 // focal length in pixels
   param.calib.cu = cam_params[2];	 // principal point (u-coordinate) in pixels
   param.calib.cv = cam_params[3];	 // principal point (v-coordinate) in pixels
   param.base = cam_params[4];

   // optional
   //   param.inlier_threshold = 1.5;       // 1.5
   //   param.ransac_iters = 200;	       // 200
   //param.bucket.bucket_height = 50;    // 50
   //param.bucket.bucket_width = 50;     // 50
   param.bucket.max_features = 3;      // 2

   // bumblebee dataset - rectified - lcd calib
   //param.calib.f  = 491.22;	    // focal length
   //param.calib.cu = 328.8657;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 249.43165;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.119744;
   ////param.base	  = 0.5;

   // libviso datasets: 00, 14
   //param.calib.f  = 718.856;	 // focal length in pixels
   //param.calib.cu = 607.1928;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 185.2157;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.53716;      // 386.1448 / 718.856

   // libviso 11 dataset
   //param.calib.f  = 707.0912;	 // focal length in pixels
   //param.calib.cu = 601.8873;	 // principal point (u-coordinate) in pixels
   //param.calib.cv = 183.1104;	 // principal point (v-coordinate) in pixels
   //param.base	  = 0.53715;      // 386.1448 / 718.856

   //Image nullimg;
   //string simdata_folder = "/home/kivan/projects/datasets/stereo_model/points_kitti_cam_nonoise_base_0.50/";
   //string simdata_xml = "/home/kivan/projects/datasets/stereo_model/stereosim_viso00path.xml";
   Mat mask = imread("/home/kivan/Projects/project-vista/config_files/mask_kitti_rect.png", CV_LOAD_IMAGE_GRAYSCALE);

   //StereoTrackerBase* tracker = new StereoTrackerSim(simdata_folder, simdata_xml);

   FeatureRefinerKLT refiner;

   FeatureDetectorGFTTCV detector(5000, 0.0001, 1, 3, true, 0.04, mask);
   StereoTrackerBFM tracker_basic(&detector, 2000, 0.9);
   StereoTrackerRefiner tracker(&tracker_basic, &refiner, tracker_basic.countFeatures());

   //StereoTrackerLibviso tracker_basic(param);
   //TrackerRefinerLibviso tracker(&tracker_basic, &refiner);

   // init visual odometry
   VisualOdometryStereo viso(param, &tracker);
   //VisualOdometryStereo viso(param, &tracker_basic);
   // LIBVISO end

   int start_frame = 0 * 2;
   //	Mat descriptors_1, descriptors_2;
   Mat img_left_prev = imread(source_folder + imagelist[start_frame], CV_LOAD_IMAGE_GRAYSCALE);
   Mat img_right_prev = imread(source_folder + imagelist[start_frame+1], CV_LOAD_IMAGE_GRAYSCALE);

   Image tmp_img;
   HelperOpencv::MatToImage(img_left_prev, tmp_img);
   ImageSetExact imgset;
   imgset.compute(tmp_img);

   // first calc margins size of gradient images
   const int gradient_margin = (imgset.kernelGrad_.size() / 2) + 1;
   // calculate feature detector mask using feature size and extra gradient margin size
   const int fhw = (FeatureData::width() / 2) + gradient_margin;
   const int fhh = (FeatureData::height() / 2) + gradient_margin;
   detector.setMask(fhw, fhh, img_left_prev.cols - fhw, img_left_prev.rows - fhh);

   //TODO try histo eq;
   Matrix pose = Matrix::eye(4);
   Matrix viso_cvtrack = Matrix::eye(4);
   //Matrix point_rt = Matrix::eye(4);
   Mat Rt_inv;
   Mat mat_I = Mat::eye(4, 4, CV_64F);
   mat_I.copyTo(Rt_inv);
   Vec<double,7> trans_vec;
   extr_params.push_back(mat_I.clone());
   Mat Rt(4, 4, CV_64F);
   Mat Rt_gt(4, 4, CV_64F);
   Mat Rt_gt_prev = Mat::eye(4, 4, CV_64F);
   Mat Rt_gt_curr = Mat::eye(4, 4, CV_64F);
   MathHelper::invTrans(Rt_inv, Rt);
   Rt_params.push_back(Rt.clone());

   Mat prev_location_viso = Mat::zeros(4, 1, CV_64F);
   Mat prev_location_viso_cvtrack = Mat::zeros(4, 1, CV_64F);
   Mat prev_location_sba = Mat::zeros(4, 1, CV_64F);

   int32_t dims[] = {img_left_prev.cols, img_left_prev.rows, img_left_prev.cols};
   // init the tracker
   if(!viso.process(img_left_prev.data, img_right_prev.data, dims))
      cout << "init frame - no estimation" << endl;

   Matches matches(features_left, features_right);

   Mat disp_libviso = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_libviso_cvtracker = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_libviso_birch = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_sba = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_camera = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);
   Mat disp_camera_right = Mat::zeros(WINDOW_HEIGHT, WINDOW_WIDTH, CV_8UC3);

   imshow("libviso_orig", disp_libviso);
   //imshow("libviso_cvtracker", disp_libviso_cvtracker);
   //imshow("SBA", disp_sba);
   //moveWindow("libviso_orig", 0, 0);
   //moveWindow("libviso_cvtracker", 682, 0);

   Mat pose_libviso = Mat::eye(4, 4, CV_64F);   // matrix transforms points in current camera coordinate system to world coord system
   CvPlotter plotter(WINDOW_WIDTH, WINDOW_HEIGHT, SCALE_FACTOR , CENTER_X, CENTER_Y);

   ifstream groundtruth_file("/home/kivan/Projects/datasets/KITTI/poses/07.txt");
   // skip first (identety) matrix
   FormatHelper::readNextMatrixKitti(groundtruth_file, Rt_gt_curr);
   ofstream reprojerr_file("stereo_reproj_error.txt");
   ofstream libviso_ptsfile("viso_points.txt");
   FormatHelper::writeMatRt(pose_libviso, libviso_ptsfile);

   // -- demo start
   //string left_out_folder = "/home/kivan/projects/master_thesis/demo/libviso_demo/left_cam/";
   //string right_out_folder = "/home/kivan/projects/master_thesis/demo/libviso_demo/right_cam/";
   //std::ostringstream frame_num;
   //frame_num << std::setw(6) << std::setfill('0') << 0;
   //cout << frame_num.str() << "\n";
   //imwrite(left_out_folder + "img_left_" + frame_num.str() + ".jpg", img_left_prev);
   //imwrite(right_out_folder + "img_right_" + frame_num.str() + ".jpg", img_right_prev);
   // -- demo end

   for(unsigned i = start_frame + 2; i < imagelist.size(); i+=(2)) {
      cout << source_folder + imagelist[i] << endl;
      Mat img_left = imread(source_folder + imagelist[i], CV_LOAD_IMAGE_GRAYSCALE);
      Mat img_right = imread(source_folder + imagelist[i+1], CV_LOAD_IMAGE_GRAYSCALE);
      cout << i/2 << " frame:" << endl;

      //if(i/2 == 5)
      //   break;

      if(viso.process(img_left.data, img_right.data, dims)) {
         vector<Matcher::p_match>& libviso_features = viso.getFeatures();
         vector<int32_t>& inliers = viso.getInliers();

         //vector<vector<KeyPoint>> matches, matches_all;
         //HelperLibviso::convertInlierMatchesToKeys(libviso_features, inliers, matches);
         ////HelperLibviso::convertAllMatchesToKeys(libviso_features, matches_all);
         //Mat disp_allfeats;
         //vector<uchar> viso_status(matches[0].size(), 1);
         //cvtColor(img_left_prev, disp_camera, COLOR_GRAY2RGB);
         //cvtColor(img_left_prev, disp_allfeats, COLOR_GRAY2RGB);
         //HelperLibviso::drawOpticalFlow(disp_camera, matches[0], matches[2], viso_status, Scalar(0,0,255));
         ////FeatureFilter::drawOpticalFlow(disp_allfeats, matches_all[0], matches_all[2], viso_status, Scalar(0,0,255));
         //imshow("camera_left", disp_camera);
         //imshow("camera_left_all", disp_allfeats);
         //moveWindow("camera left", 400, 200);

         // save imgs for demo
         // -- demo start
         //cvtColor(img_right, disp_camera_right, COLOR_GRAY2RGB);
         //FeatureFilter::drawOpticalFlow(disp_camera_right, matches[1], matches[3], viso_status, Scalar(0,0,255));
         //frame_num.str("");
         //frame_num << std::setw(6) << std::setfill('0') << (i/2);
         //imwrite(left_out_folder + "img_left_" + frame_num.str() + ".jpg", disp_camera);
         //imwrite(right_out_folder + "img_right_" + frame_num.str() + ".jpg", disp_camera_right);
         // -- demo end

         // on success, update current pose
         // TODO try without inverse
         pose = pose * Matrix::inv(viso.getMotion());
         //cout << pose << endl << endl;
         MathHelper::matrixToMat(pose, pose_libviso);
         FormatHelper::writeMatRt(pose_libviso, libviso_ptsfile);
         //viso_ptsfile << ~pose.extractCols(std::vector<int>(1,3)) << endl;

         //	 cout << "point_rt - rt * viso.getMotion:\n" << point_rt_mat << endl;
         //TODO - why Rt and point_rt not excatly the same? because of Matrix::inv?
         Matrix Rt_inv_libviso = Matrix::inv(viso.getMotion());
         Matrix Rt_libviso = viso.getMotion();
         MathHelper::matrixToMat(Rt_inv_libviso, Rt_inv);
         MathHelper::matrixToMat(Rt_libviso, Rt);
         //MathHelper::invTrans(Rt, Rt_inv);    // better
         extr_params.push_back(Rt_inv.clone());

         // output some statistics
         vector<core::Point> points_lp, points_rp, points_lc, points_rc;
         FeatureHelper::LibvisoInliersToPoints(libviso_features, inliers, points_lp, points_rp, points_lc, points_rc);
         //cout << points_lp.size() << ", " << points_rp.size() << ", " << points_lc.size() << ", " << points_rc.size() << endl;
         Mat C = HelperLibviso::getCameraMatrix(param);
         //cout << Rt_inv << "\n";

         FormatHelper::readNextMatrixKitti(groundtruth_file, Rt_gt_curr);
         Mat Rt_gt_inv = Rt_gt_prev * Rt_gt_curr;
         //cout << Rt_gt_inv << endl;
         MathHelper::invTrans(Rt_gt_inv, Rt_gt);
         //Rt_gt = Rt_gt_inv;
         //cout << "Groundtruth:\n" << Rt_gt << endl;
         //cout << "Odometry:\n" << Rt << endl;
         cout << "-------------------------------\n";
         MathHelper::invTrans(Rt_gt_curr, Rt_gt_prev);
         double reproj_error = EvalHelper::getStereoReprojError(points_lp, points_rp, points_lc, points_rc, C,
                                                                Rt, param.base);
         double reproj_error_gt = EvalHelper::getStereoReprojError(points_lp, points_rp, points_lc, points_rc, C,
                                                                   Rt_gt, param.base);
         cout << "reprojection error (libviso): " << reproj_error << "\n";
         cout << "reprojection error (groundtruth): " << reproj_error_gt << "\n";
         cout << "-----------------------------------------------------------------------\n";
         // reprojerr_file << reproj_error << "\n";
         if(reproj_error > reproj_error_gt) {
            cout << "Found better for GT!!!\n";
            waitKey(0);
         }

         double num_matches = viso.getNumberOfMatches();
         double num_inliers = viso.getNumberOfInliers();
         cout << "Matches: " << num_matches;
         cout << ", Inliers: " << num_inliers << " -> " << 100.0*num_inliers/num_matches << " %" << endl;
         Mat location_viso(pose_libviso, Range(0,4), Range(3,4)); // extract 4-th column
         //cout << location_viso << endl;
         //Matrix location = ~point_rt.extractCols(std::vector<int>(1,3));
         //for(int k = 0; k < 3; k++) location.val[0][k] = - location.val[0][k];
         //cout << point_rt * ~location << endl;
         //cout << location << endl << endl;

         // drawing
         //drawLine(disp_libviso, Point(100.5,100.5), Point(500,500));
         //Point pt1 = coordToPoint(prev_location, SCALE_FACTOR);
         //Point pt2 = coordToPoint(location, SCALE_FACTOR);
         plotter.drawLine(prev_location_viso, location_viso, disp_libviso);
         imshow("libviso_orig", disp_libviso);
         waitKey(10);
         //cout << pose << endl << endl;
         //cout << endl << location.val[0][3] << endl;

         location_viso.copyTo(prev_location_viso);
      } else {
         cout << "libviso ... failed!" << endl;
         waitKey(0);
         //writeMatRt(pose_libviso, libviso_ptsfile);
         //extr_params.push_back(Rt_inv.clone());
         exit(1);
      }

      cv::swap(img_left_prev, img_left);
      cv::swap(img_right_prev, img_right);
   }

   destroyAllWindows();
   waitKey(0);
}

int main(int argc, char	** argv)
{
   string config_file;
   string imagelistfn;
   string cam_params_file;
   string source_folder;
   string output_folder;

   try {
      po::options_description generic("Generic options");
      generic.add_options()
         ("help", "produce help message")
         ("config,c", po::value<string>(&config_file)->default_value("config.txt"), "config filename")
         ;
      po::options_description config("Config file options");
      config.add_options()
         ("camera_params,p", po::value<string>(&cam_params_file)->default_value("camera_params.txt"), "camera params file")
         ("source_folder,s", po::value<string>(&source_folder), "folder with source")
         ("output_folder,o", po::value<string>(&output_folder), "folder for output")
         ("imglist,l", po::value<string>(&imagelistfn), "file with image list")
         ;

      po::options_description cmdline_options;
      cmdline_options.add(generic).add(config);

      po::options_description config_file_options;
      config_file_options.add(config);
      po::variables_map vm;
      //po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
      po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
      notify(vm);
      if(vm.count("help")) {
         cout << generic;
         cout << config;
         return 0;
      }

      ifstream ifs(config_file.c_str());
      if (!ifs) {
         cout << "can not open config file: " << config_file << "\n";
         cout << generic;
         cout << config;
         return 0;
      }
      else {
         po::store(parse_config_file(ifs, config_file_options), vm);
         notify(vm);
      }
      cout << "Configuring done, using:" << endl;

      if(vm.count("camera_params")) {
         cout << "Camera params: ";
         cout << cam_params_file << endl;
      }
      if(vm.count("source_folder")) {
         cout << "Source folder: ";
         cout << source_folder << endl;
      }
      if(vm.count("output_folder")) {
         cout << "Output folder: ";
         cout << output_folder << endl;
      }
      if(vm.count("imglist")) {
         cout << "Image list file: ";
         cout << imagelistfn << endl;
      }
   }
   catch(std::exception& e) {
      cout << e.what() << "\n";
      return 1;
   }

   if(imagelistfn == "")
   {
      cout << "error: no xml image list given." << endl;
      return -1;
   }
   if(output_folder == "" || source_folder == "")
   {
      cout << "error: no output or source folder given." << endl;
      return -1;
   }

   vector<string> imagelist;
   bool ok = FormatHelper::readStringList(imagelistfn, imagelist);
   if(!ok || imagelist.empty())
   {
      cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
      return -1;
   }

   visualOdometry(imagelist, cam_params_file, source_folder, output_folder);

   return 0;
}


#include <vector>
#include <map>
#include <random>

#include "state_reconstructor_simple.h"
#include "rate_model.h"
#include "mcmc.h"
#include "tree.h"
#include "sequence.h"
#include "seq_utils.h"

#include <armadillo>
using namespace arma;

void sm0_mcmc(int reps, int sampleiter, Tree * tree, StateReconstructorSimple & sr,
    RateModel & rm, vector<Sequence> & seqs, vector<Sequence> & sr_seqs,
    map<string,vector<int> > & codon_pos, mat &bf, mat &K, mat &w, mat &inq) {

    int sites = (seqs[0].get_sequence().size()/3);
    double curlike = 0;
    double sw = 0.5;
    rm.selection_model = 0;
    for (int s=0; s < sites; s++) {
        create_vector_seq_codon_state_reconstructor(seqs, sr_seqs, s, codon_pos);
        sr.set_tip_conditionals(sr_seqs, s);
        curlike +=  sr.eval_likelihood(s);
        rm.set_sameQ(true);
    }
    cout << "start likelihood: " << curlike << endl;

    std::default_random_engine re;

    double newlike = 0;
    double startk = 1.0; //start
    double startw = 1.0; //startw
    for (int i=0; i < reps; i++) {
        std::uniform_real_distribution<double> unif1(startk - sw, startk + sw);
        std::uniform_real_distribution<double> unif2(startw - sw, startw + sw);
        double newk = fabs(unif1(re));
        double neww = fabs(unif2(re));
        newlike = 0;
        update_simple_goldman_yang_q(&inq, newk, neww, bf, K, w);
        rm.setup_Q(inq);
        sr.clear_map_ps();
        for (int s=0; s < sites; s++) {
            //create_vector_seq_codon_state_reconstructor(seqs,sr_seqs,s,codon_pos);
            //sr.set_tip_conditionals(sr_seqs,s);
            newlike +=  sr.eval_likelihood(s);
            rm.set_sameQ(true);
        }
        if (newlike < curlike) {
            curlike = newlike;
            startk = newk;
            startw = neww;
        }
        if (i%sampleiter == 0) {
            cout << "iter: "<< i << " like: " << curlike << " K: "<< startk <<" w: " << startw << endl;
        }
    }
}

/**
 * this should have 5 parameters
 * K = free, w0 = 0,w1 = 1,w2 = free ,p0 = free ,p1 = free ,p2 = free
 */
void sm2a_mcmc(int reps, int sampleiter, Tree * tree, StateReconstructorSimple & sr,
    RateModel & rm, vector<Sequence> & seqs, vector<Sequence> & sr_seqs,
    map<string,vector<int> > & codon_pos, mat &bf, mat &K, mat &w, mat & inq0,
    mat & inq1, mat &inq2) {

    int sites = (seqs[0].get_sequence().size()/3);
    double curlike = 0;
    double sw = 0.2;
    sr.pp0 = 0.38008; sr.pp1 = 0.28326; sr.pp2 = 0.33666;

    for (int s=0; s < sites; s++) {
        create_vector_seq_codon_state_reconstructor(seqs, sr_seqs, s, codon_pos);
        sr.set_tip_conditionals(sr_seqs, s);
        curlike +=  sr.eval_likelihood(s);
        rm.set_sameQ(true);
    }
    cout << "start likelihood: " << curlike << endl;

    std::default_random_engine re;

    double newlike = 0;
    double startk = 1.36714; //start
    double startw0 = 0.0; //start w0
    double startw2 = 8.0; //start w2
    double startp0 = 0.38008;
    double startp1 = 0.28326;
    double startp2 = 0.33666; //not free
    for (int i=0; i < reps; i++) {
        std::uniform_real_distribution<double> unif1(startk - sw, startk + sw);
        std::uniform_real_distribution<double> unif2(startw0 - sw, startw0 + sw);
        std::uniform_real_distribution<double> unif3(startw2 - sw, startw2 + sw);
        std::uniform_real_distribution<double> unif4(startp0 - sw, startp0 + sw);
        std::uniform_real_distribution<double> unif5(startp1 - sw, startp1 + sw);
        std::uniform_real_distribution<double> unif6(startp2 - sw, startp2 + sw);

        double newk = fabs(unif1(re));
        double neww0 = fabs(unif2(re));
        if (neww0 >= 1) {
            neww0 = 1 - (neww0-1);
        }
        double neww2 = fabs(unif3(re));
        double newp0 = fabs(unif4(re));
        double newp1 = fabs(unif5(re));
        double newp2 = fabs(unif6(re));

        double sum1 = newp0 + newp1 + newp2;
        newp0 = newp0/sum1; newp1 = newp1/sum1; newp2 = newp2/sum1;
        sr.pp0 = newp0; sr.pp1 = newp1; sr.pp2 = newp2;

        newlike = 0;
        //only do it with
        update_simple_goldman_yang_q(&inq0, newk, neww0, bf, K, w);
        update_simple_goldman_yang_q(&inq2, newk, neww2, bf, K, w);
        rm.set_Q_which(inq0, 0);
        rm.set_Q_which(inq2, 2);
        sr.clear_map_ps();
        for (int s=0; s < sites; s++) {
            //create_vector_seq_codon_state_reconstructor(seqs,sr_seqs,s,codon_pos);
            //sr.set_tip_conditionals(sr_seqs,s);
            newlike +=  sr.eval_likelihood(s);
            rm.set_sameQ(true);
        }
        if (newlike < curlike) {
            curlike = newlike;
            startk = newk;
            startw0 = neww0;
            startw2 = neww2;
            startp0 = newp0;
            startp1 = newp1;
            startp2 = newp2;
        }
        if (i % sampleiter == 0) {
            cout << "iter: "<< i << " like: " << curlike << " K: "<< startk <<" w0: "
            << startw0 << " w2: " << startw2 << " p0: " << startp0 << " p1: " << startp1
            << " p2: "<< startp2 << endl;
        }
    }
}


#pragma once

#include <pcl/common/common_headers.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/point_cloud.h>

#include "Geodesy.hpp"
#include <string>
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>

class SpherePoint{

public:

	//TODO encapsulate public attributes
	Eigen::Vector3d georeferencedPosition;

        double intensity;
        double lidarTime;

	Eigen::Vector3d lidarPing;

	//angles in radians
	//HPR angles
        double heading;
        double pitch;
        double roll;

	//coordinates in radians
	double latitude;
        double longitude;
        double ellipsoidalHeight;

	Eigen::Vector3d positionEcef;

	void recompute(){
		ch = cos(heading);
		sh = sin(heading);
		cp = cos(pitch);
		sp = sin(pitch);
		cr = cos(roll);
		sr = sin(roll);

		Geodesy::getPositionECEF(positionEcef,longitude,latitude,ellipsoidalHeight);
	}

	inline double getHeading() { return heading;}
	inline double getPitch() { return pitch; }
	inline double getRoll()  { return roll; }

	inline double getCh() { return ch;}
	inline double getSh() { return sh;}

	inline double getCp() { return cp;}
	inline double getSp() { return sp;}

	inline double getCr() { return cr;}
	inline double getSr() { return sr;}

private:

	//trig for HPR angles
	double ch;
	double sh;
	double cp;
	double sp;
	double cr;
	double sr;

};

class SphereReader{
public:
        SphereReader(){

        }

        ~SphereReader(){
                for(auto i=spherePoints.begin();i != spherePoints.end();i++){
                        delete *i;
                }
        }

        static bool sortByTime(SpherePoint * p1,SpherePoint * p2){
                return p1->lidarTime < p2->lidarTime;
        }

        /* Read points from file and sort them by time */
        void read(std::string & fileName){
                std::ifstream inputFile;

                inputFile.open(fileName);

                if(inputFile.is_open()){
                        std::string line;
                        int lineCount = 0;

                        while(std::getline(inputFile,line)){
                                SpherePoint * p = new SpherePoint();

				/*double xTRF,yTRF,zTRF;
				double xLGF,yLGF,zLGF;
				double intensity;
				double lidarTime,lidarX,lidarY,lidarZ,heading,pitch,roll,latitude,longitude;
				*/

				if(sscanf(line.c_str(),"%*f %*f %*f %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",&p->georeferencedPosition[0],&p->georeferencedPosition[1],&p->georeferencedPosition[2],&p->intensity,&p->lidarTime,&p->lidarPing[0],&p->lidarPing[1],&p->lidarPing[2],&p->heading,&p->pitch,&p->roll,&p->latitude,&p->longitude,&p->ellipsoidalHeight) == 14){
					//convert angles to radians
					p->heading = D2R(p->heading);
					p->pitch   = D2R(p->pitch);
					p->roll    = D2R(p->roll);
					p->latitude = D2R(p->latitude);
					p->longitude = D2R(p->longitude);

					p->recompute();
					spherePoints.push_back(p);
					lineCount++;
                                }
                                else{
#ifdef DEBUG
                                        std::cerr << "Skipping line" << std::endl;
#endif
                                }
                        }

                        if(lineCount > 0){
#ifdef DEBUG
                                std::cerr << lineCount << " lines loaded" << std::endl;
#endif
                        }
                        else{
                                throw std::domain_error("No lines found in file " + fileName);

                        }

			//std::cerr << "Loaded " << fileName << " (" << spherePoints.size() << " points loaded)" << std::endl;

                        inputFile.close();
                        std::sort (spherePoints.begin(),spherePoints.end(), &SphereReader::sortByTime);
                }
                else{
                        throw std::invalid_argument("Cannot open file " + fileName);
                }
        }

        void fit(){
                //LSQ Fit as B=Ax
                Eigen::MatrixXd A(spherePoints.size(),4);
                Eigen::VectorXd B(spherePoints.size(),1);

                int rowCount = 0;
                for(auto i=spherePoints.begin();i!=spherePoints.end();i++){
                        A(rowCount,0)=(*i)->georeferencedPosition[0];
                        A(rowCount,1)=(*i)->georeferencedPosition[1];
                        A(rowCount,2)=(*i)->georeferencedPosition[2];
                        A(rowCount,3)=1;

                        B(rowCount)=(*i)->georeferencedPosition[0] * (*i)->georeferencedPosition[0]  + (*i)->georeferencedPosition[1] * (*i)->georeferencedPosition[1]   + (*i)->georeferencedPosition[2] * (*i)->georeferencedPosition[2];
                        rowCount++;
                }

                //LSQ QR
                Eigen::VectorXd x = A.colPivHouseholderQr().solve(B);

                //LSQ SVD
                //Eigen::MatrixXd x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

		center << x(0)/2.0 , x(1)/2.0, x(2)/2.0;

                radius  = sqrt(  x(3) + center[0]*center[0] + center[1]*center[1] + center[2]*center[2] );

		/*
                //Compute squared sum of errors
                sumSquaredErrors = 0;
                for(auto i=spherePoints.begin();i!=spherePoints.end();i++){
			double error= ((*i)->xTRF - xCenter) * ((*i)->xTRF - xCenter) + ((*i)->yTRF - yCenter)*((*i)->yTRF - yCenter) + ((*i)->zTRF - zCenter)*((*i)->zTRF - zCenter) - radius * radius;
                        sumSquaredErrors += error * error;
                }
                */
        }

/*
        void fitKnownRadius(double & xCenter,double & yCenter,double & zCenter,double radius,double & sumSquaredErrors){
                //LSQ Fit as B=Ax
                Eigen::MatrixXd A(spherePoints.size(),4);
                Eigen::VectorXd B(spherePoints.size(),1);

                int rowCount = 0;
                for(auto i=spherePoints.begin();i!=spherePoints.end();i++){
                        A(rowCount,0)=(*i)->xTRF;
                        A(rowCount,1)=(*i)->yTRF;
                        A(rowCount,2)=(*i)->zTRF;
                        A(rowCount,3)=1;

                        B(rowCount)=(*i)->xTRF * (*i)->xTRF  + (*i)->yTRF * (*i)->yTRF   + (*i)->zTRF * (*i)->zTRF + radius * radius;
                        rowCount++;
                }

                //LSQ QR
                //Eigen::VectorXd x = A.colPivHouseholderQr().solve(B);

                //LSQ SVD
                Eigen::MatrixXd x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

                xCenter = x(0)/2.0;
                yCenter = x(1)/2.0;
                zCenter = x(2)/2.0;

                sumSquaredErrors = 0;
                for(auto i=spherePoints.begin();i!=spherePoints.end();i++){
			double error= ((*i)->xTRF - xCenter) * ((*i)->xTRF - xCenter) + ((*i)->yTRF - yCenter)*((*i)->yTRF - yCenter) + ((*i)->zTRF - zCenter)*((*i)->zTRF - zCenter) - radius * radius;
                        sumSquaredErrors += error * error;
                }
        }
*/
        /* Find sphere with RANSAC */
        bool find(double x,double y, double z,double radius){
                // TODO: maybe build point cloud outside of the method if many methods need it in the future ?
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

                // Generate pointcloud data
                cloud->width = spherePoints.size(); //Load this from whatever...
                cloud->height = 1;
                cloud->points.resize (cloud->width);

                int j = 0;
                for (auto i = spherePoints.begin();i != spherePoints.end();i++,j++){
                        (*cloud)[j].x =(*i)->georeferencedPosition[0];
                        (*cloud)[j].y =(*i)->georeferencedPosition[1];
                        (*cloud)[j].z =(*i)->georeferencedPosition[2];
                }

                pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr sphereModel(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));

                pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (sphereModel);
                ransac.setDistanceThreshold (0.01); //FIXME: is 10mm enough/too little ? Adjust accordingly
                ransac.computeModel();

		//TODO: maybe we'd like to use the point count to see how good of a fit we have...
                //std::vector<int> inliers;
                //ransac.getInliers(inliers);

                Eigen::VectorXf modelParameters;
                ransac.getModelCoefficients(modelParameters);

		x=modelParameters[0];
		y=modelParameters[1];
		z=modelParameters[2];
		radius=modelParameters[3];

                return true;
        }

        void georeference(Eigen::Matrix3d & ecef2ned,Eigen::Vector3d & originNedEcef,Eigen::Matrix3d & boresight,Eigen::Vector3d & offset){

		for(auto i : spherePoints){
			//georeference point
			Eigen::Matrix3d imu2ned;

			imu2ned << 	i->getCh()*i->getCp(),   i->getCh()*i->getSp()*i->getSr()-i->getCr()*i->getSh(), i->getCh()*i->getCr()*i->getSp()+i->getSr()*i->getSh(),
    					i->getCp()*i->getSh(),   i->getCh()*i->getCr()+i->getSp()*i->getSr()*i->getSh(), i->getSh()*i->getCr()*i->getSp()-i->getCh()*i->getSr(),
    					-i->getSp(),          	i->getCp()*i->getSr(),        			     i->getCr()*i->getCp();

			//Compute total NED vector
			i->georeferencedPosition = ecef2ned * ( i->positionEcef-originNedEcef ) + imu2ned * (offset + boresight * i->lidarPing);

			//TRF Eigen::Vector3d georeferencedPosition = (*i)->positionEcef + nav2ecef * imu2nav  * (boresight * lidarX + offset);
		}

        }

        static double distance(SphereReader & s1,SphereReader & s2){
        	return (s1.getCenter()-s2.getCenter()).norm();
        }

        inline std::vector<SpherePoint*> & getPoints() { return spherePoints;}
	inline double getRadius(){ return radius;}

	inline Eigen::Vector3d & getCenter(){ return center;}

private:
        std::vector<SpherePoint*> spherePoints;

        //fit variables

	Eigen::Vector3d center;
        double radius;
};


#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <fstream>
#include <sstream>
#include <boost/format.hpp>
#include <algorithm>

using std::cin;
using std::cout;
using std::endl;
using std::vector;
using std::string;
using std::ifstream;
using std::istringstream;
using std::stod;
using std::stoi;
using std::pow;
using std::min;

class dp_matching
{
    public:
        dp_matching() :
            file_total_number_(100),
            temp_file_top_num_(0),
            unk_file_top_num_(0),
            temp_file_num_store_(1),
            frame_number_(15),
            init_flag_(1),
            min_file_search(1),
            min_search_num(0),
            min_file_result_(0),
            correct_answer_rate_(0),
            min_search_num_store_(0),
            vec_one_dimensional_{1,1,1,1,1}
        {
            file_number_.emplace_back();
            file_data_capture_.emplace_back();
            template_data_.emplace_back();
            unknown_data_.emplace_back();
            local_distance_.emplace_back();
            cumulative_distance_.emplace_back();
            //word_distance_.emplace_back();
            minimum_word_distance_.emplace_back();
        }

        void select_file_number()
        {
            cout << "please select file top number" << endl;
            cout << "menue: 11 12 21 22" << endl;

            cin >> temp_file_top_num_ >> unk_file_top_num_;

            cout << "template_file: " << temp_file_top_num_ << endl;
            cout << "unknown_file:  " << unk_file_top_num_ << endl;
            cout << endl << "start DP_matching processing" << endl;
        }

        void file_read(int& file_number_tmp, int& file_number_unk)
        {
            const std::string f_te_path = (boost::format("./city_mcepdata/city%03d/city%03d_%03d.txt") %temp_file_top_num_ % temp_file_top_num_ % file_number_tmp).str();
            const std::string f_un_path = (boost::format("./city_mcepdata/city%03d/city%03d_%03d.txt") % unk_file_top_num_ % unk_file_top_num_ % file_number_unk).str();
            ifstream f_te(f_te_path,std::ios::in);
            ifstream f_un(f_un_path,std::ios::in);

            string line,field;

            while (getline(f_te, line))
            {
                istringstream stream(line);

                while (getline(stream, field, ' ') )
                {
                    template_data_[vec_one_dimensional_.at(0) - 1].push_back(field );
                }

                template_data_.resize(++vec_one_dimensional_.at(0) );
            }
            template_data_.resize(--vec_one_dimensional_.at(0) );

            while (getline(f_un, line))
            {
                istringstream stream(line);
                while (getline(stream, field) )
                {
                    unknown_data_[vec_one_dimensional_.at(1) - 1].push_back(field );
                }

                unknown_data_.resize(++vec_one_dimensional_.at(1) );
            }
            unknown_data_.resize(--vec_one_dimensional_.at(1) );
        }

        void local_distance_calculation()
        {
            int double_flag = 0;
            for (auto it_temp = template_data_.begin(); it_temp != template_data_.end(); ++it_temp)
            {
                for (auto it_unk = unknown_data_.begin(); it_unk != unknown_data_.end(); ++it_unk)
                {
                    int tuning_number_outside = 0;
                    double accumulation = 0;
                    for (auto it = (*it_temp).begin(); it != (*it_temp).end(); ++it)
                    {
                        int tuning_number_inside = 0;
                        for (auto it_t = (*it_unk).begin(); it_t != (*it_unk).end(); ++it_t)
                        {
                            if(tuning_number_outside == tuning_number_inside)
                            {
                                double it_d = 0,it_t_d = 0;
                                try
                                {
                                    it_d   = stod(*it);
                                    it_t_d = stod(*it_t);
                                    accumulation += pow(it_d - it_t_d,2);
                                    if(it_d < 10 && it_t_d < 10) double_flag = 1;
                                    else double_flag = 0;
                                    ++tuning_number_inside;
                                }
                                catch (const std::invalid_argument& e)
                                {
                                    double_flag = 0;
                                }
                            }
                            break;
                        }
                        ++tuning_number_outside;
                    }
                    if(double_flag) local_distance_[vec_one_dimensional_.at(2) - 1].push_back(accumulation);
                }
                if(double_flag) local_distance_.resize(++vec_one_dimensional_.at(2));
            }
            local_distance_.resize(--vec_one_dimensional_.at(2));
        }

        void boundary_condition_calculation()
        {
            for (auto it_t = local_distance_.begin(); it_t != local_distance_.end(); ++it_t)
            {
                for (auto it = (*it_t).begin(); it != (*it_t).end(); ++it)
                {
                    cumulative_distance_[vec_one_dimensional_.at(3) - 1].push_back(*it);
                }
                cumulative_distance_.resize(++vec_one_dimensional_.at(3));
            }
            cumulative_distance_.resize(--vec_one_dimensional_.at(3));

            for(int i = 1;i < stoi(template_data_[2][0]);i++)
            {
                cumulative_distance_.at(i).at(0) = cumulative_distance_.at(i - 1).at(0) + local_distance_.at(i).at(0);
            }

            for(int i = 1;i < stoi(unknown_data_[2][0]);i++)
            {
                cumulative_distance_.at(0).at(i) = cumulative_distance_.at(0).at(i - 1) + local_distance_.at(0).at(i);
            }

            for (int i = 1; i < stoi(template_data_[2][0]); ++i)
            {
                for (int j = 1; j < stoi(unknown_data_[2][0]); ++j)
                {
                    double vertical = cumulative_distance_[i][j - 1] + local_distance_[i][j];
                    double diagonal = cumulative_distance_[i - 1][j - 1] + (1.4142 * local_distance_[i][j]);
                    double side = cumulative_distance_[i - 1][j] + local_distance_[i][j];

                    double min_num = min ({vertical, diagonal , side});

                    if (vertical == min_num)
                    {
                        cumulative_distance_.at(i).at(j) = vertical;
                    }

                    if (diagonal == min_num)
                    {
                        cumulative_distance_.at(i).at(j) = diagonal;
                    }

                    if (side == min_num)
                    {
                        cumulative_distance_.at(i).at(j) = side;
                    }
                }
            }

            word_distance_.push_back(cumulative_distance_[stoi(template_data_[2][0]) - 1][stoi(unknown_data_[2][0]) - 1] / (stoi(template_data_[2][0]) + stoi(unknown_data_[2][0]) ) );
        }

        void min_search()
        {
            for (auto it_t =  word_distance_.begin(); it_t !=  word_distance_.end(); ++it_t)
            {
                min_search_num =  *it_t;
                if (init_flag_) min_search_num_store_ = min_search_num;
                double min_num = min ({min_search_num_store_,min_search_num});
                min_search_num_store_ = min_num;
                if (min_search_num_store_ == min_search_num) min_file_result_ = min_file_search;
                ++min_file_search;
                init_flag_ = 0;
            }
        }

        void correct_answer_rate(int& temp_file_num)
        {

            if(temp_file_num == min_file_result_) ++correct_answer_rate_;
            if(temp_file_num == file_total_number_)
            {
                correct_answer_rate_ = 100 * (correct_answer_rate_ / file_total_number_);
                cout << endl << endl << "正解率は" << correct_answer_rate_ << "%です。" << endl;
            }
        }

        void vector_memory_clear(int& temp_file_num)
        {
            file_number_.clear();
            file_data_capture_.clear();
            template_data_.clear();
            unknown_data_.clear();
            local_distance_.clear();
            cumulative_distance_.clear();
            if(temp_file_num != temp_file_num_store_) word_distance_.clear();

            file_number_.emplace_back();
            file_data_capture_.emplace_back();
            template_data_.emplace_back();
            unknown_data_.emplace_back();
            local_distance_.emplace_back();
            cumulative_distance_.emplace_back();
            vec_one_dimensional_ = {1,1,1,1,1};

            min_search_num_store_ = 0;
            min_file_search = 1;
            init_flag_ = 1;
            min_search_num = 0;
            min_file_result_ = 0;

            temp_file_num_store_ = temp_file_num;
        }

        void progress_bar(int& temp_file_num)
        {
            cout << "[";
            for(int j = 1;temp_file_num >= j;j++) cout << "=";
            cout << ">";
            for(int k = 1;file_total_number_ - temp_file_num >= k ;k++) cout << " ";
            cout << "] " << temp_file_num << " %\r";

            cout.flush();
        }

        void run()
        {
            select_file_number();

            for(int i = 1;i<= file_total_number_;i++)
            {
                for(int j = 1;j <= file_total_number_;j++)
                {
                    vector_memory_clear(i);
                    file_read(i,j);
                    local_distance_calculation();
                    boundary_condition_calculation();
                    min_search();
                }

                progress_bar(i);
                correct_answer_rate(i);
            }
        }

    private:
        double file_total_number_;
        int temp_file_top_num_;
        int unk_file_top_num_;
        int temp_file_num_store_;
        int frame_number_;
        int init_flag_;
        int min_file_search;
        double min_search_num;
        int min_file_result_;
        double correct_answer_rate_;
        double min_search_num_store_;
        vector<int> vec_one_dimensional_;
        vector<int> file_number_;
        vector<double> file_data_capture_;
        vector<vector<string>> template_data_;
        vector<vector<string>> unknown_data_;
        vector<vector<double>> local_distance_;
        vector<vector<double>> cumulative_distance_;
        vector<double> word_distance_;
        vector<double> minimum_word_distance_;
};

int main()
{
    dp_matching dp;
    dp.run();

    return 0;
}


/*
 * optimize_tnc.cpp
 *
 *  Created on: Feb 9, 2010
 *      Author: smitty
 */

#include "optimize_state_reconstructor_nlopt.h"
#include <iostream>
#include <stdio.h>
#include <nlopt.hpp>
#include <math.h>

#include "state_reconstructor.h"
#include "rate_model.h"

#include <armadillo>
using namespace arma;

StateReconstructor * nloptsr;
RateModel * nloptrm;
mat * nloptfree_variables;

//double nlopt_sr(int n, const double *x, void *state) {
//double nlopt_sr(unsigned n, const double *x, double *grad, void *my_func_data);
double nlopt_sr(unsigned n, const double *x, double *grad, void *my_func_data) {
    for (unsigned int i=0; i < nloptfree_variables->n_rows; i++) {
        for (unsigned int j=0; j < nloptfree_variables->n_cols; j++) {
            if (i != j) {
                nloptrm->set_Q_cell(i, j,x[int((*nloptfree_variables)(i, j))]);
                if (nloptrm->get_Q()(i, j) < 0 || nloptrm->get_Q()(i, j) >= 1000) {
                    return 1000000000000;
                }
            }
        }
    }
    double like;
    nloptrm->set_Q_diag();
    like = nloptsr->eval_likelihood();
    if (nloptrm->neg_p == true) {
        like = 10000000000000;
    }
    //cout << like << endl;
    if (like < 0 || like == std::numeric_limits<double>::infinity()) {
        like = 10000000000000;
    }
    return like;
}

void optimize_sr_nlopt(RateModel * _rm,StateReconstructor * _sr, mat * _free_mask, int _nfree) {
    nloptsr = _sr;
    nloptrm = _rm;
    nloptfree_variables = _free_mask;

    nlopt::opt opt(nlopt::LN_NELDERMEAD, _nfree);
    //nlopt::opt opt(nlopt::LN_BOBYQA, _nfree);
    //nlopt::opt opt(nlopt::LN_PRAXIS, _nfree);
    //nlopt::opt opt(nlopt::LN_SBPLX, _nfree);
    //nlopt::opt opt(nlopt::LN_COBYLA, _nfree);
    //nlopt::opt opt(nlopt::LN_NEWUOA, _nfree);

    opt.set_lower_bounds(0.0000);
    opt.set_upper_bounds(100000);
    opt.set_min_objective(nlopt_sr, NULL);
    opt.set_xtol_rel(0.001);
    opt.set_maxeval(5000);

    vector<double> x(_nfree,0);
    for (unsigned int i=0; i < _rm->get_Q().n_rows; i++) {
        for (unsigned int j=0; j < _rm->get_Q().n_cols; j++) {
            if (i != j) {
                x[int((*_free_mask)(i, j))] = _rm->get_Q()(i, j);
                //cout << x[int((*_free_mask)(i, j))] << " ";
            }
        }
        //cout << endl;
    }

    //double minf;
    vector<double> result = opt.optimize(x);
    for (unsigned int i=0; i < _rm->get_Q().n_rows; i++) {
        for (unsigned int j=0; j < _rm->get_Q().n_cols; j++) {
            if (i != j) {
                (*_free_mask)(i, j) = result[int((*_free_mask)(i, j))];
                //cout << x[int((*_free_mask)(i, j))] << " ";
            }
        }
        //cout << endl;
    }
    //if ((&minf) < 0) {
     //   printf("nlopt failed!\n");
    //}
    //else {
    //    printf("found minimum at %0.10g\n", x[0], x[1], minf);
    //}
}


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

////////////////////////////////////////////////////////////////////////////////
// Main generic functor class
// Documentation: http://nt2.lri.fr/sdk/functor/entity/functor.html
////////////////////////////////////////////////////////////////////////////////
#include <boost/config.hpp>
#include <boost/tr1/functional.hpp>
#include <nt2/sdk/meta/floating.hpp>
#include <nt2/sdk/meta/arithmetic.hpp>
#include <nt2/sdk/functor/forward.hpp>
#include <nt2/sdk/error/static_assert.hpp>
#include <nt2/sdk/functor/details/call.hpp>
#include <nt2/sdk/functor/details/dispatch.hpp>
#include <nt2/sdk/functor/meta/enable_call.hpp>

#if !defined(BOOST_HAS_VARIADIC_TMPL)
#include <nt2/extension/parameters.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/repetition/enum_binary_params.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>
#endif

namespace nt2
{
  template<class Tag, class Site> struct functor
  {
    template<class Sig> struct result;

    #if defined(BOOST_HAS_VARIADIC_TMPL)
    template<class This, class... Args>
    struct  result<This(Args...)>
    {
      typedef typename meta::dispatch_call<Tag(Args...),Site>::type callee;
      typedef typename std::tr1::result_of<callee(Args...)>::type   type;
    };

    template<class... Args> inline typename result<functor(Args...)>::type
    operator()( Args const& ...args ) const
    {
      typename meta::dispatch_call<Tag(Args...),Site>::type callee;
      return callee( args... );
    }
    #else

    #define M0(z,n,t)                                                         \
    template<class This, BOOST_PP_ENUM_PARAMS(n,class A) >                    \
    struct result<This(BOOST_PP_ENUM_PARAMS(n,A))>                            \
    {                                                                         \
      typedef typename                                                        \
      meta::dispatch_call<Tag(BOOST_PP_ENUM_PARAMS(n,A)),Site>::type callee;  \
      typedef typename                                                        \
      std::tr1::result_of<callee(BOOST_PP_ENUM_PARAMS(n,A))>::type  type;     \
    };                                                                        \
                                                                              \
    template<BOOST_PP_ENUM_PARAMS(n,class A)> inline                          \
    typename meta::enable_call<Tag(BOOST_PP_ENUM_PARAMS(n,A))>::type          \
    operator()( BOOST_PP_ENUM_BINARY_PARAMS(n,A,const& a)  ) const            \
    {                                                                         \
      typename                                                                \
      meta::dispatch_call<Tag(BOOST_PP_ENUM_PARAMS(n,A)),Site>::type callee;  \
      return callee( BOOST_PP_ENUM_PARAMS(n,a) );                             \
    }                                                                         \
    /**/

    BOOST_PP_REPEAT_FROM_TO(1,BOOST_PP_INC(NT2_MAX_ARITY),M0,~)
    #undef M0

    #endif
  };
}

#endif


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sstream>
#include <unsupported/Eigen/Polynomials>

namespace Eigen {

typedef Matrix<double, 6, 1> Vector6d;
typedef Matrix<double, 6, 6> Matrix6d;
typedef Stride<Dynamic, Dynamic> DynStride;

template <typename Mtype>
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
using DynMap = Map<Mtype, AlignmentType::Aligned16, DynStride>;
#elif EIGEN_VERSION_AT_LEAST(3, 2, 90)
using DynMap = Map<Mtype, Aligned, DynStride>;
#elif EIGEN_VERSION_AT_LEAST(3, 2, 9)
using DynMap = Map<Mtype, AlignmentType::Aligned, DynStride>;
#else
using DynMap = Map<Mtype, Aligned, DynStride>;
#endif
}

template <typename T, int r, int c>
Eigen::Matrix<T, r, c>
MatrixFromDynMap(const Eigen::DynMap<Eigen::Matrix<T, r, c>> &map) {
  return Eigen::Matrix<T, r, c>(map);
}

template <typename T, int r, int c, int opr, int opc>
Eigen::Matrix<T, r, opc> EigenMul(const Eigen::Matrix<T, r, c> &lhs,
                                  const Eigen::Matrix<T, opr, opc> &rhs) {
  return lhs * rhs;
}

template <typename T, int r, int c, int opr, int opc>
Eigen::Matrix<T, opr, opc>
EigenFixedMul(const Eigen::Matrix<T, r, c> &lhs,
              const Eigen::Matrix<T, opr, opc> &rhs) {
  return lhs * rhs;
}

Eigen::Vector3d EigenEulerAngles(const Eigen::Matrix3d &m, int a0, int a1,
                                 int a2) {
  return m.eulerAngles(a0, a1, a2);
}

template <typename T, int r, int c> Eigen::Matrix<T, r, c> EigenZero() {
  return Eigen::Matrix<T, r, c>::Zero();
}

template <typename T, int r, int c> Eigen::Matrix<T, r, c> EigenZero(int row) {
  return Eigen::Matrix<T, r, c>::Zero(row);
}

template <typename T, int r, int c>
Eigen::Matrix<T, r, c> EigenZero(int row, int col) {
  return Eigen::Matrix<T, r, c>::Zero(row, col);
}

template <typename T, int r, int c> Eigen::Matrix<T, r, c> EigenRandom() {
  return Eigen::Matrix<T, r, c>::Random();
}

template <typename T, int r, int c>
Eigen::Matrix<T, r, c> EigenRandom(int row) {
  return Eigen::Matrix<T, r, c>::Random(row);
}

template <typename T, int r, int c>
Eigen::Matrix<T, r, c> EigenRandom(int row, int col) {
  return Eigen::Matrix<T, r, c>::Random(row, col);
}

template <typename T, int r, int c> Eigen::Matrix<T, r, c> EigenIdentity() {
  return Eigen::Matrix<T, r, c>::Identity();
}

template <typename T, int r, int c>
Eigen::Matrix<T, r, c> EigenIdentity(int row, int col) {
  return Eigen::Matrix<T, r, c>::Identity(row, col);
}

template <typename T, int r, int c>
void EigenSetValue(Eigen::Matrix<T, r, c> &m, int row, int col, const T &v) {
  m(row, col) = v;
}

template <typename T, int r, int c>
std::string toString(const Eigen::Matrix<T, r, c> &m) {
  std::stringstream ss;
  ss << m;
  return ss.str();
}

template <typename T> std::string QtoString(const Eigen::Quaternion<T> &q) {
  std::stringstream ss;
  ss << q.coeffs().transpose();
  return ss.str();
}

template <typename T> std::string AAtoString(const Eigen::AngleAxis<T> &aa) {
  std::stringstream ss;
  ss << "Angle: " << aa.angle() << ", Axis: " << aa.axis().transpose();
  return ss.str();
}

template <typename T>
T poly_eval(const Eigen::Matrix<T, -1, 1> &m, const T &x) {
  return Eigen::poly_eval(m, x);
}

template <typename T>
Eigen::AngleAxis<T> EigenAAFromQ(const Eigen::Quaternion<T> &q) {
  return Eigen::AngleAxis<T>(q);
}

template <typename T>
Eigen::Quaternion<T> QuaternionFromM3(const Eigen::Matrix<T, 3, 3> &m) {
  return Eigen::Quaternion<T>(m);
}

std::string EigenVersion() {
  return std::to_string(EIGEN_WORLD_VERSION) + "." +
         std::to_string(EIGEN_MAJOR_VERSION) + "." +
         std::to_string(EIGEN_MINOR_VERSION);
}


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Mzero Mzero (function template)

  Generates the constant @c -0.

  @headerref{<boost/simd/constant/mzero.hpp>}

  @par Description

  1.  @code
      template<typename T> T Mzero();
      @endcode

  2.  @code
      template<typename T> T Mzero( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T  that evaluates to -0.

  This constant is the second IEEE representation of 0 and is defined so
  that the expression `1/Mzero<T>() == Minf<T>()` evaluates to @c true. When computed as an
  IntegerValue, the expression `Mzero<T>() == Zero<T>()` evaluates to @c true.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `T(-0.0)`

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/mzero.hpp>
#include <boost/simd/constant/simd/mzero.hpp>

#endif


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//  Adaptation to Boost of the libcxx
//  Copyright 2010 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <boost/chrono/duration.hpp>
#include <boost/type_traits.hpp>
#include <limits>

#if !defined(BOOST_NO_STATIC_ASSERT)
#define NOTHING ""
#endif

template <typename D, int ExpectedDigits, typename ExpectedPeriod>
void check_duration()
{
    typedef typename D::rep Rep;
    typedef typename D::period Period;
    BOOST_CHRONO_STATIC_ASSERT(boost::is_signed<Rep>::value, NOTHING, ());
    BOOST_CHRONO_STATIC_ASSERT(boost::is_integral<Rep>::value, NOTHING, ());
    BOOST_CHRONO_STATIC_ASSERT(std::numeric_limits<Rep>::digits >= ExpectedDigits, NOTHING, ());
    BOOST_CHRONO_STATIC_ASSERT((boost::is_same<Period, ExpectedPeriod >::value), NOTHING, ());
}

void test()
{
    check_duration<boost::chrono::hours, 22, boost::ratio<3600> >();
    check_duration<boost::chrono::minutes, 28, boost::ratio<60> >();
    check_duration<boost::chrono::seconds, 34, boost::ratio<1> >();
    check_duration<boost::chrono::milliseconds, 44, boost::milli >();
    check_duration<boost::chrono::microseconds, 54, boost::micro >();
    check_duration<boost::chrono::nanoseconds, 63, boost::nano >();
}


#define BOOST_TEST_MODULE MyTest
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include "pose.h"
#include <cmath>
#include <iostream>

using namespace std;

BOOST_AUTO_TEST_CASE( test_pose )
{
    Pose p = Pose (2,4,40,5,30) ;
    Pose p2 = Pose (1,3,50,6,20);

    cout << "check creation of a pose" << endl;
    BOOST_CHECK_EQUAL( p.getX(), 2 );
    BOOST_CHECK_EQUAL(p.getY(),4);
    BOOST_CHECK_EQUAL(p.getOrientation(),40);
    BOOST_CHECK_EQUAL(p.getR(), 5);
    BOOST_CHECK_EQUAL(p.getPhi(), 30);

    cout << "check distance" << endl ;
    BOOST_CHECK_EQUAL(p.getDistance(p2),sqrt(2));
}

// EVERYTHING WORK LIKE A CHARM


#include <kr_tracker_msgs/TrackerStatus.h>
#include <kr_trackers/lissajous_generator.h>

#include <Eigen/Geometry>
#include <cmath>
#include <iostream>

LissajousGenerator::LissajousGenerator()
{
  active_ = false;
  goal_reached_ = false;
  goal_set_ = false;
}

void LissajousGenerator::setParams(const kr_tracker_msgs::LissajousTrackerGoal::ConstPtr &msg)
{
  x_amp_ = msg->x_amp;
  y_amp_ = msg->y_amp;
  z_amp_ = msg->z_amp;
  yaw_amp_ = msg->yaw_amp;
  x_num_periods_ = msg->x_num_periods;
  y_num_periods_ = msg->y_num_periods;
  z_num_periods_ = msg->z_num_periods;
  yaw_num_periods_ = msg->yaw_num_periods;
  period_ = msg->period;
  num_cycles_ = msg->num_cycles;
  ramp_time_ = msg->ramp_time;

  // Set goal stuff
  total_s_ = period_ * num_cycles_;

  // Compute a3_ and a2_
  double tr = ramp_time_;
  double tr2 = tr * tr;
  double tr3 = tr2 * tr;
  double tr4 = tr3 * tr;
  double tr5 = tr4 * tr;
  double tr6 = tr5 * tr;
  double tr7 = tr6 * tr;
  double tr8 = tr7 * tr;

  a4_ = 35.0 / tr4;
  a5_ = -84.0 / tr5;
  a6_ = 70.0 / tr6;
  a7_ = -20.0 / tr7;

  // Compute ramp_s_, const_time_
  ramp_s_ = a7_ * tr8 / 8.0 + a6_ * tr7 / 7.0 + a5_ * tr6 / 6.0 + a4_ * tr5 / 5.0;
  const_time_ = total_s_ - 2.0 * ramp_s_;
  total_time_ = 2.0 * ramp_time_ + const_time_;

  // Set the start position and time
  goal_set_ = true;
  goal_reached_ = false;
}

void LissajousGenerator::setParams(const kr_tracker_msgs::LissajousAdderGoal::ConstPtr &msg, int num)
{
  x_amp_ = msg->x_amp[num];
  y_amp_ = msg->y_amp[num];
  z_amp_ = msg->z_amp[num];
  yaw_amp_ = msg->yaw_amp[num];
  x_num_periods_ = msg->x_num_periods[num];
  y_num_periods_ = msg->y_num_periods[num];
  z_num_periods_ = msg->z_num_periods[num];
  yaw_num_periods_ = msg->yaw_num_periods[num];
  period_ = msg->period[num];
  num_cycles_ = msg->num_cycles[num];
  ramp_time_ = msg->ramp_time[num];

  // Set goal stuff
  total_s_ = period_ * num_cycles_;

  // Compute a3_ and a2_
  double tr = ramp_time_;
  double tr2 = tr * tr;
  double tr3 = tr2 * tr;
  double tr4 = tr3 * tr;
  double tr5 = tr4 * tr;
  double tr6 = tr5 * tr;
  double tr7 = tr6 * tr;
  double tr8 = tr7 * tr;

  a4_ = 35.0 / tr4;
  a5_ = -84.0 / tr5;
  a6_ = 70.0 / tr6;
  a7_ = -20.0 / tr7;

  // Compute ramp_s_, const_time_
  ramp_s_ = a7_ * tr8 / 8.0 + a6_ * tr7 / 7.0 + a5_ * tr6 / 6.0 + a4_ * tr5 / 5.0;
  const_time_ = total_s_ - 2.0 * ramp_s_;
  total_time_ = 2.0 * ramp_time_ + const_time_;

  // Set the start position and time
  goal_set_ = true;
  goal_reached_ = false;
}

const kr_mav_msgs::PositionCommand::Ptr LissajousGenerator::getPositionCmd(void)
{
  if(!active_)
  {
    return kr_mav_msgs::PositionCommand::Ptr();
  }

  // Set gains
  kr_mav_msgs::PositionCommand::Ptr cmd(new kr_mav_msgs::PositionCommand);

  // Get elapsed time
  ros::Time current_time = ros::Time::now();
  ros::Duration elapsed_time = current_time - start_time_;
  double t = elapsed_time.toSec();
  double t2 = t * t;
  double t3 = t2 * t;
  double t4 = t3 * t;
  double t5 = t4 * t;
  double t6 = t5 * t;
  double t7 = t6 * t;
  double t8 = t7 * t;
  double s, sdot, sddot, sdddot;

  Eigen::Vector3f pos, vel, acc, jrk;
  double yaw, yaw_dot;
  if(t > total_time_)
  {
    pos = Eigen::Vector3f::Zero();
    yaw = 0;
    cmd->position.x = pos(0), cmd->position.y = pos(1), cmd->position.z = pos(2);
    cmd->velocity.x = 0, cmd->velocity.y = 0, cmd->velocity.z = 0;
    cmd->acceleration.x = 0, cmd->acceleration.y = 0, cmd->acceleration.z = 0;
    cmd->jerk.x = 0, cmd->jerk.y = 0, cmd->jerk.z = 0;
    cmd->yaw = yaw;
    cmd->yaw_dot = 0;
    goal_set_ = false;
    goal_reached_ = true;
  }
  else
  {
    if(t < ramp_time_)
    {
      s = a7_ * t8 / 8.0 + a6_ * t7 / 7.0 + a5_ * t6 / 6.0 + a4_ * t5 / 5.0;
      sdot = a7_ * t7 + a6_ * t6 + a5_ * t5 + a4_ * t4;
      sddot = 7.0 * a7_ * t6 + 6.0 * a6_ * t5 + 5.0 * a5_ * t4 + 4.0 * a4_ * t3;
      sdddot = 42.0 * a7_ * t5 + 30.0 * a6_ * t4 + 20.0 * a5_ * t3 + 12.0 * a4_ * t2;
    }
    else if(t < total_time_ - ramp_time_)
    {
      s = ramp_s_ + t - ramp_time_;
      sdot = 1;
      sddot = 0;
      sdddot = 0;
    }
    else
    {
      double te = total_time_ - t;
      double te2 = te * te;
      double te3 = te2 * te;
      double te4 = te3 * te;
      double te5 = te4 * te;
      double te6 = te5 * te;
      double te7 = te6 * te;
      double te8 = te7 * te;

      s = 2.0 * ramp_s_ + const_time_ - a7_ * te8 / 8.0 - a6_ * te7 / 7.0 - a5_ * te6 / 6.0 - a4_ * te5 / 5.0;
      sdot = a7_ * te7 + a6_ * te6 + a5_ * te5 + a4_ * te4;
      sddot = -7.0 * a7_ * te6 - 6.0 * a6_ * te5 - 5.0 * a5_ * te4 - 4.0 * a4_ * te3;
      sdddot = 42.0 * a7_ * te5 + 30.0 * a6_ * te4 + 20.0 * a5_ * te3 + 12.0 * a4_ * te2;
    }
    double T = period_;
    double T2 = T * T;
    double T3 = T2 * T;
    pos(0) = x_amp_ * (1 - std::cos(2 * M_PI * x_num_periods_ * s / T));
    pos(1) = y_amp_ * std::sin(2 * M_PI * y_num_periods_ * s / T);
    pos(2) = z_amp_ * std::sin(2 * M_PI * z_num_periods_ * s / T);
    vel(0) = x_amp_ * 2 * M_PI * x_num_periods_ * std::sin(2 * M_PI * x_num_periods_ * s / T) * sdot / T;
    vel(1) = y_amp_ * 2 * M_PI * y_num_periods_ * std::cos(2 * M_PI * y_num_periods_ * s / T) * sdot / T;
    vel(2) = z_amp_ * 2 * M_PI * z_num_periods_ * std::cos(2 * M_PI * z_num_periods_ * s / T) * sdot / T;
    acc(0) = x_amp_ * (4 * M_PI * M_PI * x_num_periods_ * x_num_periods_ * std::cos(2 * M_PI * x_num_periods_ * s / T) *
                           sdot * sdot / T2 +
                       2 * M_PI * x_num_periods_ * std::sin(2 * M_PI * x_num_periods_ * s / T) * sddot / T);
    acc(1) = y_amp_ * (-4 * M_PI * M_PI * y_num_periods_ * y_num_periods_ *
                           std::sin(2 * M_PI * y_num_periods_ * s / T) * sdot * sdot / T2 +
                       2 * M_PI * y_num_periods_ * std::cos(2 * M_PI * y_num_periods_ * s / T) * sddot / T);
    acc(2) = z_amp_ * (-4 * M_PI * M_PI * z_num_periods_ * z_num_periods_ *
                           std::sin(2 * M_PI * z_num_periods_ * s / T) * sdot * sdot / T2 +
                       2 * M_PI * z_num_periods_ * std::cos(2 * M_PI * z_num_periods_ * s / T) * sddot / T);
    jrk(0) = x_amp_ * (-8 * M_PI * M_PI * M_PI * x_num_periods_ * x_num_periods_ * x_num_periods_ *
                           std::sin(2 * M_PI * x_num_periods_ * s / T) * sdot * sdot * sdot / T3 +
                       4 * M_PI * M_PI * x_num_periods_ * x_num_periods_ * std::cos(2 * M_PI * x_num_periods_ * s / T) *
                           sdot * sddot / T2 +
                       2 * M_PI * x_num_periods_ * std::sin(2 * M_PI * x_num_periods_ * s / T) * sdddot / T);
    jrk(1) = y_amp_ * (-8 * M_PI * M_PI * M_PI * y_num_periods_ * y_num_periods_ * y_num_periods_ *
                           std::cos(2 * M_PI * y_num_periods_ * s / T) * sdot * sdot * sdot / T3 -
                       4 * M_PI * M_PI * y_num_periods_ * y_num_periods_ * std::sin(2 * M_PI * y_num_periods_ * s / T) *
                           sdot * sddot / T2 +
                       2 * M_PI * y_num_periods_ * std::cos(2 * M_PI * y_num_periods_ * s / T) * sdddot / T);
    jrk(2) = z_amp_ * (-8 * M_PI * M_PI * M_PI * z_num_periods_ * z_num_periods_ * z_num_periods_ *
                           std::cos(2 * M_PI * z_num_periods_ * s / T) * sdot * sdot * sdot / T3 -
                       4 * M_PI * M_PI * z_num_periods_ * z_num_periods_ * std::sin(2 * M_PI * z_num_periods_ * s / T) *
                           sdot * sddot / T2 +
                       2 * M_PI * z_num_periods_ * std::cos(2 * M_PI * z_num_periods_ * s / T) * sdddot / T);
    yaw = yaw_amp_ * (1 - std::cos(2 * M_PI * yaw_num_periods_ * s / T));
    yaw_dot = yaw_amp_ * 2 * M_PI * yaw_num_periods_ * std::sin(2 * M_PI * yaw_num_periods_ * s / T) * sdot / T;
    cmd->position.x = pos(0), cmd->position.y = pos(1), cmd->position.z = pos(2);
    cmd->velocity.x = vel(0), cmd->velocity.y = vel(1), cmd->velocity.z = vel(2);
    cmd->acceleration.x = acc(0), cmd->acceleration.y = acc(1), cmd->acceleration.z = acc(2);
    cmd->jerk.x = jrk(0), cmd->jerk.y = jrk(1), cmd->jerk.z = jrk(2);
    cmd->yaw = yaw;
    cmd->yaw_dot = yaw_dot;
  }
  return cmd;
}

void LissajousGenerator::generatePath(nav_msgs::Path &path, geometry_msgs::Point &initial_pt, double dt)
{
  if(goal_set_)
  {
    double s = 0.0;
    double T = period_;

    while(s < period_)
    {
      geometry_msgs::PoseStamped ps;
      ps.pose.position.x = x_amp_ * (1 - std::cos(2 * M_PI * x_num_periods_ * s / T)) + initial_pt.x;
      ps.pose.position.y = y_amp_ * std::sin(2 * M_PI * y_num_periods_ * s / T) + initial_pt.y;
      ps.pose.position.z = z_amp_ * std::sin(2 * M_PI * z_num_periods_ * s / T) + initial_pt.z;

      path.poses.push_back(ps);
      s += dt;  // increment by 0.1s
    }
  }
}

bool LissajousGenerator::activate(void)
{
  if(goal_set_)
  {
    active_ = true;
    start_time_ = ros::Time::now();
  }
  return active_;
}

void LissajousGenerator::deactivate(void)
{
  goal_set_ = false;
  active_ = false;
}

bool LissajousGenerator::isActive(void)
{
  return active_;
}

bool LissajousGenerator::goalIsSet(void)
{
  return goal_set_;
}

bool LissajousGenerator::status() const
{
  return goal_reached_ ? kr_tracker_msgs::TrackerStatus::SUCCEEDED : kr_tracker_msgs::TrackerStatus::ACTIVE;
}

float LissajousGenerator::timeRemaining(void)
{
  ros::Time t_now = ros::Time::now();
  float time_elapsed = (t_now - start_time_).toSec();
  return total_time_ - time_elapsed;
}

float LissajousGenerator::timeElapsed(void)
{
  ros::Time t_now = ros::Time::now();
  return (t_now - start_time_).toSec();
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_UPPER_INCLUDE
#define MTL_MATRIX_UPPER_INCLUDE

#include <boost/numeric/mtl/matrix/bands.hpp>
#include <limits>

namespace mtl { namespace mat {

namespace traits {

    template <typename Matrix>
    struct upper
    {
	typedef typename traits::bands<Matrix>::type type;
    };
}

/// Upper triangular matrix
template <typename Matrix>
typename traits::upper<Matrix>::type
inline upper(const Matrix& A)
{
    return bands(A, 0, std::numeric_limits<long>::max());
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_UPPER_INCLUDE


/*
Copyright (c) 2012, Matthew H. Reilly (kb1vc)
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

    Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
    Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in
    the documentation and/or other materials provided with the
    distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#define SAMPLE_RATE 48000
#include <complex>
#include <iostream>
#include <fstream>
#include <stdlib.h>
#include "OSFilter.hxx"
#include <time.h>
#include <fftw3.h>
#include <math.h>
#include <boost/format.hpp>

#include <stdio.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include "HilbertTransformer.hxx"

using namespace std;
#include <sys/time.h>

namespace SoDaTest {
class Histogram {
public:
  Histogram(unsigned int nb, float _min, float _max) {
    num_buckets = nb;
    min = _min;
    max = _max;

    bucket_size = (_max - _min) / ((float) num_buckets);
    buckets = new unsigned int[num_buckets];
    underflow = 0; overflow = 0;
    unsigned int i;
    for(i = 0; i < num_buckets; i++) buckets[i] = 0;
  }

  void record(float samp) {
    if(samp < min) underflow++;
    else if(samp > max) overflow++;
    else {
      unsigned int idx = (unsigned int) floor((samp - min) / bucket_size);
      buckets[idx]++;
    }
  }

  void dump(ostream & os, const std::string & tag) {
    os << boost::format("%s %f %d\n") % tag % (min - bucket_size) % underflow;
    unsigned int i;
    for(i = 0; i < num_buckets; i++) {
      if(buckets[i] == 0) continue;
      float b = min + ((float) i) * bucket_size;
      os << boost::format("%s %f %d\n") % tag % b % buckets[i];
    }
    os << boost::format("%s %f %d\n") % tag % (max + bucket_size) % overflow;
  }

  unsigned int num_buckets;
  float min;
  float max;
  unsigned int underflow, overflow;
  unsigned int * buckets;
  float bucket_size;
};
}

double curtime()
{
  double ret;
  struct timeval tp;
  gettimeofday(&tp, NULL);
  ret = ((double) tp.tv_sec) + 1e-6*((double)tp.tv_usec);

  return ret;
}

#define BUFLEN 1024
int dumpTest(int argc, char * argv[])
{
  (void) argc; (void) argv;
  SoDa::HilbertTransformer htR(BUFLEN);
  SoDa::HilbertTransformer htC(BUFLEN);
  float inbuf[BUFLEN];
  std::complex<float> cinbuf[BUFLEN];
  std::complex<float> outbuf[BUFLEN];
  std::complex<float> coutbuf[BUFLEN];
  std::complex<float> checkbuf[BUFLEN];

  int i, j, k;

  // load up the input buffer
  const int num_freqs = 8;
  float angles[num_freqs];
  float ang_incs[num_freqs];

  srandom(0x13255);

  for(i = 0; i < num_freqs; i++) {
    angles[i] = 0.0;
    long irand = random();
    ang_incs[i] = M_PI * ((float) (3.535 * ((irand % 8) + 15))) / (0.82 * ((float) (BUFLEN)));
  }

  float gain;
  for(k = 0; k < 20; k++) {

    for(i = 0; i < BUFLEN; i++) {
      checkbuf[i] = std::complex<float>(0.0, 0.0);
      inbuf[i] = 0.0;
      cinbuf[i] = std::complex<float>(0.0, 0.0);
    }

    for(i = 0; i < BUFLEN; i++) {
      if(k < 3) {
	float x = ((float) (i + BUFLEN + k * BUFLEN)) / (4.0 * ((float) BUFLEN));
	gain = 0.5 * (1.0 - cos(M_PI * x));
      }
      else {
	gain = 1.0;
      }
      for(j = 0; j < num_freqs; j++) {
	inbuf[i] += gain * sin(angles[j]);
	cinbuf[i] += std::complex<float>(gain * sin(angles[j]),0.0);
	checkbuf[i] += std::complex<float>(gain * sin(angles[j]), -1.0 * gain * cos(angles[j]));
	angles[j] += ang_incs[j];
	if(angles[j] > M_PI) angles[j] -= (2.0 * M_PI);
      }
    }

    // now do a hilbert transform.
    htR.apply(inbuf, outbuf, false, 1.0);
    htC.apply(cinbuf, coutbuf, true, 1.0);

    if(k > -1) {
      for(i = 0; i < BUFLEN; i++) {
	std::cout << boost::format("ZZZ: %d %f %f  %f %f %f %f\n")
	  % (i + k * BUFLEN) % outbuf[i].real() % outbuf[i].imag() % coutbuf[i].real() % coutbuf[i].imag() % checkbuf[i].real() % checkbuf[i].imag();
      }
    }
  }

  // Now do a modulation test...
  SoDa::HilbertTransformer htDemod(BUFLEN);
  gain = 0.0;
  for(k = 0; k < 20; k++) {
    for(i = 0; i < BUFLEN; i++) {
      checkbuf[i] = std::complex<float>(0.0, 0.0);
      cinbuf[i] = std::complex<float>(0.0, 0.0);
    }

    for(i = 0; i < BUFLEN; i++) {
      if(k < 3) {
	float x = ((float) (i + BUFLEN + k * BUFLEN)) / (4.0 * ((float) BUFLEN));
	gain = 0.5 * (1.0 - cos(M_PI * x));
      }
      else {
	gain = 1.0;
      }
      for(j = 0; j < num_freqs; j++) {
	cinbuf[i] += gain * std::complex<float>(sin(angles[j]), cos(angles[j]));
	// cinbuf[i].real() += gain * sin(angles[j]);
	// cinbuf[i].imag() += gain * cos(angles[j]);
	checkbuf[i] += gain * std::complex<float>(sin(angles[j]), -1.0 * cos(angles[j]));
	// checkbuf[i].real() = checkbuf[i].real() + gain * sin(angles[j]);
	// checkbuf[i].imag() = checkbuf[i].imag() - gain * cos(angles[j]);
	angles[j] += ang_incs[j];
	if(angles[j] > M_PI) angles[j] -= (2.0 * M_PI);
      }
    }

    // now do a hilbert transform.
    htDemod.applyIQ(cinbuf, outbuf, 1.0);

    // dump the output
    if(k > -1) {
      for(i = 0; i < BUFLEN; i++) {
	std::cout << boost::format("MMM: %d %f %f %f %f %f %f\n")
	  % (i + k * BUFLEN) % outbuf[i].real() % outbuf[i].imag() % cinbuf[i].real() % cinbuf[i].imag() % checkbuf[i].real() % checkbuf[i].imag();
      }
    }
  }
  return 0;
}

/**
 *    Applying HT twice to a real valued signal f(t) will
 *    produce a delayed version of -1 * f(t + d) at its output.
 *    The HT module's complex result, should be (a + ib) =
 *    f(t + d) + i * -1 * f(t + d).  Therefore, we should see
 *    that (a + b) == 0 and (a - b) = 2a.
 *
 *    In this test, we accumulate a histogram of the error
 *    magnitude (b + a) -- this should ideally be 0.0;
 */
int doSSBTest(int argc, char * argv[])
{
  (void) argc; (void) argv;
  // create an HT for an audio buffer size of 2304
  // (This corresponds to the magic choice in Params.hxx.)
  int buflen = 2304;
  // make sure the HT impulse response is at least 256 samples long.
  SoDa::HilbertTransformer HT_ui(buflen, 256);
  SoDa::HilbertTransformer HT_li(buflen, 256);
  SoDa::HilbertTransformer HT_usb(buflen, 256);
  SoDa::HilbertTransformer HT_lsb(buflen, 256);

  // create a test input buffer
  float inbuf[buflen];
  // output buffers
  std::complex<float> analytic_U_inbuf[buflen];
  std::complex<float> analytic_L_inbuf[buflen];
  std::complex<float> usb_outbuf[buflen];
  std::complex<float> lsb_outbuf[buflen];

  // now setup an input test signal parameters.
  // We're going to use a signal made up of eight
  // more or less random frequencies
  const int num_freqs = 16;
  float angles[num_freqs];
  float ang_incs[num_freqs];

  srandom(0x13255);

  int i, j, k;
  for(i = 0; i < num_freqs; i++) {
    angles[i] = 0.0;
    long irand = random();
    // we want arg_inc (phase advance per step) to vary
    // over the range 0 to pi / 8 (0.125 * Fnyquist).
    float frand = ((float) (irand & 0xffff)) / (8.0 * 65536.0);
    ang_incs[i] = frand * M_PI;
  }

  ang_incs[0] = 0.01 * M_PI;

  // build the error histograms
  SoDaTest::Histogram usb_sum_err(1000, -0.2, 0.2);
  SoDaTest::Histogram lsb_diff_err(1000, -0.2, 0.2);

  // now do the test over 1000 buffers
  int trial_count = 10;
  int ignore_buffer_count = 1; // ignore the first buffer, or so to get over the startup transient.
  for(k = 0; k < trial_count; k++) {
    // load the input buffer.
    for(j = 0; j < buflen; j++) {
      inbuf[j] = 0.0;
    }
    for(i = 0; i < num_freqs; i++) {
      for(j = 0; j < buflen; j++) {
	inbuf[j] += sin(angles[i]);
	angles[i] += ang_incs[i];
      }
    }

    // perform the transform.
    HT_ui.apply(inbuf, analytic_U_inbuf, false, 1.0);
    HT_li.apply(inbuf, analytic_L_inbuf, true, 1.0);
    HT_usb.applyIQ(analytic_U_inbuf, usb_outbuf, 1.0);
    HT_lsb.applyIQ(analytic_L_inbuf, lsb_outbuf, 1.0);

    // now calculate the errors
    if(k >= ignore_buffer_count) {
      float norm = 0.0;
      for(j = 0; j < buflen; j++) {
	norm += (inbuf[j] * inbuf[j]);
      }
      norm = sqrt(norm);
      for(j = 0; j < buflen; j++) {
	float usb_sum, lsb_diff;
	usb_sum = usb_outbuf[j].real() + usb_outbuf[j].imag();
	lsb_diff = lsb_outbuf[j].real() - lsb_outbuf[j].imag();

	usb_sum_err.record(usb_sum / norm);
	lsb_diff_err.record(lsb_diff / norm);
#if 0
	std::cerr << boost::format("%d %f %f %f %f %f %f %f %f %f %f\n") %
	  (j + k * buflen) %
	  usb_sum % lsb_diff %
	  usb_outbuf[j].real() % usb_outbuf[j].imag() %
	  lsb_outbuf[j].real() % lsb_outbuf[j].imag() %
	  analytic_U_inbuf[j].real() % analytic_U_inbuf[j].imag() %
	  analytic_L_inbuf[j].real() % analytic_L_inbuf[j].imag()
	  ;
#endif
      }
    }
  }

  std::cout << boost::format("# Dumps histograms of USB and LSB normalized error\n# -- all should be clustered around 0.0 -- normalized to mean squared amplitude.\n");
  usb_sum_err.dump(std::cout, std::string("USB: "));
  lsb_diff_err.dump(std::cout, std::string("LSB: "));

  return 1;
}

/**
 * doPMTest -- run a Phase Modulated signal through a hilbert transform
 * shift to baseband, then demodulate.
 */
int doPMTest(int argc, char * argv[])
{
  (void) argc; (void) argv;
  // make a PM signal and find it.
  // create an HT for an audio buffer size of 2304
  // (This corresponds to the magic choice in Params.hxx.)
  int buflen = 2304;
  // make sure the HT impulse response is at least 256 samples long.
  SoDa::HilbertTransformer HT(buflen, 256);

  // create a test input modulating buffer
  float inbuf[buflen];
  // create the FM widget
  float rf_inbuf[buflen];

  // output buffers
  std::complex<float> transform_buf[buflen];

  // now setup an input test signal parameters.
  // We're going to use a signal made up of eight
  // more or less random frequencies
  const int num_freqs = 16;
  float angles[num_freqs];
  float ang_incs[num_freqs];

  srandom(0x13255);

  int i, j, k;
  for(i = 0; i < num_freqs; i++) {
    angles[i] = 0.0;
    long irand = random();
    // we want arg_inc (phase advance per step) to vary
    // over the range 0 to pi / 64
    float frand = ((float) (irand & 0xffff)) / (64.0 * 65536.0);
    ang_incs[i] = frand * M_PI;
  }

  ang_incs[0] = 0.01 * M_PI;

  // what is the phase modulation shift?
  float phase_incr = M_PI / 100.0;

  // what is the carrier frequency ?
  // make it fsamp / 4;
  float carrier_freq = M_PI / 20.0;
  float tx_carrier_angle = 0.0;
  float rx_carrier_angle = 0.0;

  // now do the test over 1000 buffers
  int trial_count = 10;
  int ignore_buffer_count = 1; // ignore the first buffer, or so to get over the startup transient.
  for(k = 0; k < trial_count; k++) {
    // load the input buffer.
    for(j = 0; j < buflen; j++) {
      inbuf[j] = 0.0;
    }
    for(i = 0; i < num_freqs; i++) {
      for(j = 0; j < buflen; j++) {
	inbuf[j] += sin(angles[i]);
	angles[i] += ang_incs[i];
      }
    }

    for(j = 0; j < buflen; j++) {
      rf_inbuf[j] = sin(tx_carrier_angle + inbuf[j] * phase_incr);
      tx_carrier_angle += carrier_freq;
    }

    // perform the transform.
    HT.apply(rf_inbuf, transform_buf, false, 1.0);

    // now shift down to baseband
    for(j = 0; j < buflen; j++) {
      std::complex<float> rx_carrier = std::complex<float>(sin(rx_carrier_angle), cos(rx_carrier_angle));
      rx_carrier_angle += carrier_freq;
      transform_buf[j] = transform_buf[j] * rx_carrier;
    }

    // now calculate the errors
    if(k >= ignore_buffer_count) {
      float norm = 0.0;
      for(j = 0; j < buflen; j++) {
	norm += (inbuf[j] * inbuf[j]);
      }
      norm = sqrt(norm);
      for(j = 0; j < buflen; j++) {
	float demod;
	// demodulate the FM carrier
	demod = atan2(transform_buf[j].imag(), transform_buf[j].real());
#if 1
	std::cerr << boost::format("PM %d %f %f %f %f %f\n") %
	  (j + k * buflen) %
	  inbuf[j] %
	  demod %
	  rf_inbuf[j] %
	  transform_buf[j].real() % transform_buf[j].imag();
#endif
      }
    }
  }

  return 1;
}

int main(int argc, char * argv[])
{
  // Test out the Hilbert Transformer module

  // Rather than doing a comparison with a reference,
  // the HT is tested out by ensuring that the transformer
  // produces results whose properties are those we expect
  // from a Hilbert transform network.
  //
  // 1. Applying HT twice to a real valued signal f(t) will
  //    produce a delayed version of -1 * f(t + d) at its output.
  //    The HT module's complex result, should be (a + ib) =
  //    f(t + d) + i * -1 * f(t + d).  Therefore, we should see
  //    that (a + b) == 0 and (a - b) = 2a.
  //
  // 2. When HT(f(t)) -> a + ib, applying HT to a phase
  //    modulated signal f(t) = sin(\omega t + \phi(t))
  //    should recover \phi(t) = atan(b/a).
  //

  doSSBTest(argc, argv);

  doPMTest(argc, argv);
}


#pragma once

// ros
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <apriltag_msgs/msg/april_tag_detection.hpp>
#include <apriltag_msgs/msg/april_tag_detection_array.hpp>
#include <tf2_msgs/msg/tf_message.hpp>

#include <image_transport/camera_subscriber.hpp>

// apriltag
#include <apriltag.h>

#include <Eigen/Core>

class AprilTagNode : public rclcpp::Node {
public:
    AprilTagNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());

    ~AprilTagNode() override;

private:
    typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Mat3;

    apriltag_family_t* tf;
    apriltag_detector_t* const td;
    const std::string tag_family;
    const double tag_edge_size;
    const int max_hamming;
    std::unordered_map<int, std::string> tag_frames;
    std::unordered_map<int, double> tag_sizes;

    Mat3 K;

    const bool z_up;

    // function pointer for tag family creation / destruction
    static const std::map<std::string, apriltag_family_t *(*)(void)> tag_create;
    const static std::map<std::string, void (*)(apriltag_family_t*)> tag_destroy;

    const image_transport::CameraSubscriber sub_cam;
    const rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf;
    const rclcpp::Publisher<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr pub_detections;

    void onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_img, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci);

    void getPose(const matd_t& H, geometry_msgs::msg::Transform& t, const double size) const;
};


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// Distributed under the Boost
// Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
// copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_BLOOM_FILTER_TWOHASH_COUNTING_BLOOM_FILTER_HPP
#define BOOST_BLOOM_FILTER_TWOHASH_COUNTING_BLOOM_FILTER_HPP 1

#include <cmath>

#include <boost/config.hpp>
#include <boost/array.hpp>

#include <boost/static_assert.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_unsigned.hpp>

#include <boost/bloom_filter/detail/twohash_counting_apply_hash.hpp>
#include <boost/bloom_filter/detail/extenders.hpp>
#include <boost/bloom_filter/hash/default.hpp>
#include <boost/bloom_filter/hash/murmurhash3.hpp>

#ifndef BOOST_NO_0X_HDR_INITIALIZER_LIST
#include <initializer_list>
#endif

namespace boost {
  namespace bloom_filters {
    template <typename T,
	      size_t NumBins,
	      size_t BitsPerBin = 4,
	      size_t HashValues = 2,
	      size_t ExpectedInsertionCount = 0,
	      class HashFunction1 = boost_hash<T>,
	      class HashFunction2 = murmurhash3<T>,
	      class ExtensionFunction = detail::square,
	      typename Block = size_t>
    class twohash_counting_bloom_filter {

      // Block needs to be an integral type
      BOOST_STATIC_ASSERT( boost::is_integral<Block>::value == true);

      // Block needs to be an unsigned type
      BOOST_STATIC_ASSERT( boost::is_unsigned<Block>::value == true);

      // BitsPerBin needs to be greater than 0
      BOOST_STATIC_ASSERT( BitsPerBin > 0);

      // it doesn't make sense to ever support using a BitsPerBin value larger
      // than the number of bits per Block. In that case, the user shouldn't
      // be using a Bloom filter to represent their data.
      BOOST_STATIC_ASSERT( (BitsPerBin < (sizeof(Block) * 8) ) );

      // because of the nature of this implementation, the Bloom filter
      // can have internal fragmentation if the calculation for
      // bins_per_slot has a remainder. The severity of the  internal
      // fragmentation is equal to the remainder * the number of slots.
      // This check prevents internal fragmentation.
      // This also necessarily limits to bin sizes to one of:
      // [1,2,4,8,16,32(64-bit system only)] bits
      BOOST_STATIC_ASSERT( ((sizeof(Block) * 8) % BitsPerBin) == 0);

      // a slot is one element position in the array
      // a bin is a segment of a slot
      static const size_t slot_bits = sizeof(Block) * 8;
      static const size_t bin_bits = NumBins * BitsPerBin;
      static const size_t array_size = bin_bits / slot_bits + 1;

    public:
      typedef T value_type;
      typedef T key_type;
      typedef HashFunction1 hash_function1_type;
      typedef HashFunction2 hash_function2_type;
      typedef ExtensionFunction extension_function_type;
      typedef Block block_type;
      typedef twohash_counting_bloom_filter<T, NumBins, BitsPerBin, HashValues,
					    ExpectedInsertionCount,
					    HashFunction1, HashFunction2,
					    ExtensionFunction, Block> this_type;

      typedef boost::array<Block, array_size> bucket_type;
      typedef typename bucket_type::iterator bucket_iterator;
      typedef typename bucket_type::const_iterator bucket_const_iterator;

    private:
      typedef detail::twohash_counting_apply_hash<HashValues,
						  this_type> apply_hash_type;

    public:
      //! constructors
      twohash_counting_bloom_filter()
      {
	this->clear();
      }

      template <typename InputIterator>
      twohash_counting_bloom_filter(const InputIterator start,
				    const InputIterator end)
      {
	this->clear();

	for (InputIterator i = start; i != end; ++i)
	  this->insert(*i);
      }

#ifndef BOOST_NO_0X_HDR_INITIALIZER_LIST
      twohash_counting_bloom_filter(const std::initializer_list<T>& ilist)
      {
	this->clear();

	typedef typename std::initializer_list<T>::const_iterator citer;
	for (citer i = ilist.begin(), end = ilist.end(); i != end; ++i) {
	  this->insert(*i);
	}
      }
#endif

      //! meta functions
      static BOOST_CONSTEXPR size_t num_bins()
      {
	return NumBins;
      }

      static BOOST_CONSTEXPR size_t bits_per_bin()
      {
	return BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t expected_insertion_count()
      {
	return ExpectedInsertionCount;
      }

      static BOOST_CONSTEXPR size_t bins_per_slot()
      {
	return sizeof(block_type) * 8 / BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t mask()
      {
	return static_cast<Block>(0 - 1) >> (slot_bits - BitsPerBin);
      }

      static BOOST_CONSTEXPR size_t bit_capacity()
      {
        return NumBins * BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t num_hash_functions()
      {
        return HashValues;
      }

      double false_positive_rate() const
      {
        const double n = static_cast<double>(this->count());
        static const double k = static_cast<double>(HashValues);
        static const double m = static_cast<double>(NumBins);
        static const double e =
	  2.718281828459045235360287471352662497757247093699959574966;
        return std::pow(1 - std::pow(e, -k * n / m), k);
      }

      //? returns the number of bins that have at least 1 bit set
      size_t count() const
      {
	size_t ret = 0;

	for (bucket_const_iterator i = this->bits.begin(),
	       end = this->bits.end();
	     i != end; ++i) {
	  for (size_t bin = 0; bin < this->bins_per_slot(); ++bin) {
	    const size_t offset_bits = bin * BitsPerBin;
	    const size_t target_bits = (*i >> offset_bits) & this->mask();

	    if (target_bits > 0)
	      ++ret;
	  }
	}

        return ret;
      }

      bool empty() const
      {
	return this->count() == 0;
      }

      const bucket_type&
      data() const
      {
	return this->bits;
      }

      //! core ops
      void insert(const T& t)
      {
	apply_hash_type::insert(t,
				this->bits,
				this->num_bins());
      }

      template <typename InputIterator>
      void insert(const InputIterator start, const InputIterator end)
      {
	for (InputIterator i = start; i != end; ++i) {
	  this->insert(*i);
	}
      }

      void remove(const T& t)
      {
	apply_hash_type::remove(t,
				this->bits,
				this->num_bins());
      }

      template <typename InputIterator>
      void remove(const InputIterator start, const InputIterator end)
      {
	for (InputIterator i = start; i != end; ++i) {
	  this->remove(*i);
	}
      }

      bool probably_contains(const T& t) const
      {
	return apply_hash_type::contains(t,
					 this->bits,
					 this->num_bins());
      }

      //! auxiliary ops
      void clear()
      {
	for (bucket_iterator i = bits.begin(), end = bits.end();
	     i != end; ++i) {
	  *i = 0;
	}
      }

      void swap(twohash_counting_bloom_filter& other)
      {
	twohash_counting_bloom_filter tmp = other;
	other = *this;
	*this = tmp;
      }

      // equality comparison operators
      template <typename _T, size_t _Bins, size_t _BitsPerBin,
		size_t _HashValues, size_t _ExpectedInsertionCount,
		class _HashFn1, class _HashFn2, class _ExtFn,
		typename _Block>
      friend bool
      operator==(const twohash_counting_bloom_filter<_T, _Bins, _BitsPerBin,
						     _HashValues,
						     _ExpectedInsertionCount,
						     _HashFn1, _HashFn2, _ExtFn,
						     _Block>& lhs,
		 const twohash_counting_bloom_filter<_T, _Bins, _BitsPerBin,
						     _HashValues,
						     _ExpectedInsertionCount,
						     _HashFn1, _HashFn2, _ExtFn,
						     _Block>& rhs);

    private:
      bucket_type bits;
    };

    template <typename T, size_t NumBins, size_t BitsPerBin,
	      size_t HashValues, size_t ExpectedInsertionCount,
	      class HashFunction1, class HashFunction2,
	      class ExtensionFunction, typename Block>
    void
    swap(twohash_counting_bloom_filter<T, NumBins,
				       BitsPerBin,
				       HashValues,
				       ExpectedInsertionCount,
				       HashFunction1,
				       HashFunction2,
				       ExtensionFunction,
				       Block>& lhs,
	 twohash_counting_bloom_filter<T, NumBins,
				       BitsPerBin,
				       HashValues,
				       ExpectedInsertionCount,
				       HashFunction1,
				       HashFunction2,
				       ExtensionFunction,
				       Block>& rhs)
    {
      lhs.swap(rhs);
    }

    template <typename T, size_t NumBins, size_t BitsPerBin,
	      size_t HashValues, size_t ExpectedInsertionCount,
	      class HashFunction1, class HashFunction2,
	      class ExtensionFunction, typename Block>
    bool
    operator==(const twohash_counting_bloom_filter<T, NumBins,
						   BitsPerBin,
						   HashValues,
						   ExpectedInsertionCount,
						   HashFunction1,
						   HashFunction2,
						   ExtensionFunction,
						   Block>& lhs,
	       const twohash_counting_bloom_filter<T, NumBins,
						   BitsPerBin,
						   HashValues,
						   ExpectedInsertionCount,
						   HashFunction1,
						   HashFunction2,
						   ExtensionFunction,
						   Block>& rhs)
    {
      return (lhs.bits == rhs.bits);
    }

    template <typename T, size_t NumBins, size_t BitsPerBin,
	      size_t HashValues, size_t ExpectedInsertionCount,
	      class HashFunction1, class HashFunction2,
	      class ExtensionFunction, typename Block>
    bool
    operator!=(const twohash_counting_bloom_filter<T, NumBins,
						   BitsPerBin,
						   HashValues,
						   ExpectedInsertionCount,
						   HashFunction1,
						   HashFunction2,
						   ExtensionFunction,
						   Block>& lhs,
	       const twohash_counting_bloom_filter<T, NumBins,
						   BitsPerBin,
						   HashValues,
						   ExpectedInsertionCount,
						   HashFunction1,
						   HashFunction2,
						   ExtensionFunction,
						   Block>& rhs)
    {
      return !(lhs == rhs);
    }
  } // namespace bloom_filter
} // namespace boost
#endif


#include <puppet/compiler/evaluation/operators/binary/divide.hpp>
#include <puppet/compiler/evaluation/operators/binary/call_context.hpp>
#include <puppet/compiler/exceptions.hpp>
#include <boost/format.hpp>
#include <limits>
#include <cfenv>

using namespace std;
using namespace puppet::runtime::values;
using namespace puppet::runtime::types;

namespace puppet { namespace compiler { namespace evaluation { namespace operators { namespace binary {

    static value divide_by(call_context& context, int64_t left, int64_t right)
    {
        auto& evaluation_context = context.context();

        if (right == 0) {
            throw evaluation_exception("cannot divide by zero.", context.right_context(), evaluation_context.backtrace());
        }
        if (left == numeric_limits<int64_t>::min() && right == -1) {
            throw evaluation_exception(
                (boost::format("division of %1% by %2% results in an arithmetic overflow.") %
                 left %
                 right
                ).str(),
                context.right_context(),
                evaluation_context.backtrace()
            );
        }
        return left / right;
    }

    static value divide_by(call_context& context, double left, double right)
    {
        auto& evaluation_context = context.context();

        feclearexcept(FE_OVERFLOW | FE_UNDERFLOW | FE_DIVBYZERO);
        double result = left / right;
        if (fetestexcept(FE_DIVBYZERO)) {
            throw evaluation_exception("cannot divide by zero.", context.right_context(), evaluation_context.backtrace());
        } else if (fetestexcept(FE_OVERFLOW)) {
            throw evaluation_exception(
                (boost::format("division of %1% and %2% results in an arithmetic overflow.") %
                 left %
                 right
                ).str(),
                context.right_context(),
                evaluation_context.backtrace()
            );
        } else if (fetestexcept(FE_UNDERFLOW)) {
            throw evaluation_exception(
                (boost::format("division of %1% and %2% results in an arithmetic underflow.") %
                 left %
                 right
                ).str(),
                context.right_context(),
                evaluation_context.backtrace()
            );
        }
        return result;
    }

    static value divide_by(call_context& context, value const& left, value const& right)
    {
        if (left.as<int64_t>() && right.as<int64_t>()) {
            return divide_by(context, left.require<int64_t>(), right.require<int64_t>());
        }
        if (left.as<int64_t>()) {
            return divide_by(context, static_cast<double>(left.require<int64_t>()), right.require<double>());
        }
        if (right.as<int64_t>()) {
            return divide_by(context, left.require<double>(), static_cast<double>(right.require<int64_t>()));
        }
        return divide_by(context, left.require<double>(), right.require<double>());
    }

    value arithmetic_string_conversion(call_context& context, bool left = true)
    {
        try {
            return numeric::instantiate(rvalue_cast(left ? context.left() : context.right()));
        } catch (type_conversion_exception const& ex) {
            throw evaluation_exception(
                ex.what(),
                (left ? context.left_context() : context.right_context()),
                context.context().backtrace()
            );
        }
    }

    descriptor divide::create_descriptor()
    {
        binary::descriptor descriptor{ ast::binary_operator::divide };

        descriptor.add("Numeric", "Numeric", [](call_context& context) {
            return divide_by(context, context.left(), context.right());
        });
        descriptor.add("String", "Numeric", [](call_context& context) {
            return divide_by(context, arithmetic_string_conversion(context), context.right());
        });
        descriptor.add("Numeric", "String", [](call_context& context) {
            return divide_by(context, context.left(), arithmetic_string_conversion(context, false));
        });
        descriptor.add("String", "String", [](call_context& context) {
            return divide_by(context, arithmetic_string_conversion(context), arithmetic_string_conversion(context, false));
        });
        return descriptor;
    }

}}}}}  // namespace puppet::compiler::evaluation::operators::binary