#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <chrono>
#include <sys/stat.h>

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

#include <boost/program_options.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
namespace po = boost::program_options;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "../../../core/image.h"
#include "../../../core/helper.h"
#include "../../../core/math_helper.h"
#include "../../../stereo_egomotion/base/egomotion_base.h"
//#include "../../../stereo_odometry/base/visual_odometry_ransac.h"
#include "../../../tracker/stereo/experiment_factory.h"
#include "../../../tracker/stereo/stereo_tracker_base.h"
#include "../../../tracker/stereo/stereo_tracker.h"
#include "../../../tracker/refiner/feature_refiner_base.cc"
#include "../../../tracker/refiner/feature_refiner_klt.h"
#include "../../../tracker/detector/feature_detector_base.h"
//#include "../../../tracker/stereo/tracker_helper.h"
#include "../../../optimization/bundle_adjustment/bundle_adjuster_base.h"
#include "../../../optimization/calibration_bias/deformation_field_solver.h"
#include "../../math_helper.h"
#include "../../../tracker/base/eval_helper.h"
#include "../../../core/format_helper.h"
#include "../../helper_libviso.h"
#include "../../cv_plotter.h"

using namespace core;

void run_visual_odometry(const std::string& source_folder,
                         const std::string& imagelistfn,
                         const std::string& experiment_config,
                         const std::string& cparams_file,
                         const std::string& gt_filepath,
                         const std::string& save_folder_root,
                         const std::string& deformation_field_path)
{
  std::deque<cv::Mat> extr_params; // pose mat
  std::deque<cv::Mat> Rt_params;

  double cam_params[5];
  FormatHelper::readCameraParams(cparams_file, cam_params);

  std::vector<std::string> imagelist;
  bool ok = FormatHelper::readStringList(imagelistfn, imagelist);
  if (!ok || imagelist.empty())
    throw "can not open " + imagelistfn + " or the string list is empty\n";

  int start_frame = 0 * 2;
  //int start_frame = 2 * 2;
  //int start_frame = 228 * 2;
  // 01
  //int start_frame = 431 * 2;
  //int start_frame = 482 * 2;
  int end_frame = imagelist.size();

  cv::Mat img_left_prev, img_right_prev;
  std::cout << source_folder + imagelist[start_frame] << "\n";
  cv::Mat img_left = cv::imread(source_folder + imagelist[start_frame], CV_LOAD_IMAGE_GRAYSCALE);
  cv::Mat img_right = cv::imread(source_folder + imagelist[start_frame+1], CV_LOAD_IMAGE_GRAYSCALE);
  if (img_left.empty()) {
    std::cout << "Error: no images!\n";
    throw 1;
  }

  std::string output_folder;
  track::FeatureDetectorBase* detector = nullptr;
  track::TrackerBase* mono_tracker = nullptr;
  track::StereoTrackerBase* stereo_tracker = nullptr;
  egomotion::EgomotionBase* viso = nullptr;
  optim::BundleAdjusterBase* bundle_adjuster = nullptr;
  bool use_ba; // use bundle adjustment
  int img_rows = img_left.rows;
  int img_cols = img_left.cols;
  track::ExperimentFactory::create_experiment(experiment_config, deformation_field_path, cam_params,
                                              img_rows, img_cols, output_folder, &detector, &mono_tracker,
                                              &stereo_tracker, &viso, use_ba, &bundle_adjuster);

  //libviso::Matrix pose = libviso::Matrix::eye(4);
  cv::Mat pose = cv::Mat::eye(4, 4, CV_64F);
  //Matrix point_rt = Matrix::eye(4);
  //cv::Mat pose_mat = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat Rt_inv;
  cv::Mat mat_I = cv::Mat::eye(4, 4, CV_64F);
  mat_I.copyTo(Rt_inv);
  extr_params.push_back(mat_I.clone());
  cv::Mat Rt(4, 4, CV_64F);
  cv::Mat Rt_gt(4, 4, CV_64F);
  cv::Mat Rt_gt_prev = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat Rt_gt_curr = cv::Mat::eye(4, 4, CV_64F);
  core::MathHelper::invTrans(Rt_inv, Rt);
  Rt_params.push_back(Rt.clone());

  // init the tracker
  stereo_tracker->init(img_left, img_right);

  // matrix transforms points in current camera coordinate system to world coord system
  std::vector<cv::Mat> libviso_Rt_all;
  cv::Mat pose_libviso = cv::Mat::eye(4, 4, CV_64F);
  libviso_Rt_all.push_back(pose_libviso.clone());

  std::string track_name;
  bool have_gt = false;
  if (!gt_filepath.empty())
    have_gt = true;
  else throw 1;

  double error_thr = 0.0;
  std::string save_folder;
  bool using_kitti = false;
  if (experiment_config.find("tsukuba") != std::string::npos) {
    printf("---------- Using Tsukuba dataset! ------------\n");
    track_name = "00";
    save_folder = save_folder_root + "/";
    error_thr = 2.0;
  }
  else if (experiment_config.find("bb2") != std::string::npos) {
    printf("---------- Using Bumblebee dataset! ------------\n");
    track_name = "bb";
    throw 1;
  }
  // else it is KITTI
  else {
    printf("---------- Using KITTI dataset! ------------\n");
    track_name = imagelistfn.substr(imagelistfn.size()-10,2);
    using_kitti = true;
    save_folder = save_folder_root + "/kitti_" + track_name + "/";
    // try 6 and 4
    //error_thr = 5.0; // remove moving cars with 5.0 - works worssse...
    error_thr = 8.0;
  }
  //std::string save_folder = "/opt/kivan/data/tracker_data/kitti_" + track_name + "/";
  //std::string save_folder = "/mnt/ssd/kivan/datasets/tracker_data/freak/kitti_" + track_name + "/";
  //std::string save_folder = "/mnt/ssd/kivan/datasets/freak_tracker_data/kitti_" + track_name + "/";
  //std::string save_folder = "/opt/kivan/datasets/tracker_data/freak/kitti_" + track_name + "/";
  struct stat st = {0};
  if(stat(save_folder.c_str(), &st) == -1) {
    mkdir(save_folder.c_str(), 0700);
  }
  else {
    std::cout << "Folder exists!\n";
    return;
  }

  int num_of_motions = ((imagelist.size()/2) - 1);
  std::vector<cv::Mat> gt_world_motion, gt_camera_motion;
  core::FormatHelper::Read2FrameMotionFromAccCameraMotion(gt_filepath, num_of_motions,
                                                          gt_world_motion, gt_camera_motion);

  //int gt_better_cnt = 0;
  auto start = std::chrono::system_clock::now();
  //for (unsigned i = start_frame + 2; i < start_frame+60; i+=2) {
  double max_trans_error = 0.0;
  for (unsigned i = start_frame + 2; i < end_frame; i += 2) {
    int motion_idx = i / 2 - 1;
    std::cout << "motion_idx: " << motion_idx << " / " << (gt_world_motion.size() - 1) << "\n";
    std::cout << source_folder + imagelist[i] << "\n";
    cv::swap(img_left_prev, img_left);
    cv::swap(img_right_prev, img_right);
    img_left = cv::imread(source_folder + imagelist[i], CV_LOAD_IMAGE_GRAYSCALE);
    img_right = cv::imread(source_folder + imagelist[i+1], CV_LOAD_IMAGE_GRAYSCALE);

    stereo_tracker->track(img_left, img_right);

    // filter outliers with GT
    bool gt_filtering_passed = false;
    std::vector<std::vector<double>> left_tmp, right_tmp;
    gt_filtering_passed = track::EvalHelper::FilterOutliersWithGroundtruth(*stereo_tracker,
                           cam_params, gt_world_motion[motion_idx], error_thr); // the best KITTI thresh - 8.0

    //cv::Mat img_lp, img_rp, img_lc, img_rc;
    //track::EvalHelper::DrawTracksWithBigErrors(*stereo_tracker, cam_params, gt_world_motion[motion_idx],
    //                              2.0, 7.0, -1, img_left_prev, img_right_prev, img_left, img_right,
    //                              cv::Scalar(0,0,255), true, img_lp, img_rp, img_lc, img_rc);
    //cv::waitKey(0);
    //track::EvalHelper::DrawTracksWithBigErrors(*stereo_tracker, cam_params, gt_motion[i/2],
    //                              6.0, 11, img_left_prev, img_right_prev, img_left, img_right,
    //                              cv::Scalar(0,0,255), true, img_lp, img_rp, img_lc, img_rc);
    //cv::imwrite("left_prev.png", img_lp);
    //// 11, 19, 30, 53
    //track::EvalHelper::DrawTrackPatches(*stereo_tracker, 53, 15, img_lp, img_rp, img_lc, img_rc);

    std::cout << "Serializing to disk\n";
    // save data to archive
    std::ofstream ofs(save_folder + "tracks_" + std::to_string(motion_idx));
    //boost::archive::text_oarchive oarchive(ofs);
    boost::archive::binary_oarchive oarchive(ofs);
    // write class instance to archive
    oarchive << *(dynamic_cast<track::StereoTracker*>(stereo_tracker));
    // archive and stream closed when destructors are called
  }
  auto end = std::chrono::system_clock::now();
  //auto elapsed = end - start;
  std::chrono::duration<double> elapsed = end - start;
  std::cout << "Elapsed time = " << elapsed.count() << " secs\n";

  delete detector;
  delete mono_tracker;
  delete stereo_tracker;
  delete viso;
}

int main(int argc, char** argv)
{
  std::string config_file;
  std::string experiment_config;
  std::string imagelistfn;
  std::string cam_params_file;
  std::string source_folder;
  std::string gt_filepath;
  std::string deformation_field_path;
  std::string save_folder;

  if (argc != 7){
    std::cout << "Usage:\n" << argv[0] << " -c dataset_config -e experiment_config -s save_folder\n";
    return 0;
  }

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help, h", "produce help message")
      ("config,c", po::value<std::string>(&config_file), "config filename")
      ("experiment_config,e", po::value<std::string>(&experiment_config), "experiment config filename")
      ("save_folder,s", po::value<std::string>(&save_folder), "save folder");
    po::options_description cmdline_options;
    cmdline_options.add(generic);

    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")) {
      std::cout << generic;
      return 0;
    }

    po::options_description config("Config file options");
    config.add_options()
      ("camera_params,p", po::value<std::string>(&cam_params_file)->default_value("camera_params.txt"), "camera params file")
      ("source_folder,s", po::value<std::string>(&source_folder), "folder with source")
      ("imglist,l", po::value<std::string>(&imagelistfn), "file with image list")
      ("groundtruth,g", po::value<std::string>(&gt_filepath), "file with motion GT")
      ("deformation_field,d", po::value<std::string>(&deformation_field_path), "file with deformation field");
    std::ifstream ifs(config_file);
    if (!ifs) {
      throw "can not open config file: " + config_file + "\n";
    }
    else {
      po::store(parse_config_file(ifs, config, true), vm);
      notify(vm);
    }
  }
  catch(std::exception& e) {
    std::cout << e.what() << "\n";
    return -1;
  }

  std::cout << "Using track config = " << config_file << '\n';
  run_visual_odometry(source_folder, imagelistfn, experiment_config, cam_params_file, gt_filepath,
                      save_folder, deformation_field_path);

  return 0;
}


// test file for quaternion.hpp

//  (C) Copyright Hubert Holin 2001.
//  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)

#include "quaternion_mi2.h"

#include <boost/math/quaternion.hpp>

void    quaternion_mi2()
{
    ::boost::math::quaternion<float>    q0;

    q0 *= q0;
}


#include <MainWindow.h>
#include <ui_MainWindow.h>

#include <cmath>
#include <boost/shared_ptr.hpp>

#include <Common.h>
#include <GeometricCamera.h>
#include <LambertBsdf.h>
#include <Material.h>
#include <QuadLight.h>

//----------------------------------------------------------------------------------------------------------------------
MainWindow::MainWindow(QWidget *parent) :QMainWindow(parent), m_ui(new Ui::MainWindow)
{
  //Create an openGL format and pass to the new GLWidget
  QGLFormat format;
  format.setVersion(3,2);
  format.setProfile( QGLFormat::CoreProfile);

  //Setup the user interface
  m_ui->setupUi(this);

  //Create our GL window for drawing the spring
  m_gl=new GLWindow(format,m_ui->m_timerValue->value(),this);

  //Add the glWindow to the UI
  m_ui->s_mainWindowGridLayout->addWidget(m_gl,0,0,12,1);

  //Create render objects as well as tissue simulation with member pointers
  m_pathtracer = boost::shared_ptr<Engine>(new Engine());
  m_scene = boost::shared_ptr<Scene>(new Scene());
  m_settings = boost::shared_ptr<Settings>(new Settings());
  m_tissue = boost::shared_ptr<TissueShape>(new TissueShape());

  //Create other render objects that don't need to be directly accessed
  boost::shared_ptr<LambertBsdf> bsdf(new LambertBsdf());
  boost::shared_ptr<Material> material(new Material(bsdf));
  boost::shared_ptr<Primitive> primitive(new Primitive(m_tissue, material));
  boost::shared_ptr<GeometricCamera> camera(new GeometricCamera());
  boost::shared_ptr<QuadLight> light(new QuadLight());

  //Innitial render settings
  m_settings->resolution_x = m_ui->m_resolution_x->value();
  m_settings->resolution_y = m_ui->m_resolution_y->value();
  m_settings->bucket_size = m_ui->m_bucket_size->value();
  m_settings->samples = m_ui->m_samples->value();
  m_settings->depth = m_ui->m_depth->value();
  m_settings->absorption = m_ui->m_absorption->value();

  //Innitial simulation settings
  m_tissue->springFactor(m_ui->m_spring_factor->value());
  m_tissue->planarFactor(m_ui->m_planar_factor->value());
  m_tissue->bulgeFactor(m_ui->m_bulge_factor->value());
  m_tissue->linkRestLength(m_ui->m_link_rest_length->value());
  m_tissue->repulsionFactor(m_ui->m_repulsion_factor->value());
  m_tissue->repulsionRadius(m_ui->m_repulsion_radius->value());
  m_tissue->timeStep(m_ui->m_dt->value());

  //Material colour
  material->colour(Colour3f(0.9f, 0.9f, 0.9f));

  //Innitial camera settings
  camera->origin(Vector3f(0.f, 0.f, 1250.f));
  camera->direction(Vector3f(0.f, 0.f, -1.f));
  //This sets the vertical FOV to 35 degrees
  camera->distance(1.f / std::tan(30.f * 0.5f * M_PI_180));
  camera->ratio(static_cast<float>(m_settings->resolution_x) / static_cast<float>(m_settings->resolution_y));
  camera->focalDistance(1150.f);
  camera->aperture(75.f);

  //Light data
  light->origin(Vector3f(0.f, 1200.f, 0.f));
  light->direction(Vector3f(0.f, -1.f, 0.f));
  light->height(900.f);
  light->width(900.f);
  light->illumination(Colour3f(1.85f, 1.63f, 1.41f));
  light->exponent(2.f);

  //Set camera, add simulation as a primitive and add a light
  m_scene->setCamera(camera);
  m_scene->addPrimitive(primitive);
  m_scene->addLight(light);

  //Send tissue object to OpenGl as it is chared
  m_gl->setTissue(m_tissue);

  // now we wire up the UI components to the slots
  connect(m_ui->m_resolution_x,SIGNAL(valueChanged(int)),this,SLOT(setResolutionX(int)));
  connect(m_ui->m_resolution_y,SIGNAL(valueChanged(int)),this,SLOT(setResolutionY(int)));
  connect(m_ui->m_bucket_size,SIGNAL(valueChanged(int)),this,SLOT(setBucketSize(int)));
  connect(m_ui->m_samples,SIGNAL(valueChanged(int)),this,SLOT(setSamples(int)));
  connect(m_ui->m_depth,SIGNAL(valueChanged(int)),this,SLOT(setDepth(int)));
  connect(m_ui->m_absorption,SIGNAL(valueChanged(double)),this,SLOT(setAbsorption(double)));

  connect(m_ui->m_spring_factor,SIGNAL(valueChanged(double)),this,SLOT(setSpingFactor(double)));
  connect(m_ui->m_planar_factor,SIGNAL(valueChanged(double)),this,SLOT(setPlanarFactor(double)));
  connect(m_ui->m_bulge_factor,SIGNAL(valueChanged(double)),this,SLOT(setBulgeFactor(double)));
  connect(m_ui->m_link_rest_length,SIGNAL(valueChanged(double)),this,SLOT(setLinkRestLength(double)));
  connect(m_ui->m_repulsion_factor,SIGNAL(valueChanged(double)),this,SLOT(setRepulsionFactor(double)));
  connect(m_ui->m_repulsion_radius,SIGNAL(valueChanged(double)),this,SLOT(setRepulsionRadius(double)));

  connect(m_ui->m_render,SIGNAL(clicked()),this,SLOT(render()));
  connect(m_ui->m_sim,SIGNAL(clicked(bool)),this,SLOT(toggleSim(bool)));
  connect(m_ui->m_reset,SIGNAL(clicked()),m_gl,SLOT(reset()));
  connect(m_ui->m_timerValue,SIGNAL(valueChanged(int)),m_gl,SLOT(setTimerDuration(int)));
  connect(m_ui->m_dt,SIGNAL(valueChanged(double)),this,SLOT(setDT(double)));
}

//----------------------------------------------------------------------------------------------------------------------
MainWindow::~MainWindow()
{
    delete m_ui;
}

//----------------------------------------------------------------------------------------------------------------------
void MainWindow::toggleSim(bool _s)
{
  if(_s == true)
  {
    m_gl->startSimTimer();
  }
  else
  {
    m_gl->stopSimTimer();
  }
}

//Render using offline renderer
void MainWindow::render()
{
  //Update camera position to match OpenGL
  Vector3f camera_position = m_gl->getCameraPos();
  Vector3f tissue_position = m_tissue->position();
  m_scene->getCamera()->origin(camera_position + tissue_position);
  m_scene->getCamera()->direction(camera_position * -1);

  m_pathtracer->run(m_scene, m_settings);
}


#include <armadillo>
#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <mutex>
#include <signal.h>
#include <thread>
#include <time.h>
#include <unistd.h>
#include "SDL/SDL.h"

#include "astar.h"
#include "chili_landmarks.h"
#include "draw.h"
#include "ipcdb.h"
#include "mathfun.h"
#include "pfilter.h"
#include "tachikoma.h"
#include "sdldef.h"
#include "sim_landmark.h"
#include "sim_map.h"
#include "sim_robot.h"

using namespace arma;
using namespace std;

// threads in this file
void manual_input(void);
void chilitag_detect(void);
void localize_pose(void);
void robot_calcmotion(void);
void motion_plan(void);
void display_interface(void);

static double secdiff(struct timeval &t1, struct timeval &t2);

static void stoprunning(int signum)
{
	stopsig = 1;
	tachikoma.disconnect();
	alarm(1); // hack to get this to force quit
}

static void forcequit(int signum)
{
	printf("Force quitting...\n");
	exit(1);
}

static SDL_Surface *initSDL(int w, int h)
{
	if (SDL_Init(SDL_INIT_EVERYTHING) == -1)
	{
		return NULL;
	}

	SDL_Surface *screen = SDL_SetVideoMode(w,h, 32, SDL_SWSURFACE);
	if (screen == NULL)
	{
		return NULL;
	}

	SDL_WM_SetCaption("tachikoma", NULL);

	return screen;
}

static void screenblit(SDL_Surface *s, cube &frame)
{
	for (int i = 0; i < (int)frame.n_rows; i++)
	{
		for (int j = 0; j < (int)frame.n_cols; j++)
		{
			uint32_t color =
				((uint8_t)((int)round(frame(i,j,0) * 255)) << 16) |
				((uint8_t)((int)round(frame(i,j,1) * 255)) << 8) |
				((uint8_t)((int)round(frame(i,j,2) * 255)) << 0);
			((uint32_t *)s->pixels)[XY2P(j, i, s->w, s->h)] = color;
		}
	}
}

static void chilicamdetect_thread(void)
{
	chili.update(cameraFrame, camframelock);
}

int main()
{
	// preemptive init
	printf("[main] preemptive init\n");
	signal(SIGINT, stoprunning);
	signal(SIGALRM, forcequit);
	screen = initSDL(500, 500);

	if (!screen)
	{
		printf("No screen found, please check your SDL configurations\n");
		return 1;
	}

	double initial_x = 96;
	double initial_y = 24;
	double initial_t = 90;
	robot_pose = vec({ initial_x, initial_y, initial_t });
	globalmap.load("map.png"); // lower corner is (0,0)

	// start up the threads
	printf("[main] start up the threads\n");
	tachikoma.connect();
  tachikoma.stop_legs();
	thread manual_thread(manual_input);
	//thread chilicamthread(chilicamdetect_thread); // uncomment later
	thread chili_thread(chilitag_detect);
	thread pose_thread(localize_pose);
	thread path_thread(motion_plan);
	thread robot_thread(robot_calcmotion);
	thread display_thread(display_interface);

	// wait until program closes
	printf("[main] wait until program closes\n");
	while (!stopsig);
	alarm(1);

	// close all threads
	printf("[main] close all threads\n");
	tachikoma.stop_wheels();
	tachikoma.stop_legs();
	tachikoma.stop_arm();
	tachikoma.disconnect();
	chili_thread.join();
	chilicamthread.join();
	pose_thread.join();
	path_thread.join();
	robot_thread.join();
	display_thread.join();
	SDL_Quit();

	printf("Closed successfully.\n");
	return 0;
}

static bool arm_enabled = false;
static vec garm({ -15, 90, 30, 0 });

void manual_input(void)
{
	int legstate = 0;
	struct timeval starttime;
	gettimeofday(&starttime, NULL);
	while (!stopsig)
	{
		SDL_PumpEvents();
		Uint8 *keystates = SDL_GetKeyState(NULL);

		// detect for exit
		if (keystates[SDLK_x])
		{
			auto_enable = false;
			auto_confirmed = false;
			manual_confirmed = true;
			tachikoma.stop_wheels();
			tachikoma.stop_legs();
			tachikoma.stop_arm();
			kill(getpid(), SIGINT);
			continue;
		}

		// detect for autonomous enable or disable
		if (keystates[SDLK_m])
		{
			autonomous_lock.lock();
			auto_enable = false;
			auto_confirmed = false;
			manual_confirmed = true;
			autonomous_lock.unlock();
		}
		else if (keystates[SDLK_n])
		{
			autonomous_lock.lock();
			auto_enable = true;
			auto_confirmed = false;
			manual_confirmed = true;
			autonomous_lock.unlock();
		}

		// if not manual en, then continue
		if (auto_enable || !auto_confirmed)
		{
			continue;
		}

		/*if (keystates[SDLK_0]) {
			legstate = 0;
		} else if (keystates[SDLK_1]) {
			legstate = 1;
		} else if (keystates[SDLK_2]) {
			legstate = 2;
		} else if (keystates[SDLK_3]) {
			legstate = 3;
		} else if (keystates[SDLK_4]) {
			legstate = 4;
		}*/

		/*switch (legstate) {
			case 1:
				tachikoma.set_legs(0, 0, 0, 0, -45, -45, -45, -45);
				break;
			case 2:
				tachikoma.set_legs(0, 0, 0, 0, 0, 0, 0, 0);
				break;
			case 3:
				tachikoma.set_legs(0, 0, 0, 0, 45, 45, 45, 45);
				break;
			case 4:
				tachikoma.set_legs(45, -45, -45, 45, 0, 0, 0, 0);
				break;
			default:
				tachikoma.stop_legs();
				break;
		}*/

		// input arm manual feedback
		struct timeval currtime;
		gettimeofday(&currtime, NULL);
		if (secdiff(starttime, currtime) > 0.05)
		{
			if (keystates[SDLK_p]) { garm(0) += 2; }
			if (keystates[SDLK_l]) { garm(0) -= 2; }
			if (keystates[SDLK_o]) { garm(1) += 2; }
			if (keystates[SDLK_k]) { garm(1) -= 2; }
			if (keystates[SDLK_i]) { garm(2) += 2; }
			if (keystates[SDLK_j]) { garm(2) -= 2; }
			if (keystates[SDLK_t]) { garm(3) += 10; }
			if (keystates[SDLK_f]) { garm(3) -= 10; }
			memcpy(&starttime, &currtime, sizeof(struct timeval));
		}

		if (keystates[SDLK_v])
		{
			arm_enabled = true;
		}
		if (keystates[SDLK_b])
		{
			arm_enabled = false;
		}

		if (arm_enabled)
		{
			tachikoma.set_arm(garm(0), garm(1), garm(2), garm(3));
		}
		else
		{
			tachikoma.stop_arm();
		}

		// input manual feedback
		if	(keystates[SDLK_q]) tachikoma.set_wheels(-1,-1,-1,-1);
		else if (keystates[SDLK_e]) tachikoma.set_wheels(1,1,1,1);
		else if (keystates[SDLK_a]) tachikoma.set_wheels(-1,-1,1,1);
		else if (keystates[SDLK_d]) tachikoma.set_wheels(1,1,-1,-1);
		else if (keystates[SDLK_s]) tachikoma.set_wheels(-1,1,-1,1);
		else if (keystates[SDLK_w]) tachikoma.set_wheels(1,-1,1,-1);
		else if (keystates[SDLK_1]) tachikoma.set_wheels(1,0,0,0);
		else if (keystates[SDLK_2]) tachikoma.set_wheels(0,1,0,0);
		else if (keystates[SDLK_3]) tachikoma.set_wheels(0,0,1,0);
		else if (keystates[SDLK_4]) tachikoma.set_wheels(0,0,0,1);
		else tachikoma.set_wheels(0,0,0,0);

	}
}

void chilitag_detect(void)
{
	while (!stopsig)
	{
		// place the chilitags' positions into a matrix
		mat sv(3, 20, fill::zeros);
		for (int j = 0; j < 20; j++)
		{
			if (chili.tags[j][0] != 0.0)
			{
				// -y since the image is reversed in camera input
				sv.col(j) = vec({ chili.tags[j][1], -chili.tags[j][2], chili.tags[j][0] });
			}
		}

		for (int j = 0; j < 20; j++)
		{
			vec pt({ sv(0, j), sv(1, j) });
			sv(0, j) = eucdist(pt);
			sv(1, j) = angle(pt);
		}

		// store the matrix
		chili_lock.lock();
		chilitags = sv;
		chili_lock.unlock();
	}
}

void localize_pose(void)
{
	// create the landmarks (custom)
	landmarks.push_back(sim_landmark(96, 24));				// 00
	landmarks.push_back(sim_landmark(72, 24));				// 01
	landmarks.push_back(sim_landmark(48, 24));				// 02
	landmarks.push_back(sim_landmark(96, 48));				// 03
	landmarks.push_back(sim_landmark(48, 48));				// 04
	landmarks.push_back(sim_landmark(72, 72));				// 05
	landmarks.push_back(sim_landmark(24, 72));				// 06
	landmarks.push_back(sim_landmark(48, 96));				// 07
	landmarks.push_back(sim_landmark(72, 120));				// 08
	landmarks.push_back(sim_landmark(24, 120));				// 09
	landmarks.push_back(sim_landmark(48, 144));				// 10
	landmarks.push_back(sim_landmark(24, 168));				// 11
	landmarks.push_back(sim_landmark(48, 168));				// 12
	landmarks.push_back(sim_landmark(48, 192));				// 13
	landmarks.push_back(sim_landmark(72, 192));				// 14
	landmarks.push_back(sim_landmark(24, 216));				// 15
	landmarks.push_back(sim_landmark(72, 216));				// 16
	landmarks.push_back(sim_landmark(48, 216));				// 17
	//landmarks.push_back(sim_landmark(146, 480));				// 18
	//landmarks.push_back(sim_landmark(146, 240));				// 19

	// start the particle filter
	pose_lock.lock();
	int nparticles = 500;
	double initial_sigma = 100;
	double x = robot_pose(0);
	double y = robot_pose(1);
	double t = robot_pose(2);
	double vs = 10.0;
	double ws = 2.0;
	pf = pfilter(nparticles, &globalmap, landmarks, x, y, t, initial_sigma);
	pf.set_noise(vs, ws);
	pose_lock.unlock();

	// loop on the particle filter for updates on the location
	vec mu;
	mat sigma;
	struct timeval prevtime, currenttime;
	gettimeofday(&prevtime, NULL);
	vec prevenc(4, fill::zeros);
	while (!stopsig)
	{
		gettimeofday(&currenttime, NULL);
		if (secdiff(prevtime, currenttime) < 0.050)
		{ // 20hz
			return;
		}
		else
		{
			memcpy(&prevtime, &currenttime, sizeof(struct timeval));
		}
		vec a_, b_, c_, sensors;
		mat d_, e_;
		tachikoma.recv(a_, b_, d_, e_, sensors, c_);
		vec enc = sensors;
		double v, w;
		double t2iK_forward = 0.246826621412;
		vec dist = enc - prevenc;
		v = mean(dist) * t2iK_forward;
		dist -= (mean(dist) / t2iK_forward) * ones<vec>(4);
		double t2iK_rotation = 2.2857123550422 / 4;
		dist(0) *= -1;
		dist(2) *= -1;
		w = mean(dist) * t2iK_rotation;
		prevenc = enc;

		// move the robot
		pf.move(0, v, w);

		// get the chilitags
		chili_lock.lock();
		mat landmarks = chilitags;
		chili_lock.unlock();

		// observe and predict the robot's new location
		pf.observe(landmarks);
		pf.predict(mu, sigma);

		// store the new location
		pose_lock.lock();
		robot_pose = mu;
		pose_lock.unlock();
	}
}

void robot_calcmotion(void)
{
	while (!stopsig)
	{

		autonomous_lock.lock();
		if (!auto_enable)
		{
			autonomous_lock.unlock();
			continue;
		}
		autonomous_lock.unlock();

		// get the current position of the robot
		pose_lock.lock();
		vec pose = robot_pose;
		pose_lock.unlock();
		vec pos = pose(span(0,1));
		double theta = pose(2);

		// get the current plan
		path_lock.lock();
		mat path_plan = pathplan;
		vec pose_plan = poseplan;
		bool do_pose = dopose;
		double twist = twistplan;
		double grab = grabplan;
		path_lock.unlock();
		int nwaypoints = (int)path_plan.n_cols;
		if (nwaypoints < 2)
		{
			// if there is no path, stop the robot for now
			tachikoma.set_wheels(0, 0, 0, 0);
			continue;
		}
		else if (nwaypoints == 2)
		{
			//printf("Reached goal, autonomous stopping\n");
			tachikoma.set_wheels(0, 0, 0, 0);
			continue;
		}

		// do calculation of the wheels
		// first find the closest waypoint as the "start"
		vec distance(nwaypoints);
		mat diffs = path_plan - repmat(pos, 1, nwaypoints);
		uword target_index = 0;
		vec target = path_plan.col(0);
		for (int j = 0; j < nwaypoints; j++)
		{
			vec a = diffs.col(j);
			vec b = diffs.col(target_index);
			distance[j] = eucdist(a);
			if (distance[j] < eucdist(b))
			{
				target_index = j;
				target = path_plan.col(target_index);
			}
		}

		// acceptance radius && sector check
		if (distance[target_index] < 20 || (distance[target_index] < 40 && !within_value(angle(target - pos) - theta, -45, 45)))
		{
			if (target_index >= nwaypoints-1)
			{
				//printf("target reached\n");
				tachikoma.set_wheels(0, 0, 0, 0);
			}
			else
			{
				target_index++;
				target = path_plan.col(target_index);
			}
		}

		// once the target is found, then calculate the trajectory
		double theta_k_p = 0.025;
		double delta_theta = angle(target - pos) - theta;
		double v_l, v_r;
		v_l = (1.0 - theta_k_p * delta_theta);
		// - theta_k_i * r.integral_error
		// - theta_k_d * r.delta_theta_diff);
		v_r = (1.0 + theta_k_p * delta_theta);
		// + r.theta_k_i * r.integral_error
		// + r.theta_k_d * r.delta_theta_diff);

		cout << "current angle: " << theta << endl;
		cout << "delta theta: " << delta_theta << endl;
		cout << "left vel: " << v_l << endl << "right vel: " << v_r << endl;
		cout << "waypoint: " << endl << path_plan.col(path_plan.n_cols-1) << endl;
		cout << "current position: " << endl << pose << endl;
		cout << endl;

		// send the trajectory to the robot's wheels
		tachikoma.set_wheels(v_l, v_r, v_l, v_r);

		// only if we want to do a pose do we activate the pose solver
		if (!do_pose)
		{
			tachikoma.stop_arm();
			continue;
		}

			//tachikoma.stop_arm(); // hack
		/*// get the list of all possible poses
		bool feasible_found = false;
		double baseangle = angle(poseplan.col(0).subvec(0,1));
		vec enc(6);
		for (int i = 0; i <= 90; i += 5)
		{ // 5 degree granularity
			mat R = rotationMat(0, 0, baseangle);
			// if a negative pose proves to find a solution, stop
			vec negpose({ 0, cos(deg2rad(-i)), sin(deg2rad(-i)) });
			negpose = R * negpose;
			if (tachikoma.get_arm_position_placement(poseplan, negpose, twist, grab, enc))
			{
				feasible_found = true;
				break;
			}
			// if a positive pose proves to find a solution, stop
			vec pospose({ 0, cos(deg2rad(i)), sin(deg2rad(i)) });
			pospose = R * pospose;
			if (tachikoma.get_arm_position_placement(poseplan, negpose, twist, grab, enc))
			{
				feasible_found = true;
				break;
			}
		}

		// send poses to the robot arm
		if (feasible_found)
		{
			tachikoma.set_arm(enc(1), enc(2), enc(3), enc(4), enc(5));
		}
		else
		{
			tachikoma.stop_arm();
		}*/
	}

	// clean the motion
	tachikoma.stop_arm();
	tachikoma.stop_legs();
	tachikoma.stop_wheels();
}

void motion_plan(void)
{
	vec goal = vec({ 48, 200 }); // this will have to change later somehow
	// grab the map
	mat localmap = globalmap.map;

	AStar astar(localmap, goal);
	while (!stopsig)
	{
		// try and see if we are allowed to go autonomous
		bool en = false;
		autonomous_lock.lock();
		en = auto_enable && manual_confirmed;
		auto_confirmed = true;
		autonomous_lock.unlock();

		// if we are indeed enabled, then we can compute the path to take
		if (!en)
		{
			continue;
		}

		// grab the current position
		pose_lock.lock();
		vec pose = robot_pose;
		pose_lock.unlock();

		// compute the new path
		vector<MotionAction> actionpath;
		vec curr = pose(span(0,1));
		astar.compute(curr, actionpath);
		if (astar.impossible())
		{
			// store an empty path
			path_lock.lock();
			pathplan = mat(2, 0);
			dopose = false; // shut off the pose just in case
			path_lock.unlock();
			continue;
		}

		// prune bad motion vectors
		vector<vec> prunedpath;
		vec origin;
		for (int i = 0; i < actionpath.size(); i++)
		{
			if (i == 0)
			{
				origin = vec({ actionpath[i].x, actionpath[i].y });
				prunedpath.push_back(origin);
			}
			else if (i == actionpath.size() - 1)
			{
				prunedpath.push_back(vec({ actionpath[i].x, actionpath[i].y }));
			}
			else
			{
				vec target({ actionpath[i].x, actionpath[i].y });
				if (eucdist(target - origin) >= 30)
				{
					prunedpath.push_back(target);
					origin = target;
				}
			}
		}

		// store the pruned path
		mat coalescedpath(2, prunedpath.size());
		for (int i = 0; i < prunedpath.size(); i++)
		{
			coalescedpath.col(i) = vec({ prunedpath[i](0), prunedpath[i](1) });
		}
		path_lock.lock();
		pathplan = coalescedpath;
		dopose = false;
		path_lock.unlock();
	}
}

void display_interface(void)
{
	cube frame(500, 500, 3, fill::zeros);

	while (!stopsig)
	{
		frame.zeros();

		// get the position of the robot
		pose_lock.lock();
		vec pose = robot_pose;
		pose_lock.unlock();

		// create a window around the pose
		int mux = (int)round(pose(0));
		int muy = (int)round(pose(1));
		double mut = pose(2);
		int sw2 = screen->w / 2;
		int sh2 = screen->h / 2;

		// draw the map
		globalmap.blit(frame, mux, muy);

		// draw the landmarks
		vec white({ 1, 1, 1 });
		for (int i = 0; i < landmarks.size(); i++)
		{
			sim_landmark &lm = landmarks[i];
			lm.blit(frame, mux, muy, chilitags.col(i));
		}

		// draw the particle filter
		pf.blit(frame, mux, muy);

		// draw the robot's position and pose
		int x, y;
		vec yellow({ 1, 1, 0 });
		draw_circle(frame, yellow, vec({ (double)sw2, (double)sh2 }), 20);
		for (int _i = -5; _i <= 5; _i++)
		{
			for (int _j = -5; _j <= 5; _j++)
			{
				x = sw2 + _j;
				y = sh2 + _i;
				if (x < 0 || x >= (int)frame.n_cols || y < 0 || y >= (int)frame.n_rows)
				{
					continue;
				}
				frame(y,x,0) = 1;
				frame(y,x,1) = 0;
				frame(y,x,2) = 0;
			}
		}
		x = (int)round(sw2 + (10 * cos(deg2rad(mut))));
		y = (int)round(sh2 + (10 * sin(deg2rad(mut))));
		vec color({ 0, 1, 1 });
		draw_line(frame, color, vec({(double)sw2,(double)sh2-1}), vec({(double)x,(double)y-1}));
		draw_line(frame, color, vec({(double)sw2,(double)sh2+1}), vec({(double)x,(double)y+1}));
		draw_line(frame, color, vec({(double)sw2-1,(double)sh2}), vec({(double)x-1,(double)y}));
		draw_line(frame, color, vec({(double)sw2+1,(double)sh2}), vec({(double)x+1,(double)y}));
		draw_line(frame, color, vec({(double)sw2,(double)sh2}), vec({(double)x,(double)y}));

		// draw A*
		path_lock.lock();
		mat path_plan = pathplan;
		path_lock.unlock();
		if (auto_enable)
		{
			vec purple({ 1, 0, 1 });
			for (int j = 0; j < (int)path_plan.n_cols; j++)
			{
				vec action = path_plan.col(j) + vec({ (double)(sw2 - mux), (double)(sh2 - muy) });
				draw_circle(frame, purple, vec({ action(0), action(1) }), 2.0);
			}
		}

		// push onto the screen
		screenblit(screen, frame);
		SDL_Flip(screen);
		SDL_Delay(25);
	}
}

static double secdiff(struct timeval &t1, struct timeval &t2)
{
	double usec = (double)(t2.tv_usec - t1.tv_usec) / 1000000.0;
	double sec = (double)(t2.tv_sec - t1.tv_sec);
	return sec + usec;
}


#pragma once

#include "Tableau.hpp"
#include "Solids.hpp"
#include "ReplacementDictionary.hpp"

#include <Eigen/Dense>

#include <utility>

namespace AQSystemSolver{
    struct SolidPresent{
        Eigen::Index solidIndex;
        double concentration;

        [[nodiscard]] bool friend operator<(const SolidPresent& solid1, const SolidPresent& solid2){
            return solid1.solidIndex<solid2.solidIndex;
        }
    };

    struct SolidNotPresent{
        Eigen::Index solidIndex;
        double solubilityProduct;

        [[nodiscard]] bool friend operator<(const SolidNotPresent& solid1, const SolidNotPresent& solid2){
            return solid1.solidIndex<solid2.solidIndex;
        }
    };

    class Equilibrium {
    private:
        TableauWithTotals<> tableau;
        Tableau<> solidsTableau;
        ReplacementDict finalReplacements;
        Eigen::RowVectorXd finalSolution;

        Eigen::VectorXd tableauConcentrations;

        SM_utils::flat_set<SolidPresent> solidsPresent;
        SM_utils::flat_set<SolidNotPresent> solidsNotPresent;
    public:

        [[nodiscard]] const auto& getTableauConcentrations() const {
            return tableauConcentrations;
        }
        [[nodiscard]] const auto& getSolidsPresent() const {
            return solidsPresent;
        }
        [[nodiscard]] const auto& getSolidsNotPresent() const {
            return solidsNotPresent;
        }
        [[nodiscard]] auto cols() const {
            return tableau.cols();
        }

        //this is useful a) to know error and b) to get the totals if you have a replacement
        [[nodiscard]] Eigen::RowVectorXd getTotalConcentrations() const {
            Eigen::RowVectorXd totalConcentrations=tableau.evalWithoutTotal(tableauConcentrations);
            Eigen::VectorXd solidConcentrations(solidsPresent.size());
            {
                auto concIt=solidConcentrations.begin();
                for(const auto& solidPresent: solidsPresent){
                    *concIt=solidPresent.concentration;
                    ++concIt;
                }
            }
            totalConcentrations+=solidsTableau.reducedCopy(SolidIndexIndexer{solidsPresent}, Eigen::all).eval(solidConcentrations);
            return totalConcentrations;
        }
        [[nodiscard]] Eigen::VectorXd getExtraSolubilityProducts(const Tableau<>& extraSolids) const {
            return finalReplacements.createReplacedTableau(extraSolids).evalTerms(finalSolution);
        }

        Equilibrium(
            TableauWithTotals<> tableau_,
            ReplacementDict replacementDict_, Eigen::RowVectorXd finalSolution_, Eigen::VectorXd speciesConcentrations_,
            SolidSystem solidSystem_, const Eigen::VectorXd& solidAmts_, const Eigen::VectorXd& solubilityProducts_
        ) :
        tableau{std::move(tableau_)},
        solidsTableau{std::move(solidSystem_).getTableau()},
        finalReplacements{std::move(replacementDict_)},
        finalSolution{std::move(finalSolution_)},
        tableauConcentrations{std::move(speciesConcentrations_)}
        {
            for(const Solid * solidPresent: solidSystem_.getSolidsPresent()){
                solidsPresent.insert(SolidPresent{solidPresent->solidIndex, solidAmts_.coeff(solidSystem_.getSolidsPresent().presenceIndexOf(solidPresent))});
            }
            for(const Solid * solidNotPresent: solidSystem_.getSolidsNotPresent()){
                solidsNotPresent.insert(SolidNotPresent{solidNotPresent->solidIndex, solubilityProducts_.coeff(solidSystem_.getSolidsNotPresent().presenceIndexOf(solidNotPresent))});
            }
        }
    };
} //namespace AQSystemSolver


#include <boost/python.hpp>
#include <boost/python/module.hpp>
#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>

#include "opengm/graphicalmodel/weights.hxx"
#include "opengm/functions/learnable/lpotts.hxx"
#include "opengm/functions/learnable/lunary.hxx"
#include "opengm/functions/learnable/lweightedsum_of_functions.hxx"

#include "../opengmcore/functionGenBase.hxx"

namespace bp = boost::python;
namespace op = opengm::python;
namespace ol = opengm::learning;
namespace ofl = opengm::functions::learnable;
namespace opengm{

    template<class GM_ADDER,class GM_MULT>
    class LPottsFunctionGen :
    public FunctionGeneratorBase<GM_ADDER,GM_MULT>
    {
    public:
        typedef typename GM_ADDER::ValueType ValueType;
        typedef typename GM_ADDER::IndexType IndexType;
        typedef typename GM_ADDER::LabelType LabelType;
        typedef ol::Weights<ValueType> WeightType;
        typedef  ofl::LPotts<ValueType, IndexType, LabelType> FType;

        LPottsFunctionGen(
            WeightType & weights,
            const size_t numFunctions,
            const size_t numLabels,
            op::NumpyView<ValueType, 2> features,
            op::NumpyView<IndexType, 1> weightIds,
            const bool addConstFeature
        ):
        FunctionGeneratorBase<GM_ADDER,GM_MULT>(),
        weights_(weights),
        numFunctions_(numFunctions),
        numLabels_(numLabels),
        features_(features.view()),
        weightIds_(weightIds.begin(), weightIds.end()),
        addConstFeature_(addConstFeature)
        {
            OPENGM_CHECK_OP(features.shape(0), == , numFunctions, "wrong shape");
            OPENGM_CHECK_OP(features.shape(1)+int(addConstFeature), == , weightIds.shape(0), "wrong shape");
        }

        template<class GM>
        std::vector< typename GM::FunctionIdentifier > * addFunctionsGeneric(GM & gm)const{

            typedef typename GM::FunctionIdentifier Fid;
            typedef std::vector<Fid> FidVector;
            FidVector * fidVector = new FidVector(numFunctions_);

            const size_t nFeat =features_.shape(1);
            std::vector<ValueType> fFeat(nFeat+int(addConstFeature_));
            for(size_t  i=0;i<numFunctions_;++i){
                for(size_t f=0; f<nFeat; ++f){
                    fFeat[f] = features_(i,f);
                }
                if(addConstFeature_){
                    fFeat[nFeat] = 1.0;
                }
                const FType f(weights_, numLabels_, weightIds_, fFeat);
                (*fidVector)[i] = gm.addFunction(f);
            }
            return fidVector;
        }

        virtual std::vector< typename GM_ADDER::FunctionIdentifier > * addFunctions(GM_ADDER & gm)const{
            return this-> template addFunctionsGeneric<GM_ADDER>(gm);
        }
        virtual std::vector< typename GM_MULT::FunctionIdentifier >  * addFunctions(GM_MULT & gm)const{
            throw RuntimeError("Wrong Operator for Learning");
            return NULL;
        }
    private:
        WeightType & weights_;
        size_t numFunctions_;
        size_t numLabels_;
        marray::Marray<ValueType>  features_;
        std::vector<size_t>  weightIds_;
        bool addConstFeature_;
    };

    template<class GM_ADDER,class GM_MULT>
    class LUnarySharedFeatFunctionGen :
    public FunctionGeneratorBase<GM_ADDER,GM_MULT>
    {
    public:
        typedef typename GM_ADDER::ValueType ValueType;
        typedef typename GM_ADDER::IndexType IndexType;
        typedef typename GM_ADDER::LabelType LabelType;
        typedef ol::Weights<ValueType> WeightType;
        typedef  ofl::LUnary<ValueType, IndexType, LabelType> FType;

        LUnarySharedFeatFunctionGen(
            WeightType & weights,
            const size_t numFunctions,
            const size_t numLabels,
            op::NumpyView<ValueType, 2> & features,
            op::NumpyView<IndexType, 2> & weightIds,
            const bool makeFirstEntryConst,
            const bool addConstFeature
        ):
        FunctionGeneratorBase<GM_ADDER,GM_MULT>(),
        weights_(weights),
        numFunctions_(numFunctions),
        numLabels_(numLabels),
        features_(features.view()),
        //weightIds_(weightIds),
        makeFirstEntryConst_(makeFirstEntryConst),
        addConstFeature_(addConstFeature)
        {
            //std::cout<<"constructor\n";

            //std::cout<<"    features (1000,1)"<<features(1000,1)<<"\n";
            //std::cout<<"    features_(1000,1)"<<features_(1000,1)<<"\n";
            OPENGM_CHECK_OP(features.shape(0), == , numFunctions, "wrong shape");
            OPENGM_CHECK_OP(weightIds.shape(1), == , features.shape(1) + int(addConstFeature), "wrong shape");
            OPENGM_CHECK_OP(weightIds.shape(0)+int(makeFirstEntryConst), == ,numLabels, "wrong shape");

            const size_t nFeat =features_.shape(1);
            const size_t nWPerL = nFeat+int(addConstFeature_);
            const size_t wShape[2] = {numLabels_- int(makeFirstEntryConst_) ,nWPerL};

            wIds_ = marray::Marray<size_t>(wShape, wShape+2);

            //std::cout<<"assignment\n";
            //std::cout<<"passed wi shape "<<weightIds.shape(0)<<" "<<weightIds.shape(1)<<" given "<<wShape[0]<<" "<<wShape[1]<<"\n";
            //std::cout<<"wIds_  shape "<<wIds_.shape(0)<<" "<<wIds_.shape(1)<<"\n";

            for(size_t ll=0; ll<wShape[0]; ++ll){
                for(size_t wi=0; wi<wShape[1]; ++wi){
                    //std::cout<<"ll "<<ll<<" wi "<<wi<<"\n";
                    size_t passed =  weightIds(ll,wi);
                    //std::cout<<"passed "<<passed<<"\n";
                    wIds_(ll,wi) = passed;
                }
            }
            //std::cout<<"constructor done\n";
        }

        template<class GM>
        std::vector< typename GM::FunctionIdentifier > * addFunctionsGeneric(GM & gm)const{
            //std::cout<<"&** features_(1000,1)"<<features_(1000,1)<<"\n";

            typedef typename GM::FunctionIdentifier Fid;
            typedef std::vector<Fid> FidVector;
            FidVector * fidVector = new FidVector(numFunctions_);

            const size_t nFeat =features_.shape(1);
            const size_t nWPerL = nFeat+int(addConstFeature_);
            marray::Marray<ValueType> fFeat(&nWPerL,&nWPerL+1);

            // copy the weights once!
            const size_t wShape[2] = {numLabels_- int(makeFirstEntryConst_) ,nWPerL};
            marray::Marray<size_t> _weightIds(wShape, wShape+2);

            //for(size_t ll=0; ll<wShape[0]; ++ll)
            //for(size_t wi=0; wi<wShape[1]; ++wi){
            //    _weightIds(ll,wi) = weightIds_(ll,wi);
            //}

            for(size_t  i=0;i<numFunctions_;++i){
                // copy the features for that instance
                for(size_t f=0; f<nFeat; ++f){
                    //std::cout<<"added feat:"<<features_(i,f)<<"\n";
                    fFeat(f) = features_(i,f);
                }
                if(addConstFeature_){
                    fFeat(nFeat) = 1.0;
                }
                FType f(weights_, numLabels_, wIds_, fFeat, makeFirstEntryConst_);

                //std::cout<<"INTERNAL TEST\n";
                //for(size_t l=0;l<numLabels_; ++l){
                //    std::cout<<"l "<<l<<" f(l) = "<<f(&l)<<"\n";
                //}

                (*fidVector)[i] = gm.addFunction(f);
            }
            return fidVector;
        }

        virtual std::vector< typename GM_ADDER::FunctionIdentifier > * addFunctions(GM_ADDER & gm)const{
            return this-> template addFunctionsGeneric<GM_ADDER>(gm);
        }
        virtual std::vector< typename GM_MULT::FunctionIdentifier >  * addFunctions(GM_MULT & gm)const{
            throw RuntimeError("Wrong Operator for Learning");
            return NULL;
        }
    private:
        WeightType & weights_;
        size_t numFunctions_;
        size_t numLabels_;

        marray::Marray<ValueType> features_;
        //op::NumpyView<ValueType, 2>  features_;
        op::NumpyView<IndexType, 2>  weightIds_;
        bool makeFirstEntryConst_;
        bool addConstFeature_;
        marray::Marray<size_t> wIds_;
    };

    template<class GM_ADDER,class GM_MULT>
    FunctionGeneratorBase<GM_ADDER,GM_MULT> * lunarySharedFeatFunctionGen(
        ol::Weights<typename GM_ADDER::ValueType> & weights,
        const size_t numFunctions,
        const size_t numLabels,
        opengm::python::NumpyView<typename GM_ADDER::ValueType,2> features,
        opengm::python::NumpyView<typename GM_ADDER::IndexType,2> weightIds,
        const bool makeFirstEntryConst,
        const bool addConstFeature
    ){
        FunctionGeneratorBase<GM_ADDER,GM_MULT> * ptr =
            new LUnarySharedFeatFunctionGen<GM_ADDER,GM_MULT>(weights,numFunctions,numLabels,
                                                              features,weightIds,makeFirstEntryConst,
                                                              addConstFeature);
        return ptr;
    }

    template<class GM_ADDER,class GM_MULT>
    FunctionGeneratorBase<GM_ADDER,GM_MULT> * lpottsFunctionGen(
        ol::Weights<typename GM_ADDER::ValueType> & weights,
        const size_t numFunctions,
        const size_t numLabels,
        opengm::python::NumpyView<typename GM_ADDER::ValueType,2> features,
        opengm::python::NumpyView<typename GM_ADDER::IndexType,1> weightIds,
        const bool addConstFeature
    ){
        FunctionGeneratorBase<GM_ADDER,GM_MULT> * ptr =
            new LPottsFunctionGen<GM_ADDER,GM_MULT>(weights,numFunctions,numLabels,features,weightIds, addConstFeature);
        return ptr;
    }

    template<class GM_ADDER,class GM_MULT>
    void export_lfunction_generator(){
        typedef LPottsFunctionGen<GM_ADDER, GM_MULT> FGen;

         bp::def("_lpottsFunctionsGen",&lpottsFunctionGen<GM_ADDER,GM_MULT>,
                bp::return_value_policy<bp::manage_new_object>(),
            (
                bp::arg("weights"),
                bp::arg("numFunctions"),
                bp::arg("numLabels"),
                bp::arg("features"),
                bp::arg("weightIds"),
                bp::arg("addConstFeature")
            )
        );

         bp::def("_lunarySharedFeatFunctionsGen",&lunarySharedFeatFunctionGen<GM_ADDER,GM_MULT>,
                bp::with_custodian_and_ward_postcall<0, 4, bp::return_value_policy<bp::manage_new_object> >(),
            (
                bp::arg("weights"),
                bp::arg("numFunctions"),
                bp::arg("numLabels"),
                bp::arg("features"),
                bp::arg("weightIds"),
                bp::arg("makeFirstEntryConst"),
                bp::arg("addConstFeature")
            )
        );

    }

}

template void opengm::export_lfunction_generator<op::GmAdder,op::GmMultiplier>();


//          Copyright Carl Philipp Reh 2006 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef SGE_BVH_CONSTRUCT_MEDIAN_CUT_HPP_INCLUDED
#define SGE_BVH_CONSTRUCT_MEDIAN_CUT_HPP_INCLUDED

#include <sge/bvh/bounding_box.hpp>
#include <sge/bvh/empty_node.hpp>
#include <sge/bvh/traits/box.hpp>
#include <sge/core/exception.hpp>
#include <fcppt/text.hpp>
#include <fcppt/math/size_type.hpp>
#include <fcppt/math/to_array.hpp>
#include <fcppt/math/box/center.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <algorithm>
#include <iterator>
#include <fcppt/config/external_end.hpp>

namespace sge::bvh
{
namespace detail
{

// TODO(philipp): This is a mess
class construct_median_cut_comparator
{
public:
  explicit construct_median_cut_comparator(fcppt::math::size_type const _axis) : axis_(_axis) {}

  template <typename LeafWrapper>
  bool operator()(LeafWrapper const &_left, LeafWrapper const &_right) const
  {
    using leaf = typename LeafWrapper::leaf;

    return fcppt::math::box::center(sge::bvh::traits::box<leaf>::extract_box(_left.value()))
               .get_unsafe(axis_) <
           fcppt::math::box::center(sge::bvh::traits::box<leaf>::extract_box(_right.value()))
               .get_unsafe(axis_);
  }

  [[nodiscard]] fcppt::math::size_type axis() const { return axis_; }

private:
  fcppt::math::size_type axis_;
};

}

template <typename Traits>
void construct_median_cut(
    typename Traits::leaf_wrapper_sequence _leaves, typename Traits::tree_representation &_tree)
{
  if (!_tree.empty())
  {
    throw sge::core::exception{FCPPT_TEXT("construct_median_cut: Tree not empty!")};
  }

  if (_leaves.size() <= 1U)
  {
    _tree.value(typename Traits::node_or_leaf_variant(_leaves));
    return;
  }

  typename Traits::box const bounding_box = sge::bvh::bounding_box<Traits>(_leaves);

  typename Traits::box::dim const bounding_box_size = bounding_box.size();

  _tree.value(typename Traits::node_or_leaf_variant(
      typename Traits::node_wrapper(typename Traits::node(bounding_box), bounding_box)));

  sge::bvh::detail::construct_median_cut_comparator comparator{
      [bounding_box_size]
      {
        auto const array(fcppt::math::to_array(bounding_box_size));

        return static_cast<fcppt::math::size_type>(
            std::distance(array.begin(), boost::range::max_element(array)));
      }()};

  std::nth_element(
      _leaves.begin(),
      std::next(
          _leaves.begin(),
          static_cast<typename Traits::leaf_wrapper_sequence::difference_type>(
              _leaves.size() / 2U)),
      _leaves.end(),
      comparator);

  typename Traits::leaf_wrapper const &median(*std::next(
      _leaves.begin(),
      static_cast<typename Traits::leaf_wrapper_sequence::difference_type>(_leaves.size() / 2U)));

  std::partition(
      _leaves.begin(),
      _leaves.end(),
      [median, &comparator](typename Traits::leaf_wrapper const &_value)
      { return comparator(_value, median); });

  /*
	typename Traits::leaf_wrapper_sequence left_side,right_side;
	left_side.reserve(
		_leaves.size()/2U);
	right_side.reserve(
		_leaves.size()/2U);
	for(
		typename Traits::leaf_wrapper_sequence::iterator it =
			_leaves.begin();
		it != _leaves.end();
		++it)
		if(comparator((*it),median))
			left_side.push_back(
				*it);
		else
			right_side.push_back(
				*it);
				*/
  /*
	boost::range::sort(
		_leaves,
		sge::bvh::detail::construct_median_cut_comparator(
			static_cast<fcppt::math::size_type>(
				std::distance(
					bounding_box_size.begin(),
					boost::range::max_element(
						bounding_box_size)))));
	*/

  /*
	if(_leaves.size() == 2)
	{
		std::cout
			<<
				fcppt::math::box::center(
					sge::bvh::traits::box<typename Traits::leaf_wrapper::leaf>::extract_box(
						_leaves[0].value()))[comparator.axis()]
			<<
			", "
			<<
				fcppt::math::box::center(
					sge::bvh::traits::box<typename Traits::leaf_wrapper::leaf>::extract_box(
						_leaves[1].value()))[comparator.axis()]
			<<
			", median: "
			<<
				fcppt::math::box::center(
					sge::bvh::traits::box<typename Traits::leaf_wrapper::leaf>::extract_box(
						median.value()))[comparator.axis()]
			<<
			"\n";
	}
	*/
  /*
	std::cout << "input length: " << _leaves.size() << "\n";
	std::cout << "left side: " << left_side.size() << "\n";
	std::cout << "right side: " << right_side.size() << "\n";
	*/

  _tree.push_back(typename Traits::node_or_leaf_variant(sge::bvh::empty_node()));

  _tree.push_back(typename Traits::node_or_leaf_variant(sge::bvh::empty_node()));

  sge::bvh::construct_median_cut<Traits>(
      typename Traits::leaf_wrapper_sequence(
          _leaves.begin(),
          std::next(
              _leaves.begin(),
              static_cast<typename Traits::leaf_wrapper_sequence::difference_type>(
                  _leaves.size() / 2U))),
      _tree.front().get_unsafe().get());

  sge::bvh::construct_median_cut<Traits>(
      typename Traits::leaf_wrapper_sequence(
          std::next(
              _leaves.begin(),
              static_cast<typename Traits::leaf_wrapper_sequence::difference_type>(
                  _leaves.size() / 2U)),
          _leaves.end()),
      *std::next(_tree.begin()));
}
}

#endif


#include "teca_vertical_coordinate_transform.h"

#include "teca_arakawa_c_grid.h"
#include "teca_curvilinear_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::cerr;
using std::endl;
using std::cos;

//#define TECA_DEBUG

namespace {

/*  unsigned long nx = ext[1] - ext[0] + 1;
    unsigned long ny = ext[3] - ext[2] + 1;
    unsigned long nz = ext[5] - ext[4] + 1;
    unsigned long nxy = nx*ny;*/

// the inputs xi, yi are 2D fields, eta is a 1D field
// the outputs xo,yo, and phd are 3D fields
template <typename num_t>
void transform_wrf_v3(unsigned long nx, unsigned long ny,
    unsigned long nz, unsigned long nxy, const num_t *xi,
    const num_t *yi, const num_t *eta, const num_t *ps, num_t pt,
    num_t* xo, num_t *yo, num_t *ph)
{
    unsigned long nxy_bytes = nxy*sizeof(num_t);
    for (unsigned long k = 0; k < nz; ++k)
    {
        unsigned long knxy = k*nxy;

        // copy x
        memcpy(xo + knxy, xi, nxy_bytes);

        // copy y
        memcpy(yo + knxy, yi, nxy_bytes);

        // transform z
        num_t eta_k = eta[k];
        for (unsigned long j = 0; j < ny; ++j)
        {
            unsigned long jnx = j*nx;
            unsigned long knxy_jnx = knxy + jnx;

            // transform from mass vertical coordinate into hydrostatic dry pressure
            // see "A descritpion  of Advanced Research WRF Model Version 4" page 8
            // note: this is the WRF ARW 3 coordinate system
            const num_t *ps_jnx = ps + jnx;
            num_t *ph_knxy_jnx = ph + knxy_jnx;
            for (unsigned long i = 0; i < nx; ++i)
                ph_knxy_jnx[i] = eta_k*(ps_jnx[i] - pt) + pt;
        }
    }
}

};

// --------------------------------------------------------------------------
teca_vertical_coordinate_transform::teca_vertical_coordinate_transform() :
    mode(mode_wrf_v3)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_vertical_coordinate_transform::~teca_vertical_coordinate_transform()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_vertical_coordinate_transform::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_vertical_coordinate_transform":prefix));

    opts.add_options()
        TECA_POPTS_GET(int, prefix, mode,
            "Sets the coordinate transform mode. The modes are: mode_wrf_v3")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_vertical_coordinate_transform::set_properties(
    const string &prefix, variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, int, prefix, mode)
}
#endif

// --------------------------------------------------------------------------
teca_metadata teca_vertical_coordinate_transform::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_vertical_coordinate_transform::get_output_metadata" << endl;
#endif
    (void)port;

    teca_metadata out_md(input_md[0]);

    // get coordinate metadata
    teca_metadata coords;
    if (out_md.get("coordinates", coords))
    {
        TECA_FATAL_ERROR("metadata issue, missing coordinate metadata")
        return teca_metadata();
    }

    // get array metadata
    teca_metadata atrs;
    if (out_md.get("attributes", atrs))
    {
        TECA_FATAL_ERROR("failed to get array attributes")
        return teca_metadata();
    }

    switch (this->mode)
    {
        case mode_wrf_v3:
        {
            // update the z coordinate axes variable names so that down
            // stream algorithms correctly identify them
            coords.set("m_z_variable", "ZPDM");
            coords.set("w_z_varibale", "ZPDW");

            // pass metadata for the atrrays we generate
            teca_metadata ps_atts;
            if (atrs.get("PSFC", ps_atts))
            {
                TECA_FATAL_ERROR("failed to get PSFC attributes")
                return teca_metadata();
            }
            atrs.set("ZPDM", ps_atts);
            atrs.set("ZPDW", ps_atts);
            break;
        }
        default:
        {
            TECA_FATAL_ERROR("Invlaid mode " << this->mode)
            return teca_metadata();
        }
    }

    out_md.set("coordinates", coords);
    out_md.set("attributes", atrs);

    return out_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata>
teca_vertical_coordinate_transform::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    (void)port;
    (void)input_md;

    vector<teca_metadata> up_reqs;

    // copy the incoming request to preserve the downstream
    // requirements and add the arrays we need
    teca_metadata req(request);

    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);

    switch (this->mode)
    {
        case mode_wrf_v3:
        {
            arrays.insert("PSFC");
            arrays.insert("P_TOP");
            break;
        }
        default:
            TECA_FATAL_ERROR("Invlaid mode " << this->mode)
            return up_reqs;
    }

    // update the request
    req.set("arrays", arrays);

    // if/when bounds based requests are
    // implemented the transform from pressure coordinates to
    // sigma coordinates would be handled here.

    // send it up
    up_reqs.push_back(req);

    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_vertical_coordinate_transform::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_vertical_coordinate_transform::execute" << endl;
#endif
    (void)port;
    (void)request;

    // get the input mesh
    const_p_teca_arakawa_c_grid in_mesh
        = std::dynamic_pointer_cast<const teca_arakawa_c_grid>(input_data[0]);

    if (!in_mesh)
    {
        TECA_FATAL_ERROR("teca_arakawa_c_grid is required")
        return nullptr;
    }

    // create the output mesh. it is a curvilinear mesh because
    // of the fully 3D realization of the coordinate arrays.
    // also cell centered values are moved to node centering, i.e.
    // this is the dual mesh.
    p_teca_curvilinear_mesh out_mesh = teca_curvilinear_mesh::New();
    out_mesh->copy_metadata(in_mesh);

    switch (this->mode)
    {
        case mode_wrf_v3:
        {
            // get the input coordinate system
            std::string x_coord_name;
            std::string y_coord_name;

            in_mesh->get_m_x_coordinate_variable(x_coord_name);
            in_mesh->get_m_y_coordinate_variable(y_coord_name);

            const_p_teca_variant_array xi = in_mesh->get_m_x_coordinates();
            const_p_teca_variant_array yi = in_mesh->get_m_y_coordinates();
            const_p_teca_variant_array eta = in_mesh->get_m_z_coordinates();

            const_p_teca_variant_array pt = in_mesh->get_information_arrays()->get("P_TOP");
            if (!pt)
            {
                TECA_FATAL_ERROR("Failed to get P_TOP")
                return nullptr;
            }

            const_p_teca_variant_array ps = in_mesh->get_cell_arrays()->get("PSFC");
            if (!ps)
            {
                TECA_FATAL_ERROR("Failed to get PSFC")
                return nullptr;
            }

            // get the mesh dimensions
            unsigned long extent[6] = {0};
            in_mesh->get_extent(extent);

            unsigned long nx = extent[1] - extent[0] + 1;
            unsigned long ny = extent[3] - extent[2] + 1;
            unsigned long nz = extent[5] - extent[4] + 1;
            unsigned long nxy = nx*ny;
            unsigned long nxyz = nxy*nz;

            // allocate the output coordinates
            p_teca_variant_array xo = xi->new_instance(nxyz);
            p_teca_variant_array yo = xi->new_instance(nxyz);
            p_teca_variant_array ph = xi->new_instance(nxyz);

            TEMPLATE_DISPATCH(teca_variant_array_impl,
                xo.get(),

                const NT *ppt = dynamic_cast<const TT*>(pt.get())->get();
                const NT *pps = dynamic_cast<const TT*>(ps.get())->get();
                const NT *pxi = dynamic_cast<const TT*>(xi.get())->get();
                const NT *pyi = dynamic_cast<const TT*>(yi.get())->get();
                const NT *peta = dynamic_cast<const TT*>(eta.get())->get();

                NT *pxo = dynamic_cast<TT*>(xo.get())->get();
                NT *pyo = dynamic_cast<TT*>(yo.get())->get();
                NT *pph = dynamic_cast<TT*>(ph.get())->get();

                ::transform_wrf_v3(nx, ny, nz, nxy, pxi, pyi,
                    peta, pps, ppt[0], pxo, pyo, pph);
                )

            // pass coordinates to output
            out_mesh->set_x_coordinates(x_coord_name, xo);
            out_mesh->set_y_coordinates(y_coord_name, yo);
            out_mesh->set_z_coordinates("ZPHM", ph);

            // pass arrays to the output
            out_mesh->get_point_arrays() =
                std::const_pointer_cast<teca_array_collection>
                    (in_mesh->get_cell_arrays());

            break;
        }
        default:
        {
            TECA_FATAL_ERROR("Invlaid mode " << this->mode)
            return nullptr;
        }
    }

    return out_mesh;
}


#ifndef _POSE_ESTIMATION_MEASUREMENT_HPP
#define _POSE_ESTIMATION_MEASUREMENT_HPP

#include <Eigen/Core>

#define MEASUREMENT(NAME, DIM) \
struct NAME \
{ \
typedef Eigen::Matrix<double, DIM, 1> Mu; \
typedef Eigen::Matrix<double, DIM, DIM> Cov; \
 \
NAME() : mu(Mu::Zero()), cov(Cov::Identity()) {} \
 \
Mu mu; \
Cov cov; \
};

#endif


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013 MetaScale SAS
//
//          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_BENCH_STATS_MEDIAN_HPP_INCLUDED
#define NT2_SDK_BENCH_STATS_MEDIAN_HPP_INCLUDED

#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <string>

namespace nt2 { namespace bench { namespace stats
{
  /*!
    @brief Median statistic

    Used as a Metric parameter, median_ make the metric computes the
    median value of its samples.

    @note The median algorithm requires a minimal amount of samples to be
    correct. Currently, this minimum is set to 3 and any experiment providing
    less than this amount of samples will make use the average value of samples
    instead.
  **/
  struct median_
  {
    /// @brief Samples processing function
    template<typename Samples> static inline double evaluate(Samples const& d)
    {
      if( boost::accumulators::count(d) < 3 )
        return boost::accumulators::mean(d);
      else
        return boost::accumulators::median(d);
    }

    /// @brief Statistics display
    static inline std::string unit() { return " (med)"; }
  };
} } }

#endif


//
// Created by martin on 21.05.15.
//

#include "test_data_generator.h"
#include <iostream>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

void test_data_generator::init() {
    parameter_names.reserve(TREE_SIZE);
    boost::random::uniform_int_distribution<> dist_tree_depth(0, TREE_DEPTH - 1);
    std::string name;
    while (parameter_names.size() < TREE_SIZE) {
        name.clear();
        int node_depth = dist_tree_depth(rng);
        add_parameter("", 0, node_depth);
    }
    generate_access_order();
}

std::string test_data_generator::generate_parameter_name() {
    const std::string chars("abcdefghijklmnopqrstuvwxyz");
    boost::random::uniform_int_distribution<> dist_char_index(0, static_cast<int>(chars.size() - 1));
    boost::random::normal_distribution<> dist_parameter_name_length(PARAMETER_LENGTH_AVG, 3);
    std::string name;
    int parameter_name_size = static_cast<int>(dist_parameter_name_length(rng));
    for(int name_index = 0; name_index < parameter_name_size; ++name_index) {
        name.push_back(chars[dist_char_index(rng)]);
    }
    return name;
}

void test_data_generator::add_parameter(std::string prefix, int layer, int node_depth) {
    std::string parameter_name;
    if(!prefix.empty()) {
        parameter_name.append(prefix);
        parameter_name.push_back('.');
    }
    parameter_name.append(generate_parameter_name());
    if(layer == node_depth) {
        parameter_names.push_back(parameter_name);
        return;
    }
    boost::random::normal_distribution<> dist_node_leafes(NODE_LEAFES_AVG, 1.5);
    int node_leafes = static_cast<int>(dist_node_leafes(rng));
    for(int node_leaf = 0; node_leaf < node_leafes; node_leaf++) {
        add_parameter(parameter_name, layer + 1, node_depth);
    }
}

const std::vector<std::string> &test_data_generator::get_parameter_names() const {
    return parameter_names;
}

const std::vector<std::size_t> &test_data_generator::get_access_order() const {
    return access_order;
}

void test_data_generator::generate_access_order() {
    boost::random::uniform_int_distribution<> dist_parameter_access(0, static_cast<int>(TREE_SIZE) - 1);
    std::size_t access_list_length = 1000000;
    access_order.clear();
    access_order.reserve(access_list_length);
    for(std::size_t i = 0; i < access_list_length; i++) {
        access_order.push_back(dist_parameter_access(rng));
    }
}


#include "ScanMatch.h"
#include "common/feature_utils.h"
#include "common/math_utils.h"
#include "common/nanoflann_pcl.h"
#include "common/ros_utils.h"
#include "common/transform_utils.h"

#include <pcl/kdtree/kdtree_flann.h>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace lidar_slam {

using std::sqrt;
using std::fabs;
using std::asin;
using std::atan2;
using std::pow;

ScanMatch::ScanMatch(const size_t maxIterations)
    : _maxIterations(maxIterations), _deltaTAbort(0.05), _deltaRAbort(0.05),
      _useScore(true), _match_count(0), _fail_match_count(0), _total_score(0),
      _score_threshold(800), _match_percentage_threshold(0.4),
      _referenceCornerCloudDS(new CloudI()),
      _referenceSurfCloudDS(new CloudI()), _CornerCloudDS(new CloudI()),
      _SurfCloudDS(new CloudI()) {

  _downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
  _downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);

  _fineScore = false;
}

ScanMatch::~ScanMatch() {
  std::cout << "[ScanMatch]\n"
            << " ,match_count:" << _match_count
            << " ,fail_match_count:" << _fail_match_count
            << " ,averageScore:" << getAverageScore() << std::endl;
}

double ScanMatch::getScore(const CloudI &coeffCloud) {
  double score = 0;
  for (int i = 0; i < coeffCloud.points.size(); ++i) {
    const PointI &p = coeffCloud.points[i];
    score += std::exp(-fabs(p.intensity));
  }
  return score;
}

bool ScanMatch::scanMatchScan(const CloudIConPtr &referenceCornerCloud,
                              const CloudIConPtr &referenceSurfCloud,
                              const CloudIConPtr &CornerCloud,
                              const CloudIConPtr &SurfCloud,
                              Twist &transformf) {

  if (referenceCornerCloud->points.size() < 50 ||
      referenceSurfCloud->points.size() < 100) {
    std::cout << "reference cloud points too few." << std::endl;
    return false;
  }
  Twist transform = transformf;

  PointI pointSel, pointOri, pointProj, coeff;
  std::vector<int> pointSearchInd(5, 0);
  std::vector<float> pointSearchSqDis(5, 0);

    nanoflann::KdTreeFLANN<PointI> kdtreeCorner;
    nanoflann::KdTreeFLANN<PointI> kdtreeSurf;
      /*
  pcl::KdTreeFLANN<PointI> kdtreeCorner;
  pcl::KdTreeFLANN<PointI> kdtreeSurf;
  */

  kdtreeCorner.setInputCloud(referenceCornerCloud);
  kdtreeSurf.setInputCloud(referenceSurfCloud);

  bool converge = false;
  bool isDegenerate = false;
  Eigen::Matrix<float, 6, 6> matP;

  size_t CornerNum = CornerCloud->points.size();
  size_t SurfNum = SurfCloud->points.size();

  pcl::PointCloud<PointI> laserCloudOri;
  pcl::PointCloud<PointI> coeffSel;

  int line_match_count = 0;
  int plane_match_count = 0;
  size_t iterCount;
  for (iterCount = 0; iterCount < _maxIterations; iterCount++) {
    laserCloudOri.clear();
    coeffSel.clear();
    line_match_count = 0;
    plane_match_count = 0;

    for (int i = 0; i < CornerNum; i++) {
      pointOri = CornerCloud->points[i];
      pointAssociateToMap(transform, pointOri, pointSel);
      kdtreeCorner.nearestKSearch(pointSel, 5, pointSearchInd,
                                  pointSearchSqDis);
      if (pointSearchSqDis[4] < 5.0) {
        const Eigen::Vector3f &point = pointSel.getVector3fMap();
        Eigen::Vector3f lineA, lineB;
        if (findLine(*referenceCornerCloud, pointSearchInd, lineA, lineB)) {
          PointI coefficients;
          if (getCornerFeatureCoefficients(lineA, lineB, point, coefficients)) {
            laserCloudOri.push_back(pointOri);
            coeffSel.push_back(coefficients);
          }
          line_match_count++;
        }
      }
    }

    for (int i = 0; i < SurfNum; i++) {
      pointOri = SurfCloud->points[i];
      pointAssociateToMap(transform, pointOri, pointSel);
      kdtreeSurf.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
      if (pointSearchSqDis[4] < 5.0) {
        Eigen::Vector4f planeCoef;
        if (findPlane(*referenceSurfCloud, pointSearchInd, 0.2, planeCoef)) {
          PointI coefficients;
          if (getSurfaceFeatureCoefficients(planeCoef, pointSel,
                                            coefficients)) {
            laserCloudOri.push_back(pointOri);
            coeffSel.push_back(coefficients);
          }
          plane_match_count++;
        }
      }
    }

    float srx = transform.rot_x.sin();
    float crx = transform.rot_x.cos();
    float sry = transform.rot_y.sin();
    float cry = transform.rot_y.cos();
    float srz = transform.rot_z.sin();
    float crz = transform.rot_z.cos();

    size_t laserCloudSelNum = laserCloudOri.points.size();
    if (laserCloudSelNum < 50) {
      ROS_WARN("matched cloud points too few. Matched/Input:  %d / %d", laserCloudSelNum, CornerNum+SurfNum);
      break;
    }

    Eigen::Matrix<float, Eigen::Dynamic, 6> matA(laserCloudSelNum, 6);
    Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, laserCloudSelNum);
    Eigen::Matrix<float, 6, 6> matAtA;
    Eigen::VectorXf matB(laserCloudSelNum);
    Eigen::VectorXf matAtB;
    Eigen::VectorXf matX;

    for (int i = 0; i < laserCloudSelNum; i++) {
      pointOri = laserCloudOri.points[i];
      coeff = coeffSel.points[i];
      /*
      float arx = (crx * sry * srz * pointOri.x + crx * crz * sry * pointOri.y -
                   srx * sry * pointOri.z) *
                      coeff.x +
                  (-srx * srz * pointOri.x - crz * srx * pointOri.y -
                   crx * pointOri.z) *
                      coeff.y +
                  (crx * cry * srz * pointOri.x + crx * cry * crz * pointOri.y -
                   cry * srx * pointOri.z) *
                      coeff.z;

      float ary = ((cry * srx * srz - crz * sry) * pointOri.x +
                   (sry * srz + cry * crz * srx) * pointOri.y +
                   crx * cry * pointOri.z) *
                      coeff.x +
                  ((-cry * crz - srx * sry * srz) * pointOri.x +
                   (cry * srz - crz * srx * sry) * pointOri.y -
                   crx * sry * pointOri.z) *
                      coeff.z;

      float arz = ((crz * srx * sry - cry * srz) * pointOri.x +
                   (-cry * crz - srx * sry * srz) * pointOri.y) *
                      coeff.x +
                  (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y +
                  ((sry * srz + cry * crz * srx) * pointOri.x +
                   (crz * sry - cry * srx * srz) * pointOri.y) *
                      coeff.z;
        */
        float arx = ((crz*sry*crx + srz*srx)* pointOri.y +(srz*crx-crz*sry*srx)* pointOri.z)*coeff.x +
        ((srz*sry*crx-crz*srx)*pointOri.y -(srz*sry*srx+crz*crx)*pointOri.z)*coeff.y +
        (cry*crx*pointOri.y-cry*srx*pointOri.z)*coeff.z;

        float ary = (-crz*sry*pointOri.x+crz*cry*srx*pointOri.y+crz*cry*crx*pointOri.z)*coeff.x +
        (-srz*sry*pointOri.x+srz*cry*srx*pointOri.y +srz*cry*crx*pointOri.z)*coeff.y +
        (-cry*pointOri.x-sry*srx*pointOri.y-sry*crx*pointOri.z)*coeff.z;

        float arz = (-srz*cry*pointOri.x -(srz*sry*srx+crz*crx)*pointOri.y+(crz*srx-srz*sry*crx)*pointOri.z)*coeff.x+
        (crz*cry*pointOri.x+ (crz*sry*srx-srz*crx)*pointOri.y+crz*sry*crx+srz*srx*pointOri.z)*coeff.y+
        0*coeff.z;

      matA(i, 0) = arx;
      matA(i, 1) = ary;
      matA(i, 2) = arz;
      matA(i, 3) = coeff.x;
      matA(i, 4) = coeff.y;
      matA(i, 5) = coeff.z;
      matB(i, 0) = -coeff.intensity;
    }

    matAt = matA.transpose();
    matAtA = matAt * matA;
    matAtB = matAt * matB;
    matX = matAtA.colPivHouseholderQr().solve(matAtB);

    if (iterCount == 0) {
      Eigen::Matrix<float, 1, 6> matE;
      Eigen::Matrix<float, 6, 6> matV;
      Eigen::Matrix<float, 6, 6> matV2;

      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 6, 6>> esolver(matAtA);
      matE = esolver.eigenvalues().real();
      matV = esolver.eigenvectors().real();

      matV2 = matV;

      isDegenerate = false;
      float eignThre[6] = {100, 100, 100, 100, 100, 100};
      for (int i = 0; i < 6; i++) {
        if (matE(0, i) < eignThre[i]) {
          for (int j = 0; j < 6; j++) {
            matV2(i, j) = 0;
          }
          isDegenerate = true;
        } else {
          break;
        }
      }
      matP = matV.inverse() * matV2;
    }

    if (isDegenerate) {
      Eigen::Matrix<float, 6, 1> matX2(matX);
      matX = matP * matX2;
    }

    transform.rot_x += matX(0, 0);
    transform.rot_y += matX(1, 0);
    transform.rot_z += matX(2, 0);
    transform.pos.x() += matX(3, 0);
    transform.pos.y() += matX(4, 0);
    transform.pos.z() += matX(5, 0);

    float deltaR =
        sqrt(pow(rad2deg(matX(0, 0)), 2) + pow(rad2deg(matX(1, 0)), 2) +
             pow(rad2deg(matX(2, 0)), 2));
    float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + pow(matX(4, 0) * 100, 2) +
                        pow(matX(5, 0) * 100, 2));

    // std::cout << "iterator:" << iterCount << std::endl;
    // transform.print();
    if (deltaR < _deltaRAbort && deltaT < _deltaTAbort) {
      converge = true;
      break;
    }
  }

  if (converge && _useScore) {

    double score = getScore(coeffSel);

    double match_count = line_match_count + plane_match_count;
    float percent = match_count / (CornerNum + SurfNum);
    std::cout << "scan match score:" << score << ",per:" << percent
              << std::endl;

    if (_fineScore) {
      laserCloudOri.clear();
      coeffSel.clear();
      line_match_count = 0;
      plane_match_count = 0;
      for (int i = 0; i < CornerNum; i++) {
        pointOri = CornerCloud->points[i];
        pointAssociateToMap(transform, pointOri, pointSel);
        kdtreeCorner.nearestKSearch(pointSel, 5, pointSearchInd,
                                    pointSearchSqDis);
        if (pointSearchSqDis[0] < 0.02) {
          const Eigen::Vector3f &point = pointSel.getVector3fMap();
          Eigen::Vector3f lineA, lineB;
          if (findLine(*referenceCornerCloud, pointSearchInd, lineA, lineB)) {
            PointI coefficients;
            if (getCornerFeatureCoefficients(lineA, lineB, point,
                                             coefficients)) {
              laserCloudOri.push_back(pointOri);
              coeffSel.push_back(coefficients);
            }
            line_match_count++;
          }
        }
      }

      for (int i = 0; i < SurfNum; i++) {
        pointOri = SurfCloud->points[i];
        pointAssociateToMap(transform, pointOri, pointSel);
        kdtreeSurf.nearestKSearch(pointSel, 5, pointSearchInd,
                                  pointSearchSqDis);
        if (pointSearchSqDis[0] < 0.05) {
          Eigen::Vector4f planeCoef;
          if (findPlane(*referenceSurfCloud, pointSearchInd, 0.2, planeCoef)) {
            PointI coefficients;
            if (getSurfaceFeatureCoefficients(planeCoef, pointSel,
                                              coefficients)) {
              laserCloudOri.push_back(pointOri);
              coeffSel.push_back(coefficients);
            }
            plane_match_count++;
          }
        }
      }

      double score2 = getScore(coeffSel);
      match_count = line_match_count + plane_match_count;
      float percent2 = match_count / (CornerNum + SurfNum);
      std::cout << "scan match score2:" << score2 << " ,per2:" << percent2
                << std::endl;
    }

    if (score < _score_threshold) {
      transformf = transform;
      _fail_match_count++;
      std::cout << "low score!!" << score << ",per:" << percent << std::endl;
      return false;
    }

    if (percent < _match_percentage_threshold) {
      transformf = transform;
      _fail_match_count++;
      std::cout << "low percent!!" << score << ",per:" << percent << std::endl;
      return false;
    }
    _total_score += score;
    _match_count++;
    transformf = transform;

    return true;
  }
  //??? should we take the result when not converge?
  transformf = transform;
  _fail_match_count++;

  return false;
}

bool ScanMatch::scanMatchScan(const CloudIConPtr &referenceCornerCloud,
                              const CloudIConPtr &referenceSurfCloud,
                              const CloudIConPtr &CornerCloud,
                              const CloudIConPtr &SurfCloud,
                              Eigen::Isometry3f &relative_pose) {
  Twist transform;
  convertTransform(relative_pose, transform);
  bool success = scanMatchScan(referenceCornerCloud, referenceSurfCloud,
                               CornerCloud, SurfCloud, transform);
  convertTransform(transform, relative_pose);
  return success;
}

bool ScanMatch::scanMatchLocal(const CloudIConPtr &referenceCornerCloud,
                               const CloudIConPtr &referenceSurfCloud,
                               const CloudIConPtr &CornerCloud,
                               const CloudIConPtr &SurfCloud,
                               Eigen::Isometry3f &relative_pose) {
  Twist transform;
  convertTransform(relative_pose, transform);
  bool success = scanMatchLocal(referenceCornerCloud, referenceSurfCloud,
                                CornerCloud, SurfCloud, transform);
  convertTransform(transform, relative_pose);
  return success;
}

bool ScanMatch::scanMatchLocal(const CloudIConPtr &referenceCornerCloud,
                               const CloudIConPtr &referenceSurfCloud,
                               const CloudIConPtr &CornerCloud,
                               const CloudIConPtr &SurfCloud,
                               Twist &transform) {
  _referenceCornerCloudDS->clear();
  _downSizeFilterCorner.setInputCloud(referenceCornerCloud);
  _downSizeFilterCorner.filter(*_referenceCornerCloudDS);

  _referenceSurfCloudDS->clear();
  _downSizeFilterSurf.setInputCloud(referenceSurfCloud);
  _downSizeFilterSurf.filter(*_referenceSurfCloudDS);

  _CornerCloudDS->clear();
  _downSizeFilterCorner.setInputCloud(CornerCloud);
  _downSizeFilterCorner.filter(*_CornerCloudDS);

  _SurfCloudDS->clear();
  _downSizeFilterSurf.setInputCloud(SurfCloud);
  _downSizeFilterSurf.filter(*_SurfCloudDS);

  return scanMatchScan(_referenceCornerCloudDS, _referenceSurfCloudDS,
                       _CornerCloudDS, _SurfCloudDS, transform);
}

} // end namespace lidar_slam


/* ============================================================================
 * p2/cell/vertex.hh
 * ========================================================================= */

#ifndef vertexINCLUDED
#define vertexINCLUDED

#include <vec3.h>
#include <Eigen/Eigen>

#include "edge.hh"

class Cell;

/* ----------------------------------------------------------------------------
 * Vertex
 * ------------------------------------------------------------------------- */

/*
 * A vertex of a cell, with an outgoing set of directed edges.
 *
 * pos    the three-dimensional position of the vertex
 * Cell   the cell that the vertex belongs to;
 *        nonnull
 * ID     the ID number assigned to the vertex by its cell (or the client);
 *        positive
 * data   generic data attached to the vertex by the client
 * normal a storage place to store surface normal vector at this vertex
 * curv   a storage place for surface curvature
 * Edges  the edges in the orbit of the vertex;
 *        all are nonnull
 */
class Vertex
{

  /* -- public class methods ----------------------------------------------- */

  public:

  /*
   * Return a new vertex at the origin with no outgoing edges.
   * cell -> the cell that the vertex belongs to;
   *         must be nonnull
   * <- the new vertex;
   *    will be nonnull
   */
  static Vertex *make(Cell *cell);

  /*
   * Release the storage occupied by a given vertex.
   * vertex -> the vertex to kill;
   *           must be nonnull
   */
  static void kill(Vertex *vertex);

  /* -- public instance variables ------------------------------------------ */

  /*
   * The three-dimensional position of this vertex.
   */
  vec3 pos;

  /*
   * The three-dimensional position of this vertex.
   */
  vec3 subdivPos;

  /*
   * The surface normal at this vertex.
   */
  vec3 normal;

  /*
   * The vector of weights, which describe the influence
   * of control vertex positions to this vertex's position.
   */
  Eigen::VectorXd weights;

  /*
   * The surface curvature at this vertex.
   */
  float curv;

  /*
   * The average distance to neighboring vertices.
   */
  float dist;

  /*
   * Subdivision level.
   */
  unsigned level;

  /*
   * Subdivision level.
   */
  unsigned valence;

  /*
   * Current step size.
   */
  float stepSize;

  /*
   * Last encountered H-value.
   */
  float lastH;

  /*
   * Last encountered intensity value.
   */
  float lastIntensity;

  /* -- public instance methods -------------------------------------------- */

  public:

  /*
   * Return the cell for this vertex.
   * <- the cell that the vertex belongs to;
   *    will be nonnull
   */
  Cell *getCell();

  /*
   * Return the ID of this vertex.
   * <- the ID of this vertex;
   *    will be positive
   */
  unsigned int getID();

  /*
   * Change the ID of this vertex.
   * id -> the new id for this vertex;
   *       must be positive
   */
  void setID(unsigned int id);

  /*
   * Return an arbitrary outgoing edge from this vertex.
   * <- an edge whose origin is this vertex;
   *    null if isolated
   */
  Edge *getEdge();

  /*
   * Add a given outgoing edge to this vertex.
   * edge -> an edge whose origin is this vertex;
   *         must be nonnull
   */
  void addEdge(Edge *edge);

  /*
   * Remove a given outgoing edge from this vertex.
   * edge -> an edge whose origin is no longer at this vertex;
   *         must be nonnull
   */
  void removeEdge(Edge *edge);

  /*
   * Index of this vertex in its parent's cell vertex array (0 - n-1).
   */
  unsigned realIndex();

  /* -- protected instance methods ----------------------------------------- */

  protected:

  /*
   * Initialize this vertex at the origin with no outgoing edges.
   * cell -> the cell that this vertex belongs to;
   *         must be nonnull
   */
  Vertex(Cell *cell);

  /*
   * Release the storage occupied by the contents of this vertex.
   */
  ~Vertex();

  /* -- private instance variables ----------------------------------------- */

  private:

  /*
   * The cell that this vertex belongs to.
   * Nonnull.
   */
  Cell *cell;

  /*
   * The ID of this vertex.
   * Positive.
   */
  unsigned int id;

  /*
   * Index of this vertex in its parent's cell vertex array (0 - n-1),
   * or -1 if it is not yet calculated.
   */
  int _realIndex;

  /*
   * An arbitrary outgoing edge of this vertex.
   * Null if isolated.
   */
  Edge *edge;

};

/* -- inline instance methods ---------------------------------------------- */

inline Cell *Vertex::getCell()
{
  return cell;
}

inline unsigned int Vertex::getID()
{
  return id;
}

inline Edge *Vertex::getEdge()
{
  return edge;
}

/* ----------------------------------------------------------------------------
 * VertexEdgeIterator
 * ------------------------------------------------------------------------- */

/*
 * Enumerates the outgoing edges of a given vertex in counterclockwise order.
 */
class VertexEdgeIterator
{

  /* -- public instance methods -------------------------------------------- */

  public:

  /*
   * Initialize this edge iterator over a given vertex.
   * vertex -> the vertex to iterate the edges of;
   *           must be nonnull
   */
  VertexEdgeIterator(Vertex *vertex)
  {
    // pick an arbitrary edge in the vertex orbit

    start = vertex->getEdge();
    edge  = start;
  }

  /*
   * Release the storage occupied by this edge iterator.
   */
  ~VertexEdgeIterator()
  {
  }

  /*
   * Return the next edge of this edge iterator, if any.
   * <- the next edge of this edge iterator;
   *    null if none
   */
  Edge *next()
  {
    // check for degeneracy or exhausted iteration

    Edge *current = edge;

    if (current==0)
	return 0;

    // get the next edge in the counterclockwise orbit of the vertex, but
    // return the current edge
    // reset to null if we've come back to the start

    Edge *next = current->Onext();

    edge = next!=start ? next : 0;

    return current;
  }

  /* -- private instance variables ----------------------------------------- */

  private:

  /*
   * The first edge to be iterated.
   * Nonnull.
   */
  Edge *start;

  /*
   * The next edge to be iterated.
   * Null if exhausted.
   */
  Edge *edge;

};

#endif /* #ifndef vertexINCLUDED */