/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the MIT License
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the MIT License for further details.
 *
 * Contact information
 * ------------------------------------------
 * Thomas Jespersen, TKJ Electronics
 * Web      :  http://www.tkjelectronics.dk
 * e-mail   :  thomasj@tkjelectronics.dk
 * ------------------------------------------
 */

#include <ros/ros.h>

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

#include "MPC.h"
#include "Trajectory.h"
#include "Path.h"

#include <tf/tf.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <kugle_msgs/StateEstimate.h>
#include <kugle_msgs/BalanceControllerReference.h>
#include <std_srvs/Empty.h>

#include <boost/math/quaternion.hpp> // see https://www.boost.org/doc/libs/1_66_0/libs/math/doc/html/quaternions.html
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

/* For plotting/visualization */
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

boost::math::quaternion<double> Quaternion_eul2quat_zyx(const float yaw, const float pitch, const float roll);
boost::math::quaternion<double>  QuaternionReference(boost::math::quaternion<double> q, boost::math::quaternion<double> W, double dt);
void Quaternion_quat2eul_zyx(boost::math::quaternion<double> q, float yaw_pitch_roll[3]);
void OdometryCallback(const nav_msgs::Odometry::ConstPtr& msg);
void cmdCallback(const kugle_msgs::BalanceControllerReference::ConstPtr& msg);
void StateEstimateCallback(const kugle_msgs::StateEstimate::ConstPtr& msg);
void GetLocalizationBasedPositionEstimate(tf::TransformListener * tfListener, std::string map_frame, std::string base_link_frame);
void GazeboReset(ros::NodeHandle &n);
void GazeboPause(ros::NodeHandle &n);
void GazeboResume(ros::NodeHandle &n);

nav_msgs::Odometry odom;
kugle_msgs::StateEstimate stateEstimate;
kugle_msgs::BalanceControllerReference cmdmsg;
struct {
    double time;
    Eigen::Vector2d position;
    boost::math::quaternion<double> quaternion;
} localizationEstimate;

int main(int argc, char **argv) {
	std::string nodeName = "mpc_test";
	ros::init(argc, argv, nodeName.c_str());
	ros::NodeHandle n;
    ros::NodeHandle nParam("~"); // default/current namespace node handle
    kugle_msgs::BalanceControllerReference cmd_vel;
    //Define the quaternion reference and initialize it:
    boost::math::quaternion<double> Quaternionref;
    Quaternionref = boost::math::quaternion<double>(1.0, 0.0, 0.0, 0.0);
	double rate=10;
    if (!nParam.getParam("rate", rate)) {
        ROS_WARN_STREAM("Loop rate not set (Parameter: rate). Defaults to: 10 Hz");
    }
else { ROS_INFO("Rate set %f", rate);
}
    std::string map_frame = "map";
    if (!nParam.getParam("map_frame", map_frame)) {
        ROS_WARN_STREAM("Map frame not set (Parameter: map_frame). Defaults to: map");
    }

    std::string base_link_frame = "base_link";
    if (!nParam.getParam("base_link_frame", base_link_frame)) {
        ROS_WARN_STREAM("base_link frame not set (Parameter: base_link_frame). Defaults to: base_link");
    }

    bool use_localization = false;
    if (!nParam.getParam("use_localization", use_localization)) {
        ROS_WARN_STREAM("Use localization flag not set (Parameter: use_localization). Defaults to: false");
    }

    double desired_velocity = 0.25;
    if (!nParam.getParam("desired_velocity", use_localization)) {
        ROS_WARN_STREAM("Desired velocity not set (Parameter: desired_velocity). Defaults to: 0.25 m/s");
    }

    ros::Rate loop_rate(rate); // Loop rate
    ros::Publisher pub_cmd_vel_angular_inertial = n.advertise<kugle_msgs::BalanceControllerReference>("cmd_combined_inertial", 1000);
    ros::Publisher pub_cost_func = n.advertise<geometry_msgs::PointStamped>("Cost_Value", 1000);
    ros::Subscriber sub_odom = n.subscribe("odom", 1000, &OdometryCallback);
    ros::Subscriber sub_cmd = n.subscribe("cmd_combined_inertial", 1000, &cmdCallback);
    ros::Subscriber sub_stateestimate = n.subscribe("state_estimate", 1000, &StateEstimateCallback);
    tf::TransformListener tfListener;
    //Make sure that the robot is having the correct initial heading:
	cmd_vel.omega.x = 0.0;
        cmd_vel.omega.y = 0.0;
        cmd_vel.omega.z = 0.0;
        cmd_vel.q.x = 0;
        cmd_vel.q.y = 0;
        cmd_vel.q.z = 0;
	cmd_vel.q.w = 1;
        pub_cmd_vel_angular_inertial.publish(cmd_vel);
    // Wait for first state estimate message
    stateEstimate.mcu_time = 0;
    localizationEstimate.time = 0;
    while (ros::ok())
    {
        if (stateEstimate.mcu_time > 0 && (localizationEstimate.time > 0 || !use_localization)) break;

        if (use_localization)
            GetLocalizationBasedPositionEstimate(&tfListener, map_frame, base_link_frame);

        if (use_localization)
            std::cout << "Waiting for localization" << std::endl;
        else
            std::cout << "Waiting for State Estimate message" << std::endl;

        /*std::cout << "Position estimate" << std::endl;
        std::cout << "odom = " << std::endl << odom.pose.pose.position;
        std::cout << "stateEst = " << std::endl << "(" << stateEstimate.position[0] << ", " << stateEstimate.position[1] << ")" << std::endl;
        std::cout << "tf = " << std::endl << localizationEstimate.position << std::endl;
        std::cout << std::endl;*/

        ros::spinOnce(); // walks the callback queue and calls registered callbacks for any outstanding events (incoming msgs, svc reqs, timers)
        loop_rate.sleep();
    }

	MPC::MPC mpc;
    mpc.setDesiredVelocity(desired_velocity);

    boost::math::quaternion<double> RobotQuaternion;
    Eigen::Vector2d RobotPos;
    Eigen::Vector2d RobotVelocity; // inertial frame
    MPC::MPC::state_t state;

    RobotQuaternion = boost::math::quaternion<double>(stateEstimate.q.w, stateEstimate.q.x, stateEstimate.q.y, stateEstimate.q.z);
    if (use_localization) {
        RobotPos(0) = localizationEstimate.position[0];
        RobotPos(1) = localizationEstimate.position[1];
    } else {
        RobotPos(0) = stateEstimate.position[0];
        RobotPos(1) = stateEstimate.position[1];
    }
    RobotVelocity(0) = stateEstimate.velocity[0];
    RobotVelocity(1) = stateEstimate.velocity[1];
    Eigen::Vector2d offset_internal;
    MPC::Trajectory trajectory = MPC::Trajectory::GenerateCircleTrajectory(RobotPos, 1);

   // trajectory.offset_internal;
	//MPC::Trajectory trajectory = MPC::Trajectory::GeneratePointTrajectory(RobotPos);
	//MPC::Trajectory trajectory = MPC::Trajectory::GenerateinfTrajectory(RobotPos, 0.5);
	mpc.setTrajectory(trajectory, RobotPos, Eigen::Vector2d(0.01,0), RobotQuaternion);
	//mpc.setXYreferencePosition(RobotPos(0), RobotPos(1));
	mpc.setCurrentState(RobotPos, Eigen::Vector2d(0.01,0), RobotQuaternion);
	//Adding the obstacles:
	std::vector<MPC::Obstacle> ob;
	MPC::Obstacle ob1;
	ob1.x = (RobotPos(0)-1)+1*cos(M_PI+M_PI/3);
	ob1.y = RobotPos(1)+1*sin(M_PI+M_PI/3);
	ob1.radius = 0.15;
	MPC::Obstacle ob2;
	ob2.x = (RobotPos(0)-1);
	ob2.y = RobotPos(1);
	ob2.radius = 0.2;
	MPC::Obstacle ob3;
	ob3.x = (RobotPos(0)-1)+1.03*cos(M_PI/2+M_PI/12);
	ob3.y = RobotPos(1)+1.03*sin(M_PI/2+M_PI/12);
	ob3.radius = 0.11;
	MPC::Obstacle ob4;
	ob4.x = (RobotPos(0)-1)+1.05*cos(3*M_PI/2+M_PI/4);
	ob4.y = RobotPos(1)+1.05*sin(3*M_PI/2+M_PI/4);
	ob4.radius = 0.11;
	ob.push_back(ob1);
	ob.push_back(ob2);
	ob.push_back(ob3);
	ob.push_back(ob4);
	mpc.setObstacles(ob);
	double offsetx = RobotPos(0);
	double offsety = RobotPos(1);

    	ROS_WARN_STREAM("Obstacle set");

	while (ros::ok())
	{
		/*if ((i % 10) == 0) {
		    mpc.setTrajectory(trajectory, state.position, state.velocity, state.quaternion);
		    mpc.setObstacles(obstacles);
		    //mpc.setObstacles(obstacles);
		}*/

        if (use_localization)
            GetLocalizationBasedPositionEstimate(&tfListener, map_frame, base_link_frame);

        RobotQuaternion = boost::math::quaternion<double>(stateEstimate.q.w, stateEstimate.q.x, stateEstimate.q.y, stateEstimate.q.z);
        if (use_localization) {
            RobotPos(0) = localizationEstimate.position[0];
            RobotPos(1) = localizationEstimate.position[1];
        } else {
            RobotPos(0) = stateEstimate.position[0];
            RobotPos(1) = stateEstimate.position[1];
        }
        RobotVelocity(0) = stateEstimate.velocity[0];
        RobotVelocity(1) = stateEstimate.velocity[1];

		mpc.setTrajectory(trajectory, RobotPos, RobotVelocity, RobotQuaternion);
		mpc.setObstacles(ob);
		mpc.setCurrentState(RobotPos, RobotVelocity, RobotQuaternion);

		// cv::Mat imgTrajectory = cv::Mat( 500, 1333, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
		cv::Mat imgTrajectory = cv::Mat( 500, 500, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
		trajectory.plot(imgTrajectory, cv::Scalar(0, 0, 255), false, false, -5, -5, 5, 5);
		mpc.PlotRobot(imgTrajectory, cv::Scalar(255, 0, 0), false, -5, -5, 5, 5);
		mpc.PlotObstacles(imgTrajectory, cv::Scalar(255, 0, 0), cv::Scalar(0, 255, 0), false, -5, -5, 5, 5);
		cv::imshow("Trajectory", imgTrajectory);

		cv::Mat imgWindowTrajectory = cv::Mat( 500, 500, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
		mpc.getCurrentTrajectory().plot(imgWindowTrajectory, cv::Scalar(0, 255, 0), true, false, -4, -4, 4, 4);
		mpc.PlotRobotInWindow(imgWindowTrajectory, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		mpc.PlotObstaclesInWindow(imgWindowTrajectory, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		cv::imshow("Window", imgWindowTrajectory);

		cv::Mat imgWindowPath = cv::Mat( 500, 500, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
		mpc.getCurrentPath().plot(imgWindowPath, cv::Scalar(0, 255, 0), true, -4, -4, 4, 4);
		mpc.getCurrentPath().PlotPoint(mpc.getClosestPointOnPath(), imgWindowPath, cv::Scalar(0, 0, 255), true, -4, -4, 4, 4);
		mpc.PlotRobotInWindow(imgWindowPath, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		mpc.PlotObstaclesInWindow(imgWindowPath, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		cv::imshow("Path", imgWindowPath);

		mpc.Step();

		Eigen::Vector2d angularVelocityReference(0.0, 0.0);
		if (mpc.getStatus() != MPC::MPC::SUCCESS) {
		    std::cout << "MPC Solver failed" << std::endl;
		    cv::waitKey(0);
		        cmd_vel.omega.x = 0;
        		cmd_vel.omega.y = 0;
        		cmd_vel.omega.z = 0;
       			cmd_vel.q.x = 0;
        		cmd_vel.q.y = 0;
        		cmd_vel.q.z = 0;
			cmd_vel.q.w = 1;
        		pub_cmd_vel_angular_inertial.publish(cmd_vel);
			ros::spinOnce(); // walks the callback queue and calls registered callbacks for any outstanding events (incoming msgs, svc reqs, timers)
		}
		else {
		state = mpc.getHorizonState();
		std::cout << "Predicted states:" << std::endl;
		std::cout << "path distance = " << state.pathDistance << std::endl;
		std::cout << "position = " << std::endl << state.position << std::endl;
		std::cout << "velocity = " << std::endl << state.velocity << std::endl;
		std::cout << "quaternion = " << std::endl << state.quaternion << std::endl;
		std::cout << std::endl;
		angularVelocityReference = mpc.getInertialAngularVelocity();
		//angularVelocityReference[1]=angularVelocityReference[1]-0.15;
		std::cout << "Control output (angular velocity) Horizon 22:" << std::endl;
		std::cout << "   x = " << angularVelocityReference[0] << std::endl;
		std::cout << "   y = " << angularVelocityReference[1] << std::endl;

		cv::Mat imgPredicted = cv::Mat( 500, 500, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
		mpc.PlotPredictedTrajectory(imgPredicted, -4, -4, 4, 4);
		mpc.PlotObstaclesInWindow(imgPredicted, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		mpc.PlotRobotInWindow(imgPredicted, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		cv::imshow("Predicted", imgPredicted);

		//Publish Cost function value:
		geometry_msgs::PointStamped var;
		var.header.stamp=ros::Time::now();
		var.point.x=mpc.SolverCostValue_;
		pub_cost_func.publish(var);
	//Calculate reference quaternion:
	//angularVelocityReference[0]=omegax_ref_filt.Filter(angularVelocityReference[0]);
	//angularVelocityReference[1]=omegay_ref_filt.Filter(angularVelocityReference[1]);
	//Integrate angular velocity commands to get quaternion reference
	Quaternionref=QuaternionReference(Quaternionref,boost::math::quaternion<double>(0, angularVelocityReference[0], angularVelocityReference[1], 0), 1/rate);
        cmd_vel.omega.x = angularVelocityReference[0];
        cmd_vel.omega.y = angularVelocityReference[1];
        cmd_vel.omega.z = 0.0;
        cmd_vel.q.x = Quaternionref.R_component_2();
        cmd_vel.q.y = Quaternionref.R_component_3();
        cmd_vel.q.z = Quaternionref.R_component_4();
	cmd_vel.q.w = Quaternionref.R_component_1();
        pub_cmd_vel_angular_inertial.publish(cmd_vel);

        cv::waitKey(1);
		ros::spinOnce(); // walks the callback queue and calls registered callbacks for any outstanding events (incoming msgs, svc reqs, timers)
		loop_rate.sleep();
		}
	}
}

void OdometryCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    //ROS_INFO_STREAM("Received odometry, translational velocity: (" << msg->twist.twist.linear.x << ", " << msg->twist.twist.linear.y << ")");
    odom = *msg;
}

void cmdCallback(const kugle_msgs::BalanceControllerReference::ConstPtr& msg)
{
    cmdmsg = *msg;
}

void StateEstimateCallback(const kugle_msgs::StateEstimate::ConstPtr& msg)
{
    //ROS_INFO_STREAM("Received StateEstimate message");
    stateEstimate = *msg;
}

void GetLocalizationBasedPositionEstimate(tf::TransformListener * tfListener, std::string map_frame, std::string base_link_frame)
{
    ros::Time currentTime = ros::Time::now();

    // Look-up current estimate of robot in map
    tf::StampedTransform tf_base_link;
    try {
	tfListener->waitForTransform(map_frame, base_link_frame, ros::Time(0), ros::Duration(3.0));
        tfListener->lookupTransform(map_frame, base_link_frame, ros::Time(0), tf_base_link);
        Eigen::Vector2d robot_center(tf_base_link.getOrigin().x(), tf_base_link.getOrigin().y());
        tf::Quaternion q = tf_base_link.getRotation();

        localizationEstimate.quaternion =  boost::math::quaternion<double>(q.w(), q.x(), q.y(), q.z());
	//std::cout << "Quaternion: " << localizationEstimate.quaternion << std::endl;
        localizationEstimate.position = robot_center;
	//std::cout << "Position: " << localizationEstimate.position << std::endl;
        localizationEstimate.time = currentTime.toSec();
	//std::cout << "Time: " << localizationEstimate.time << std::endl;
    }
    catch (tf::TransformException &ex) {
        ROS_WARN("%s", ex.what());
    }
}

void GazeboReset(ros::NodeHandle &n)
{
    ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/gazebo/reset_world");
    std_srvs::Empty srv;
    if (client.call(srv))
    {
        ROS_INFO("Success calling /gazebo/reset_world");
    }
    else
    {
        ROS_ERROR("Failed calling /gazebo/reset_world");
    }
}

void GazeboPause(ros::NodeHandle &n)
{
    ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/gazebo/pause_physics");
    std_srvs::Empty srv;
    if (client.call(srv))
    {
        ROS_INFO("Success calling /gazebo/pause_physics");
    }
    else
    {
        ROS_ERROR("Failed calling /gazebo/pause_physics");
    }
}

void GazeboResume(ros::NodeHandle &n)
{
    ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/gazebo/unpause_physics");
    std_srvs::Empty srv;
    if (client.call(srv))
    {
        ROS_INFO("Success calling /gazebo/unpause_physics");
    }
    else
    {
        ROS_ERROR("Failed calling /gazebo/unpause_physics");
    }
}

boost::math::quaternion<double> Quaternion_eul2quat_zyx(const float yaw, const float pitch, const float roll)
{
	const float cx = cosf(roll/2);
	const float cy = cosf(pitch/2);
	const float cz = cosf(yaw/2);
	const float sx = sinf(roll/2);
	const float sy = sinf(pitch/2);
	const float sz = sinf(yaw/2);

	double q[4];
	q[0] = cz*cy*cx+sz*sy*sx;
	q[1] = cz*cy*sx-sz*sy*cx;
	q[2] = cz*sy*cx+sz*cy*sx;
	q[3] = sz*cy*cx-cz*sy*sx;

	return boost::math::quaternion<double>(q[0], q[1], q[2], q[3]);
}

void Quaternion_quat2eul_zyx(boost::math::quaternion<double> q, float yaw_pitch_roll[3])
{
	// Normalize quaternion
	q /= norm(q);

	float qw = q.R_component_1();
	float qx = q.R_component_2();
	float qy = q.R_component_3();
	float qz = q.R_component_4();

	float aSinInput = -2*(qx*qz-qw*qy);
	aSinInput = fmax(fmin(aSinInput, 1.f), -1.f);

	yaw_pitch_roll[0] = atan2( 2*(qx*qy+qw*qz), qw*qw + qx*qx - qy*qy - qz*qz ); // yaw
	yaw_pitch_roll[1] = asin( aSinInput ); // pitch
	yaw_pitch_roll[2] = atan2( 2*(qy*qz+qw*qx), qw*qw - qx*qx - qy*qy + qz*qz ); // roll
}

    boost::math::quaternion<double>  QuaternionReference(boost::math::quaternion<double> q, boost::math::quaternion<double> W, double dt)
	{
	double normW=norm(W);
	boost::math::quaternion<double> qe;
	if (normW > 0) {
        double sinOmeg = sinf(0.5f * dt * normW);
	//Quaternion exponential:
	qe = boost::math::quaternion<double>(cosf(0.5f * dt * normW), sinOmeg * W.R_component_2() / normW, sinOmeg * W.R_component_3() / normW, sinOmeg * W.R_component_4() / normW);
    } else {
	qe = boost::math::quaternion<double>(1.0, 0.0, 0.0, 0.0); //Angular velocity is zero (No rotation)
    }
	return qe*q;
}


#pragma once

#include <boost/multiprecision/cpp_int.hpp>
#include <array>
#include <cstdint>
#include <iterator>
#include <type_traits>
#include <limits>
#include <algorithm>
#include <vector>
#include "sha512.hpp"

namespace cryptb
{
	// Give the constructor nothing to automatically initizlize with a truly
	// random state.
	// Give the constructor a seed to (instead) initialize the state deterministrically
	// based on the seed.
	class random_engine
	{
		sha512 m_state;
	public:
		// m_state has the following values:
		//
		// m_hash_values: 64 bytes == 2 ^ 512 valid combinations
		// m_message_block: 128 bytes == 2 ^ 1024 valid combinations
		// m_num_bytes_filled == 128 valid combinations
		// m_bits_counter == 2 ^ 128 valid combinations
		//
		// Altogether that's: log2(2**(1024+512+128)*128) bits required
		// to achieve a similar number of combinations.
		//
		// 1671 bits. A close enough number is 208 bytes.
		//
		static constexpr int optimal_seed_size_bytes = 208;
		// Generates a truly random number
		random_engine()
			: random_engine(random_engine::gen_truly_random_bytes()) {}
		random_engine(const random_engine&) = default;
		random_engine(random_engine&&) = default;
		random_engine& operator=(const random_engine&) = default;
		random_engine& operator=(random_engine&&) = default;
		// Supports using a specific size (optimal size) std::array as a seed
		random_engine(const std::array<std::uint8_t, random_engine::optimal_seed_size_bytes>& seed_bytes) :
			m_state(seed_bytes.data(), seed_bytes.size()) {}

		// Supports using any boost::multiprecision::cpp_int type as a seed.
		// For example:
		//
		//	boost::multiprecision::cpp_int
		// Or:
		//	boost::multiprecision::something_bits
		//
		template <unsigned MinBits, unsigned MaxBits, boost::multiprecision::cpp_integer_type SignType, boost::multiprecision::cpp_int_check_type Checked, class Allocator>
		random_engine(const boost::multiprecision::number<boost::multiprecision::cpp_int_backend<MinBits, MaxBits, SignType, Checked, Allocator>>& seed)
		{
			// Helper class that can be used with std::back_inserter_iterator.
			// This is to avoid using a std::vector with dynamic memory allocation.
			class back_inserter_t_for_sha512
			{
				sha512& m_obj;
			public:
				using value_type = std::uint8_t;
				back_inserter_t_for_sha512(sha512& obj) : m_obj(obj) {}
				void push_back(const std::uint8_t& elem)
				{
					this->m_obj.update(&elem, 1);
				}
			};
			back_inserter_t_for_sha512 dummy_back_inserter{ this->m_state };
			// The "msv_first == true" means that if the received number is 0xff01230
			// then it'll be layed-out in the byte array as:
			// { 0x0f, 0xf0, 0x12, 0x30 }
			//
			// The alternative ("msv_first == false") would make the byte array look like:
			// { 0x30, 0x12, 0xf0, 0x0f }
			//
			// But it doesn't matter that much. As long as it's random.
			boost::multiprecision::export_bits(seed, std::back_inserter(dummy_back_inserter), 8, true);
		}

		// iter_T could be for example:
		//	std::vector<std::uint8_t>::const_iterator
		//
		template <typename iter_T>
		random_engine(const iter_T& begin, const iter_T& end)
		{
			using value_type = decltype(*begin);
			static_assert(std::is_integral<value_type>::value, "Must be iterator with an integer value type");
			static_assert(sizeof(value_type) == 8, "Must be iterator with an integer value type of size 8 bytes");
			std::for_each(begin, end,
				[&state = this->m_state](const value_type& val) -> void
				{
					const std::uint8_t cpy{ val };
					state.update(&cpy, 1);
				}
			);
		}

		// Get n random number with n number of bytes.
		boost::multiprecision::cpp_int operator()(int num_bytes);
		// Pseudo random number
		std::array<std::uint8_t, 64> gen_512_bit_random_number();
		// Truly random number
		std::array<std::uint8_t, random_engine::optimal_seed_size_bytes> gen_truly_random_bytes();
	};
}


//Copyright (c) 2015 Zachary Kann
//
//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.

// ---
// Author: Zachary Kann

#include "boost/multi_array.hpp"
#include <armadillo>
#include "z_constants.hpp"

using namespace arma;

typedef boost::multi_array<double, 4> hypercube;

#ifndef _Z_VEC_HPP_
#define _Z_VEC_HPP_

class Grid
{
    private:
        static const int dims = 3;
        rowvec length;
        urowvec points;

    public:
        double spacing;
        cube rho;
        hypercube grid;
        cube upper_mesh;
        cube lower_mesh;
        Grid(rowvec box, double grid_spacing = 0.1);
        void zero_rho();
        void read_mesh(std::fstream &mesh_file);
        double avg_mesh();
        unsigned  size_x() const { return points(0); }
        unsigned  size_y() const { return points(1); }
        unsigned  size_z() const { return points(2); }
        urowvec size() const { return points; }
};

extern void setupDX(cube& xoo, cube& xio, cube& xii, mat& roo2, mat& rio2, mat& rii2, const mat& x, const mat& xion, icube& shiftoo,
                    icube& shiftio, icube& shiftii, const unsigned  numMols, const unsigned  numIons, const rowvec& box);
extern void setupDX_io(cube& xio, const mat& x, const mat& xion, icube& shiftio, const unsigned  numMols, const unsigned  numIons, const rowvec& box);
extern void setupShift_oo(const mat& x, icube& shiftoo, const unsigned  numMols, const rowvec& box);
extern void findDx_noShift(rowvec& result, const rowvec& vec1, const rowvec& vec2, const rowvec& box);
extern void findDx(rowvec& result, const rowvec& vec1, const rowvec& vec2, const rowvec& box, irowvec& shift);

template <typename type1>
rowvec veclike_to_row(const type1& vec1, const unsigned  length)
{
    rowvec result(length);
    for (int i=0; i<length; i++)
    {
        result[i] = vec1[i];
    }
    return result;
}

inline rowvec useDx(const rowvec& vec1, const rowvec& vec2, const rowvec& box, const irowvec& shift) { return vec1 - vec2 + shift%box; }

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_SCALAR_POPCNT_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SCALAR_POPCNT_HPP_INCLUDED
#include <boost/simd/bitwise/functions/popcnt.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/scalar/lo.hpp>
#include <boost/simd/include/functions/scalar/hi.hpp>
#include <boost/simd/include/functions/scalar/sbits.hpp>
#include <boost/simd/include/constants/digits.hpp>

#ifdef BOOST_MSVC
#include <intrin.h>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::popcnt(boost::simd::sbits(a0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)
                            , (scalar_< ints8_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
    #ifdef BOOST_MSVC
      return result_type(__popcnt16(int16_t(a0) & 0xFF));
    #else
      return __builtin_popcount(int32_t(a0) & 0xFF);
    #endif
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)
                            , (scalar_< ints16_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
    #ifdef BOOST_MSVC
      return __popcnt16(a0);
    #else
      return __builtin_popcount(int32_t(a0) & 0xFFFF);
    #endif
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)
                            , (scalar_< ints32_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
    #ifdef BOOST_MSVC
      return __popcnt(a0);
    #else
      return __builtin_popcount(a0);
    #endif
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)
                            , (scalar_< ints64_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
    #if defined BOOST_MSVC && defined _WIN64
      return __popcnt64(a0);
    #elif defined BOOST_MSVC
      return  __popcnt( hi(a0) )
            + __popcnt( lo(a0) );
    #else
      return  __builtin_popcount( hi(a0) )
            + __builtin_popcount( lo(a0) );
    #endif
    }
  };
} } }

#endif


#include <boost/geometry/algorithms/buffer.hpp>


#include "SPlisHSPlasH/Common.h"
#include "GL/glew.h"
#include "Visualization/MiniGL.h"
#include "GL/glut.h"
#include "SPlisHSPlasH/TimeManager.h"
#include <Eigen/Dense>
#include <iostream>
#include "SPlisHSPlasH/Utilities/Timing.h"
#include "Utilities/PartioReaderWriter.h"
#include "PositionBasedDynamicsWrapper/PBDRigidBody.h"
#include "Utilities/OBJLoader.h"
#include "SPlisHSPlasH/Utilities/PoissonDiskSampling.h"
#include "PositionBasedDynamicsWrapper/PBDWrapper.h"
#include "Demos/Common/DemoBase.h"
#include "Utilities/FileSystem.h"

// Enable memory leak detection
#ifdef _DEBUG
#ifndef EIGEN_ALIGN
	#define new DEBUG_NEW
#endif
#endif

using namespace SPH;
using namespace Eigen;
using namespace std;

void timeStep ();
void initBoundaryData();
void render ();
void renderBoundary();
void reset();
void updateBoundaryParticles(const bool forceUpdate);
void updateBoundaryForces();
void simulationMethodChanged();

DemoBase base;
PBDWrapper pbdWrapper;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS;

	base.init(argc, argv, "DynamicBoundaryDemo");

	//////////////////////////////////////////////////////////////////////////
	// PBD
	//////////////////////////////////////////////////////////////////////////
	pbdWrapper.initShader();
	pbdWrapper.readScene(base.getSceneFile());

	initBoundaryData();
	base.buildModel();
	base.setSimulationMethodChangedFct(simulationMethodChanged);
	pbdWrapper.initGUI();

	pbdWrapper.initModel(TimeManager::getCurrent()->getTimeStepSize());

	MiniGL::setClientIdleFunc(50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setClientSceneFunc(render);

	glutMainLoop ();

	base.cleanup();

	Timing::printAverageTimes();
	Timing::printTimeSums();

	return 0;
}

void reset()
{
	Timing::printAverageTimes();
	Timing::reset();

	//////////////////////////////////////////////////////////////////////////
	// PBD
	//////////////////////////////////////////////////////////////////////////
	pbdWrapper.reset();

	updateBoundaryParticles(true);

	base.getSimulationMethod().simulation->reset();
	TimeManager::getCurrent()->setTime(0.0);
}

void timeStep ()
{
	if ((base.getPauseAt() > 0.0) && (base.getPauseAt() < TimeManager::getCurrent()->getTime()))
		base.setPause(true);

	if (base.getPause())
		return;

	// Simulation code
	for (unsigned int i = 0; i < base.getNumberOfStepsPerRenderUpdate(); i++)
	{
		START_TIMING("SimStep");
		base.getSimulationMethod().simulation->step();
		STOP_TIMING_AVG;

		updateBoundaryForces();

		//////////////////////////////////////////////////////////////////////////
		// PBD
		//////////////////////////////////////////////////////////////////////////
		START_TIMING("SimStep - PBD");
		pbdWrapper.timeStep();
		STOP_TIMING_AVG;

		updateBoundaryParticles(false);
	}
}

void simulationMethodChanged()
{
	pbdWrapper.initGUI();
	reset();
}

void renderBoundary()
{
	DemoBase::SimulationMethod &simulationMethod = base.getSimulationMethod();
	Shader &shader = base.getShader();
	Shader &meshShader = base.getMeshShader();
	SceneLoader::Scene &scene = base.getScene();
	const int renderWalls = base.getRenderWalls();

	float wallColor[4] = { 0.1f, 0.6f, 0.6f, 1.0f };
	if ((renderWalls == 1) || (renderWalls == 2))
	{
		if (MiniGL::checkOpenGLVersion(3, 3))
		{
			shader.begin();
			glUniform3fv(shader.getUniform("color"), 1, &wallColor[0]);
			glEnableVertexAttribArray(0);
			for (int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
			{
				if ((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
				{
					FluidModel::RigidBodyParticleObject *rb = simulationMethod.model.getRigidBodyParticleObject(body);
					glVertexAttribPointer(0, 3, GL_DOUBLE, GL_FALSE, 0, &simulationMethod.model.getPosition(body + 1, 0));
					glDrawArrays(GL_POINTS, 0, rb->numberOfParticles());
				}
			}
			glDisableVertexAttribArray(0);
			shader.end();
		}
		else
		{
			glDisable(GL_LIGHTING);
			glPointSize(4.0f);

			glBegin(GL_POINTS);
			for (int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
			{
				if ((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
				{
					FluidModel::RigidBodyParticleObject *rb = simulationMethod.model.getRigidBodyParticleObject(body);
					for (unsigned int i = 0; i < rb->numberOfParticles(); i++)
					{
						glColor3fv(wallColor);
						glVertex3v(&simulationMethod.model.getPosition(body + 1, i)[0]);
					}
				}
			}
			glEnd();
			glEnable(GL_LIGHTING);
		}
	}
}

void render()
{
	MiniGL::coordinateSystem();

	base.renderFluid();
	renderBoundary();

	//////////////////////////////////////////////////////////////////////////
	// PBD
	//////////////////////////////////////////////////////////////////////////

	PBD::SimulationModel &model = pbdWrapper.getSimulationModel();
	PBD::SimulationModel::RigidBodyVector &rb = model.getRigidBodies();

	const int renderWalls = base.getRenderWalls();
	SceneLoader::Scene &scene = base.getScene();
	if ((renderWalls == 3) || (renderWalls == 4))
	{
		for (size_t i = 0; i < rb.size(); i++)
		{
			const PBD::VertexData &vd = rb[i]->getGeometry().getVertexData();
			const PBD::IndexedFaceMesh &mesh = rb[i]->getGeometry().getMesh();
			if ((renderWalls == 3) || (!scene.boundaryModels[i]->isWall))
			{
				float *col = &scene.boundaryModels[i]->color[0];
				if (!scene.boundaryModels[i]->isWall)
				{
					base.meshShaderBegin(col);
					pbdWrapper.drawMesh(vd, mesh, 0, col);
					base.meshShaderEnd();
				}
				else
				{
					base.meshShaderBegin(col);
					pbdWrapper.drawMesh(vd, mesh, 0, col);
					base.meshShaderEnd();
				}
			}
		}
	}

	pbdWrapper.renderTriangleModels();
	pbdWrapper.renderTetModels();
	pbdWrapper.renderConstraints();
	pbdWrapper.renderBVH();
}

void initBoundaryData()
{
	std::string base_path = FileSystem::getFilePath(base.getSceneFile());
	SceneLoader::Scene &scene = base.getScene();
	const bool useCache = base.getUseParticleCaching();

	for (unsigned int i = 0; i < scene.boundaryModels.size(); i++)
	{
		std::vector<Vector3r> boundaryParticles;
		if (scene.boundaryModels[i]->samplesFile != "")
		{
			string particleFileName = base_path + "/" + scene.boundaryModels[i]->samplesFile;
			PartioReaderWriter::readParticles(particleFileName, Vector3r::Zero(), Matrix3r::Identity(), scene.boundaryModels[i]->scale[0], boundaryParticles);
		}

		// Cache sampling
		std::string mesh_base_path = FileSystem::getFilePath(scene.boundaryModels[i]->meshFile);
		std::string mesh_file_name = FileSystem::getFileName(scene.boundaryModels[i]->meshFile);
		std::string scene_path = FileSystem::getFilePath(base.getSceneFile());
		std::string scene_file_name = FileSystem::getFileName(base.getSceneFile());
		string cachePath = scene_path + "/" + mesh_base_path + "/Cache";
		string particleFileName = FileSystem::normalizePath(cachePath + "/" + scene_file_name + "_" + mesh_file_name + "_" + std::to_string(i) + ".bgeo");

		PBD::SimulationModel &model = pbdWrapper.getSimulationModel();
		PBD::SimulationModel::RigidBodyVector &rigidBodies = model.getRigidBodies();
		PBDRigidBody *rb = new PBDRigidBody(rigidBodies[i]);
		PBD::RigidBodyGeometry &geo = rigidBodies[i]->getGeometry();
		PBD::IndexedFaceMesh &mesh = geo.getMesh();
		PBD::VertexData &vd = geo.getVertexData();

		if (scene.boundaryModels[i]->samplesFile == "")
		{
			bool foundCacheFile = false;
			if (useCache)
			{
				foundCacheFile = PartioReaderWriter::readParticles(particleFileName, Vector3r::Zero(), Matrix3r::Identity(), 1.0, boundaryParticles);
				if (foundCacheFile)
					std::cout << "Loaded cached boundary sampling: " << particleFileName << "\n";
			}

			if (!useCache || !foundCacheFile)
			{
				std::cout << "Surface sampling of " << scene.boundaryModels[i]->meshFile << "\n";
				START_TIMING("Poisson disk sampling");
				PoissonDiskSampling sampling;
				sampling.sampleMesh(mesh.numVertices(), &vd.getPosition(0), mesh.numFaces(), mesh.getFaces().data(), scene.particleRadius, 10, 1, boundaryParticles);
				STOP_TIMING_AVG;

				// Cache sampling
				if (useCache && (FileSystem::makeDir(cachePath) == 0))
				{
					std::cout << "Save particle sampling: " << particleFileName << "\n";
					PartioReaderWriter::writeParticles(particleFileName, (unsigned int)boundaryParticles.size(), boundaryParticles.data(), NULL, scene.particleRadius);
				}
			}
			// transform back to local coordinates
			for (unsigned int j = 0; j < boundaryParticles.size(); j++)
				boundaryParticles[j] = rb->getRotation().transpose() * (boundaryParticles[j] - rb->getPosition());
		}
		base.getSimulationMethod().model.addRigidBodyObject(rb, static_cast<unsigned int>(boundaryParticles.size()), &boundaryParticles[0]);
	}
	updateBoundaryParticles(true);
}

void updateBoundaryParticles(const bool forceUpdate = false)
{
	SceneLoader::Scene &scene = base.getScene();
	const unsigned int nObjects = base.getSimulationMethod().model.numberOfRigidBodyParticleObjects();
	for (unsigned int i = 0; i < nObjects; i++)
	{
		FluidModel::RigidBodyParticleObject *rbpo = base.getSimulationMethod().model.getRigidBodyParticleObject(i);
		RigidBodyObject *rbo = rbpo->m_rigidBody;
		if (rbo->isDynamic() || forceUpdate)
		{
			#pragma omp parallel default(shared)
			{
				#pragma omp for schedule(static)
				for (int j = 0; j < (int)rbpo->numberOfParticles(); j++)
				{
					rbpo->m_x[j] = rbo->getRotation() * rbpo->m_x0[j] + rbo->getPosition();
					rbpo->m_v[j] = rbo->getAngularVelocity().cross(rbpo->m_x[j] - rbo->getPosition()) + rbo->getVelocity();
				}
			}
		}
	}
}

void updateBoundaryForces()
{
	Real h = TimeManager::getCurrent()->getTimeStepSize();
	SceneLoader::Scene &scene = base.getScene();
	const unsigned int nObjects = base.getSimulationMethod().model.numberOfRigidBodyParticleObjects();
	for (unsigned int i = 0; i < nObjects; i++)
	{
		FluidModel::RigidBodyParticleObject *rbpo = base.getSimulationMethod().model.getRigidBodyParticleObject(i);
		RigidBodyObject *rbo = rbpo->m_rigidBody;
		if (rbo->isDynamic())
		{
			((PBDRigidBody*)rbo)->updateTimeStepSize();
			Vector3r force, torque;
			force.setZero();
			torque.setZero();

			for (int j = 0; j < (int)rbpo->numberOfParticles(); j++)
			{
				force += rbpo->m_f[j];
				torque += (rbpo->m_x[j] - rbo->getPosition()).cross(rbpo->m_f[j]);
				rbpo->m_f[j].setZero();
			}
			rbo->addForce(force);
			rbo->addTorque(torque);
		}
	}
}


//#include <windows.h>
#include "calibrate.h"
#include <iostream>
#include <stdlib.h>
#include <string>
#include <opencv2/core/core_c.h>

//#include "atlbase.h"
//#include "atlstr.h"
//#include "comutil.h"

extern bool bshow;

#ifndef _CRT_SECURE_NO_WARNINGS
# define _CRT_SECURE_NO_WARNINGS
#endif

//#include <boost/timer/timer.hpp>
//#include <boost/format.hpp>

static void help()
{
    cout <<  "This is a camera calibration sample." << endl
         <<  "Usage: calibration configurationFile"  << endl
         <<  "Near the sample file you'll find the configuration file, which has detailed help of "
             "how to edit it.  It may be any OpenCV supported file format XML/YAML." << endl;
}

static void read(const FileNode& node, Settings& x, const Settings& default_value = Settings())
{
    if(node.empty())
        x = default_value;
    else
        x.read(node);
}

enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };

int runcalib(int mod)
{

	 Settings s;
	  string inputSettingsFile =  "d:\\james\\images\\default.xml";

	  if (mod == 1)
	  {
		  inputSettingsFile = "d:\\james\\images\\Ldefault.xml";
	  }
	  if (mod == 2)
	  {
		  inputSettingsFile = "d:\\james\\images\\Rdefault.xml";
	  }

    FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
    if (!fs.isOpened())
    {
        cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
        return -1;
    }
    fs["Settings"] >> s;
    fs.release();                                         // close Settings file

    if (!s.goodInput)
    {
        cout << "Invalid input detected. Application stopping. " << endl;
        return -1;
    }

    vector<vector<Point2f> > imagePoints;
    Mat cameraMatrix, distCoeffs;
	vector<Mat> rvecs, tvecs;
    Size imageSize;
    int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
	  clock_t prevTimestamp = 0;
    const Scalar RED(0,0,255), GREEN(0,255,0);
	const char ESC_KEY = 27;
    for(int i = 0;;++i)
    {
      Mat view;
      bool blinkOutput = false;

      view = s.nextImage();

      //-----  If no more image, or got enough, then stop calibration and show result -------------
      if( mode == CAPTURING && imagePoints.size() >= (unsigned)s.nrFrames )
      {
          if( runCalibrationAndSave(s, imageSize,  cameraMatrix, distCoeffs, imagePoints,rvecs, tvecs))
              mode = CALIBRATED;
          else
              mode = DETECTION;
      }
      if(view.empty())          // If no more images then run calibration, save and stop loop.
      {
            if( imagePoints.size() > 0 && mode != CALIBRATED )
                runCalibrationAndSave(s, imageSize,  cameraMatrix, distCoeffs, imagePoints,rvecs, tvecs);
            break;
      }

        imageSize = view.size();  // Format input image.
        if( s.flipVertical )    flip( view, view, 0 );

        vector<Point2f> pointBuf;

        bool found;
        switch( s.calibrationPattern ) // Find feature points on the input format
        {
        case Settings::CHESSBOARD:
		{
			found = findChessboardCorners(view, s.boardSize, pointBuf,
				CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);

			if (!found)
			{

				char fn[1024];
				int find=0;
				strcpy(fn, s.imageList[s.atImageList].c_str());
				sscanf(fn, "%*[^f]f%d.bmp", &find);

				int indx = 1;
				while (!found && indx < 30)
				{
					sprintf(fn, "images//f%d.bmp", find + indx);
					indx++;
					view = imread(fn);
					if (view.dims == 0)
						break;
					found = findChessboardCorners(view, s.boardSize, pointBuf,
						CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);

				}
			}
		}
            break;
        case Settings::CIRCLES_GRID:
            found = findCirclesGrid( view, s.boardSize, pointBuf );
            break;
        case Settings::ASYMMETRIC_CIRCLES_GRID:
            found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID );
            break;
        default:
            found = false;
            break;
        }

        if ( found)                // If done with success,
        {
              // improve the found corners' coordinate accuracy for chessboard
                if( s.calibrationPattern == Settings::CHESSBOARD)
                {
                    Mat viewGray;
                    cvtColor(view, viewGray, COLOR_BGR2GRAY);
                    cornerSubPix( viewGray, pointBuf, Size(11,11),
                        Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
                }

                if( mode == CAPTURING &&  // For camera only take new samples after delay time
                    (!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) )
                {
                    imagePoints.push_back(pointBuf);
                    prevTimestamp = clock();
                    blinkOutput = s.inputCapture.isOpened();
                }

                // Draw the corners.
                drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found );
        }

        //----------------------------- Output Text ------------------------------------------------
        string msg = (mode == CAPTURING) ? "100/100" :
                      mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
        int baseLine = 0;
        Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
        Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10);

        /*if( mode == CAPTURING )
        {
            if(s.showUndistorsed)
                msg = format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames );
            else
                msg = format( "%d/%d", (int)imagePoints.size(), s.nrFrames );
        }*/

        putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ?  GREEN : RED);

        if( blinkOutput )
            bitwise_not(view, view);

        //------------------------- Video capture  output  undistorted ------------------------------
        if( mode == CALIBRATED && s.showUndistorsed )
        {
            Mat temp = view.clone();
            undistort(temp, view, cameraMatrix, distCoeffs);
        }

        //------------------------------ Show image and check for input commands -------------------
       // imshow("Image View", view);
//		imwrite( "save.bmp",view);

		//char key;
		/*if ( i ==0 )
			key =waitKey();
		else*/
		//key = (char)waitKey(1);// s.inputCapture.isOpened() ? 50 : s.delay);

       // if( key  == ESC_KEY )
        //    break;

        //if( key == 'u' && mode == CALIBRATED )
       //    s.showUndistorsed = !s.showUndistorsed;

       /* if( s.inputCapture.isOpened() && key == 'g' )
        {
            mode = CAPTURING;
            imagePoints.clear();
        }*/
    }

	 return 1;
}

int mymain(int argc, char* argv[])
{
    help();
    Settings s;
    const string inputSettingsFile = argc > 1 ? argv[1] : "d:\\james\\images\\Ddefault.xml";

    FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
    if (!fs.isOpened())
    {
        cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
        return -1;
    }
    fs["Settings"] >> s;
    fs.release();                                         // close Settings file

    if (!s.goodInput)
    {
        cout << "Invalid input detected. Application stopping. " << endl;
        return -1;
    }

    vector<vector<Point2f> > imagePoints;
    Mat cameraMatrix, distCoeffs;
	vector<Mat> rvecs, tvecs;
    Size imageSize;
    int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
    clock_t prevTimestamp = 0;
    const Scalar RED(0,0,255), GREEN(0,255,0);
    const char ESC_KEY = 27;

	if (! readCameraParams(  s.outputFileName, imageSize,  cameraMatrix, distCoeffs,  rvecs,   tvecs ))
	{

	}

	//boost::timer::cpu_timer timer;

	//timer.start();

    // -----------------------Show the undistorted image for the image list ------------------------
    if( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed )
    {
        Mat view, rview, map1, map2;
		Mat  nview, nmap1, nmap2;
		Mat newMatrix;
		newMatrix= getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0);
        initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
           newMatrix, imageSize, CV_16SC2, map1, map2);

	   float ss = s.squareSize ;

		vector<Point3f>  newpoints;
		vector<Point2f> imagePoints2;

		float bw=169;//195;//169;//ss*5;
		float bh=230;//265;//228;//ss*7;
		float bt=31.5;//19;//32;//ss;

		float newx = ss*7;
		float newy =ss*5;

		newpoints.push_back(Point3f(0,newy -bw,0));   //oldbase (0,0,0)  viewed from the newbase (0,newy,0)
		newpoints.push_back(Point3f(bh,newy-bw,0));   // x
		newpoints.push_back(Point3f(0,newy,0)); //y
		newpoints.push_back(Point3f(bh,newy ,0)); //x,y
		newpoints.push_back(Point3f(bh,newy,-bt));  //x,y
		newpoints.push_back(Point3f(0,newy-bw,-bt));  //oldbase
		newpoints.push_back(Point3f(bh,newy-bw,-bt));  //x
		newpoints.push_back(Point3f(0,newy,-bt));
		newpoints.push_back(Point3f(0,0,0));

        //for(int i = 0; i < (int)s.imageList.size(); i++ )
		int i=0;
        {
            view = imread("d:\\james\\images\\captured.png", 1);
			//view = imread(s.imageList[i], 1);
           // if(view.empty())
            //    continue;
            remap(view, rview, map1, map2, INTER_LINEAR);

			/*distCoeffs.at<double>(0) = 0;
			distCoeffs.at<double>(1) = 0;
			distCoeffs.at<double>(2) = 0;
			distCoeffs.at<double>(3) = 0;
			distCoeffs.at<double>(4) = 0;*/

			 projectPoints( Mat(newpoints), rvecs[i], tvecs[i], newMatrix,
                       distCoeffs, imagePoints2);

			  nmap1.create( view.size(), CV_32FC1 );
			  nmap2.create( view.size(), CV_32FC1 );

			  float scale=4.0f;

			vector<Point3f>  map3d;
		    vector<Point2f> mapp;

			//boost::timer::cpu_times elapsed = timer.elapsed(); // nano seconds...

			//float t = (elapsed.wall / 1000.0f / 1000.0f / 1000.0f) / (float)100;

			//float fps = 1.0f / t ;

			//if (t > 0.001) {

			//	std::cout << "t=" << t << ", fps=" << fps << std::endl;

			//}

			//timer.start();

			  for( int j = 0; j < nmap1.rows; j++ )
			  {
					for( int i = 0; i < nmap1.cols; i++ )
					{
						  if ( (i < scale*bw +50) && (j < scale*bh +50))
						  {

						map3d.push_back(Point3d(bh-j/scale ,newy -bw+i/scale,-bt));

						}

					}
	            }

		 //elapsed = timer.elapsed(); // nano seconds...

			// t = (elapsed.wall / 1000.0f / 1000.0f / 1000.0f) / (float)100;

			// fps = 1.0f / t ;

			//if (t > 0.001) {

			//	std::cout << "t=" << t << ", fps=" << fps << std::endl;

			//}

		//	timer.start();

						 projectPoints( Mat(map3d), rvecs[0], tvecs[0], newMatrix,
								distCoeffs, mapp);

// elapsed = timer.elapsed(); // nano seconds...

	//		 t = (elapsed.wall / 1000.0f / 1000.0f / 1000.0f) / (float)100;

		/*	 fps = 1.0f / t ;

			if (t > 0.001) {

				std::cout << "t=" << t << ", fps=" << fps << std::endl;

			}

			timer.start();
*/

                int k = 0 ;
			    for( int j = 0; j < nmap1.rows; j++ )
			  {
					for( int i = 0; i < nmap1.cols; i++ )
					{
						  if ( (i < scale*bw +50) && (j < scale*bh +50))
						  {

						  nmap1.at<float>(j,i) = mapp[k].x ;
						  nmap2.at<float>(j,i) = mapp[k].y ;
						  k++;

						}else
						{
							 nmap1.at<float>(j,i) = 0;
							  nmap2.at<float>(j,i) = 0 ;
						}

					}
	            }

		//	 elapsed = timer.elapsed(); // nano seconds...

		/*	 t = (elapsed.wall / 1000.0f / 1000.0f / 1000.0f) / (float)100;

			 fps = 1.0f / t ;

			if (t > 0.001) {

				std::cout << "t=" << t << ", fps=" << fps << std::endl;

			}

			timer.start();*/

			// updatmap( nmap1,nmap2, imagePoints2 ,bw,bh,scale);
			  remap(rview, nview, nmap1, nmap2, INTER_LINEAR);

			  //�b�ܥ����ʭ��W���u
			   line(nview,Point2f(0,0*scale),Point2f(bw*scale,0*scale), Scalar( 0, 0, 255 ),2,8);
			  line(nview,Point2f(0,60*scale),Point2f(bw*scale,60*scale), Scalar( 0, 255, 0 ),2,8);
			  line(nview,Point2f(0,77*scale),Point2f(bw*scale,77*scale), Scalar( 0, 0, 255 ),2,8);
			  line(nview,Point2f(0,bh*scale),Point2f(bw*scale,bh*scale), Scalar( 0, 0, 255 ),2,8);

			 ellipse( rview, imagePoints2[8],Size( 5, 5 ),0,0,360,Scalar( 255, 0, 0 ),2,8 );
			 line(rview,imagePoints2[0],imagePoints2[1], Scalar( 0, 255, 0 ),2,8);
			 line(rview,imagePoints2[0],imagePoints2[2], Scalar( 0, 0, 255 ),2,8);
			 line(rview,imagePoints2[1],imagePoints2[3], Scalar( 0, 0, 255 ),2,8);
			 line(rview,imagePoints2[2],imagePoints2[3], Scalar( 0, 0, 255 ),2,8);
			 line(rview,imagePoints2[3],imagePoints2[4], Scalar( 0, 255, 255 ),2,8);
			 line(rview,imagePoints2[0],imagePoints2[5], Scalar( 0, 255, 255 ),2,8);
			 line(rview,imagePoints2[5],imagePoints2[4], Scalar( 0, 255, 255 ),2,8);
			 line(rview,imagePoints2[1],imagePoints2[6], Scalar( 0, 255, 255 ),2,8);
			 line(rview,imagePoints2[2],imagePoints2[7], Scalar( 0, 255, 255 ),2,8);

			   imshow("map View", nview);
			   imwrite("map.png",nview);
            imshow("Image View", rview);
			imwrite("result.png",rview);
//
		 //elapsed = timer.elapsed(); // nano seconds...

			// t = (elapsed.wall / 1000.0f / 1000.0f / 1000.0f) / (float)100;

			// fps = 1.0f / t ;

			//if (t > 0.001) {

			//	std::cout << "t=" << t << ", fps=" << fps << std::endl;

			//}

			//timer.start();

            char c = (char)waitKey();
           /* if( c  == ESC_KEY || c == 'q' || c == 'Q' )
                break;*/
        }
    }

    return 0;
}

void Barycentric(Point2f p, Point2f a, Point2f b, Point2f c, float &u, float &v, float &w)
{
    Point2f v0 = b - a;
	Point2f v1 = c - a;
	Point2f v2 = p - a;
    float d00 = v0.dot( v0);
    float d01 = v0.dot(v1);
    float d11 = v1.dot( v1);
    float d20 = v2.dot( v0);
    float d21 = v2.dot( v1);
    float denom = d00 * d11 - d01 * d01;
    v = (d11 * d20 - d01 * d21) / denom;
    w = (d00 * d21 - d01 * d20) / denom;
    u = 1.0f - v - w;
}

Point2f  cartesian( Point2f p0, Point2f p1, Point2f p2, float u, float v, float w)
{
      return u * p0 + v * p1 + w * p2;
}

//7,4,5,6
void  updatmap(Mat& nmap1, Mat& nmap2,vector<Point2f> & imagePoints2 , int bw,int bh, float scale )
{

	float u,v,w;
	Point2f ret ;

	for( int j = 0; j < nmap1.rows; j++ )
    {
		for( int i = 0; i < nmap1.cols; i++ )
		{
			 ret= Point2f(0,0);
			  if ( (j < scale*bw) || (i < scale*bh))
			  {
				    Barycentric(Point2f(i,j),Point2f(0,0),Point2f(scale*bw,0),Point2f(0,scale*bh),u,v,w);
					if (( u+v+w)<=1.001 && ( u>=0 && v>=0 && w>=0))
					{
						ret = cartesian(imagePoints2[6],imagePoints2[4],imagePoints2[5],u,v,w);

					}else
					{

					 Barycentric(Point2f(i,j),Point2f(scale*bw,scale*bh),Point2f(scale*bw,0),Point2f(0,scale*bh),u,v,w);
					 if (( u+v+w)<=1.001 && ( u>=0 && v>=0 && w>=0))
						  ret = cartesian(imagePoints2[7],imagePoints2[4],imagePoints2[5],u,v,w);

					}

			  }
			  nmap1.at<float>(j,i) = ret.x ;
              nmap2.at<float>(j,i) = ret.y ;
		}
	}
}

static double computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
                                         const vector<vector<Point2f> >& imagePoints,
                                         const vector<Mat>& rvecs, const vector<Mat>& tvecs,
                                         const Mat& cameraMatrix , const Mat& distCoeffs,
                                         vector<float>& perViewErrors)
{
    vector<Point2f> imagePoints2;
    int i, totalPoints = 0;
    double totalErr = 0, err;
    perViewErrors.resize(objectPoints.size());

    for( i = 0; i < (int)objectPoints.size(); ++i )
    {
        projectPoints( Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix,
                       distCoeffs, imagePoints2);
        err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);

        int n = (int)objectPoints[i].size();
        perViewErrors[i] = (float) std::sqrt(err*err/n);
        totalErr        += err*err;
        totalPoints     += n;
    }

    return std::sqrt(totalErr/totalPoints);
}

static void calcBoardCornerPositions(Size boardSize, float squareSize, vector<Point3f>& corners,
                                     Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
{
    corners.clear();

    switch(patternType)
    {
    case Settings::CHESSBOARD:
    case Settings::CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; ++i )
            for( int j = 0; j < boardSize.width; ++j )
                corners.push_back(Point3f(float( j*squareSize ), float( i*squareSize ), 0));
        break;

    case Settings::ASYMMETRIC_CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; i++ )
            for( int j = 0; j < boardSize.width; j++ )
                corners.push_back(Point3f(float((2*j + i % 2)*squareSize), float(i*squareSize), 0));
        break;
    default:
        break;
    }
}

static bool runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
                            vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs, vector<Mat>& tvecs,
                            vector<float>& reprojErrs,  double& totalAvgErr)
{

    cameraMatrix = Mat::eye(3, 3, CV_64F);
    if( s.flag & CALIB_FIX_ASPECT_RATIO )
        cameraMatrix.at<double>(0,0) = 1.0;

    distCoeffs = Mat::zeros(8, 1, CV_64F);

    vector<vector<Point3f> > objectPoints(1);
    calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0], s.calibrationPattern);

    objectPoints.resize(imagePoints.size(),objectPoints[0]);

    //Find intrinsic and extrinsic camera parameters
    double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
                                 distCoeffs, rvecs, tvecs, s.flag|CALIB_FIX_K4|CALIB_FIX_K5);

    cout << "Re-projection error reported by calibrateCamera: "<< rms << endl;

    bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);

    totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
                                             rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);

    return ok;
}

// Print camera parameters to the output file
static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
                              const vector<Mat>& rvecs, const vector<Mat>& tvecs,
                              const vector<float>& reprojErrs, const vector<vector<Point2f> >& imagePoints,
                              double totalAvgErr )
{

    FileStorage fs( s.outputFileName, FileStorage::WRITE );

    time_t tm;
    time( &tm );
    struct tm *t2 = localtime( &tm );
    char buf[1024];
    strftime( buf, sizeof(buf)-1, "%c", t2 );

    fs << "calibration_Time" << buf;

    if( !rvecs.empty() || !reprojErrs.empty() )
        fs << "nrOfFrames" << (int)std::max(rvecs.size(), reprojErrs.size());
    fs << "image_Width" << imageSize.width;
    fs << "image_Height" << imageSize.height;
    fs << "board_Width" << s.boardSize.width;
    fs << "board_Height" << s.boardSize.height;
    fs << "square_Size" << s.squareSize;

    if( s.flag & CALIB_FIX_ASPECT_RATIO )
        fs << "FixAspectRatio" << s.aspectRatio;

    if( s.flag )
    {
        sprintf( buf, "flags: %s%s%s%s",
            s.flag & CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess" : "",
            s.flag & CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "",
            s.flag & CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point" : "",
            s.flag & CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : "" );
        fs.writeComment( buf, 0 );

    }

    fs << "flagValue" << s.flag;

    fs << "Camera_Matrix" << cameraMatrix;
    fs << "Distortion_Coefficients" << distCoeffs;

    fs << "Avg_Reprojection_Error" << totalAvgErr;
    if( !reprojErrs.empty() )
        fs << "Per_View_Reprojection_Errors" << Mat(reprojErrs);

    if( !rvecs.empty() && !tvecs.empty() )
    {
        CV_Assert(rvecs[0].type() == tvecs[0].type());
        Mat bigmat((int)rvecs.size(), 6, rvecs[0].type());
        for( int i = 0; i < (int)rvecs.size(); i++ )
        {
            Mat r = bigmat(Range(i, i+1), Range(0,3));
            Mat t = bigmat(Range(i, i+1), Range(3,6));

            CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
            CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
            //*.t() is MatExpr (not Mat) so we can use assignment operator
            r = rvecs[i].t();
            t = tvecs[i].t();
        }
        fs.writeComment(  "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
        fs << "Extrinsic_Parameters" << bigmat;
    }

    if( !imagePoints.empty() )
    {
        Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2);
        for( int i = 0; i < (int)imagePoints.size(); i++ )
        {
            Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols);
            Mat imgpti(imagePoints[i]);
            imgpti.copyTo(r);
        }
        fs << "Image_points" << imagePtMat;
    }
}

// Print camera parameters to the output file
 bool readCameraParams( cv::String  s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
                               vector<Mat>& rvecs,  vector<Mat>& tvecs
                               )
{
    FileStorage fs( s, FileStorage::READ );

	if ( !fs.isOpened())
		return false;

	fs["image_Width"]>>imageSize.width;
	fs["image_Height"]>>imageSize.height;

    fs["Camera_Matrix"]>> cameraMatrix ;
    fs["Distortion_Coefficients"] >> distCoeffs;

	 Mat bigmat;
	 fs["Extrinsic_Parameters"]>>bigmat;
	 int nrows=bigmat.rows;

	 for( int i = 0 ; i < nrows; i++)
	 {
		    Mat r = bigmat(Range(i, i+1), Range(0,3));
            Mat t = bigmat(Range(i, i+1), Range(3,6));
			rvecs.push_back(r);
            tvecs.push_back(t);
	 }

	 return true;
}

bool runCalibrationAndSave(Settings& s, Size imageSize, Mat&  cameraMatrix, Mat& distCoeffs,vector<vector<Point2f> > imagePoints,vector<Mat>& rvecs, vector<Mat> &tvecs )
{
   // vector<Mat> rvecs, tvecs;
    vector<float> reprojErrs;
    double totalAvgErr = 0;

    bool ok = runCalibration(s,imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs,
                             reprojErrs, totalAvgErr);
    cout << (ok ? "Calibration succeeded" : "Calibration failed")
        << ". avg re projection error = "  << totalAvgErr ;

    if( ok )
        saveCameraParams( s, imageSize, cameraMatrix, distCoeffs, rvecs ,tvecs, reprojErrs,
                            imagePoints, totalAvgErr);
    return ok;
}


#ifndef RAY_HPP_TVFS3AKQ
#define RAY_HPP_TVFS3AKQ

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

namespace ca
{
namespace scrollgrid
{

template<typename Scalar>
class Ray3 {
 public:
  typedef Eigen::Matrix<Scalar, 3, 1> Vec3;

 public:
  /**
   * @param origin: origin of ray
   * @param direction: *unit* direction of ray
   *
   * if you want to use origin/endpoint
   * Ray3(origin, (endpoint-origin).normalized());
   *
   */
  Ray3(const Vec3& origin,
       const Vec3& direction) :
         origin(origin),
         direction(direction),
         tmin(Scalar(0)),
         tmax(std::numeric_limits<Scalar>::max()),
         invdir(Scalar(1.) / direction.array())
  {
    sign[0] = (invdir.x() < 0);
    sign[1] = (invdir.y() < 0);
    sign[2] = (invdir.z() < 0);
  }

 public:
  Vec3 point_at(Scalar t) const {
    return origin + t*direction;
  }

 public:
  Vec3 origin, direction;
  Scalar tmin, tmax; /// ray min and max distances
  Vec3 invdir; // for convenience in AABB intersection
  int sign[3];

};

}
} /* ca */
#endif /* end of include guard: RAY_HPP_TVFS3AKQ */


#include <fstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include "MatrixUtil.h"

void MatrixUtil::matrixRemoveRow(Eigen::MatrixXd & matrix, unsigned int rowToRemove)
{
    unsigned int numRows = matrix.rows()-1;
    unsigned int numCols = matrix.cols();

    if( rowToRemove < numRows )
        matrix.block(rowToRemove,0,numRows-rowToRemove,numCols) = matrix.block(rowToRemove+1,0,numRows-rowToRemove,numCols);

    matrix.conservativeResize(numRows,numCols);
}

void MatrixUtil::matrixRemoveColumn(Eigen::MatrixXd& matrix, unsigned int colToRemove)
{
    unsigned int numRows = matrix.rows();
    unsigned int numCols = matrix.cols()-1;

    if( colToRemove < numCols )
        matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.block(0,colToRemove+1,numRows,numCols-colToRemove);

    matrix.conservativeResize(numRows,numCols);
}

Eigen::MatrixXd MatrixUtil::loadMatrix(std::string filename, char delimiter)
{
    std::ifstream ifs(filename, std::ifstream::in);
    if (!ifs)
    {
        throw std::runtime_error("Cannot read file '" + filename + "'!");
    }

    // Read number of columns from first line
    std::string delim(1, delimiter);
    std::string line;
    std::getline(ifs, line);
    std::vector<std::string> strs;
    boost::split(strs, line, boost::is_any_of(delim));
    auto cols = boost::lexical_cast<unsigned>(strs.back());

    Eigen::MatrixXd mat = Eigen::MatrixXd::Zero(0, cols);

    // Read matrix entries from rest of file
    unsigned i = 0;
    while (std::getline(ifs, line))
    {
        mat.conservativeResize(i+1, cols);

        strs.clear();
        boost::split(strs, line, boost::is_any_of(delim));

        for (unsigned j = 0; j < cols; j++)
        {
            auto val = boost::lexical_cast<float>(strs[j]);
            mat(i, j) = val;
        }
        i++;
    }

    ifs.close();

    return mat;
}

void MatrixUtil::saveMatrix(const Eigen::MatrixXd & mat, std::string filename, char delimiter)
{
    std::ofstream ofs(filename, std::ifstream::out);

    if (!ofs)
    {
        throw std::runtime_error("Cannot open file '" + filename + "' for writing!");
    }

    unsigned rows = mat.rows();
    unsigned cols = mat.cols();

    // Write first line with number of dimensions
    ofs << cols << '\n';

    // Write rest of matrix to file
    for (unsigned i = 0; i < rows; i++)
    {
        for (unsigned j = 0; j < cols; j++)
        {
            ofs << mat(i, j);
            if (j < cols - 1)
            {
                ofs << delimiter;
            }
        }
        ofs << '\n';
    }

    ofs.close();
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE regular_grid_test

// Standard includes
#include <fstream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/orbitals.h"
#include "votca/xtp/regular_grid.h"

using namespace votca::xtp;
using namespace std;

BOOST_AUTO_TEST_SUITE(regular_grid_test)

AOBasis CreateBasis(const QMMolecule& mol) {

  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/regular_grid/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, mol);
  return aobasis;
}

BOOST_AUTO_TEST_CASE(regular_grid_build) {

  QMMolecule mol("none", 0);

  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/regular_grid/molecule.xyz");
  AOBasis aobasis = CreateBasis(mol);

  Regular_Grid grid;
  Eigen::Array<votca::Index, 3, 1> steps(5, 5, 5);
  Eigen::Array3d padding(1.0, 1.0, 1.0);
  grid.GridSetup(steps, padding, mol, aobasis);

  BOOST_CHECK_EQUAL(grid.getGridSize(), 125);
  BOOST_CHECK_EQUAL(grid.getBoxesSize(), 1);

  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[0].x(), -2.1888606344960548, 1e-5);
  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[1].z(), -1.0944303172480274, 1e-5);
  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[2].z(), 0, 1e-5);
  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[30].y(), -1.0944303172480274, 1e-5);
  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[45].z(), -2.18886063, 1e-5);
}

BOOST_AUTO_TEST_CASE(regular_grid_build_large) {

  QMMolecule mol("none", 0);

  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/regular_grid/molecule.xyz");
  auto extend = mol.CalcSpatialMinMax();
  AOBasis aobasis = CreateBasis(mol);

  Regular_Grid grid;
  Eigen::Array<votca::Index, 3, 1> steps(30, 30, 30);
  Eigen::Array3d padding(1.0, 1.0, 1.0);
  grid.GridSetup(steps, padding, mol, aobasis);
  auto max = extend.second.array() + padding + 1e-9;
  auto min = extend.first.array() - padding - 1e-9;
  for (auto& a : grid) {
    for (auto& point : a.getGridPoints()) {
      bool check_larger = (point.array() > max).any();
      BOOST_CHECK_EQUAL(check_larger, false);
      if (check_larger) {
        std::cout << point.transpose() << std::endl;
        std::cout << max.transpose() << std::endl;
      }
      bool check_smaller = (point.array() < min).any();
      BOOST_CHECK_EQUAL(check_smaller, false);
      if (check_smaller) {
        std::cout << point.transpose() << std::endl;
        std::cout << min.transpose() << std::endl;
      }
    }
  }

  BOOST_CHECK_EQUAL(grid.getGridSize(), 27000);
  BOOST_CHECK_EQUAL(grid.getBoxesSize(), 54);

  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[0].x(), min.x(), 1e-5);
  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[0].y(), min.y(), 1e-5);
  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[0].z(), min.z(), 1e-5);
  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[30].y(), -2.0379047286687406, 1e-5);
  BOOST_CHECK_CLOSE(grid[0].getGridPoints()[45].z(), 0.075477952913657109,
                    1e-5);
}

BOOST_AUTO_TEST_SUITE_END()


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(Image_ImageData)
//****************************************************************************
/**
 * @file
 * @author Christian Winne
 * \ingroup Image_ImageData
 */

#ifndef openOR_Image_Image3DRawData_hpp
#define openOR_Image_Image3DRawData_hpp

#include <boost/tr1/memory.hpp>

#include <openOR/Plugin/CreateInterface.hpp>
#include <openOR/Math/vector.hpp>
#include <openOR/Utility/Types.hpp>

namespace openOR {
   namespace Image {

     /**
      * \brief Image3DRawData
      * \ingroup Image_ImageData
      */
     template<class Type>
     class Image3DRawData
     {
       public:

         typedef Type ValueType;
         typedef ValueType* Pointer;
         typedef ValueType const* ConstPointer;
         typedef ValueType& Reference;
         typedef ValueType const& ConstReference;

         typedef ValueType value_type;
         typedef Pointer pointer;
         typedef ConstPointer const_pointer;
         typedef Reference reference;
         typedef ConstReference const_reference;

         /**
         * Getter for the volume-size as a Size3UI object in indices
         * @return The volume size in indices
         */
         virtual Math::Vector3ui size() const = 0;

         /**
         * Setter for the volume-size as a Size3UI object in indices
         * @param size The volume size in indices
         */
         virtual void setSize(const Math::Vector3ui& size) = 0;

         virtual Pointer mutableData() = 0;

         virtual ConstPointer data() const = 0;

     };

     typedef Image3DRawData<uint8> Image3DRawDataUI8;
     typedef Image3DRawData<uint16> Image3DRawDataUI16;

   }

  /**
  * Getter for the width of the whole volume object in indices
  * @return The width of the volume object
  */
  template<class Type>
  const uint width(std::tr1::shared_ptr<const Image::Image3DRawData<Type> > pVolumeData) { return pVolumeData->size()(0); }

  /**
  * Getter for the height of the whole volume object in indices
  * @return The height of the volume object
  */
  template<class Type>
  const uint height(std::tr1::shared_ptr<const Image::Image3DRawData<Type> > pVolumeData) { return pVolumeData->size()(1); }

  /**
  * Getter for the depth of the whole volume object in indices
  * @return The depth of the volume object
  */
  template<class Type>
  const uint depth(std::tr1::shared_ptr<const Image::Image3DRawData<Type> > pVolumeData) { return pVolumeData->size()(2); }

  template<class Type>
  unsigned int indexPosition(std::tr1::shared_ptr<const Image::Image3DRawData<Type> > pVolumeData, const Math::Vector3ui& vecPosition)
  {
     return vecPosition(0) + width(pVolumeData) * (vecPosition(1) + height(pVolumeData) * vecPosition(2));
  }

  template<class Type>
  unsigned int indexPosition(std::tr1::shared_ptr<const Image::Image3DRawData<Type> > pVolumeData, unsigned int x, unsigned int y, unsigned int z)
  {
     return x + width(pVolumeData) * (y + height(pVolumeData) * z);
  }

  template<class Type>
  Type& voxelIndexRef(std::tr1::shared_ptr<Image::Image3DRawData<Type> > pVolumeData, const Math::Vector3ui& vecPosition)
  {
    std::tr1::shared_ptr<const Image::Image3DRawData<Type> > pConstVolumeData = pVolumeData;
    return *(pVolumeData->mutableData() + vecPosition(0) + width(pConstVolumeData) * (vecPosition(1) + height(pConstVolumeData) * vecPosition(2)));
  }

  template<class Type>
  const Type& voxelIndexRef(std::tr1::shared_ptr<const Image::Image3DRawData<Type> > pVolumeData, const Math::Vector3ui& vecPosition)
  {
    return *(pVolumeData->data() + vecPosition(0) + width(pVolumeData) * (vecPosition(1) + height(pVolumeData) * vecPosition(2)));
  }

  template<class Type>
  Type& voxelIndexRef(std::tr1::shared_ptr<Image::Image3DRawData<Type> > pVolumeData, const uint index)
  {
    return *(pVolumeData->mutableData() + index);
  }

  template<class Type>
  const Type& voxelIndexRef(std::tr1::shared_ptr<const Image::Image3DRawData<Type> > pVolumeData, const uint index)
  {
    return *(pVolumeData->data() + index);
  }

  /************************************************************************/
  /*                                                                      */
  /************************************************************************/

  ///**
  // * \brief VolumeCreator
  // * \ingroup Volume_VolumeRawData
  // */
  //template<class Volume>
  //struct VolumeCreator
  //{
  //  typedef Volume VolumeType;

  //  virtual std::tr1::shared_ptr<VolumeType> operator()() const = 0;
  //  virtual void setVolume(std::tr1::shared_ptr<VolumeType> pVolume) = 0;
  //  virtual std::tr1::shared_ptr<VolumeType> getVolume() = 0;
  //};

  //typedef VolumeCreator<VolumeRawDataUI8> VolumeRawDataUI8Creator;
  //typedef VolumeCreator<VolumeRawDataUI16> VolumeRawDataUI16Creator;

  /************************************************************************/
  /*                                                                      */
  /************************************************************************/

}

OPENOR_CREATE_TEMPLATED_INTERFACE_1(Type, openOR::Image::Image3DRawData<Type>)

  Math::Vector3ui size() const { return adaptee()->size(); }
  void setSize(const Math::Vector3ui& size) { adaptee()->setSize(size); }

  Type* mutableData() { return adaptee()->mutableData(); }
  const Type* data() const { return adaptee()->data(); }

OPENOR_CREATE_INTERFACE_END

//
//OPENOR_CREATE_TEMPLATED_INTERFACE_1(Volume, openOR::VolumeCreator<Volume>)
//  std::tr1::shared_ptr<Volume> operator()() const { return adaptee()->operator()(); }
//  void setVolume(std::tr1::shared_ptr<Volume> pVolume) { adaptee()->setVolume(pVolume); }
//  std::tr1::shared_ptr<Volume> getVolume() { return adaptee()->getVolume(); }
//
//OPENOR_CREATE_INTERFACE_END

#endif


#pragma once

#include <boost/hana/set.hpp>
#include <boost/hana/type.hpp>
#include <boost/hana/size.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/drop_while.hpp>
#include <boost/hana/integral_constant.hpp>

namespace ecfw {
namespace detail {

    namespace bh = boost::hana;

    template <typename T>
    using type = bh::basic_type<T>;

    template <typename T>
    constexpr auto type_v = type<T>{};

    template <typename... Ts>
    using type_list = bh::tuple<bh::basic_type<Ts>...>;

    template <typename... Ts>
    constexpr auto type_list_v = type_list<Ts...>{};

    template <typename Iterable>
    constexpr auto is_unique(const Iterable& iterable) {
        return bh::size(bh::to<bh::set_tag>(iterable)) == bh::size(iterable);
    }

    template <typename T, typename Iterable>
    constexpr auto index_of(const T& element, const Iterable& iterable) {
        auto size = decltype(bh::size(iterable)){};
        auto dropped = decltype(bh::size(
            bh::drop_while(iterable, bh::not_equal.to(element))
        )){};
        return size - dropped;
    }

} // namespace detail
} // namespace ecfw


#include <Common/Timer.h>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/tuple/tuple.hpp>
#include <cstdlib>
#include <iostream>
#include <random>
#include <vector>
#include <memory>

class MemBlock
{
public:
  template <typename Generator_t>
  MemBlock(size_t size, Generator_t& g)
      : m_size(size)
  {
    m_rawMemory = (char*)malloc(size);
    for (size_t i = 0; i < (size) / sizeof(size_t); ++i)
    {
      *(size_t*)(m_rawMemory + i) = g();
    }
  }

  ~MemBlock()
  {
    free(m_rawMemory);
  }
  size_t m_size;
  char* m_rawMemory;
};

template <typename Generator_t>
double* GetDoublePointer(MemBlock& m, size_t length, Generator_t& g)
{
  auto position = g();
  while ((position + length * (sizeof(double) / (sizeof(char*)))) >= m.m_size)
  {
    position = g();
  }

  return reinterpret_cast<double*>(m.m_rawMemory + position);
}

int main(int argc, char* argv[])
{
  size_t size = boost::lexical_cast<size_t>(argv[1]);

  std::uniform_int_distribution<size_t> distribution(0, size / 10);
  std::mt19937 engine;
  auto generator = [&]
  {
    return distribution(engine);
  };

  MemBlock m(size, generator);

  size_t length = size / 10;

  auto theFunction = [](double a, double b, double c, double d)
  {
    return (a + b) / (c + d);
  };

  double sum1 = 0.0;
  {
    Timer t("Simple loops");
    for (size_t pass = 0; pass < 100; ++pass)
    {
      double* ws = GetDoublePointer(m, length, generator);
      double* xs = GetDoublePointer(m, length, generator);
      double* ys = GetDoublePointer(m, length, generator);
      double* zs = GetDoublePointer(m, length, generator);

      for (int i = 0; i < length; ++i)
      {
        sum1 += theFunction(ws[i], xs[i], ys[i], zs[i]);
      }
    }
  }

  double sum2 = 0.0;
  {
    Timer t("Zip iterators");
    for (size_t pass = 0; pass < 100; ++pass)
    {
      double* ws = GetDoublePointer(m, length, generator);
      double* xs = GetDoublePointer(m, length, generator);
      double* ys = GetDoublePointer(m, length, generator);
      double* zs = GetDoublePointer(m, length, generator);

      std::for_each(boost::make_zip_iterator(boost::make_tuple(ws, xs, ys, zs)),
                    boost::make_zip_iterator(boost::make_tuple(
                        ws + length, xs + length, ys + length, zs + length)),
                    [&](const boost::tuple<double, double, double, double>& t)
                    {
                      sum2 += theFunction(t.get<0>(), t.get<1>(), t.get<2>(),
                                          t.get<3>());
                    });
    }
  }

  return sum1 == sum2;
}


/*
Copyright (c) 2013, Sibt ul Hussain <sibt.ul.hussain at gmail dot com>
All rights reserved.
Released under BSD License
-------------------------
For license terms please see license.lic
*/
#include "includeHeaders.h"

#include "processImage.h"
#include <boost/program_options.hpp>
namespace po = boost::program_options;
UINT Features::xcell = 8, Features::ycell = 8, Features::norient = 36,
		Features::foffset = 8, Features::cellsize = 8;
//Pyramid Features::pyobj(64,128,10,Bilinear);
//PyramidType Features::sspace = NoPyramid;
#include <iterator>
class Process {
public:
	Process(po::variables_map &obj, HistParams &histparams) {
		ProcessInput(obj, histparams);
	}
	void ProcessInput(po::variables_map &obj, HistParams &histparams);
	~Process() {
		delete fobj;
		delete hogroot;
	}
private:
	string cname;
	Features *fobj;
	HOGRoot *hogroot;
	ProcessImage *pim;
};

void Process::ProcessInput(po::variables_map &obj, HistParams &histparams) {
	cname = obj["Class"].as<string> ();
	UINT height = obj["Win-Height"].as<int> (), width =
			obj["Win-Width"].as<int> (),
			cellsize = obj["Cell-Size"].as<int> (), radius = obj["Radius"].as<
					int> (), npoints = obj["Number-Points"].as<int> (),
			xstride = obj["X-Stride"].as<int> (), ystride = obj["Y-Stride"].as<
					int> (), nplevels = obj["Number-Levels"].as<int> (),
			nflevels = obj["Feature-Levels"].as<int> (), lbpstride =
					obj["LBP-Stride"].as<int> (), ftype =
					obj["FeatureType"].as<int> (), extent = obj["Extent"].as<
					int> (), norient = obj["NORIENT"].as<int> (), ltol =
					obj["LTP-TLevels"].as<int> (), plsdim = obj["PLS-Dim"].as<
					int> (), nbins = obj["LBP-NBins"].as<int> (), skip =
					obj["Skip"].as<int> (), detvalue =
					obj["Detector"].as<int> (), csize =
					obj["CacheSize"].as<int> ();
	plsdim = (plsdim > 0 ? plsdim : 0);
	csize = pow(2.0, 20.0) * csize;
	NORM norm = (NORM) obj["Normalization"].as<int> ();
	Channel ch = (Channel) obj["Color-Channel"].as<int> ();

	REAL svmj = obj["SVM-J"].as<REAL> (), svmc = obj["SVM-C"].as<REAL> (),
			gnorm = obj["Gamma-Normalization"].as<REAL> (), tolerance =
					((REAL) obj["LTP-Tolerance"].as<int> ()) / 255.0, alpha =
					obj["Cont-Alpha"].as<REAL> (), tou =
					(REAL) obj["Cont-Tou"].as<int> () / 255, k1 =
					obj["DoG-K1"].as<REAL> (), k2 = obj["DoG-K2"].as<REAL> ();

	bool dog = obj.count("DoG"), coneq = obj.count("Cont-Equal"), foldfeat =
			obj.count("Fold-Features"), embedSVM = (obj["Embed-SVM"].as<int> ()
			== 1 ? true : false), addrgb =
			(obj["Add-RGBLBP"].as<int> () == 1 ? true : false), nseparately =
			(obj["Norm-Sep"].as<int> () == 1 ? true : false), qda = (detvalue
			== 2 ? true : false);

	pim = new ProcessImage(gnorm, dog, k1, k2, coneq, tou, alpha, ch);
	cout << "\n Folding Parameter = " << foldfeat << endl;
	LBPParams lbpparam;
	lbpparam.cellsize = cellsize;
	lbpparam.stride = xstride;
	lbpparam.nbins = nbins;
	lbpparam.radius = radius;
	lbpparam.npoints = npoints;
	lbpparam.addfeat = addrgb;
	lbpparam.normfeatsep = nseparately;
	lbpparam.featnorm = norm;
	lbpparam.nflevels = nflevels;
	lbpparam.folded = foldfeat;
	lbpparam.gridtype = (GridType) obj["LBPGridType"].as<int> ();
	lbpparam.tolerance = tolerance;// used for the ltp features
	lbpparam.nthrlevels = 1;
	lbpparam.extent = extent;
	lbpparam.useblocknorm = obj.count("BlockNorm");
	lbpparam.issigned = obj.count("Signed");
	lbpparam.usegradmag = obj.count("UseGradMag");

	HOGParams hogparams;
	hogparams.cell = 8;
	hogparams.norient = norient;
	hogparams.chan = ch;

	if (obj.count("Neg-Train-File"))
		histparams.nfname = obj["Neg-Train-File"].as<string> ();
	else
		histparams.nfname = "";

	if (obj.count("Pos-Train-File"))
		histparams.pfname = obj["Pos-Train-File"].as<string> ();
	else
		histparams.pfname = "";

	int sparse = 0;
	if (obj.count("HF-Type")) {
		histparams.ftype = (WindowFeatureTypes) ftype;
		HistFeatures type = (HistFeatures) obj["HF-Type"].as<int> ();
		HistogramFeatures *histfeat;
		switch (type) {
		case HF_BIN:
			histfeat = new HistogramFeatures(lbpparam, hogparams, width,
					height, nplevels, *pim, histparams);
			histfeat->GenerateHistogramInformation();
			sparse = histparams.nbins; // use sparse svm learning mechanisms...
			break;
		case HF_BIN_TRAIN:
			histfeat = new HistogramFeaturesTrain(lbpparam, hogparams, width,
					height, nplevels, *pim, histparams);
			histfeat->GenerateHistogramInformation();
			break;
		case HF_LHRATIO:// liklihood ratio
		default:
			histfeat = new HistogramFeaturesLLRatio(lbpparam, hogparams, width,
					height, nplevels, *pim, histparams);
			histfeat->GenerateHistogramInformation();

		}
		fobj = histfeat;
	} else if (obj.count("Combined-Feature-Type") || obj.count(
			"Feature-ConfigFile")) {
		fobj = new MultipleWindowFeatures(lbpparam, width, height, nplevels,
				*pim, obj);

	} else {
		switch (ftype) {
		case WFT_LBP:
			fobj = new LBPFeatures(lbpparam, width, height, nplevels, *pim);
			break;
		case WFT_HOG:
			fobj = new HOGFeatures(width, height, nplevels);
			break;
		case WFT_HOG_LBP:
			fobj = new HOGLBPFeatures(cellsize, npoints, radius, width, height,
					nplevels, norm, nflevels, lbpstride, *pim);
			//		fobj =new ProjectedLBPFeatures(cellsize,npoints,radius,
			//				width,height,nplevels,norm,nflevels,lbpstride,*pim,foldfeat,
			//				norient);
			break;
		case WFT_HOG_COLOR:
			fobj = new HOGColorFeatures(width, height, nplevels);
			break;
		case WFT_LBP_LTP:
			fobj = new LTPFeatures(cellsize, npoints, radius, width, height,
					nplevels, norm, nflevels, lbpstride, *pim, foldfeat,
					tolerance);
			break;
		case WFT_HOG_LBP_LTP:
			fobj
					= new HOGLTPFeatures(cellsize, npoints, radius, width,
							height, nplevels, norm, nflevels, lbpstride, *pim,
							false, tolerance);
			break;
		case WFT_PROJ_HOG:
			fobj = new ProjectedFeatures(width, height, nplevels, norient, ch);
			break;
		case WFT_LTPX:
			fobj = new OnlyLTPFeaturesX(cellsize, npoints, radius, width,
					height, nplevels, norm, nflevels, lbpstride, *pim,
					foldfeat, tolerance, ltol);
			break;
		case WFT_LBP256:
			fobj = new LBPFeatures256(cellsize, npoints, radius, width, height,
					nplevels, norm, nflevels, lbpstride, *pim);
			break;

		case WFT_CSLTP:
			//			fobj = new CSLTPFeatures(cellsize, npoints, radius, width, height,
			//					nplevels, norm, nflevels, lbpstride, *pim, foldfeat,
			//					tolerance);
			cout << " Not Defined THis option CS_LTP";
			exit(EXIT_FAILURE);
			break;
		case WFT_LTP:
			fobj = new OnlyLTPFeatures(cellsize, npoints, radius, width,
					height, nplevels, norm, nflevels, lbpstride, *pim,
					foldfeat, tolerance);
			break;
		case WFT_PROJ_HOG_LBP_LTP_COLOR:
			fobj = new ProjectedLTPFeaturesRGB(cellsize, npoints, radius,
					width, height, nplevels, norm, nflevels, lbpstride, *pim,
					foldfeat, addrgb, nbins, nseparately, tolerance, norient,
					Circular);
			break;
		case WFT_PROJ_HOG_LBP_COLOR:
			fobj = new ProjectedLBPFeaturesRGB(cellsize, npoints, radius,
					width, height, nplevels, norm, nflevels, lbpstride, *pim,
					foldfeat, addrgb, nbins, nseparately, norient);
			break;
		case WFT_LBP_COLOR:
			if (obj.count("UseMeanValue"))
				fobj = new MeanLBPFeaturesRGB(lbpparam, width, height,
						nplevels, *pim);
			else
				fobj = new LBPFeaturesRGB(lbpparam, width, height, nplevels,
						*pim);
			break;
		case WFT_HOG_LBP_COLOR:
			fobj = new HOGLBPFeaturesRGB(lbpparam, width, height, nplevels,
					*pim, hogparams);
			break;
		case WFT_LTP_COLOR:
			fobj = new OnlyLTPFeaturesRGB(lbpparam, width, height, nplevels,
					*pim);
			break;
		case WFT_HOG_LTP_COLOR:
			fobj = new HOGOnlyLTPFeaturesRGB(lbpparam, width, height, nplevels,
					*pim, hogparams);
			break;
		case WFT_LBP_LTP_COLOR:
			fobj = new LTPFeaturesRGB(lbpparam, width, height, nplevels, *pim);
			break;

		case WFT_HOG_LBP_LTP_COLOR:
			fobj = new HOGLTPFeaturesRGB(lbpparam, width, height, nplevels,
					*pim, hogparams);
		case WFT_CSLBP:
			fobj = new CSLBPFeatures(lbpparam, width, height, nplevels, *pim);
			break;
		case WFT_CSLBP_COLOR:
			fobj
					= new CSLBPFeaturesRGB(lbpparam, width, height, nplevels,
							*pim);
			break;
		case WFT_CSLTP_COLOR:
			fobj
					= new CSLTPFeaturesRGB(lbpparam, width, height, nplevels,
							*pim);
			break;
		default:
			fobj = new LBPFeatures(lbpparam, width, height, nplevels, *pim);
		}
	}
#ifdef TMPDEBUG
	//	fobj->TestRoutine();
#endif
	hogroot = new HOGRoot(cname, width, height, xstride, ystride, extent,
			*fobj, plsdim, embedSVM, skip, qda, sparse, svmc);
	if (obj.count("Train")) {
		if (!obj.count("Pos-Train-File") || !obj.count("Neg-Train-File")) {
			cout
					<< "TRAIN : Positive or Negative Training List file is missing "
					<< endl << " So Aborting .... " << endl;
			return;
		}
		string plfile = obj["Pos-Train-File"].as<string> (), nlfile =
				obj["Neg-Train-File"].as<string> ();
		hogroot->Run(cname, plfile, nlfile, svmj, csize);

	} else if (obj.count("Plot")) {
		PLOTYPE ptype = (PLOTYPE) obj["Plot"].as<int> ();
		string lpimages, lnimages;
		if (ptype == 1) {
			if (!obj.count("Pos-Test-File") || !obj.count("Neg-Test-File")) {
				cout
						<< "DET Plot: Positive or Negative Test List fileM is missing "
						<< endl << " So Aborting .... " << endl;
				return;
			}
			lpimages = obj["Pos-Test-File"].as<string> ();
			lnimages = obj["Neg-Test-File"].as<string> ();
		}
		if (ptype == 2) {
			if (!obj.count("Test-File")) {
				cout << "PR Plot: Test Images List file is missing " << endl
						<< " So Aborting .... " << endl;
				return;
			}
			lpimages = obj["Test-File"].as<string> ();
		}
		string lmfile;
		if (!obj.count("Model-File"))
			lmfile = cname + ".mod";
		REAL rth = obj["ReduceThreshold"].as<REAL> ();
		hogroot->GeneratePlotData(cname, lpimages, lnimages, lmfile, ptype, rth);
	} else if (obj.count("Test")) {

	} else {
		cout << " Neither of Training, Plotting or Testing Option Selected  "
				<< endl << " So Aborting .... " << endl;
		return;
	}
}
int main(int ac, char* av[]) {
	try {

		po::options_description mainopt("Detector Options");
		mainopt.add_options()("help", "produce help message")("Class",
				po::value<string>()->default_value("person"), "Class Name")(
				"Train", "Train Detector")("Test", po::value<string>(),
				"Test Detector: Name of File")("Plot",
				po::value<int>()->default_value(1),
				"Generate Plot Data \n 1.DET Curve \n 2.PR Curve")("Detector",
				po::value<int>()->default_value(1),
				"Detector to be Used \n 1.Linear SVM \n 2.QDA Classifier")(
				"Color-Channel", po::value<int>()->default_value(4),
				"Image Color Channel to use "
					"\n 0. Red Channel \n"
					" 1. Green Channel \n"
					" 2. Blue Channel \n"
					" 3. Gray Scale \n"
					" 4. RGB (Default) \n"
					" 5. O1: luminance component...\n"
					" 6. O2 :Red-Green channel \n"
					" 7. O3 :Blue-Yellow channel "
					" 8. HSL ,\n"
					" 9. LAB ,\n"
					" 10. WandellOCS ,\n"
					" 11. SandeOCS ,\n"
					" 12. ABSGradientMag")(
				"FeatureType",
				po::value<int>()->default_value(1),
				"Features Type \n1. WFT_LBP,"
					"\n2. WFT_HOG,"
					"\n3. WFT_HOG_LBP,"
					"\n4. WFT_HOG_COLOR(HOG+Color Histogram),"
					"\n5. WFT_LBP_LTP,"
					"\n6. WFT_HOG_LBP_LTP,"
					"\n7. WFT_PROJ_HOG,"
					"\n8. WFT_LTPX,"
					"\n9. WFT_LBP256,"
					"\n10. WFT_CSLBP,"
					"\n11. WFT_CSLTP,"
					"\n12. WFT_LTP,"
					"\n13. WFT_PROJ_HOG_LBP_LTP_COLOR,  // BUG IN COMPUTATION OF DET, NO PROBLEM FOR PR,"
					"\n14. WFT_PROJ_HOG_LBP_COLOR	,"
					"\n15. WFT_LBP_COLOR,"
					"\n16. WFT_LTP_COLOR,"
					"\n17. WFT_LBP_LTP_COLOR,"
					"\n18. WFT_HOG_LBP_COLOR,"
					"\n19. WFT_HOG_LTP_COLOR,"
					"\n20. WFT_HOG_LBP_LTP_COLOR"
					"\n21. WFT_CSLBP_COLOR"
					"\n22. WFT_CSLTP_COLOR")("Add-RGBLBP",
				po::value<int>()->default_value(1),
				"1. Add RGB-Channels Hist\n 2. Concat RGB-Channels Hist")(
				"BlockNorm",
				"Use Neigbhour block normalization for LBP(by default false)")(
				"Norm-Sep", po::value<int>()->default_value(1),
				"1. Normalize Each RGB channel & then add"
					" \n 0.Add RGB channels and then Normalize")("Embed-SVM",
				po::value<int>()->default_value(1), "Include SVM training ...")(
				"Skip", po::value<int>()->default_value(0),
				"Skip Number of Pixels on Borders")("LBP-NBins",
				po::value<int>()->default_value(59),
				"Number of LBPBins can be either 256 or 59")("Fold-Features",
				"Valid for Feature-Type 1,5,7 & 8")("PLS-Dim",
				po::value<int>()->default_value(0), "PLS Projection Dimension")// features will be projected to that dimension
		("Cell-Size", po::value<int>()->default_value(16),
				"LBP Feature: Cell size")("Radius",
				po::value<int>()->default_value(1), "Radius of LBP Features")(
				"Number-Points", po::value<int>()->default_value(8),
				"Number of Points around each point")("Feature-Levels",
				po::value<int>()->default_value(1),
				"Number of Feature Levels(default = 1)")("LTP-Tolerance",
				po::value<int>()->default_value(5),
				"+,- Threshold for the LTP Features")("LTP-TLevels", po::value<
				int>()->default_value(1), "Number of Tolerance Levels for LTPs")(
				"NORIENT", po::value<int>()->default_value(36),
				"Number of Orientations for Projected HOG")(
				"Gamma-Normalization", po::value<REAL>()->default_value(1),
				"Gamma Normalization for LBP & LTP Features")("DoG",
				"Apply DoG Filter")("DoG-K1", po::value<REAL>()->default_value(
				1), "DoG filter Inner kernel Stdev")("DoG-K2",
				po::value<REAL>()->default_value(3),
				"DoG filter outer kernel Stdev")("Cont-Equal",
				"Perform Contrast Equalization")("Cont-Alpha",
				po::value<REAL>()->default_value(0.1),
				"Contrast Equalization:Alpha value")("Cont-Tou",
				po::value<int>()->default_value(10),
				"Contrast Equalization:Threshold value")("Normalization",
				po::value<int>()->default_value(L1sqrt),
				"Block Normalization:\n 0. L1 Norm  \n 1. L1Sqrt Norm"
					" \n 2. L2 Norm \n 3. LOG2 \n 4. LOG2 followed by L1-Norm")(
				"Number-Levels", po::value<int>()->default_value(10),
				"Number of Levels in each Pyramid Octave")("X-Stride",
				po::value<int>()->default_value(8), "X Stride in Pixels")(
				"Y-Stride", po::value<int>()->default_value(8),
				"Y Stride in Pixels")("LBP-Stride",
				po::value<int>()->default_value(16),
				"LBP Block Stride in Pixels")("Win-Width",
				po::value<int>()->default_value(64), "Detection Window Width")(
				"Win-Height", po::value<int>()->default_value(128),
				"Detection Window Height")("Extent",
				po::value<int>()->default_value(16), "Pos Cropped Image Border")(
				"SVM-J", po::value<REAL>()->default_value(3),
				"SVM J: Relative Weighting of +ve to -ve Examples")(
				"Pos-Train-File", po::value<string>(), "Pos Train:  List File")(
				"Neg-Train-File", po::value<string>(), "Neg Train: List File")(
				"Pos-Test-File", po::value<string>(),
				"Pos Test:  List File for DET Computation")("Neg-Test-File",
				po::value<string>(), "Neg Test:  List File for DET Computation")(
				"Test-File", po::value<string>(),
				"PR Curve: List File of Images")("Model-File",
				po::value<string>(), "Name of Learned Model file")("CacheSize",
				po::value<int>()->default_value(1024),
				"Cache Size for the 2nd Stage Training of the detector")(
				"SVM-C", po::value<REAL>()->default_value(1),
				"SVM-C Paramters(1 means select by Heuristic)")("LBPGridType",
				po::value<int>()->default_value(0),
				"0. Circular\n 1. Rectangular")("UseGradMag",
				"If Specified Use Gradient Magnitude for quantization in LBP/CSLBP/LTP")(
				"Signed", "Use Signed CSLBP/LTP")("UseMeanValue",
				"Compare with mean of patch instead of center pixel value")(
				"Combined-Feature-Type,F", po::value<vector<int> >(),
				"Specify Multiple features to have combined features from the Feature-Type")(
				"Feature-ConfigFile", po::value<vector<string> >(),
				"Feature Configuration File")("ReduceThreshold",
				po::value<REAL>()->default_value(0),
				"Reduce the Threshold by subtracting this value");

		HistParams histparams;

		po::options_description hfdesc("Histogram of Features:");
		hfdesc.add_options()("HF-Type", po::value<int>(),
				"Histogram Feature Options \n 1. BIN,"
					"\n 2.BIN_TRAIN,"
					"\n 3.LHRATIO,// liklihood ratio")("HF-Nbins", po::value<
				UINT>(&histparams.nbins)->default_value(30),
				"Histogram Features Number of Bins")(
				"HF-Sigma",
				po::value<REAL>(&histparams.sigma)->default_value(5),
				"Gaussian Sigma for Smoothing of Histograms(Only in LHRatio)(0 Means No Smoothing)")(
				"HF-Epsilon",
				po::value<REAL>(&histparams.lhratio_const)->default_value(1e-5),
				"Epsilon For likelihood ratio constant")("HF-Norm", po::value<
				int>()->default_value(0),
				"Same as Original Feature Normalization");
		po::options_description desc;

		desc.add(mainopt).add(hfdesc);
		po::variables_map vm;
		po::store(po::parse_command_line(ac, av, desc), vm);
		po::notify(vm);

		if (vm.count("help")) {
			cout << desc << "\n";
			return 1;
		} else {
			histparams.norm = (NORM) vm["HF-Norm"].as<int> ();
			Process pobj(vm, histparams);
		}
	} catch (exception& e) {
		cerr << "error: " << e.what() << "\n";
		return 1;
	} catch (...) {
		cerr << "Exception of unknown type!\n";
	}

	return 0;
}


/**
 *  @file
 *  @copyright defined in evt/LICENSE.txt
 */
#include <evt/chain/asset.hpp>

// This fixes the issue in safe_numerics in boost 1.69
#include <evt/chain/workaround/boost/safe_numerics/exception.hpp>
#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/rational.hpp>
#include <fc/reflect/variant.hpp>

namespace evt { namespace chain {

symbol
symbol::from_string(const string& from) {
    try {
        auto s = fc::trim(from);

        // Find comma in order to split precision and symbol id
        auto c = s.find(',');
        EVT_ASSERT(c != string::npos, symbol_type_exception, "Symbol's precision and id should be separated with comma");
        FC_ASSERT(s.substr(c + 1, 2) == "S#");

        auto p  = std::stoul(s.substr(0, c));
        auto id = std::stoul(s.substr(c + 3));

        return symbol(p, id);
    }
    EVT_CAPTURE_AND_RETHROW(symbol_type_exception, (from));
}

string
symbol::to_string() const {
    auto str = fc::to_string(precision());
    str.append(",S#");
    str.append(fc::to_string(id()));
    return str;
}

string
asset::to_string() const {
    auto sign = amount_ < 0 ? "-" : "";
    auto abs_amount = std::abs(amount_);

    auto str = fc::to_string(abs_amount);

    if(precision() >= str.size()) {
        auto zeros = precision() - str.size();
        str.insert(0, "0.");
        str.insert(2, zeros, '0');
    }
    else if(precision() > 0) {
        str.insert(str.size() - precision(), 1, '.');
    }

    str.insert(0, sign);

    str.append(" S#");
    str.append(fc::to_string(sym_.id()));
    return str;
}

asset
asset::from_string(const string& from) {
    using namespace boost::safe_numerics;

    try {
        string s = fc::trim(from);

        // Find space in order to split amount and symbol
        auto space_pos = s.find(' ');
        EVT_ASSERT((space_pos != string::npos), asset_type_exception,
                   "Asset's amount and symbol should be separated with space");
        FC_ASSERT(s.substr(space_pos + 1, 2) == "S#");

        auto symbol_str = s.substr(space_pos + 3);
        auto amount_str = s.substr(0, space_pos);

        // Ensure that if decimal point is used (.), decimal fraction is specified
        auto dot_pos = amount_str.find('.');
        if(dot_pos != string::npos) {
            EVT_ASSERT((dot_pos != amount_str.size() - 1), asset_type_exception,
                       "Missing decimal fraction after decimal point");
        }

        // Parse symbol
        auto precision = 0u;
        auto sym_id    = fc::to_uint64(symbol_str);

        if(dot_pos != string::npos) {
            precision = amount_str.size() - dot_pos - 1;
        }

        // Parse amount
        auto amount = safe<int64_t>();
        if(dot_pos != string::npos) {
            amount_str.erase(dot_pos, 1);
        }
        amount = fc::to_int64(amount_str);

        return asset((int64_t)amount, symbol(precision, sym_id));
    }
    EVT_CAPTURE_AND_RETHROW(asset_type_exception, (from));
}

}}  // namespace evt::chain