/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

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

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

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

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE 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.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>

#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <cmath>
#include <ctime>
#include <time.h>

#include "pseudo_inverse.h"

using namespace std;
using namespace Eigen;
using namespace KDL;

// take it from yaml file
int n_rows;
int n_cols;
int quality_index;
string relative_path_file;
string file_name;
std::vector<double> Quality;
int n_c_max = 19; // number of contact point, assumed that 1 contact point for each phalanx

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

	ros::init(argc, argv, "Hand_Jacobian");	// ROS node
	ros::NodeHandle nh;

  nh.param<int>("n_rows_file",n_rows,108);
  nh.param<int>("n_cols_file",n_cols,86);
  nh.param<int>("quality_index",quality_index,0);
  nh.param<std::string>("file_name", relative_path_file, "/db/box_db_2.csv" );

  std::string path = ros::package::getPath("grasp_learning");
  file_name = path + relative_path_file ;
  ifstream file(file_name);

  std::cout << "file: " << file_name.c_str() << " is " << (file.is_open() == true ? "already" : "not") << " open" << std::endl;
  if(!file.is_open())
  return 0;

  ////////////////////////////////////////////////////////////////////////////////////
	 /////////////////////////////////////////////////////////////////////////

  //	TAKE DATA_BASE : load a file .csv and put the values into Eigen::MatrixXd

  ////////////////////////////////////////////////////////////////////////////////////
	 /////////////////////////////////////////////////////////////////////////

  int count_row = 0;
  int count_col = 0;

  bool count_cols_only_one = true;

  for(std::string line; getline ( file, line, '\n' ); ) // ciclo sulla riga
  {
    count_row++;
    std::istringstream iss_line(line);

    if(count_cols_only_one)
    {

      for(std::string value; getline(iss_line, value, ',' ); )
          count_col++;

      count_cols_only_one = false;
    }
  }

  cout << "ROWS : " << count_row << endl; // 40
  cout << "COLS : " << count_col << endl; // 189

  Eigen::MatrixXd data_set(count_row,count_col);

	int i = 0;
	int j = 0;

	for(std::string line; getline( file, line, '\n' ); ) // ciclo sulla riga
	{

    std::istringstream iss_line(line);
    for(std::string value; getline(iss_line, value, ',' ); )
    {
    	data_set(i,j) = stod(value);
    	j++;
    }

    //cout << endl;
    j=0;
    i++;
	}

	file.close();

//	cout << " DATA_SET" << data_set << endl;

///////////////////////////// 		END		///////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

//								TAKE SOFT_HAND

////////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

  KDL::Tree hand_tree;

  std::string robot_desc_string;
  nh.param("robot_description", robot_desc_string, string());  // robot description is the name in the launch file
  if (!kdl_parser::treeFromString(robot_desc_string, hand_tree))
  { ROS_ERROR("Failed to construct kdl tree"); return false;}

  KDL::Chain chains_hand_finger[5];
  KDL::Jacobian hand_jacob[5];

  //std::string root_name = "right_hand_softhand_base";
  //std::string root_name = "right_hand_palm_link";
  std::string root_name = "world";
  std::string end_chain_name[5];
  end_chain_name[0] = "right_hand_thumb_distal_link";
  end_chain_name[1] = "right_hand_index_distal_link";
  end_chain_name[2] = "right_hand_middle_distal_link";
  end_chain_name[3] = "right_hand_ring_distal_link";
  end_chain_name[4] = "right_hand_little_distal_link";

  hand_tree.getChain(root_name, end_chain_name[0], chains_hand_finger[0]);      //thumb
  hand_tree.getChain(root_name, end_chain_name[1], chains_hand_finger[1]);      //index
  hand_tree.getChain(root_name, end_chain_name[2], chains_hand_finger[2]);      //middle
  hand_tree.getChain(root_name, end_chain_name[3], chains_hand_finger[3]);      //ring
  hand_tree.getChain(root_name, end_chain_name[4], chains_hand_finger[4]);      //little

  int nq_hand = hand_tree.getNrOfJoints();  // 34
  int ns_hand = hand_tree.getNrOfSegments(); // 39

  cout << "number_of_joint_in_hand : " << nq_hand << endl;
  cout << "number_of_segment_in_hand : " << ns_hand << endl;

///////////////////////////// 		END		  ///////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

//			CALCULATE THE GRASP_MATRIX AND HAND_JACOBIAN FOR EACH ROW
//	I supposed to have 19 joint variables and a single point of contact for each phalanx

////////////////////////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////////////

	unsigned int nj_0 = chains_hand_finger[0].getNrOfJoints();  // 5
  unsigned int nj_1 = chains_hand_finger[1].getNrOfJoints();  // 7
  unsigned int nj_2 = chains_hand_finger[2].getNrOfJoints();  // 7
  unsigned int nj_3 = chains_hand_finger[3].getNrOfJoints();  // 7
  unsigned int nj_4 = chains_hand_finger[4].getNrOfJoints();  // 7

  KDL::JntArray q_thumb = JntArray(nj_0);    // thumb
  KDL::JntArray q_index = JntArray(nj_1);    // forefinger
  KDL::JntArray q_middle = JntArray(nj_2);   // middlefinger
  KDL::JntArray q_ring = JntArray(nj_3);     // ringfinger
  KDL::JntArray q_little = JntArray(nj_4);   // littlefinger

  cout << "nj_0 : " << nj_0 << endl;
  cout << "nj_1 : " << nj_1 << endl;
  cout << "nj_2 : " << nj_2 << endl;
  cout << "nj_3 : " << nj_3 << endl;
  cout << "nj_4 : " << nj_4 << endl;

  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_0;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_1;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_2;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_3;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_4;

  // constructs the kdl solvers in non-realtime
  jnt_to_jac_solver_0.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[0]));
  jnt_to_jac_solver_1.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[1]));
  jnt_to_jac_solver_2.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[2]));
  jnt_to_jac_solver_3.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[3]));
  jnt_to_jac_solver_4.reset(new KDL::ChainJntToJacSolver(chains_hand_finger[4]));

  // resizes the joint state vectors in non-realtime
  q_thumb.resize(chains_hand_finger[0].getNrOfJoints());
  q_index.resize(chains_hand_finger[1].getNrOfJoints());
  q_middle.resize(chains_hand_finger[2].getNrOfJoints());
  q_ring.resize(chains_hand_finger[3].getNrOfJoints());
  q_little.resize(chains_hand_finger[4].getNrOfJoints());

  hand_jacob[0].resize(chains_hand_finger[0].getNrOfJoints());
  hand_jacob[1].resize(chains_hand_finger[1].getNrOfJoints());
  hand_jacob[2].resize(chains_hand_finger[2].getNrOfJoints());
 	hand_jacob[3].resize(chains_hand_finger[3].getNrOfJoints());
  hand_jacob[4].resize(chains_hand_finger[4].getNrOfJoints());

	for(int r = 0; r < n_rows ; r++)  // for each row calculation the grasp matrix and hand jacobian
	{
		//take the value of joints from data
		// initialize jntarray then be able to calculate the Jacobian to that point of contact

		q_thumb(0) = data_set(r,10);

		q_thumb(1) = data_set(r,11) / 2;
		q_thumb(2) = data_set(r,11) / 2;

		q_thumb(3) = data_set(r,12) / 2;
		q_thumb(4) = data_set(r,12) / 2;

		q_index(0) = data_set(r,13);

		q_index(1) = data_set(r,14) / 2;
		q_index(2) = data_set(r,14) / 2;

		q_index(3) = data_set(r,15) / 2;
		q_index(4) = data_set(r,15) / 2;

		q_index(5) = data_set(r,16) / 2;
		q_index(6) = data_set(r,16) / 2;

		q_middle(0) = data_set(r,17);

		q_middle(1) = data_set(r,18) / 2;
		q_middle(2) = data_set(r,18) / 2;

		q_middle(3) = data_set(r,19) / 2;
		q_middle(4) = data_set(r,19) / 2;

		q_middle(5) = data_set(r,20) / 2;
		q_middle(6) = data_set(r,20) / 2;

		q_ring(0) = data_set(r,21);

		q_ring(1) = data_set(r,22) / 2;
		q_ring(2) = data_set(r,22) / 2;

		q_ring(3) = data_set(r,23) / 2;
		q_ring(4) = data_set(r,23) / 2;

		q_ring(5) = data_set(r,24) / 2;
		q_ring(6) = data_set(r,24) / 2;

		q_little(0) = data_set(r,25);

		q_little(1) = data_set(r,26) / 2;
		q_little(2) = data_set(r,26) / 2;

		q_little(3) = data_set(r,27) / 2;
		q_little(4) = data_set(r,27) / 2;

		q_little(5) = data_set(r,28) / 2;
		q_little(6) = data_set(r,28) / 2;

		Eigen::MatrixXd Contacts(n_c_max,3); // I build a matrix of contact points for convenience only
										 // each row is a contact point
										 // row 0, 1, 2 for thumb
										 // row 3, 4, 5, 6 for index
										 // row 7, 8, 9, 10 for middle
										 // row 11, 12, 13, 14 for ring
										 // row 15, 16, 17, 18 for little
										 // n_c = 19 ( number of contact)

	 int start = 29; // the first value for the position of the joint variables
	 int gap_point_coordinate = 0;

		for(int i = 0; i < n_c_max; i++)
		{
			for(int j = 0; j < 3; j++ )
				Contacts(i,j) = data_set(r, start + gap_point_coordinate + j);  // check if it is correct

			gap_point_coordinate += 3;
		}

    cout << "Contacts : " << Contacts << endl;

    //for each contact point
		std::vector<Eigen::MatrixXd> Grasp_Matrix_ ;
    std::vector<Eigen::MatrixXd> Hand_Jacobian_ ;

    int k = 1;

    for(int i = 0 ; i < n_c_max ; i++) //calc the grasp_matrix and hand_jacobian for each contact point
    {

      if(Contacts(i,0) != 9999) //9999 similar to NaN in dataset
      {

        Eigen::MatrixXd Grasp_Matrix(6,6);
 				Eigen::MatrixXd Skew_Matrix(3,3);
  			Eigen::MatrixXd Rotation(3,3);

  			Rotation <<  MatrixXd::Identity(3,3);

        //check if the values of the skew matrix are expressed in the correct reference system
      	Skew_Matrix(0,0) = Skew_Matrix(1,1) = Skew_Matrix(2,2) = 0;
        Skew_Matrix(0,1) = - Contacts(i,2); // -rz
        Skew_Matrix(0,2) = Contacts(i,1);   // ry
        Skew_Matrix(1,0) = Contacts(i,2);   // rz
        Skew_Matrix(2,0) = - Contacts(i,1); // -ry
        Skew_Matrix(1,2) = - Contacts(i,0); // -rx
        Skew_Matrix(2,1) = Contacts(i,0);   // rx

        Grasp_Matrix.block<3,3>(0,0) = Rotation;
        Grasp_Matrix.block<3,3>(3,3) = Rotation;
        Grasp_Matrix.block<3,3>(0,3) = Skew_Matrix * Rotation;
        Grasp_Matrix.block<3,3>(3,0) = MatrixXd::Zero(3,3);

        cout << "Grasp_Matrix" << endl;
        cout << Grasp_Matrix << endl;

        Grasp_Matrix_.push_back(Grasp_Matrix);

        jnt_to_jac_solver_0->JntToJac(q_thumb, hand_jacob[0], 0);
      	jnt_to_jac_solver_1->JntToJac(q_index, hand_jacob[1], 0);
      	jnt_to_jac_solver_2->JntToJac(q_middle, hand_jacob[2], 0);
      	jnt_to_jac_solver_3->JntToJac(q_ring, hand_jacob[3], 0);
   			jnt_to_jac_solver_4->JntToJac(q_little, hand_jacob[4], 0);

   			cout << "Hand_Jacobian_ THUMB" << endl;
      	cout <<  hand_jacob[0].data << endl;
      	cout << "Hand_Jacobian_ INDEX" << endl;
    		cout <<  hand_jacob[1].data << endl;
  			cout << "Hand_Jacobian_ MIDDLE"<< endl;
   			cout <<  hand_jacob[2].data << endl;
      	cout << "Hand_Jacobian_ RING"  << endl;
    		cout <<  hand_jacob[3].data << endl;
  			cout << "Hand_Jacobian_ LITTLE"<< endl;
  			cout <<  hand_jacob[4].data << endl;

      	int which_finger = 0;
        int which_falange = 0;

        if((0 <= i) && (i <= 2)){ which_finger = 0; which_falange = i+k; k++; if(i==2) k = 1;} // control flag which_falange there may be some error
        if((3 <= i) && (i <= 6)){ which_finger = 1; which_falange = i-3+k; k++; if(i==6) k = 1;}
        if((7 <= i) && (i <= 10)){ which_finger = 2; which_falange = i-7+k; k++; if(i==10) k = 1;}
        if((11 <= i) && (i <= 14)){ which_finger = 3; which_falange = i-11+k; k++; if(i==14) k = 1;}
        if((15 <= i) && (i <= 18)){ which_finger = 4; which_falange = i-15+k; k++; if(i==18) k = 1;}

        cout << "i : " << i << endl;
        cout << "which_finger : " << which_finger << endl;
        cout << "which_falange : " << which_falange << endl;

        switch(which_finger)
        {
          case 0: // thumb
      						jnt_to_jac_solver_0->JntToJac(q_thumb, hand_jacob[0], which_falange);
      						break;

	  			case 1: // index
      						jnt_to_jac_solver_1->JntToJac(q_index, hand_jacob[1], which_falange);
      						break;

	  			case 2: // middle
							jnt_to_jac_solver_2->JntToJac(q_middle, hand_jacob[2], which_falange);
      						break;

	  			case 3: // ring
      						jnt_to_jac_solver_3->JntToJac(q_ring, hand_jacob[3], which_falange);
	  						break;

	  			case 4: // little
      						jnt_to_jac_solver_4->JntToJac(q_little, hand_jacob[4], which_falange);
	  						break;
	  		}// end switch

	  		cout << " Jacobian thumb " << hand_jacob[0].data << endl ;
	  		cout << " Jacobian index " << hand_jacob[1].data << endl ;
	  	  cout << " Jacobian middle " << hand_jacob[2].data << endl ;
			  cout << " Jacobian ring " << hand_jacob[3].data << endl ;
			  cout << " Jacobian little " << hand_jacob[4].data << endl ;

	  		Hand_Jacobian_.push_back(hand_jacob[0].data); // 5
	  		Hand_Jacobian_.push_back(hand_jacob[1].data); // 7
	 		  Hand_Jacobian_.push_back(hand_jacob[2].data); // 7
	  		Hand_Jacobian_.push_back(hand_jacob[3].data); // 7
	  		Hand_Jacobian_.push_back(hand_jacob[4].data); // 7

      }// end if  contact

      }	// end for contact

      cout << " Dim of the vectors of the matrices grasp " << Grasp_Matrix_.size() << endl;
      cout << " Dim of the hand jacobian " << Hand_Jacobian_.size() << endl;

      int n_c_eff = Grasp_Matrix_.size();

      Eigen::MatrixXd Hand_Jacobian_Contact(6*n_c_eff,nq_hand-1);
      Eigen::MatrixXd Grasp_Matrix_Contact(6,6*n_c_eff);

      int s = 0; // dimension
      int s_ = 0;

      for(int i = 0; i < n_c_eff; i++)
      {

        Grasp_Matrix_Contact.block<6,6>(0,s) = Grasp_Matrix_[i];

        cout << "i : " << i << endl;

      	Hand_Jacobian_Contact.block<6,5>(s,0) = Hand_Jacobian_[s_];
      	Hand_Jacobian_Contact.block<6,7>(s,5) = Hand_Jacobian_[s_+1];
      	Hand_Jacobian_Contact.block<6,7>(s,12) = Hand_Jacobian_[s_+2];
      	Hand_Jacobian_Contact.block<6,7>(s,19) = Hand_Jacobian_[s_+3];
      	Hand_Jacobian_Contact.block<6,7>(s,26) = Hand_Jacobian_[s_+4];

  			s_+=5;
       	s+=6;
      }

/*
        file_output << "Grasp_Matrix_Contact : " << endl;
        file_output <<  Grasp_Matrix_Contact << endl;
        file_output << "__________________________________________________" << endl;
        file_output << "Hand_Jacobian_Contact : " << endl;
        file_output << Hand_Jacobian_Contact << endl;
*/

        // GRASP JACOBIAN

      Eigen::MatrixXd GRASP_Jacobian(6,nq_hand-1);
      Eigen::MatrixXd Grasp_Matrix_pseudo(Grasp_Matrix_Contact.rows(),Grasp_Matrix_Contact.cols()) ;
      pseudo_inverse(Grasp_Matrix_Contact,Grasp_Matrix_pseudo);

      GRASP_Jacobian = Grasp_Matrix_pseudo.transpose() * Hand_Jacobian_Contact;

      cout << "GRASP_Jacobian : " << endl;
      cout << GRASP_Jacobian << endl;

      int quality_ = 9999;
      Eigen::VectorXd Singular;
      double sigma_min ;
		  double sigma_max ;

      switch(quality_index)
      {

        case 0: // "minimum_singular_value_of_G"  Q = sigma_min(G)
				      {
        	   		JacobiSVD<MatrixXd> svd0(Grasp_Matrix_Contact, ComputeThinU | ComputeThinV);
        				Singular = svd0.singularValues();

				    		Quality.push_back(Singular[Singular.size()-1]);
				      }
        		  break;

        case 1: // "Volume of the ellipsoid in the wrench space"  Q = K sqrt(det(GG.t)) = k ( sigma_0 **** sigma_d)
        		  {
                Eigen::MatrixXd G_G_t = Grasp_Matrix_Contact * Grasp_Matrix_Contact.transpose();
					      JacobiSVD<MatrixXd> svd1(G_G_t, ComputeThinU | ComputeThinV);
					      Singular = svd1.singularValues();

				        for(int i = 0 ; i < Singular.size() ; i++)
						        quality_ *= Singular[i];

					     Quality.push_back(quality_);
				      }
        		  break;

        case 2: // "Grasp isotropy index" Q = sigma_min(G) / sigma_max(G)
        		 {
        			  JacobiSVD<MatrixXd> svd2(Grasp_Matrix_Contact, ComputeThinU | ComputeThinV);
					      Singular = svd2.singularValues();
                sigma_min = Singular[Singular.size()-1];
					      sigma_max = Singular[0];

					      Quality.push_back(sigma_min/sigma_max);
				      }
				      break;

			case 3: // "Distance to singular configuration" Q = sigma_min(H) H = G.pseudo_inverse.transpose * J
				    {
					    JacobiSVD<MatrixXd> svd3(GRASP_Jacobian, ComputeThinU | ComputeThinV);
					    Singular = svd3.singularValues();

              Quality.push_back(Singular[Singular.size()-1]);
				    }
				    break;

			case 4: // "Volume of manipulability ellipsoid"
				{
					Eigen::MatrixXd H_H_t = GRASP_Jacobian * GRASP_Jacobian.transpose();

					JacobiSVD<MatrixXd> svd4(H_H_t, ComputeThinU | ComputeThinV);

					Singular = svd4.singularValues();

					for(int i = 0 ; i < Singular.size() ; i++)
						quality_ *= Singular[i];

					Quality.push_back(quality_);
				}
				break;

			case 5: // "Uniformity of transformations"
				{
					JacobiSVD<MatrixXd> svd5(GRASP_Jacobian, ComputeThinU | ComputeThinV);

					Singular = svd5.singularValues();

					sigma_min = Singular[Singular.size()-1];
					sigma_max = Singular[0];

					Quality.push_back(sigma_min/sigma_max);
				}
				break;
      }//end switch

    cout << "Quality : " << endl;

    for(int i = 0; i < Quality.size(); i++)
			cout << Quality[i] << ' ';
    cout << endl;
	}
  // end for each row

  cout << " YEAH ENJOY " << endl;
  cout << "   fine   " << endl;

	ros::spin();
	return 0;
}


/*
 * Copyright (c) 2011-2017, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 */

#ifndef DART_MATH_CONFIGURATIONSPACE_HPP_
#define DART_MATH_CONFIGURATIONSPACE_HPP_

#include <Eigen/Dense>
#include "dart/math/MathTypes.hpp"
#include "dart/math/Geometry.hpp"

namespace dart {
namespace math {

//==============================================================================
template <std::size_t Dimension>
struct RealVectorSpace
{
  static constexpr std::size_t NumDofs = Dimension;
  static constexpr int NumDofsEigen = static_cast<int>(Dimension);

  using TangentSpace = RealVectorSpace<NumDofs>;

  using Point          = Eigen::Matrix<double, NumDofs, 1>;
  using EuclideanPoint = Eigen::Matrix<double, NumDofs, 1>;
  using Vector         = Eigen::Matrix<double, NumDofs, 1>;
  using Matrix         = Eigen::Matrix<double, NumDofs, NumDofs>;
  using JacobianMatrix = Eigen::Matrix<double, 6, NumDofs>;
};

//==============================================================================
//
// These namespace-level definitions are required to enable ODR-use of static
// constexpr member variables.
//
// See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426
//
template <std::size_t Dimension>
constexpr std::size_t RealVectorSpace<Dimension>::NumDofs;
template <std::size_t Dimension>
constexpr int RealVectorSpace<Dimension>::NumDofsEigen;

using NullSpace = RealVectorSpace<0u>;
using R1Space = RealVectorSpace<1u>;
using R2Space = RealVectorSpace<2u>;
using R3Space = RealVectorSpace<3u>;

//==============================================================================
struct SO3Space
{
  static constexpr std::size_t NumDofs = 3u;
  static constexpr int NumDofsEigen = 3;

  using TangentSpace = RealVectorSpace<NumDofs>;

  using Point          = Eigen::Matrix3d;
  using EuclideanPoint = Eigen::Vector3d;
  using Vector         = Eigen::Vector3d;
  using Matrix         = Eigen::Matrix3d;
  using JacobianMatrix = Eigen::Matrix<double, 6, NumDofs>;
};

//==============================================================================
struct SE3Space
{
  static constexpr std::size_t NumDofs = 6u;
  static constexpr int NumDofsEigen = 6;

  using TangentSpace = RealVectorSpace<NumDofs>;

  using Point          = Eigen::Isometry3d;
  using EuclideanPoint = Eigen::Vector6d;
  using Vector         = Eigen::Vector6d;
  using Matrix         = Eigen::Matrix6d;
  using JacobianMatrix = Eigen::Matrix6d;
};

struct MapsToManifoldPoint {};

//==============================================================================
template <typename SpaceT>
typename SpaceT::Matrix inverse(const typename SpaceT::Matrix& mat);

//==============================================================================
template <typename SpaceT>
typename SpaceT::EuclideanPoint
toEuclideanPoint(const typename SpaceT::Point& point);

//==============================================================================
template <typename SpaceT>
typename SpaceT::Point
toManifoldPoint(const typename SpaceT::EuclideanPoint& point);

//==============================================================================
template <typename SpaceT>
typename SpaceT::Point integratePosition(
    const typename SpaceT::Point& pos,
    const typename SpaceT::Vector& vel,
    double dt);

//==============================================================================
template <typename SpaceT>
typename SpaceT::Vector integrateVelocity(
    const typename SpaceT::Vector& vel,
    const typename SpaceT::Vector& acc,
    double dt);

} // namespace math
} // namespace dart

#include "dart/math/detail/ConfigurationSpace.hpp"

#endif // DART_MATH_CONFIGURATIONSPACE_HPP_


#include <svgpp/parser/number.hpp>

#include <limits>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtest/gtest.h>

using namespace svgpp;

namespace
{
  struct Context
  {
    Context()
      : value_(std::numeric_limits<double>::max())
    {}

    void set(tag::attribute::by, double val)
    {
      value_ = val;
    }

    double value_;
  };
}

TEST(value_parser, number)
{
  {
    Context ctx;
    EXPECT_TRUE(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("1234"), tag::source::attribute()));
    EXPECT_EQ(1234, ctx.value_);
    EXPECT_TRUE(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("+1234"), tag::source::attribute()));
    EXPECT_EQ(1234, ctx.value_);
    EXPECT_TRUE(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("-567"), tag::source::attribute()));
    EXPECT_EQ(-567, ctx.value_);
    EXPECT_TRUE(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("+0"), tag::source::attribute()));
    EXPECT_EQ(0, ctx.value_);
    EXPECT_TRUE(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("-.4"), tag::source::attribute()));
    EXPECT_EQ(-0.4, ctx.value_);
    EXPECT_TRUE(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("14.56"), tag::source::attribute()));
    EXPECT_EQ(14.56, ctx.value_);
    EXPECT_TRUE(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("-5.4e+10"), tag::source::attribute()));
    EXPECT_EQ(-5.4e+10, ctx.value_);
    EXPECT_TRUE(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("-5.4e-10"), tag::source::attribute()));
    EXPECT_EQ(-5.4e-10, ctx.value_);
    EXPECT_TRUE(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("7E10"), tag::source::attribute()));
    EXPECT_EQ(7e+10, ctx.value_);

    EXPECT_THROW(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("5."), tag::source::attribute()), std::exception);
    EXPECT_THROW(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("."), tag::source::attribute()), std::exception);
    EXPECT_THROW(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string(""), tag::source::attribute()), std::exception);
    EXPECT_THROW(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string(" "), tag::source::attribute()), std::exception);
    EXPECT_THROW(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("1N8"), tag::source::attribute()), std::exception);
    EXPECT_THROW(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("+-9"), tag::source::attribute()), std::exception);
    EXPECT_THROW(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string(" 5"), tag::source::attribute()), std::exception);
    EXPECT_THROW(value_parser<tag::type::number>::parse(tag::attribute::by(), ctx, std::string("5 "), tag::source::attribute()), std::exception);
  }
}

namespace
{
  typedef boost::tuple<const char *, double, boost::optional<double> > number_optional_number_test_t;

  class number_optional_number_valid : public ::testing::TestWithParam<number_optional_number_test_t>
  {};

  number_optional_number_test_t number_optional_number_valid_tests[] = {
    number_optional_number_test_t("1e34 -3.5", 1e34, -3.5),
    number_optional_number_test_t("8e-4 ,-7.5", 8e-4, -7.5),
    number_optional_number_test_t("8e4 ,-7.5", 8e+4, -7.5),
    number_optional_number_test_t("8e4,-7.5", 8e+4, -7.5),
    number_optional_number_test_t("8e4, -7.5", 8e+4, -7.5),
    number_optional_number_test_t("8e4     -7.5", 8e+4, -7.5),
    number_optional_number_test_t("-6.12", -6.12)
  };

  std::ostream & operator<< (std::ostream & os, number_optional_number_test_t const & val)
  {
    os << "\"" << val.get<0>() << "\"";
    return os;
  }

  class number_optional_number_invalid : public ::testing::TestWithParam<const char *>
  {};

  const char * number_optional_number_invalid_tests[] = {
    "-6.12 ",
    " -6.12",
    "-6.12 4.5 ",
    "-6.12 4.5 1",
    "-6.12,,4.5",
    "-6.12, ,4.5"
  };
}

namespace
{
  struct Context2
  {
    Context2()
      : value1_(std::numeric_limits<double>::max())
    {}

    void set(tag::attribute::by, double val)
    {
      value1_ = val;
    }

    void set(tag::attribute::by, double val, double val2)
    {
      value1_ = val;
      value2_ = val2;
    }

    double value1_;
    boost::optional<double> value2_;
  };
}

TEST_P(number_optional_number_valid, t1)
{
  {
    Context2 ctx;
    EXPECT_TRUE(value_parser<tag::type::number_optional_number>::parse(tag::attribute::by(), ctx, std::string(GetParam().get<0>()), tag::source::attribute()));
    EXPECT_EQ(GetParam().get<1>(), ctx.value1_);
    EXPECT_EQ(GetParam().get<2>(), ctx.value2_);
  }
}

INSTANTIATE_TEST_CASE_P(value_parser,
                        number_optional_number_valid,
                        ::testing::ValuesIn(number_optional_number_valid_tests));

namespace
{
  struct Context3
  {
    void set(tag::attribute::by, double) {}
    void set(tag::attribute::by, double, double) {}
  };
}

TEST_P(number_optional_number_invalid, t1)
{
  Context3 ctx;
  EXPECT_THROW(value_parser<tag::type::number_optional_number>::parse(tag::attribute::by(), ctx, std::string(GetParam()), tag::source::attribute()), std::exception);
}

INSTANTIATE_TEST_CASE_P(value_parser,
                        number_optional_number_invalid,
                        ::testing::ValuesIn(number_optional_number_invalid_tests));


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_GSTMERC_HPP
#define BOOST_GEOMETRY_PROJECTIONS_GSTMERC_HPP

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_phi2.hpp>
#include <boost/geometry/srs/projections/impl/pj_tsfn.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace gstmerc
    {
            template <typename T>
            struct par_gstmerc
            {
                T lamc;
                T phic;
                T c;
                T n1;
                T n2;
                T XS;
                T YS;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_gstmerc_spheroid
                : public base_t_fi<base_gstmerc_spheroid<T, Parameters>, T, Parameters>
            {
                par_gstmerc<T> m_proj_parm;

                inline base_gstmerc_spheroid(const Parameters& par)
                    : base_t_fi<base_gstmerc_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    T L, Ls, sinLs1, Ls1;

                    L= this->m_proj_parm.n1*lp_lon;
                    Ls= this->m_proj_parm.c+this->m_proj_parm.n1*log(pj_tsfn(-1.0*lp_lat,-1.0*sin(lp_lat),this->m_par.e));
                    sinLs1= sin(L)/cosh(Ls);
                    Ls1= log(pj_tsfn(-1.0*asin(sinLs1),0.0,0.0));
                    xy_x= (this->m_proj_parm.XS + this->m_proj_parm.n2*Ls1)*this->m_par.ra;
                    xy_y= (this->m_proj_parm.YS + this->m_proj_parm.n2*atan(sinh(Ls)/cos(L)))*this->m_par.ra;
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T L, LC, sinC;

                    L= atan(sinh((xy_x*this->m_par.a - this->m_proj_parm.XS)/this->m_proj_parm.n2)/cos((xy_y*this->m_par.a - this->m_proj_parm.YS)/this->m_proj_parm.n2));
                    sinC= sin((xy_y*this->m_par.a - this->m_proj_parm.YS)/this->m_proj_parm.n2)/cosh((xy_x*this->m_par.a - this->m_proj_parm.XS)/this->m_proj_parm.n2);
                    LC= log(pj_tsfn(-1.0*asin(sinC),0.0,0.0));
                    lp_lon= L/this->m_proj_parm.n1;
                    lp_lat= -1.0*pj_phi2(exp((LC-this->m_proj_parm.c)/this->m_proj_parm.n1),this->m_par.e);
                }

                static inline std::string get_name()
                {
                    return "gstmerc_spheroid";
                }

            };

            // Gauss-Schreiber Transverse Mercator (aka Gauss-Laborde Reunion)
            template <typename Parameters, typename T>
            inline void setup_gstmerc(Parameters& par, par_gstmerc<T>& proj_parm)
            {
                proj_parm.lamc= par.lam0;
                proj_parm.n1= sqrt(T(1)+par.es*math::pow(cos(par.phi0),4)/(T(1)-par.es));
                proj_parm.phic= asin(sin(par.phi0)/proj_parm.n1);
                proj_parm.c= log(pj_tsfn(-1.0*proj_parm.phic,0.0,0.0))
                           - proj_parm.n1*log(pj_tsfn(-1.0*par.phi0,-1.0*sin(par.phi0),par.e));
                proj_parm.n2= par.k0*par.a*sqrt(1.0-par.es)/(1.0-par.es*sin(par.phi0)*sin(par.phi0));
                proj_parm.XS= 0;/* -par.x0 */
                proj_parm.YS= -1.0*proj_parm.n2*proj_parm.phic;/* -par.y0 */
            }

    }} // namespace detail::gstmerc
    #endif // doxygen

    /*!
        \brief Gauss-Schreiber Transverse Mercator (aka Gauss-Laborde Reunion) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin
         - lon_0: Central meridian
         - k_0: Scale factor
        \par Example
        \image html ex_gstmerc.gif
    */
    template <typename T, typename Parameters>
    struct gstmerc_spheroid : public detail::gstmerc::base_gstmerc_spheroid<T, Parameters>
    {
        template <typename Params>
        inline gstmerc_spheroid(Params const& , Parameters const& par)
            : detail::gstmerc::base_gstmerc_spheroid<T, Parameters>(par)
        {
            detail::gstmerc::setup_gstmerc(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_gstmerc, gstmerc_spheroid, gstmerc_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(gstmerc_entry, gstmerc_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(gstmerc_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(gstmerc, gstmerc_entry);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_GSTMERC_HPP


#include "eigen_config.h"
#include <matplot/matplot.h>

#include <Eigen/Dense>

// #include "cfar.h"

#include "pulse_doppler.h"
using namespace plasma;
using namespace matplot;
using namespace Eigen;
int main() {
  // Eigen::MatrixXd x = Eigen::MatrixXd::Ones(10, 1);
  // size_t num_samps_delay = 30;
  // size_t nfft = 30;
  // double fs = 1e6;
  // double ts = 1 / fs;
  // double t = num_samps_delay * ts;
  // auto y = plasma::delay(x, t, nfft, fs);

  // auto yv = std::vector<double>(y.data(), y.data() + y.size());
  // matplot::plot(yv);
  // matplot::show();
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// cross_validation::k_fold::partition.hpp                                  //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef  BOOST_STATISTICS_DETAIL_CROSS_VALIDATION_DATA_K_FOLD_PARTITION_HPP_ER_2009
#define  BOOST_STATISTICS_DETAIL_CROSS_VALIDATION_DATA_K_FOLD_PARTITION_HPP_ER_2009
#include <stdexcept>
#include <iterator>
#include <vector>
#include <ostream>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/range.hpp>
#include <boost/format.hpp>
#include <boost/utility.hpp>
#include <boost/circular_buffer.hpp>
#include <boost/iterator/iterator_traits.hpp>

#include <boost/statistics/detail/cross_validation/extractor/meta_range.hpp>
//#include <boost/statistics/detail/tuple/meta/include.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace cross_validation{
namespace k_fold{

    // k = 4
    // train     test
    // [0][1][2] (3)    j = 3
    // -------------
    // [1][2][3] (0)    j = 0
    // [2][3][0] (1)    j = 1
    // [3][0][1] (2)    j = 2
    // [0][1][2] (3)    j = 3

    // Partitions a dataset into 2 disjoint subsets, of size n(k-1) and n.
    //
    // U is the type of one observation unit, u, which must contain enough
    // information to train and test an estimator. For example, t = (x,y)
    // is needed to train a regression estimator, x is the input to the
    // predictor, and y is a value against which the output is compared
    // Each of t, x, and y are extracted from u using instances
    // of Ft, Fi, and Fo, respectively.

    template<
        typename U,
        typename Ft = extractor::identity,
        typename Fi = extractor::identity,
        typename Fo = extractor::identity
    >
    class partition{

        // Warning:
        // Since the dataset is kept internally, U must not be a reference or
        // contain a reference

        typedef std::string str_;
        public:
        typedef boost::circular_buffer<U>                       subset1_type;
        typedef std::vector<U>                                  subset2_type;
        typedef long int                                        int_;

        struct meta_training_range
            : extractor::meta::range<Ft,const subset1_type>{};
        struct meta_input_range
            : extractor::meta::range<Fi,const subset2_type>{};
        struct meta_output_range
            : extractor::meta::range<Fo,const subset2_type>{};

        partition();
        template<typename It>
        partition(int_ k, It b,It e);
        template<typename It>
        partition(
            const Ft& ft,
            const Fi& fi,
            const Fo& fo,
            int_ k, It b,It e
        );
        partition(const partition&);
        partition& operator=(const partition&);

        template<typename It>
        void initialize(int_ k, It b,It e); // j = 0
        void initialize(); //restores state to j = 0
        void increment(); // ++j

        // Access
        const subset1_type& subset1()const;
        const subset2_type& subset2()const;

        typename meta_training_range::type   training_range()const;
        typename meta_input_range::type      input_range()const;
        typename meta_output_range::type     output_range()const;

        static str_ description_header;

        const int_& n_test()const{ return this->n(); }
        const int_& index()const{ return this->j(); }
        const int_& n_folds()const{ return this->k(); }

        int_ n_training()const{ return this->n() * (this->k()-1); }

        protected:
        const int_& n()const;      // size of test data
        const int_& j()const;      // index of current iteration
        const int_& k()const;      // number of iterations

        int_ k_;
        int_ j_;
        int_ n_;
        subset1_type subset1_;
        subset2_type subset2_;
        Ft ft_;
        Fi fi_;
        Fo fo_;
    };
    template<typename U,typename Ft,typename Fi,typename Fo>
    typename partition<U,Ft,Fi,Fo>::str_
    partition<U,Ft,Fi,Fo>::description_header ="(n_test,n_index,n_folds)";

    template<typename U,typename Ft,typename Fi,typename Fo>
    std::ostream& operator<<(
        std::ostream& out,const partition<U,Ft,Fi,Fo>& that){
        format f("partition(%1%,%2%,%3%)");
        f % that.n_test() % that.index() % that.n_folds();
        return (out << f.str());
    }

    // Implementation //

    template<typename U,typename Ft,typename Fi,typename Fo>
    partition<U,Ft,Fi,Fo>::partition()
    :k_(0),j_(0),n_(0){}

    template<typename U,typename Ft,typename Fi,typename Fo>
        template<typename It>
    partition<U,Ft,Fi,Fo>::partition(int_ k, It b,It e)
    {
        this->initialize(k,b,e);
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    template<typename It>
    partition<U,Ft,Fi,Fo>::partition(
        const Ft& ft,
        const Fi& fi,
        const Fo& fo,
        int_ k, It b,It e
    )
    :ft_(ft),fi_(fi),fo_(fo)
    {
        this->initialize(k,b,e);
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    partition<U,Ft,Fi,Fo>::partition(const partition& that)
    :k_(that.k_),j_(that.j_),n_(that.n_),
    subset1_(that.subset1_),
    subset2_(that.subset2_),
    ft_(that.ft_),
    fi_(that.fi_),
    fo_(that.fo_){}

    template<typename U,typename Ft,typename Fi,typename Fo>
    partition<U,Ft,Fi,Fo>& partition<U,Ft,Fi,Fo>::operator=(const partition& that)
    {
        this->k_ = that.k_;
        this->j_ = that.j_;
        this->n_ = that.n_;
        this->subset1_ = that.subset1_;
        this->subset2_  = that.subset2_;
        this->ft_ = that.ft_;
        this->fi_ = that.fi_;
        this->fo_ = that.fo_;
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
        template<typename It>
    void partition<U,Ft,Fi,Fo>::initialize(int_ k,It b,It e)
    {
        BOOST_ASSERT(k>1);
        typedef typename iterator_difference<It>::type  diff_;
        this->k_ = k;
        this->j_ = -1;
        diff_ d = std::distance( b, e);
        if(d % this->k() != 0){
            static const str_ msg
                = str_("k_fold_estimate : distance(b,e)")
                    + "%1% not a multiple of k = %2%";
            throw std::runtime_error( ( format(msg) % d % k ).str() );
        }
        this->n_ = d / this->k();

        It i = boost::next( b, this->n() * (k-1) );
        this->subset2_.clear();
        this->subset2_.reserve(this->n());
        std::copy(
            i,
            e,
            std::back_inserter(this->subset2_)
        );
        this->subset1_.assign(
            this->n() * (k-1),
            b,
            i
        );
        this->increment();
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    void partition<U,Ft,Fi,Fo>::initialize()
    {
        while(this->j()<this->k()){
            this->increment();
        }
        this->j_ = 0;
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    void partition<U,Ft,Fi,Fo>::increment(){
        static subset2_type tmp;
        if( !( this->j()<this->k() ) )
        {
            static const char* msg = "partition: !j<k=%1%";
            throw std::runtime_error(
                ( format( msg ) % this->k() ).str()
            );
        }
        tmp.clear();
        std::copy(
            boost::begin(this->subset1()),
            boost::next(
                boost::begin(this->subset1()),
                this->n()
            ),
            std::back_inserter(tmp)
        );
        this->subset1_.insert(
            boost::end( this->subset1_ ),
            boost::begin( this->subset2() ),
            boost::end( this->subset2() )
        );
        this->subset2_ = tmp;
        ++this->j_;
    }

    // Access
    template<typename U,typename Ft,typename Fi,typename Fo>
    const typename partition<U,Ft,Fi,Fo>::subset1_type&
    partition<U,Ft,Fi,Fo>::subset1()const
    {
        return this->subset1_;
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    const typename partition<U,Ft,Fi,Fo>::subset2_type&
    partition<U,Ft,Fi,Fo>::subset2()const
    {
        return this->subset2_;
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    typename partition<U,Ft,Fi,Fo>::meta_training_range::type
    partition<U,Ft,Fi,Fo>::training_range()const
    {
        return meta_training_range::make(
            this->ft_,
            boost::begin(this->subset1()),
            boost::end(this->subset1())
        );
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    typename partition<U,Ft,Fi,Fo>::meta_input_range::type
    partition<U,Ft,Fi,Fo>::input_range()const
    {
        return meta_input_range::make(
            this->fi_,
            boost::begin(this->subset2()),
            boost::end(this->subset2())
        );
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    typename partition<U,Ft,Fi,Fo>::meta_output_range::type
    partition<U,Ft,Fi,Fo>::output_range()const
    {
        return meta_output_range::make(
            this->fo_,
            boost::begin(this->subset2()),
            boost::end(this->subset2())
        );
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    const typename partition<U,Ft,Fi,Fo>::int_&
    partition<U,Ft,Fi,Fo>::n()const
    {
        return this->n_;
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    const typename partition<U,Ft,Fi,Fo>::int_&
    partition<U,Ft,Fi,Fo>::j()const
    {
        return this->j_;
    }

    template<typename U,typename Ft,typename Fi,typename Fo>
    const typename partition<U,Ft,Fi,Fo>::int_&
    partition<U,Ft,Fi,Fo>::k()const
    {
        return this->k_;
    }

}// k_fold
}// cross_validation
}// detail
}// statistics
}// boost

#endif


#ifndef LATTICE_SQUARELATTICE_CONNECTED_TAGGEDPARTICLE_HPP
#define LATTICE_SQUARELATTICE_CONNECTED_TAGGEDPARTICLE_HPP

#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>

namespace lattice {

class SquareLatticeConnectedTaggedParticle {
public:
  static std::string name() { return "SquareLattice lasso (total must be even)";}
  int set_num_particles(int Ns){return Ns*Ns+1;}
  SquareLatticeConnectedTaggedParticle(unsigned int LatticeSize) : dim_(LatticeSize) {}
  void create_table(std::vector<std::vector < int > >& table){
  //for bulk
    for (int i = 0; i < dim_; ++i){
      for (int j = 0; j < dim_; ++j){
          int l = numberize(i,j);
          table[l][4] = l;
          int temp = 0;
          for(int d = -1; d < 2; d =d+2){
            table[l][temp] = numberize(i+d,j);
            temp += 1;
            table[l][temp] = numberize(i,j+d);
            temp += 1;
          }
        }
      }
   int l = numberize((dim_-1)/2,(dim_-1)/2);
   int tagg = dim_ * dim_ ;
   table[l][4] = tagg;
   table[tagg][0] = l;
   for(int temp = 1; temp < 5 ; ++temp) table[tagg][temp] = tagg;
    }

  int numberize(const int i, const int j){
    int i_t = (i + dim_);
    int j_t = (j + dim_);
    i_t %= dim_ ;
    j_t %= dim_ ;
    return i_t + j_t * dim_;
  }

  int latticize(int d, const int l){ //d is direction(0 ~ ) ex. 0 denotes x, 1 denotes y,~
    int j = l/dim_ ;
    int i = (l - dim_ *j);
    if(d==0) return i;
    else return j;
  }

  int number_adjacent() {return 5;}

private:
  unsigned int dim_;
};

}//end namespace

#endif //LATTICE_SQUARELATTICE_CONNECTED_TAGGEDPARTICLE_HPP


/// \file
/// Maintainer: Luzian Hug
/// Created: 23.04.2018
///

#include "single_cluster.h"

#include <boost/test/unit_test.hpp>
namespace utf = boost::unit_test;

BOOST_AUTO_TEST_CASE(single_cluster) {
  MouseTrack::PointCloud pc;
  pc.resize(10, 0);
  for (int i = 0; i < 10; i += 1) {
    pc[i].y(0);
    pc[i].z(0);
    pc[i].intensity(0);
  }
  pc[0].x(1.0);
  pc[1].x(2.0);
  pc[2].x(2.0);
  pc[3].x(3.0);
  pc[4].x(10.0);
  pc[5].x(11.0);
  pc[6].x(12.0);
  pc[7].x(100.0);
  pc[8].x(101.0);
  pc[9].x(102.0);

  MouseTrack::SingleCluster sc = MouseTrack::SingleCluster();

  std::vector<MouseTrack::Cluster> clusters = sc(pc);

  BOOST_CHECK_EQUAL(clusters.size(), 1);
  BOOST_CHECK_EQUAL(clusters[0].points().size(), pc.size());
}


#include <boost/test/tools/floating_point_comparison.hpp>


/*
 * This file is part of bogus, a C++ sparse block matrix library.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@gmail.com>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#include "Block.hpp"
#include "Block/Streams.hpp"

#ifdef BOGUS_WITH_BOOST_SERIALIZATION
#include <boost/serialization/version.hpp>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/utility.hpp>

#ifndef BOGUS_WITHOUT_EIGEN
#include "Eigen/EigenSerialization.hpp"
#endif

#include "Block/Serialization.hpp"
#endif

#ifndef BOGUS_BLOCK_WITHOUT_EIGEN_SPARSE
#include "Eigen/SparseConversions.hpp"
#endif


// 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.

#include <iostream>
#include <cmath>
#include <complex>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>
#include <boost/numeric/mtl/operation/norms.hpp>
#include <boost/numeric/mtl/operation/sum.hpp>
#include <boost/numeric/mtl/operation/product.hpp>
#include <boost/numeric/mtl/operation/unroll.hpp>

using namespace std;

template <typename Vector, typename Value>
void extra_test(Vector& u, Vector& v, Value)
{
    u+= 4;
    std::cout << "u+= 4 = " << u << "\n";

    u= v + 4;
    std::cout << "u= v + 4 = " << u << "\n";

    u= 4 + v;
    std::cout << "u= 4 + v = " << u << "\n";

    u-= 4;
    std::cout << "u-= 4 = " << u << "\n";

    u= v - 4;
    std::cout << "u= v - 4 = " << u << "\n";

    u= 4 - v;
    std::cout << "u= 4 - v = " << u << "\n";

    v= min(4, u);
    std::cout << "v= min(4, u) = " << v << "\n";

    v= min(u, 4);
    std::cout << "v= min(u, 4) = " << v << "\n";

    v= max(4, u);
    std::cout << "v= max(4, u) = " << v << "\n";

    v= max(u, 4);
    std::cout << "v= max(u, 4) = " << v << "\n";

    u= max(v - 4, 1);
    std::cout << "u= max(v - 4, 3) is " << u << "\n";
}

// not complex
template <typename Vector, typename Value>
void extra_test(Vector&, Vector&, std::complex<Value>) {}

template <typename Vector>
void test(Vector& v, const char* name)
{
    typedef typename mtl::Collection<Vector>::value_type value_type;
    typedef typename mtl::Collection<Vector>::size_type  size_type;

    using mtl::sum; using mtl::product; using mtl::one_norm; using mtl::unroll;

    for (size_type i= 0; i < size(v); i++)
	v[i]= value_type(double(i+1) * pow(-1.0, int(i))); // Amb. in MSVC

    std::cout << "\n" << name << "  --- v = " << v << std::endl;

    Vector w(v + v), u;
    std::cout << "w= v + v = " << w << "\n";

    u= w - v;
    std::cout << "u= w - v = " << u << "\n";

    u= -v;
    std::cout << "u= -v = " << u << "\n";

    unroll<3>(u)= 4. * w - v;
    std::cout << "unroll<3>(u)= 4. * w - v = " << u << "\n";

    extra_test(u, v, u[0]);
}

int main(int, char**)
{
    using namespace mtl;
    using mtl::vec::parameters;

    dense_vector<float>   u(5);
    dense_vector<double>  x(5);
    dense_vector<std::complex<double> >  xc(5);

    std::cout << "Testing vector operations\n";

    test(u, "test float");
    test(x, "test double");

    test(xc, "test complex<double>");

    dense_vector<float, parameters<row_major> >   ur(5);
    test(ur, "test float in row vector");

    // dense_vector<dense_vector<float> > z(5, u), z2(5);
    // z2= 4.5f * z;
    // z2= z + z;

    return 0;
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#include "poplibs_support/ContiguousRegionsByTile.hpp"

#include <cassert>

#if defined(ALTERNATE_IMPLEMENTATION__)
#include <boost/icl/interval_map.hpp>
#include <boost/optional.hpp>
#endif

namespace poplibs {

#if defined(ALTERNATE_IMPLEMENTATION__)

std::vector<std::vector<std::vector<poplar::Interval>>>
getSortedContiguousRegionsByTile(
    const poplar::Graph &graph, const poplar::Tensor &A,
    const poplar::Graph::TileToTensorMapping &mapping) {

  // Get the sorted contiguous regions up-front.
  const auto scrs = graph.getSortedContiguousRegions(A, {{0, A.numElements()}});

  // Convert the mapping to boost ICL format.
  auto mappingIcl = tileMappingToIntervalMap(mapping);

  // Make sure that the mapping is complete. This should always be the case
  // since getTileMapping() throws an exception if it isn't (by default).
  assert(mappingIcl.size() == A.numElements());

  // The result - a set of sorted contiguous regions for each tile.
  std::vector<std::vector<std::vector<poplar::Interval>>> out(mapping.size());

  // For each sorted contiguous region:
  for (const auto &scr : scrs) {
    if (scr.empty())
      continue;

    // The tile that the last bit of this contiguous region was mapped to.
    boost::optional<unsigned> lastTile;

    // For each tensor region in this contiguous variable region:
    for (const auto &re : scr) {
      if (re.size() == 0)
        continue;

      // Find the parts of this region that are mapped to different tiles.
      // It is possible to iterate through `mappingIcl & re` but that is slow.

      auto begin = mappingIcl.find(re.begin());
      auto end = mappingIcl.find(re.end());

      // For each part on a different tile:
      for (auto it = begin; it != end; ++it) {
        // Get the region.
        auto lower = std::max(it->first.lower(), re.begin());
        auto upper = std::min(it->first.upper(), re.end());

        // The tile that this part is mapped to.
        const auto &tile = it->second;

        // If it is the first part of this contiguous region, or if the
        // last part was on a different tile, start a new contiguous region
        // on this tile.
        if (!lastTile || tile != lastTile) {
          lastTile = tile;
          out[tile].emplace_back();
        }

        // And append it to the current contiguous region.
        out[tile].back().emplace_back(lower, upper);
      }
    }
  }

  return out;
}

#else

std::vector<std::vector<std::vector<poplar::Interval>>>
getSortedContiguousRegionsByTile(
    const poplar::Graph &graph, const poplar::Tensor &A,
    const poplar::Graph::TileToTensorMapping &mapping) {

  std::vector<std::vector<std::vector<poplar::Interval>>>
      contiguousRegionsByTile;

  for (const auto &m : mapping) {
    contiguousRegionsByTile.emplace_back(
        graph.getSortedContiguousRegions(A, m));
  }

  return contiguousRegionsByTile;
}

#endif

} // namespace poplibs


#pragma once

#include <boost/container/pmr/monotonic_buffer_resource.hpp>
#include <boost/container/scoped_allocator.hpp>
#include <unordered_set>

#include "base_column_statistics.hpp"
#include "column_statistics.hpp"
#include "resolve_type.hpp"
#include "storage/segment_iterate.hpp"
#include "storage/table.hpp"

namespace opossum {

/**
 * Generate the statistics of a single column. Used by generate_table_statistics()
 */
template <typename ColumnDataType>
std::shared_ptr<BaseColumnStatistics> generate_column_statistics(const Table& table, const ColumnID column_id) {
  // distinct_set is thrown away at the end of this method, so we don't want proper heap allocations for the strings
  // stored within. The initial size of the buffer is a completely random guess, but better than zero.

  auto temp_buffer = boost::container::pmr::monotonic_buffer_resource(table.row_count() * sizeof(ColumnDataType));
  auto distinct_set =
      std::unordered_set<ColumnDataType, std::hash<ColumnDataType>, std::equal_to<ColumnDataType>,
                         PolymorphicAllocator<ColumnDataType>>(PolymorphicAllocator<ColumnDataType>{&temp_buffer});
  distinct_set.reserve(table.row_count());

  auto null_value_count = size_t{0};

  auto min = std::numeric_limits<ColumnDataType>::max();
  auto max = std::numeric_limits<ColumnDataType>::lowest();

  for (ChunkID chunk_id{0}; chunk_id < table.chunk_count(); ++chunk_id) {
    const auto base_segment = table.get_chunk(chunk_id)->get_segment(column_id);

    segment_iterate<ColumnDataType>(*base_segment, [&](const auto& position) {
      if (position.is_null()) {
        ++null_value_count;
      } else {
        distinct_set.insert(position.value());
        min = std::min(min, position.value());
        max = std::max(max, position.value());
      }
    });
  }

  const auto null_value_ratio =
      table.row_count() > 0 ? static_cast<float>(null_value_count) / static_cast<float>(table.row_count()) : 0.0f;
  const auto distinct_count = static_cast<float>(distinct_set.size());

  if (distinct_count == 0.0f) {
    min = std::numeric_limits<ColumnDataType>::min();
    max = std::numeric_limits<ColumnDataType>::max();
  }

  return std::make_shared<ColumnStatistics<ColumnDataType>>(null_value_ratio, distinct_count, min, max);
}

template <>
std::shared_ptr<BaseColumnStatistics> generate_column_statistics<std::string>(const Table& table,
                                                                              const ColumnID column_id);

}  // namespace opossum


#include <iostream>

#include <harp_mpi_test.hpp>

#include <boost/random.hpp>

using namespace std;
using namespace harp;

#define SIZE 50
#define SPECSIZE 100
#define NSPEC 10
#define MAX 1000.0

void harp::mpi_test_linalg ( string const & datadir ) {

  boost::mpi::communicator comm;
  El::mpi::Comm el_comm ( comm );

  int np = comm.size();
  int myp = comm.rank();

  const El::Grid grid ( el_comm );

  typedef boost::ecuyer1988 base_generator_type;
  typedef boost::uniform_01<> distribution_type;
  typedef boost::variate_generator < base_generator_type&, distribution_type > gen_type;

  base_generator_type generator(42u);
  gen_type gen ( generator, distribution_type() );

  if ( myp == 0 ) {
    cout << "Testing elemental / ublas conversions..." << endl;
  }

  vector_double intest ( SIZE );
  vector_double outtest ( SIZE );

  for ( size_t i = 0; i < SIZE; ++i ) {
    intest[i] = gen();
  }

  elem_matrix_local outtest_local ( SIZE, 1 );

  ublas_to_elem ( intest, outtest_local );

  elem_to_ublas ( outtest_local, outtest );

  for ( size_t i = 0; i < SIZE; ++i ) {
    if ( fabs ( (outtest[i] - intest[i]) / intest[i] ) > std::numeric_limits < float > :: epsilon() ) {
      cerr << "FAIL on proc " << myp << ", local vector element " << i << ", " << outtest[i] << " != " << intest[i] << endl;
      exit(1);
    }
  }

  mpi_matrix outtest_mpi ( SIZE, 1, grid );

  ublas_to_elem ( intest, outtest_mpi );

  elem_to_ublas ( outtest_mpi, outtest );

  for ( size_t i = 0; i < SIZE; ++i ) {
    if ( fabs ( (outtest[i] - intest[i]) / intest[i] ) > std::numeric_limits < float > :: epsilon() ) {
      cerr << "FAIL on proc " << myp << ", mpi vector element " << i << ", " << outtest[i] << " != " << intest[i] << endl;
      exit(1);
    }
  }

  if ( myp == 0 ) {
    cout << "  (PASSED)" << endl;
  }

  if ( myp == 0 ) {
    cout << "Testing elemental collective Set()..." << endl;
  }

  mpi_matrix settest ( SIZE, 1 );

  for ( size_t i = 0; i < SIZE; ++i ) {
    settest.Set ( i, 0, (double)i );
  }

  size_t hlocal = settest.LocalHeight();
  size_t wlocal = settest.LocalWidth();

  size_t rowoff = settest.ColShift();
  size_t rowstride = settest.ColStride();
  size_t row;

  if ( wlocal > 0 ) {
    for ( size_t j = 0; j < hlocal; ++j ) {
      // the global element of sub_data
      row = rowoff + j * rowstride;
      double myval = settest.GetLocal ( j, 0 );
      if ( fabs ( myval ) > std::numeric_limits < double > :: epsilon() ) {
        if ( fabs ( (myval - (double)row) / (double)row ) > std::numeric_limits < float > :: epsilon() ) {
          cerr << "FAIL on proc " << myp << ", global vector element " << row << ", " << myval << " != " << (double)row << endl;
          exit(1);
        }
      }
    }
  }

  if ( myp == 0 ) {
    cout << "  (PASSED)" << endl;
  }

  if ( myp == 0 ) {
    cout << "Testing elemental collective Get()..." << endl;
  }

  mpi_matrix gettest ( SIZE, 1 );

  hlocal = gettest.LocalHeight();
  wlocal = gettest.LocalWidth();

  rowoff = gettest.ColShift();
  rowstride = gettest.ColStride();

  //cerr << "   proc " << gettest.Grid().Rank() << " has local storage " << hlocal << " x " << wlocal << endl;

  if ( wlocal > 0 ) {
    for ( size_t j = 0; j < hlocal; ++j ) {
      // the global element of sub_data
      row = rowoff + j * rowstride;
      gettest.SetLocal ( j, 0, (double)row );
    }
  }

  vector_double gettest_local ( SIZE );
  elem_to_ublas ( gettest, gettest_local );

  for ( size_t i = 0; i < SIZE; ++i ) {
    double myval = gettest.Get ( i, 0 );
    //cerr << "p(" << myp << ")[" << i << "] = " << myval << endl;
    if ( fabs ( gettest_local[i] ) > std::numeric_limits < double > :: epsilon() ) {
      if ( fabs ( (myval - gettest_local[i]) / gettest_local[i] ) > std::numeric_limits < float > :: epsilon() ) {
        cerr << "FAIL on proc " << myp << ", local vector element " << i << ", " << myval << " != " << gettest_local[i] << endl;
        exit(1);
      }
    }
  }

  if ( myp == 0 ) {
    cout << "  (PASSED)" << endl;
  }

  if ( myp == 0 ) {
    cout << "Testing elemental eigendecomposition..." << endl;
  }

  cerr.precision(16);

  // construct random matrices

  mpi_matrix a1 ( SIZE, SIZE, grid );
  mpi_matrix a2 ( SIZE, SIZE, grid );

  for ( size_t i = 0; i < SIZE; ++i ) {
    for ( size_t j = 0; j < SIZE; ++j ) {
      a1.Set( i, j, MAX * gen() );
      a2.Set( i, j, MAX * gen() );
    }
  }

  // construct symmetric test matrix

  mpi_matrix sym ( SIZE, SIZE, grid );

  El::Gemm ( El::TRANSPOSE, El::NORMAL, 1.0, a1, a1, 0.0, sym );
  El::Gemm ( El::TRANSPOSE, El::NORMAL, 1.0, a2, a2, 1.0, sym );

  // get eigenvectors and eigenvalues

  mpi_matrix w ( SIZE, 1, grid );
  mpi_matrix Z ( SIZE, SIZE, grid );

  mpi_eigen_decompose ( sym, w, Z, false );

  mpi_matrix symprod ( SIZE, SIZE, grid );
  mpi_matrix eprod ( SIZE, SIZE, grid );
  mpi_matrix wdiag ( SIZE, SIZE, grid );

  mpi_matrix_zero ( wdiag );
  for ( size_t i = 0; i < SIZE; ++i ) {
    double wval = w.Get(i,0);
    wdiag.Set ( i, i, wval );
  }

  El::Gemm ( El::NORMAL, El::NORMAL, 1.0, sym, Z, 0.0, symprod );
  El::Gemm ( El::NORMAL, El::NORMAL, 1.0, Z, wdiag, 0.0, eprod );

  double relerr;
  double eval, sval;

  for ( size_t i = 0; i < SIZE; ++i ) {
    for ( size_t j = 0; j < SIZE; ++j ) {
      eval = eprod.Get ( j, i );
      sval = symprod.Get ( j, i );
      relerr = fabs ( ( eval - sval ) / sval );
      if ( relerr > std::numeric_limits < float > :: epsilon() ) {
        cerr << "FAIL on matrix element (" << j << ", " << i << ") Av = " << sval << ", ev = " << eval << " rel err = " << relerr << endl;
        exit(1);
      }
    }
  }

  if ( myp == 0 ) {
    cout << "  (PASSED)" << endl;
  }

  if ( myp == 0 ) {
    cout << "Testing re-composition..." << endl;
  }

  mpi_matrix outcomp ( Z );

  mpi_eigen_compose ( EIG_NONE, w, Z, outcomp );

  double inval;
  double outval;

  for ( size_t i = 0; i < SIZE; ++i ) {
    for ( size_t j = 0; j < SIZE; ++j ) {
      inval = sym.Get ( j, i );
      outval = outcomp.Get ( j, i );
      relerr = fabs ( ( outval - inval ) / inval );
      if ( relerr > std::numeric_limits < float > :: epsilon() ) {
        cerr << "FAIL on matrix element (" << j << ", " << i << ") input = " << inval << ", output = " << outval << " rel err = " << relerr << endl;
        exit(1);
      }
    }
  }

  mpi_matrix mat_rt ( Z );
  mpi_eigen_compose ( EIG_SQRT, w, Z, mat_rt );

  mpi_matrix mat_invrt ( Z );
  mpi_eigen_compose ( EIG_INVSQRT, w, Z, mat_invrt );

  mpi_matrix w_inv ( w );
  mpi_matrix Z_inv ( Z );

  mpi_eigen_decompose ( mat_invrt, w_inv, Z_inv, false );

  for ( size_t i = 0; i < SIZE; ++i ) {
    double val = w_inv.Get(i,0);
    val *= val;
    w_inv.Set ( i, 0, val );
  }

  mpi_matrix comp_rt ( mat_rt );

  mpi_eigen_compose ( EIG_INVSQRT, w_inv, Z_inv, comp_rt );

  for ( size_t i = 0; i < SIZE; ++i ) {
    for ( size_t j = 0; j < SIZE; ++j ) {
      inval = mat_rt.Get ( j, i );
      outval = comp_rt.Get ( j, i );
      relerr = fabs ( outval - inval ) / inval;
      if ( relerr > std::numeric_limits < float > :: epsilon() ) {
        cerr << "FAIL on doubly inverted sqrt matrix element (" << j << ", " << i << ") original = " << inval << ", output = " << outval << " rel err = " << relerr << endl;
        exit(1);
      }
    }
  }

  if ( myp == 0 ) {
    cout << "  (PASSED)" << endl;
  }

  if ( myp == 0 ) {
    cout << "Testing gang-parallel (re)distribution..." << endl;
  }

  int gangsize = (int)( np / 2 );
  if ( gangsize < 1 ) {
    gangsize = 1;
  }

  int ngang = (int)( np / gangsize );
  int gangtot = ngang * gangsize;
  if ( myp == 0 ) {
    cout << "  Using " << ngang << " gangs of " << gangsize << " processes each" << endl;
  }
  if ( gangtot < np ) {
    if ( myp == 0 ) {
      cout << "  WARNING: " << (np-gangtot) << " processes are idle" << endl;
    }
  }
  int gang = (int)( myp / gangsize );
  int grank = myp % gangsize;
  if ( gang >= ngang ) {
    gang = MPI_UNDEFINED;
    grank = MPI_UNDEFINED;
  }

  boost::mpi::communicator gcomm = comm.split ( gang, grank );
  El::mpi::Comm el_gcomm ( gcomm );

  // create gang process grids

  const El::Grid gang_grid ( el_gcomm );

  mpi_matrix redist_comp ( outcomp );
  mpi_matrix gout_comp ( SIZE, SIZE, gang_grid );

  mpi_gang_distribute ( outcomp, gout_comp );

  mpi_gang_accum ( gout_comp, redist_comp );

  for ( size_t i = 0; i < SIZE; ++i ) {
    for ( size_t j = 0; j < SIZE; ++j ) {
      inval = (double)ngang * outcomp.Get ( j, i );
      outval = redist_comp.Get ( j, i );
      relerr = fabs ( outval - inval ) / inval;
      if ( relerr > std::numeric_limits < float > :: epsilon() ) {
        cerr << "FAIL on matrix element (" << j << ", " << i << ") input = " << inval << ", output = " << outval << " rel err = " << relerr << endl;
        exit(1);
      }
    }
  }

  if ( myp == 0 ) {
    cout << "  (PASSED)" << endl;
  }

  return;
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_SCAL_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_SCAL_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/has_linear_array.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>

//
// The BLAS-backend is selected by defining a pre-processor variable,
//  which can be one of
// * for CBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
// * for CUBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
// * netlib-compatible BLAS is the default
//
#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
#include <boost/numeric/bindings/blas/detail/cblas.h>
#include <boost/numeric/bindings/blas/detail/cblas_option.hpp>
#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
#include <boost/numeric/bindings/blas/detail/cublas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#else
#include <boost/numeric/bindings/blas/detail/blas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#endif

namespace boost {
namespace numeric {
namespace bindings {
namespace blas {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end BLAS-routine.
//
namespace detail {

#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * float value-type.
//
inline void scal( const int n, const float a, float* x, const int incx ) {
    cblas_sscal( n, a, x, incx );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * double value-type.
//
inline void scal( const int n, const double a, double* x, const int incx ) {
    cblas_dscal( n, a, x, incx );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * combined float and complex<float> value-type.
//
inline void scal( const int n, const float a, std::complex<float>* x,
        const int incx ) {
  cblas_csscal( n, a, OPENBLAS_FLOAT_CAST(x), incx );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * combined double and complex<double> value-type.
//
inline void scal( const int n, const double a, std::complex<double>* x,
        const int incx ) {
  cblas_zdscal( n, a, OPENBLAS_DOUBLE_CAST(x), incx );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<float> value-type.
//
inline void scal( const int n, const std::complex<float> a,
        std::complex<float>* x, const int incx ) {
  cblas_cscal( n, OPENBLAS_CONST_FLOAT_CAST(&a), OPENBLAS_FLOAT_CAST(x), incx );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<double> value-type.
//
inline void scal( const int n, const std::complex<double> a,
        std::complex<double>* x, const int incx ) {
  cblas_zscal( n, OPENBLAS_CONST_DOUBLE_CAST(&a), OPENBLAS_DOUBLE_CAST(x), incx );
}

#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * float value-type.
//
inline void scal( const int n, const float a, float* x, const int incx ) {
    cublasSscal( n, a, x, incx );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * double value-type.
//
inline void scal( const int n, const double a, double* x, const int incx ) {
    cublasDscal( n, a, x, incx );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * combined float and complex<float> value-type.
//
inline void scal( const int n, const float a, std::complex<float>* x,
        const int incx ) {
    cublasCsscal( n, a, x, incx );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * combined double and complex<double> value-type.
//
inline void scal( const int n, const double a, std::complex<double>* x,
        const int incx ) {
    cublasZdscal( n, a, x, incx );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<float> value-type.
//
inline void scal( const int n, const std::complex<float> a,
        std::complex<float>* x, const int incx ) {
    cublasCscal( n, a, x, incx );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<double> value-type.
//
inline void scal( const int n, const std::complex<double> a,
        std::complex<double>* x, const int incx ) {
    cublasZscal( n, a, x, incx );
}

#else
//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * float value-type.
//
inline void scal( const fortran_int_t n, const float a, float* x,
        const fortran_int_t incx ) {
    BLAS_SSCAL( &n, &a, x, &incx );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * double value-type.
//
inline void scal( const fortran_int_t n, const double a, double* x,
        const fortran_int_t incx ) {
    BLAS_DSCAL( &n, &a, x, &incx );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * combined float and complex<float> value-type.
//
inline void scal( const fortran_int_t n, const float a,
        std::complex<float>* x, const fortran_int_t incx ) {
    BLAS_CSSCAL( &n, &a, x, &incx );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * combined double and complex<double> value-type.
//
inline void scal( const fortran_int_t n, const double a,
        std::complex<double>* x, const fortran_int_t incx ) {
    BLAS_ZDSCAL( &n, &a, x, &incx );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<float> value-type.
//
inline void scal( const fortran_int_t n, const std::complex<float> a,
        std::complex<float>* x, const fortran_int_t incx ) {
    BLAS_CSCAL( &n, &a, x, &incx );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<double> value-type.
//
inline void scal( const fortran_int_t n, const std::complex<double> a,
        std::complex<double>* x, const fortran_int_t incx ) {
    BLAS_ZSCAL( &n, &a, x, &incx );
}

#endif

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to scal.
//
template< typename Value >
struct scal_impl {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;
    typedef void result_type;

    //
    // Static member function that
    // * Deduces the required arguments for dispatching to BLAS, and
    // * Asserts that most arguments make sense.
    //
    template< typename ScalarA, typename VectorX >
    static result_type invoke( const ScalarA a, VectorX& x ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::has_linear_array< VectorX >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorX >::value) );
        detail::scal( bindings::size(x), a, bindings::begin_value(x),
                bindings::stride(x) );
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. Calls
// to these functions are passed to the scal_impl classes. In the
// documentation, the const-overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for scal. Its overload differs for
//
template< typename ScalarA, typename VectorX >
inline typename scal_impl< typename bindings::value_type<
        VectorX >::type >::result_type
scal( const ScalarA a, VectorX& x ) {
    scal_impl< typename bindings::value_type<
            VectorX >::type >::invoke( a, x );
}

} // namespace blas
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#include "aikido/planner/vectorfield/VectorFieldPlanner.hpp"

#include <boost/numeric/odeint.hpp>

#include "aikido/constraint/TestableIntersection.hpp"
#include "aikido/constraint/dart/JointStateSpaceHelpers.hpp"
#include "aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp"
#include "aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp"
#include "aikido/planner/vectorfield/VectorFieldUtil.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp"
#include "aikido/trajectory/Spline.hpp"

#include "detail/VectorFieldIntegrator.hpp"
#include "detail/VectorFieldPlannerExceptions.hpp"

using aikido::planner::vectorfield::MoveEndEffectorOffsetVectorField;
using aikido::planner::vectorfield::MoveEndEffectorPoseVectorField;
using aikido::planner::vectorfield::detail::VectorFieldIntegrator;
using aikido::statespace::dart::MetaSkeletonStateSaver;

namespace aikido {
namespace planner {
namespace vectorfield {

constexpr double integrationTimeInterval = 10.0;

//==============================================================================
aikido::trajectory::UniqueInterpolatedPtr followVectorField(
    const aikido::planner::vectorfield::VectorField& vectorField,
    const aikido::statespace::StateSpace::State& startState,
    const aikido::constraint::Testable& constraint,
    std::chrono::duration<double> timelimit,
    double initialStepSize,
    double checkConstraintResolution,
    planner::Planner::Result* result)
{
  using namespace std::placeholders;
  using errorStepper = boost::numeric::odeint::runge_kutta_dopri5<
      Eigen::VectorXd,
      double,
      Eigen::VectorXd,
      double,
      boost::numeric::odeint::vector_space_algebra>;

  std::size_t dimension = vectorField.getStateSpace()->getDimension();
  auto integrator = std::make_shared<detail::VectorFieldIntegrator>(
      &vectorField, &constraint, timelimit.count(), checkConstraintResolution);

  integrator->start();

  try
  {
    Eigen::VectorXd initialQ(dimension);
    vectorField.getStateSpace()->logMap(&startState, initialQ);
    // The current implementation works only in real vector spaces.

    // Integrate the vector field to get a configuration space path.
    boost::numeric::odeint::integrate_adaptive(
        errorStepper(),
        std::bind(
            &detail::VectorFieldIntegrator::step,
            integrator,
            std::placeholders::_1,
            std::placeholders::_2,
            std::placeholders::_3),
        initialQ,
        0.,
        integrationTimeInterval,
        initialStepSize,
        std::bind(
            &detail::VectorFieldIntegrator::check,
            integrator,
            std::placeholders::_1,
            std::placeholders::_2));
  }
  // VectorFieldTerminated is an exception that is raised internally to
  // terminate
  // integration, which does not indicate that an error has occurred.
  catch (const detail::VectorFieldTerminated& e)
  {
    if (result)
    {
      result->setMessage(e.what());
    }
  }
  catch (const detail::VectorFieldError& e)
  {
    dtwarn << e.what() << std::endl;
    if (result)
    {
      result->setMessage(e.what());
    }
    return nullptr;
  }

  if (integrator->getCacheIndex() <= 1)
  {
    // no enough waypoints cached to make a trajectory output.
    if (result)
    {
      result->setMessage("No segment cached.");
    }
    return nullptr;
  }

  std::vector<detail::Knot> newKnots(
      integrator->getKnots().begin(),
      integrator->getKnots().begin() + integrator->getCacheIndex());
  // See https://github.com/personalrobotics/aikido/issues/512
  // auto outputTrajectory
  //     = detail::convertToSpline(newKnots, vectorField.getStateSpace());
  auto outputTrajectory
      = detail::convertToInterpolated(newKnots, vectorField.getStateSpace());

  // evaluate constraint satisfaction on last piece of trajectory
  double lastEvaluationTime = integrator->getLastEvaluationTime();
  if (outputTrajectory->getEndTime() > lastEvaluationTime)
  {
    if (!vectorField.evaluateTrajectory(
            *outputTrajectory,
            &constraint,
            checkConstraintResolution,
            lastEvaluationTime,
            true))
    {
      result->setMessage("Constraint violated.");
      return nullptr;
    }
  }
  return outputTrajectory;
}

//==============================================================================
aikido::trajectory::UniqueInterpolatedPtr planToEndEffectorOffset(
    const aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr& stateSpace,
    const statespace::dart::MetaSkeletonStateSpace::State& startState,
    dart::dynamics::MetaSkeletonPtr metaskeleton,
    const dart::dynamics::ConstBodyNodePtr& bn,
    const aikido::constraint::ConstTestablePtr& constraint,
    const Eigen::Vector3d& direction,
    double minDistance,
    double maxDistance,
    double positionTolerance,
    double angularTolerance,
    double initialStepSize,
    double jointLimitTolerance,
    double constraintCheckResolution,
    std::chrono::duration<double> timelimit,
    planner::Planner::Result* result)
{
  // ensure that no two planners run at the same time
  if (metaskeleton->getNumBodyNodes() == 0)
  {
    throw std::runtime_error("MetaSkeleton doesn't have any body nodes.");
  }
  auto robot = metaskeleton->getBodyNode(0)->getSkeleton();
  std::lock_guard<std::mutex> lock(robot->getMutex());
  // TODO(JS): The above code should be replaced by
  // std::lock_guard<std::mutex> lock(metaskeleton->getLockableReference())
  // once https://github.com/dartsim/dart/pull/1011 is released.

  // TODO: Check compatibility between MetaSkeleton and MetaSkeletonStateSpace

  // Save the current state of the space
  auto saver = MetaSkeletonStateSaver(
      metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS);
  DART_UNUSED(saver);

  stateSpace->setState(metaskeleton.get(), &startState);

  auto vectorfield
      = dart::common::make_aligned_shared<MoveEndEffectorOffsetVectorField>(
          stateSpace,
          metaskeleton,
          bn,
          direction,
          minDistance,
          maxDistance,
          positionTolerance,
          angularTolerance,
          initialStepSize,
          jointLimitTolerance);

  auto compoundConstraint
      = std::make_shared<constraint::TestableIntersection>(stateSpace);
  compoundConstraint->addConstraint(constraint);
  compoundConstraint->addConstraint(
      constraint::dart::createTestableBounds(stateSpace));
  return followVectorField(
      *vectorfield,
      startState,
      *compoundConstraint,
      timelimit,
      initialStepSize,
      constraintCheckResolution,
      result);
}

//==============================================================================
aikido::trajectory::UniqueInterpolatedPtr planToEndEffectorPose(
    const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
    dart::dynamics::MetaSkeletonPtr metaskeleton,
    const dart::dynamics::BodyNodePtr& bn,
    const aikido::constraint::TestablePtr& constraint,
    const Eigen::Isometry3d& goalPose,
    double poseErrorTolerance,
    double conversionRatioInGeodesicDistance,
    double initialStepSize,
    double jointLimitTolerance,
    double constraintCheckResolution,
    std::chrono::duration<double> timelimit,
    planner::Planner::Result* result)
{
  // TODO: Check compatibility between MetaSkeleton and MetaSkeletonStateSpace

  // Save the current state of the space
  auto saver = MetaSkeletonStateSaver(
      metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS);
  DART_UNUSED(saver);

  auto vectorfield
      = dart::common::make_aligned_shared<MoveEndEffectorPoseVectorField>(
          stateSpace,
          metaskeleton,
          bn,
          goalPose,
          poseErrorTolerance,
          conversionRatioInGeodesicDistance,
          initialStepSize,
          jointLimitTolerance);

  auto compoundConstraint
      = std::make_shared<aikido::constraint::TestableIntersection>(stateSpace);
  compoundConstraint->addConstraint(constraint);
  compoundConstraint->addConstraint(
      constraint::dart::createTestableBounds(stateSpace));

  auto startState
      = stateSpace->getScopedStateFromMetaSkeleton(metaskeleton.get());
  return followVectorField(
      *vectorfield,
      *startState,
      *compoundConstraint,
      timelimit,
      initialStepSize,
      constraintCheckResolution,
      result);
}

} // namespace vectorfield
} // namespace planner
} // namespace aikido


#pragma once
#ifndef _pow_observatory_hpp
#define _pow_observatory_hpp

#include "galaxy.hpp"
#include "camera.hpp"
#include "siphash.hpp"
#include "dipper_detector.hpp"
#include "checked_cast.hpp"
#include <boost/thread.hpp>
#include <fstream>
#include <math.h>

#ifndef DIPPER_DONT_USE_NAMESPACE
namespace prologcoin { namespace pow {
#endif

template<size_t N, typename T> class observatory {
public:
    inline observatory(const siphash_keys &keys) : keys_(keys), galaxy_(keys_) { init(); }

    void init(size_t num_stars = 0);

    inline void set_keys(const siphash_keys &keys) {
	keys_ = keys;
	galaxy_.clear();
	init();
    }

    inline const siphash_keys & keys() const {
	return keys_;
    }

    void set_target(const vec3<T> &v, size_t cam_id = 0);
    const vec3<T> & get_target(size_t cam_id = 0) const;
    void set_target(uint64_t nonce_offset, uint32_t nonce, size_t cam_id = 0);

    size_t new_camera();

    inline star get_star(uint32_t id) const {
	uint64_t out[3];
	siphash(keys_, checked_cast<uint64_t>(3*id),
		       checked_cast<uint64_t>(3*id+3), out);
	return star(id, out[0], out[1], out[2]);
    }

    void take_picture(std::vector<projected_star> &stars, size_t cam_id = 0) const;

    bool scan(uint64_t nonce_offset, projected_star &first_visible, std::vector<projected_star> &found, uint32_t &nonce);

    void status() const;
    void memory() const;

    size_t num_buckets() const;
    T step_vector_length() const;

private:
    inline galaxy<N, T> & get_galaxy() {
	return galaxy_;
    }

    inline camera<N, T> & get_camera(size_t id = 0) {
	return cameras_[id];
    }

    inline const galaxy<N, T> & get_galaxy() const {
	return galaxy_;
    }

    inline const camera<N, T> & get_camera(size_t id = 0) const {
	return cameras_[id];
    }

    siphash_keys keys_;
    galaxy<N, T> galaxy_;
    std::vector<camera<N, T> > cameras_;
};

template<size_t N, typename T> void observatory<N,T>::init(size_t num_stars)
{
    if (num_stars == 0) {
	galaxy_.init();
    } else {
	galaxy_.init(num_stars);
    }
    cameras_.clear();
    new_camera(); // Default camera id=0
}

template<size_t N, typename T> size_t observatory<N,T>::new_camera() {
    size_t id = cameras_.size();
    camera<N, T> cam(galaxy_, id);
    cameras_.push_back(cam);
    return id;
}

template<size_t N, typename T> size_t observatory<N,T>::num_buckets() const {
    return get_galaxy().num_buckets;
}

template<size_t N, typename T> T observatory<N,T>::step_vector_length() const {
    return get_galaxy().step_vector_length();
}

template<size_t N, typename T> void observatory<N,T>::status() const {
    get_galaxy().status();
}

template<size_t N, typename T> void observatory<N,T>::memory() const {
    get_galaxy().memory();
}

template<size_t N, typename T> void observatory<N,T>::set_target(const vec3<T> &v, size_t cam_id)
{
    get_camera(cam_id).set_target(v);
}

template<size_t N, typename T> void observatory<N,T>::set_target(uint64_t nonce_offset, uint32_t nonce, size_t cam_id)
{
    get_camera(cam_id).set_target(nonce_offset, nonce);
}

template<size_t N, typename T> const vec3<T> & observatory<N,T>::get_target(size_t cam_id) const
{
    return get_camera(cam_id).get_target();
}

template<size_t N, typename T> void observatory<N,T>::take_picture(std::vector<projected_star> &stars, size_t cam_id) const
{
    get_camera(cam_id).take_picture(stars);
}

template<size_t N, typename T> class worker_pool;

template<size_t N, typename T> class worker {
public:
    worker(worker_pool<N,T> &workers);

    worker(const worker &other) = delete;
    void operator = (const worker &other) = delete;

    inline void join() {

    }

    inline void kill() {
	killed_ = true;
	set_idle(false);
    }

    inline void wait_idle() const {
    }

    inline void set_idle(bool r) {
	boost::unique_lock<boost::mutex> lockit(idle_lock_);
	idle_ = r;
	idle_cv_.notify_one();
    }

    inline bool is_done() const {
	return found_done_;
    }

    inline void set_nonce_range(size_t nonce_offset, size_t nonce_start, size_t nonce_end) {
	nonce_offset_ = nonce_offset;
	nonce_ = nonce_start;
	nonce_start_ = nonce_start;
	nonce_end_ = nonce_end;
	found_done_ = false;
	has_first_visible_ = false;
	first_visible_.clear();
    }

    inline size_t nonce_start() const {
        return nonce_start_;
    }

    inline size_t nonce_end() const {
        return nonce_end_;
    }

    inline size_t nonce() const {
	return nonce_;
    }

    inline const projected_star & first_visible() const {
        return first_visible_;
    }

    inline bool has_first_visible() const {
        return has_first_visible_;
    }

    void set_target(uint64_t nonce_offset, uint32_t nonce);
    void take_picture();
    void run();

    inline const std::vector<projected_star> & get_found() const {
	return found_;
    }

private:
    worker_pool<N,T> &workers_;
    std::vector<projected_star> stars_;
    std::vector<projected_star> found_;
    projected_star first_visible_;
    bool has_first_visible_;
    dipper_detector detector_;
    size_t cam_id_;
    bool idle_;
    bool killed_;
    boost::mutex idle_lock_;
    boost::condition_variable idle_cv_;
    size_t nonce_offset_, nonce_, nonce_start_, nonce_end_;
    bool found_done_;
};

template<size_t N, typename T> class worker_pool {
public:
    static const size_t DEFAULT_NUM_WORKERS = 16;

    worker_pool(observatory<N,T> &obs, size_t num_workers = DEFAULT_NUM_WORKERS) : observatory_(obs), num_workers_(num_workers), busy_count_(0) {
	smallest_nonce_ = std::numeric_limits<size_t>::max();
	for (size_t i = 0; i < num_workers_; i++) {
	    auto *w = new worker<N,T>(*this);
	    all_workers_.push_back(w);
	    ready_workers_.push_back(w);
	}
	for (auto *w : ready_workers_) {
	    threads_.create_thread( [=]{w->run();} );
	}
    }

    void kill_all_workers() {
	std::vector<worker<N,T> *> all_workers;
	{
	    boost::unique_lock<boost::mutex> lockit(workers_lock_);
	    for (auto *w : all_workers_) {
		all_workers.push_back(w);
	    }
	}
	for (auto *w : all_workers) {
	    w->kill();
	}
    }

    size_t smallest_nonce() {
	boost::unique_lock<boost::mutex> lockit(workers_lock_);
	return smallest_nonce_;
    }

    void found_nonce(size_t nonce) {
	boost::unique_lock<boost::mutex> lockit(workers_lock_);
	if (nonce < smallest_nonce_) {
	    smallest_nonce_ = nonce;
	}
    }

    worker<N,T> & find_ready_worker() {
	boost::unique_lock<boost::mutex> lockit(workers_lock_);
	while (busy_count_ == num_workers_) {
	    workers_cv_.wait(lockit);
	}
	worker<N,T> *w = ready_workers_.back();
	ready_workers_.pop_back();
	busy_count_++;
	return *w;
    }

    void wait_until_no_more_busy_workers() {
	{
	    boost::unique_lock<boost::mutex> lockit(workers_lock_);
	    while (busy_count_ != 0) {
		workers_cv_.wait(lockit);
	    }
	}
	threads_.join_all();
    }

    worker<N,T> * find_successful_worker() {
	worker<N,T> *best_worker = nullptr;
	for (auto *worker : all_workers_) {
	    if (worker->is_done()) {
		if (best_worker == nullptr || worker->nonce() < best_worker->nonce()) {
		    best_worker = worker;
		}
	    }
	}
	return best_worker;
    }

    worker<N,T> * find_first_nonce_worker() {
	for (auto *worker : all_workers_) {
	    if (worker->has_first_visible()) {
	        return worker;
	    }
	}
	return nullptr;
    }

    void add_ready_worker(worker<N,T> *w) {
	boost::unique_lock<boost::mutex> lockit(workers_lock_);
	busy_count_--;
	ready_workers_.push_back(w);
	workers_cv_.notify_all();
    }

private:
    observatory<N,T> &observatory_;
    size_t num_workers_;
    std::vector<worker<N,T> *> all_workers_;
    std::vector<worker<N,T> *> ready_workers_;
    boost::mutex workers_lock_;
    boost::condition_variable workers_cv_;
    size_t busy_count_;
    boost::thread_group threads_;
    size_t smallest_nonce_;

    friend class worker<N,T>;
};

template<size_t N, typename T> worker<N,T>::worker(worker_pool<N,T> &workers)
    : workers_(workers), has_first_visible_(false), detector_(stars_), idle_(true), killed_(false), nonce_offset_(0), nonce_(0), nonce_end_(0), found_done_(false) {
    cam_id_ = workers_.observatory_.new_camera();
}

template<size_t N, typename T> void worker<N,T>::set_target(uint64_t nonce_offset, uint32_t nonce) {
    workers_.observatory_.set_target(nonce_offset, nonce, cam_id_);
}

template<size_t N, typename T> void worker<N,T>::take_picture() {
    workers_.observatory_.take_picture(stars_, cam_id_);
}

template<size_t N, typename T> void worker<N,T>::run() {
    for (;;) {
	boost::unique_lock<boost::mutex> lockit(idle_lock_);
	while (idle_ && !killed_) {
	    idle_cv_.wait(lockit);
	}
	if (killed_) {
	    break;
	}
	for (; nonce_ != nonce_end_ && nonce_ < workers_.smallest_nonce(); nonce_++) {
	    set_target(nonce_offset_, nonce_);
	    take_picture();
	    if (nonce_ == 0 && stars_.size() >= 1) {
	        has_first_visible_ = true;
	        first_visible_ = stars_[0];
	    }
	    if (detector_.search(found_)) {
	         workers_.found_nonce(nonce_);
	         found_done_ = true;
	         break;
	    }
	}
	idle_ = true;
	workers_.add_ready_worker(this);
    }
}

template<size_t N, typename T> bool observatory<N,T>::scan(uint64_t nonce_offset, projected_star &first_visible, std::vector<projected_star> &found, uint32_t &nonce_out)
{
    worker_pool<N,T> workers(*this);

    uint32_t nonce = 0, nonce_delta = 100;
    bool first_visible_found = false;

    first_visible.clear();

    for (;;) {
	auto &worker = workers.find_ready_worker();
	if (worker.nonce_start() == 0) {
	    if (worker.has_first_visible()) {
	        first_visible_found = true;
	        first_visible = worker.first_visible();
	    }
	}
	if (worker.is_done()) {
	    workers.add_ready_worker(&worker);
	    break;
	}
	worker.set_nonce_range(nonce_offset, nonce, nonce + nonce_delta);
	nonce += nonce_delta;
	worker.set_idle(false);
    }
    workers.kill_all_workers();
    workers.wait_until_no_more_busy_workers();

    if (!first_visible_found) {
        if (auto *wn = workers.find_first_nonce_worker()) {
	    if (wn->has_first_visible()) {
	        first_visible_found = true;
		first_visible = wn->first_visible();
	    }
	}
    }

    auto *w = workers.find_successful_worker();
    if (w) {
	found = w->get_found();
	nonce_out = w->nonce();
    }

    return w != nullptr && first_visible_found;
}

#ifndef DIPPER_DONT_USE_NAMESPACE
}}
#endif

#endif