#ifndef TRAFFIC_GENERATOR_HH
#define TRAFFIC_GENERATOR_HH

#include <thread>
#include <vector>

#include <boost/random/uniform_int_distribution.hpp>

#include "exponential.hh"
#include "random.hh"
#include "remycc.hh"

template<class T>
class TrafficGenerator{
private:
	enum TrafficType {EXPONENTIAL_ON_OFF, DETERMINISTIC_ON_OFF};
	enum SwitchType {TIME_SWITCHED, BYTE_SWITCHED};

	TrafficType _traffic_type;
	SwitchType _switch_type;

	union TrafficParams {
		struct{
			double _mean_off_unit;
			double _mean_on_unit;
			unsigned int num_cycles;
		} _on_off;
	} _traffic_params;

	T &_ctcp;

	std::vector< std::thread > _senders;

	void send_data(int seed, int id);

public:
	TrafficGenerator(T &s_ctcp, double s_mean_on_unit, double s_mean_off_unit, std::string traffic_params)
		:	_traffic_type(TrafficType::EXPONENTIAL_ON_OFF),
			_switch_type(SwitchType::TIME_SWITCHED),
			_traffic_params({ s_mean_off_unit, s_mean_on_unit, (unsigned int)-1 }),
			_ctcp(s_ctcp),
			_senders()
	{
		_traffic_params._on_off._mean_on_unit = s_mean_on_unit;
		_traffic_params._on_off._mean_off_unit = s_mean_off_unit;

		// parse traffic_params
		size_t start_pos = 0;
		while (start_pos < traffic_params.length()) {
			size_t end_pos = traffic_params.find(',', start_pos);
			if (end_pos == std::string::npos)
				end_pos = traffic_params.length();

			std::string arg = traffic_params.substr(start_pos, end_pos - start_pos);
			if (arg == "exponential")
				_traffic_type = TrafficType::EXPONENTIAL_ON_OFF;
			else if (arg == "deterministic")
				_traffic_type = TrafficType::DETERMINISTIC_ON_OFF;
			else if (arg == "byte_switched"){
				_switch_type = SwitchType::BYTE_SWITCHED;
			}
			else if (arg.substr(0, 11) == "num_cycles=")
				_traffic_params._on_off.num_cycles = (unsigned int) \
					atoi(arg.substr(11).c_str());
			else
				std::cout << "Unrecognised parameter: " << arg << endl;

			start_pos = end_pos + 1;
		}
	}

	void spawn_senders(int num_senders);
};

template<class T>
void TrafficGenerator<T>::send_data(int seed, int id) {
	PRNG prng(seed);
	Exponential on (1 / _traffic_params._on_off._mean_on_unit, prng);
	Exponential off (1 / _traffic_params._on_off._mean_off_unit, prng);

	unsigned int flow_id = 0;
	while (1) {
		double off_duration = off.sample();
		double on_duration = on.sample();

		if (_traffic_type == TrafficType::DETERMINISTIC_ON_OFF) {
			off_duration = _traffic_params._on_off._mean_off_unit;
			on_duration = _traffic_params._on_off._mean_on_unit;
		}

		std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(unsigned(off_duration)));

		bool byte_switched = (_switch_type == SwitchType::BYTE_SWITCHED);
		_ctcp.send_data(on_duration, byte_switched, flow_id, id);

		++ flow_id;
		std::cout<<"Sender: "<<id<<", Flow: "<<flow_id<<". Transmitted for "<<on_duration<<(byte_switched?" bytes.":" ms.")<<endl<<std::flush;

		if (flow_id >= _traffic_params._on_off.num_cycles)
			break;
	}
}

template<class T>
void TrafficGenerator<T>::spawn_senders(int num_senders) {
	PRNG prng(global_PRNG());

	// Have only one sender for now, since the NAT creates issues for multiple senders anyway
	assert(num_senders == 1); // feature not yet implemented

	int seed = boost::random::uniform_int_distribution<>()(prng);
	int src_id = boost::random::uniform_int_distribution<>()(prng);
	std::cout<<"Assigning Source ID: "<<src_id<<std::endl;
	send_data(seed, src_id);
}

#endif


/*
 *  Software License Agreement (BSD License)
 *
 *  Robot Operating System code by the University of Osnabrück
 *  Copyright (c) 2015, University of Osnabrück
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   1. Redistributions of source code must retain the above
 *      copyright notice, this list of conditions and the following
 *      disclaimer.
 *
 *   2. Redistributions in binary form must reproduce the above
 *      copyright notice, this list of conditions and the following
 *      disclaimer in the documentation and/or other materials provided
 *      with the distribution.
 *
 *   3. Neither the name of the copyright holder nor the names of its
 *      contributors may be used to endorse or promote products derived
 *      from this software without specific prior written permission.
 *
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 *  TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 *  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 *  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 *  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *
 *
 *  mesh_msgs_transforms.cpp
 *
 *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
 */

#include "mesh_msgs_transform/transforms.h"
#include <Eigen/Eigen>

namespace mesh_msgs_transform{

inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3d& eigen_vec){
  eigen_vec(0) = tf_vec[0];
  eigen_vec(1) = tf_vec[1];
  eigen_vec(2) = tf_vec[2];
}

inline void pointMsgToEigen(const geometry_msgs::Point& gm_point, Eigen::Vector3d& eigen_point){
  eigen_point(0) = gm_point.x;
  eigen_point(1) = gm_point.y;
  eigen_point(2) = gm_point.z;
}

inline void pointEigenToMsg(const Eigen::Vector3d& eigen_point, geometry_msgs::Point& gm_point){
  gm_point.x = eigen_point(0);
  gm_point.y = eigen_point(1);
  gm_point.z = eigen_point(2);
}

bool transformTriangleMeshNoTime(
    const std::string& target_frame,
    const mesh_msgs::TriangleMeshStamped& mesh_in,
    const std::string& fixed_frame,
    mesh_msgs::TriangleMeshStamped& mesh_out,
    const tf::TransformListener&  tf_listener
)
{
  tf::StampedTransform transform;
  try{
    tf_listener.lookupTransform (
      target_frame,
      ros::Time(0), mesh_in.header.frame_id,
      ros::Time(0),
      fixed_frame,
      transform
    );
  }
  catch (tf::LookupException &e){
   ROS_ERROR ("%s", e.what ());
   return false;
  }
  catch (tf::ExtrapolationException &e){
   ROS_ERROR ("%s", e.what ());
   return false;
  }

  tf::Quaternion quaternion = transform.getRotation ();
  Eigen::Quaterniond rotation (
    quaternion.w (),
    quaternion.x (),
    quaternion.y (),
    quaternion.z ()
  );

  tf::Vector3 origin = transform.getOrigin ();
  Eigen::Vector3d eigen_origin;
  vectorTfToEigen(origin, eigen_origin);
  Eigen::Translation3d translation ( eigen_origin );
  Eigen::Affine3d transformation = translation * rotation;

  if (&mesh_in != &mesh_out){
    mesh_out.header = mesh_in.header;
    mesh_out.header.stamp = ros::Time::now();
    mesh_out.mesh.triangles = mesh_in.mesh.triangles;
    mesh_out.mesh.vertex_colors = mesh_in.mesh.vertex_colors;
    mesh_out.mesh.triangle_colors = mesh_in.mesh.triangle_colors;
    mesh_out.mesh.vertex_texture_coords = mesh_in.mesh.vertex_texture_coords;
    mesh_out.mesh.face_materials = mesh_in.mesh.face_materials;
    mesh_out.mesh.textures = mesh_in.mesh.textures;
    mesh_out.mesh.clusters = mesh_in.mesh.clusters;
  }

  mesh_out.mesh.vertices.resize(mesh_in.mesh.vertices.size());
  mesh_out.mesh.vertex_normals.resize(mesh_in.mesh.vertex_normals.size());

  // transform vertices
  for(size_t i = 0; i < mesh_in.mesh.vertices.size(); i++)
  {
    Eigen::Vector3d in_vec, out_vec;
    pointMsgToEigen(mesh_in.mesh.vertices[i], in_vec);
    out_vec = transformation * in_vec;
    pointEigenToMsg(out_vec, mesh_out.mesh.vertices[i]);
  }

  // rotate normals
  for(size_t i = 0; i < mesh_in.mesh.vertex_normals.size(); i++)
  {
    Eigen::Vector3d in_vec, out_vec;
    pointMsgToEigen(mesh_in.mesh.vertex_normals[i], in_vec);
    out_vec = transformation.rotation() * in_vec;
    pointEigenToMsg(out_vec, mesh_out.mesh.vertex_normals[i]);
  }

  mesh_out.header.frame_id = target_frame;
  mesh_out.header.stamp = ros::Time::now();

  return true;
}

bool transformGeometryMeshNoTime(
    const std::string& target_frame,
    const mesh_msgs::MeshGeometryStamped& mesh_in,
    const std::string& fixed_frame,
    mesh_msgs::MeshGeometryStamped& mesh_out,
    const tf::TransformListener&  tf_listener
)
{
  tf::StampedTransform transform;
  try{
    tf_listener.lookupTransform (
      target_frame,
      ros::Time(0), mesh_in.header.frame_id,
      ros::Time(0),
      fixed_frame,
      transform
    );
  }
  catch (tf::LookupException &e){
   ROS_ERROR ("%s", e.what ());
   return false;
  }
  catch (tf::ExtrapolationException &e){
   ROS_ERROR ("%s", e.what ());
   return false;
  }

  tf::Quaternion quaternion = transform.getRotation ();
  Eigen::Quaterniond rotation (
    quaternion.w (),
    quaternion.x (),
    quaternion.y (),
    quaternion.z ()
  );

  tf::Vector3 origin = transform.getOrigin ();
  Eigen::Vector3d eigen_origin;
  vectorTfToEigen(origin, eigen_origin);
  Eigen::Translation3d translation ( eigen_origin );
  Eigen::Affine3d transformation = translation * rotation;

  if (&mesh_in != &mesh_out){
    mesh_out.header = mesh_in.header;
    mesh_out.header.stamp = ros::Time::now();
    mesh_out.mesh_geometry.faces = mesh_in.mesh_geometry.faces;
  }

  mesh_out.mesh_geometry.vertices.resize(mesh_in.mesh_geometry.vertices.size());
  mesh_out.mesh_geometry.vertex_normals.resize(mesh_in.mesh_geometry.vertex_normals.size());

  // transform vertices
  for(size_t i = 0; i < mesh_in.mesh_geometry.vertices.size(); i++)
  {
    Eigen::Vector3d in_vec, out_vec;
    pointMsgToEigen(mesh_in.mesh_geometry.vertices[i], in_vec);
    out_vec = transformation * in_vec;
    pointEigenToMsg(out_vec, mesh_out.mesh_geometry.vertices[i]);
  }

  // rotate normals
  for(size_t i = 0; i < mesh_in.mesh_geometry.vertex_normals.size(); i++)
  {
    Eigen::Vector3d in_vec, out_vec;
    pointMsgToEigen(mesh_in.mesh_geometry.vertex_normals[i], in_vec);
    out_vec = transformation.rotation() * in_vec;
    pointEigenToMsg(out_vec, mesh_out.mesh_geometry.vertex_normals[i]);
  }

  mesh_out.header.frame_id = target_frame;
  mesh_out.header.stamp = ros::Time::now();

  return true;
}

bool transformTriangleMesh(
    const std::string& target_frame,
    const ros::Time& target_time,
    const mesh_msgs::TriangleMeshStamped& mesh_in,
    const std::string& fixed_frame,
    mesh_msgs::TriangleMeshStamped& mesh_out,
    const tf::TransformListener&  tf_listener
){

  tf::StampedTransform transform;
  try{
    tf_listener.lookupTransform (
      target_frame,
      target_time, mesh_in.header.frame_id,
      mesh_in.header.stamp,
      fixed_frame,
      transform
    );
  }
  catch (tf::LookupException &e){
   ROS_ERROR ("%s", e.what ());
   return false;
  }
  catch (tf::ExtrapolationException &e){
   ROS_ERROR ("%s", e.what ());
   return false;
  }

  tf::Quaternion quaternion = transform.getRotation ();
  Eigen::Quaterniond rotation (
    quaternion.w (),
    quaternion.x (),
    quaternion.y (),
    quaternion.z ()
  );

  tf::Vector3 origin = transform.getOrigin ();
  Eigen::Vector3d eigen_origin;
  vectorTfToEigen(origin, eigen_origin);
  Eigen::Translation3d translation ( eigen_origin );
  Eigen::Affine3d transformation = translation * rotation;

  if (&mesh_in != &mesh_out){
    mesh_out.header = mesh_in.header;
    mesh_out.mesh.triangles = mesh_in.mesh.triangles;
    mesh_out.mesh.vertex_colors = mesh_in.mesh.vertex_colors;
    mesh_out.mesh.triangle_colors = mesh_in.mesh.triangle_colors;
    mesh_out.mesh.vertex_texture_coords = mesh_in.mesh.vertex_texture_coords;
    mesh_out.mesh.face_materials = mesh_in.mesh.face_materials;
    mesh_out.mesh.textures = mesh_in.mesh.textures;
    mesh_out.mesh.clusters = mesh_in.mesh.clusters;
  }

  mesh_out.mesh.vertices.resize(mesh_in.mesh.vertices.size());
  mesh_out.mesh.vertex_normals.resize(mesh_in.mesh.vertex_normals.size());

  // transform vertices
  for(size_t i = 0; i < mesh_in.mesh.vertices.size(); i++)
  {
    Eigen::Vector3d in_vec, out_vec;
    pointMsgToEigen(mesh_in.mesh.vertices[i], in_vec);
    out_vec = transformation * in_vec;
    pointEigenToMsg(out_vec, mesh_out.mesh.vertices[i]);
  }

  // rotate normals
  for(size_t i = 0; i < mesh_in.mesh.vertex_normals.size(); i++)
  {
    Eigen::Vector3d in_vec, out_vec;
    pointMsgToEigen(mesh_in.mesh.vertex_normals[i], in_vec);
    out_vec = transformation.rotation() * in_vec;
    pointEigenToMsg(out_vec, mesh_out.mesh.vertex_normals[i]);
  }

  mesh_out.header.frame_id = target_frame;
  mesh_out.header.stamp = target_time;

  return true;
}

} /* namespace mesh_msgs_transforms */


//  Copyright (c) 2011 Helge Bahmann
//
//  Distributed under the Boost Software License, Version 1.0.
//  See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

// Attempt to determine whether the operations on atomic variables
// do in fact behave atomically: Let multiple threads race modifying
// a shared atomic variable and verify that it behaves as expected.
//
// We assume that "observable race condition" events are exponentially
// distributed, with unknown "average time between observable races"
// (which is just the reciprocal of exp distribution parameter lambda).
// Use a non-atomic implementation that intentionally exhibits a
// (hopefully tight) race to compute the maximum-likelihood estimate
// for this time. From this, compute an estimate that covers the
// unknown value with 0.995 confidence (using chi square quantile).
//
// Use this estimate to pick a timeout for the race tests of the
// atomic implementations such that under the assumed distribution
// we get 0.995 probability to detect a race (if there is one).
//
// Overall this yields 0.995 * 0.995 > 0.99 confidence that the
// operations truly behave atomic if this test program does not
// report an error.

#include <algorithm>

#include <boost/atomic.hpp>
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/time_formatters.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/thread.hpp>

/* helper class to let two instances of a function race against each
other, with configurable timeout and early abort on detection of error */
class concurrent_runner {
public:
    /* concurrently run the function in two threads, until either timeout
    or one of the functions returns "false"; returns true if timeout
    was reached, or false if early abort and updates timeout accordingly */
    static bool
    execute(
        const boost::function<bool(size_t)> & fn,
        boost::posix_time::time_duration & timeout)
    {
        concurrent_runner runner(fn);
        runner.wait_finish(timeout);
        return !runner.failure();
    }

    concurrent_runner(
        const boost::function<bool(size_t)> & fn)
        : finished_(false), failure_(false),
        first_thread_(boost::bind(&concurrent_runner::thread_function, this, fn, 0)),
        second_thread_(boost::bind(&concurrent_runner::thread_function, this, fn, 1))
    {
    }

    void
    wait_finish(boost::posix_time::time_duration & timeout)
    {
        boost::system_time start = boost::get_system_time();
        boost::system_time end = start + timeout;

        {
            boost::mutex::scoped_lock guard(m_);
            while (boost::get_system_time() < end && !finished())
                c_.timed_wait(guard, end);
        }

        finished_.store(true, boost::memory_order_relaxed);

        first_thread_.join();
        second_thread_.join();

        boost::posix_time::time_duration duration = boost::get_system_time() - start;
        if (duration < timeout)
            timeout = duration;
    }

    bool
    finished(void) const throw() {
        return finished_.load(boost::memory_order_relaxed);
    }

    bool
    failure(void) const throw() {
        return failure_;
    }
private:
    void
    thread_function(boost::function<bool(size_t)> function, size_t instance)
    {
        while (!finished()) {
            if (!function(instance)) {
                boost::mutex::scoped_lock guard(m_);
                failure_ = true;
                finished_.store(true, boost::memory_order_relaxed);
                c_.notify_all();
                break;
            }
        }
    }

    boost::mutex m_;
    boost::condition_variable c_;

    boost::atomic<bool> finished_;
    bool failure_;

    boost::thread first_thread_;
    boost::thread second_thread_;
};

bool
racy_add(volatile unsigned int & value, size_t instance)
{
    size_t shift = instance * 8;
    unsigned int mask = 0xff << shift;
    for (size_t n = 0; n < 255; n++) {
        unsigned int tmp = value;
        value = tmp + (1 << shift);

        if ((tmp & mask) != (n << shift))
            return false;
    }

    unsigned int tmp = value;
    value = tmp & ~mask;
    if ((tmp & mask) != mask)
        return false;

    return true;
}

/* compute estimate for average time between races being observable, in usecs */
static double
estimate_avg_race_time(void)
{
    double sum = 0.0;

    /* take 10 samples */
    for (size_t n = 0; n < 10; n++) {
        boost::posix_time::time_duration timeout(0, 0, 10);

        volatile unsigned int value(0);
        bool success = concurrent_runner::execute(
            boost::bind(racy_add, boost::ref(value), _1),
            timeout
        );

        if (success) {
            BOOST_FAIL("Failed to establish baseline time for reproducing race condition");
        }

        sum = sum + timeout.total_microseconds();
    }

    /* determine maximum likelihood estimate for average time between
    race observations */
    double avg_race_time_mle = (sum / 10);

    /* pick 0.995 confidence (7.44 = chi square 0.995 confidence) */
    double avg_race_time_995 = avg_race_time_mle * 2 * 10 / 7.44;

    return avg_race_time_995;
}

template<typename value_type, size_t shift_>
bool
test_arithmetic(boost::atomic<value_type> & shared_value, size_t instance)
{
    size_t shift = instance * 8;
    value_type mask = 0xff << shift;
    value_type increment = 1 << shift;

    value_type expected = 0;

    for (size_t n = 0; n < 255; n++) {
        value_type tmp = shared_value.fetch_add(increment, boost::memory_order_relaxed);
        if ( (tmp & mask) != (expected << shift) )
            return false;
        expected ++;
    }
    for (size_t n = 0; n < 255; n++) {
        value_type tmp = shared_value.fetch_sub(increment, boost::memory_order_relaxed);
        if ( (tmp & mask) != (expected << shift) )
            return false;
        expected --;
    }

    return true;
}

template<typename value_type, size_t shift_>
bool
test_bitops(boost::atomic<value_type> & shared_value, size_t instance)
{
    size_t shift = instance * 8;
    value_type mask = 0xff << shift;

    value_type expected = 0;

    for (size_t k = 0; k < 8; k++) {
        value_type mod = 1 << k;
        value_type tmp = shared_value.fetch_or(mod << shift, boost::memory_order_relaxed);
        if ( (tmp & mask) != (expected << shift))
            return false;
        expected = expected | mod;
    }
    for (size_t k = 0; k < 8; k++) {
        value_type tmp = shared_value.fetch_and( ~ (1 << (shift + k)), boost::memory_order_relaxed);
        if ( (tmp & mask) != (expected << shift))
            return false;
        expected = expected & ~(1<<k);
    }
    for (size_t k = 0; k < 8; k++) {
        value_type mod = 255 ^ (1 << k);
        value_type tmp = shared_value.fetch_xor(mod << shift, boost::memory_order_relaxed);
        if ( (tmp & mask) != (expected << shift))
            return false;
        expected = expected ^ mod;
    }

    value_type tmp = shared_value.fetch_and( ~mask, boost::memory_order_relaxed);
    if ( (tmp & mask) != (expected << shift) )
        return false;

    return true;
}

int test_main(int, char *[])
{
    boost::posix_time::time_duration reciprocal_lambda;

    double avg_race_time = estimate_avg_race_time();

    /* 5.298 = 0.995 quantile of exponential distribution */
    const boost::posix_time::time_duration timeout = boost::posix_time::microseconds((long)(5.298 * avg_race_time));

    {
        boost::atomic<unsigned int> value(0);

        /* testing two different operations in this loop, therefore
        enlarge timeout */
        boost::posix_time::time_duration tmp(timeout * 2);

        bool success = concurrent_runner::execute(
            boost::bind(test_arithmetic<unsigned int, 0>, boost::ref(value), _1),
            tmp
        );

        BOOST_CHECK_MESSAGE(success, "concurrent arithmetic");
    }

    {
        boost::atomic<unsigned int> value(0);

        /* testing three different operations in this loop, therefore
        enlarge timeout */
        boost::posix_time::time_duration tmp(timeout * 3);

        bool success = concurrent_runner::execute(
            boost::bind(test_bitops<unsigned int, 0>, boost::ref(value), _1),
            tmp
        );

        BOOST_CHECK_MESSAGE(success, "concurrent bitops");
    }
    return 0;
}


#include <istream>
#include <iostream>
#include <random>
#include <boost/program_options.hpp>
#include "sphere.h"
#include "surface_list.h"
#include "float.h"
#include "camera.h"
#include "math_util.h"
#include "material.h"
#include "lodepng/lodepng.h"
#include <SDL/SDL.h>
#include <GL/gl.h>
#include <thread>
#include <algorithm>

namespace po = boost::program_options;

vec3 color(const Ray& r, Surface *scene, int depth);
Surface* randomScene(int varA, int varB);
void render(std::vector<std::uint8_t> *img, int width, int height, int numRaysPixel, Surface* scene, vec3 lookFrom, vec3 lookAt, float focalDistance, float aperture, float vfov, bool shuffle);
int preview(std::vector<std::uint8_t> *img, int width, int height, int pwidth, int pheight);

int main(int argc, const char *argv[])
{
    int width;
    int height;
    int numRaysPixel;
    float focalDistance;
    float aperture;
    float vfov;
    int seed;
    int varA;
    int varB;
    int pwidth;
    int pheight;
    bool shuffle;
    std::string filename;

    po::options_description desc("A very simple ray tracer (╯°□°)╯︵ ┻━┻\n\nSupported parameters");
    desc.add_options()
    ("help", "print available parameters")
    ("filename", po::value<std::string>(&filename), "filename to store the rendered scene as png-file")
    ("width", po::value<int>(&width)->default_value(1280), "width for the rendered scene")
    ("height", po::value<int>(&height)->default_value(720), "height for the rendered scene")
    ("num-rays", po::value<int>(&numRaysPixel)->default_value(100), "number of rays per pixel ('Anti-Aliasing')")
    ("vfov", po::value<float>(&vfov)->default_value(20), "field of view for the camera")
    ("aperture", po::value<float>(&aperture)->default_value(0.01), "aperture of the camera")
    ("focal-distance", po::value<float>(&focalDistance)->default_value(10), "focal distance of the camera")
    ("seed", po::value<int>(&seed)->default_value(42), "random seed for the scene")
    ("var-a", po::value<int>(&varA)->default_value(11), "controls the number of random spheres")
    ("var-b", po::value<int>(&varB)->default_value(11), "controls the number of random spheres")
    ("pwidth", po::value<int>(&pwidth)->default_value(1280), "width for the preview frame")
    ("pheight", po::value<int>(&pheight)->default_value(720), "height for the preview frame")
    ("shuffle", po::bool_switch(&shuffle)->default_value(false), "randomizes the order of computation of the pixels");

    po::positional_options_description p;
    p.add("filename", -1);
    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if(vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 1;
    }
    else if(!vm.count("filename"))
    {
        std::cout << "Please provide a filename to store the rendered scene." << std::endl << std::endl << desc << std::endl;
        return 1;
    }

    srand48(seed);
    Surface* scene = randomScene(varA, varB);
    vec3 lookFrom = vec3(13,2,3);
    vec3 lookAt = vec3(0,0,0);

    std::vector<std::uint8_t> img;
    img.resize(width*height*4);

    std::thread t1(preview, &img, width, height, pwidth, pheight);

    render(&img, width, height, numRaysPixel, scene, lookFrom, lookAt, focalDistance, aperture, vfov, shuffle);

    unsigned error = lodepng::encode(filename, img, width, height);
    if(error)
        std::cout << "encoder error " << error << ": "<< lodepng_error_text(error) << std::endl;

    SDL_Event sdlevent;
    sdlevent.type = SDL_QUIT;
    SDL_PushEvent(&sdlevent);

    t1.join();

    std::cout << "Done. Rendere scene saved as " << filename << std::endl;
}

void render(std::vector<std::uint8_t> *img, int width, int height, int numRaysPixel, Surface* scene, vec3 lookFrom, vec3 lookAt, float focalDistance, float aperture, float vfov, bool shuffle)
{
    Camera cam(lookFrom, lookAt, vec3(0,1,0), vfov, float(width)/float(height), aperture, focalDistance);
    std::vector<int> indices;
    if(shuffle){
        indices.resize(width*height);
        for(int i = 0; i < width*height; ++i)
            indices[i] = i;

        std::random_shuffle(indices.begin(), indices.end());

    }

    #pragma omp parallel for
    for(int i = 0; i < height*width; ++i){
        int j = i;

        if(shuffle){
            j = indices[i];
        }

        int x = j % width;
        int y = j / width;

        vec3 col(0, 0, 0);
        for (int i = 0; i < numRaysPixel; ++i) {
            float u = float(x + drand48()) / float(width);
            float v = float(y + drand48()) / float(height);
            Ray r = cam.getRay(u, v);
            col += color(r, scene, 0);
        }
        col /= float(numRaysPixel);
        col = vec3(sqrt(col[0]), sqrt(col[1]), sqrt(col[2]));
        (*img)[4 * width * (height - y - 1) + 4 * x + 0] = int(255.99 * col[0]);
        (*img)[4 * width * (height - y - 1) + 4 * x + 1] = int(255.99 * col[1]);
        (*img)[4 * width * (height - y - 1) + 4 * x + 2] = int(255.99 * col[2]);
        (*img)[4 * width * (height - y - 1) + 4 * x + 3] = 255;
    }
}

vec3 color(const Ray& r, Surface *scene, int depth)
{
    hitRecord hitRec;
    if(scene->hit(r, 0.001, MAXFLOAT, hitRec))
    {
        Ray scattered;
        vec3 attenuation;
        if(depth < 150 && hitRec.mat->scatter(r, hitRec, attenuation, scattered))
        {
            return attenuation*color(scattered, scene, depth+1);
        }
        else
        {
            return vec3(0,0,0);
        }
    }
    else
    {
        vec3 dir = unitVector(r.getDirection());
        float t = 0.5*(dir.y() + 1.0);
        return (1.0-t)*vec3(1.0, 1.0, 1.0) + t*vec3(0.5, 0.7, 1.0);
    }
}

Surface* randomScene(int varA, int varB)
{
    int n = 500;
    Surface **list = new Surface*[n+1];
    list[0] = new Sphere(vec3(0,-1000,0), 1000, new Lambertian(vec3(0.5,0.5,0.5)));
    int i = 1;
    for(int a = -varA; a < varA; ++a)
    {
        for(int b = -varB; b < varB; ++b)
        {
            float randMat = drand48();
            vec3 center(a+0.9*drand48(), 0.2, b+drand48());
            if((center-vec3(4,0.2,0)).length() > 0.9)
            {
                if(randMat < 0.8)
                {
                    list[i++] = new Sphere(center, 0.2, new Lambertian(vec3(drand48()*drand48(), drand48()*drand48(), drand48()*drand48())));
                }
                else if(randMat < 0.95)
                {
                    list[i++] = new Sphere(center, 0.2,
                                           new Metal(vec3(0.5*(1+drand48()), 0.5*(1+drand48()), 0.5*(1+drand48())), 0.5*drand48()));
                }
                else
                {
                    list[i++] = new Sphere(center, 0.2, new Dielectric(1.5+(drand48()*2 - 1.0)));
                }
            }

        }
    }
    list[i++] = new Sphere(vec3(0, 1, 0), 1.0, new Dielectric(1.3));
    list[i++] = new Sphere(vec3(-4, 1, 0), 1.0, new Lambertian(vec3(0.4, 0.2, 0.1)));
    list[i++] = new Sphere(vec3(4, 1, 0), 1.0, new Metal(vec3(0.7, 0.6, 0.5), 0.0));

    return new SurfaceList(list, i);
}

int preview(std::vector<std::uint8_t> *img, int width, int height, int pwidth, int pheight){

    if(pwidth > width)
        pwidth = width;
    if(pheight > height)
        pheight = height;

    if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_NOPARACHUTE) < 0) {
        std::cout << "Error: Unable to init SDL: " << SDL_GetError() << std::endl;
        return 1;
    }

    SDL_Surface* scr = SDL_SetVideoMode(pwidth, pheight, 32, SDL_OPENGL);

    if(scr == 0) {
        std::cout << "Error: Unable to set video. SDL error message: " << SDL_GetError() << std::endl;
        return 1;
    }

    glViewport(0, 0, pwidth, pheight);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(0, pwidth, pheight, 0, -1, 1);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    glDisable(GL_CULL_FACE);
    glDisable(GL_DEPTH_TEST);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glEnable(GL_BLEND);
    glDisable(GL_ALPHA_TEST);

    if(glGetError() != GL_NO_ERROR) {
        std::cout << "Error initing GL" << std::endl;
        return 1;
    }

    size_t u2 = 1; while(u2 < width) u2 *= 2;
    size_t v2 = 1; while(v2 < height) v2 *= 2;
    double u3 = (double)width / u2;
    double v3 = (double)height / v2;

    std::vector<unsigned char> image2(u2 * v2 * 4);

    glEnable(GL_TEXTURE_2D);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glTexImage2D(GL_TEXTURE_2D, 0, 4, u2, v2, 0, GL_RGBA, GL_UNSIGNED_BYTE, &image2[0]);

    bool done = false;
    SDL_Event event = {0};
    glColor4ub(255, 255, 255, 255);

    while(!done) {
        while(SDL_PollEvent(&event)) {
            if(event.type == SDL_QUIT) done = 1;
        }
        for(size_t y = 0; y < height; y++) {
            for (size_t x = 0; x < width; x++) {
                for (size_t c = 0; c < 4; c++) {
                    image2[4 * u2 * y + 4 * x + c] = (*img)[4 * width * y + 4 * x + c];
                }
            }
        }
        glTexImage2D(GL_TEXTURE_2D, 0, 4, u2, v2, 0, GL_RGBA, GL_UNSIGNED_BYTE, &image2[0]);
        glBegin(GL_QUADS);
        glTexCoord2d( 0,  0); glVertex2f(    0,      0);
        glTexCoord2d(u3,  0); glVertex2f(width,      0);
        glTexCoord2d(u3, v3); glVertex2f(width, height);
        glTexCoord2d( 0, v3); glVertex2f(    0, height);
        glEnd();

        SDL_GL_SwapBuffers();
        glClearColor(0, 0, 0, 0);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        SDL_Delay(16);
    }

    return 0;
}


/*******************************************************************************
*
*  Filename    : SummaryTable.cc
*  Description : Generating latex summary tables
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "ManagerUtils/Maths/interface/Parameter.hpp"
#include "ManagerUtils/SampleMgr/interface/MultiFile.hpp"
#include "ManagerUtils/SampleMgr/interface/SampleGroup.hpp"
#include "TstarAnalysis/Common/interface/GetEventWeight.hpp"
#include "TstarAnalysis/Common/interface/InitSample.hpp"
#include "TstarAnalysis/CompareDataMC/interface/Compare_Common.hpp"
#include "TstarAnalysis/CompareDataMC/interface/MakeTable.hpp"

#include "DataFormats/FWLite/interface/Handle.h"
#include "DataFormats/PatCandidates/interface/Jet.h"

#include <boost/format.hpp>
#include <fstream>
#include <vector>

using namespace std;
using namespace mgr;
/*******************************************************************************
*   Static string and format settings
*******************************************************************************/
static const string hlinestr = "\\hline";
static boost::format samplelinefmt( "%-45s & %35s & %35s & %15s & %35s \\\\" );
static boost::format simplelinefmt( "%-40s & %25s &%20s\\\\" );
static boost::format lumilinefmt(   "%-55s & %35s & %10s & %35s &  %20s\\\\" );

/*******************************************************************************
*   Main control functions
*******************************************************************************/
void
SummaryComplete(
  const vector<SampleTableMgr>& siglist,
  const vector<SampleTableMgr>& bkglist,
  const SampleTableMgr&         data )
{
  ofstream selfile( compnamer.TexFileName( "summary" ) );
  selfile << "\\begin{tabular}{|l|ccc|c|}" << endl
          << hlinestr << endl
          << samplelinefmt
    % "Sample"
    % "Cross Section (pb)"
    % "Selection Efficiency"
    % "Weight Errors"
    % "Expected Yield"
          << endl
          << hlinestr << endl;

  for( const auto& sig : siglist ){
    selfile << SampleLine( sig.Sample() ) << endl;
  }

  selfile << hlinestr << endl;

  Parameter expyield( 0, 0, 0 );
  Parameter obsyield( data.ExpectedYield(), 0, 0 );

  for( const auto& grp : bkglist ){
    for( const auto& smp : grp.SampleList() ){
      selfile << SampleLine( smp ) << endl;
      const Parameter smpyield = smp.ExpectedYield()// double
                                 * Prod( smp.CrossSection().NormParam()// 1+- rel error
                                       , smp.SelectionEfficiency().NormParam()
                                       , GetWeightError( smp )
        );
      expyield += smpyield;
    }

    selfile << hlinestr << endl;
  }

  selfile << CountLine( "MC background", expyield ) << endl
          << hlinestr << endl
          << CountLine( "Data", obsyield ) << endl
          <<  hlinestr << endl
          << "\\end{tabular}" << endl;
  selfile.close();
}

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

void
SummaryBrief(
  const vector<SampleTableMgr>& siglist,
  const vector<SampleTableMgr>& bkglist,
  const SampleTableMgr&         data )
{
  ofstream brieffile( compnamer.TexFileName( "summary", "brief" ) );

  brieffile << "\\begin{tabular}{|l|cc|c|}" << endl
            << hlinestr << endl
            << simplelinefmt  % "Process" % "Average Efficiency" % "Expected Yield" << endl
            << hlinestr << endl;

  double expyield = 0;
  double obsyield = data.ExpectedYield();

  for( const auto& grp : siglist ){
    brieffile << SimpleLine( grp ) << endl;
  }

  brieffile << hlinestr << endl;

  for( const auto& grp : bkglist ){
    brieffile << SimpleLine( grp ) << endl;
    expyield += grp.ExpectedYield();
  }

  brieffile << hlinestr << endl
            << SimpleCount( "Bkg. Total", Parameter( expyield, 0, 0 ) ) << endl
            << hlinestr << endl
            << SimpleCount( "Data", Parameter( obsyield, 0, 0 ) ) << endl
            << hlinestr << endl
            << "\\end{tabular}" << endl;

  brieffile.close();
}

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

void
SummaryMCLumi(
  const vector<SampleTableMgr>& siglist,
  const vector<SampleTableMgr>& bkglist )
{
  ofstream lumifile( compnamer.TexFileName( "summary", "lumi" ) );

  lumifile <<  "\\begin{tabular}{|l|ccc|c|}" << endl
           << hlinestr << endl
           << lumilinefmt
    % "Process"
    % "Cross section ($pb$)"
    % "K Factor"
    % "Equiv. \\Lumi (\\pbinv)"
    % "Generator"
           << endl
           << hlinestr << endl;

  for( const auto& sig : siglist ){
    lumifile << LumiLine( sig.Sample() ) << endl;
  }

  for( const auto& grp : bkglist ){
    lumifile << hlinestr << endl;

    for( const auto& smp : grp.SampleList() ){
      lumifile << LumiLine( smp ) << endl;
    }
  }

  lumifile << hlinestr << endl
           << "\\end{tabular}" << endl;

  lumifile.close();
}

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

string
SampleLine( const SampleMgr& x )
{
  const string namecol = x.LatexName();
  const string xseccol = x.Name().find( "Tstar" ) != string::npos ?
                         Scientific( x.CrossSection(), 2 ) :
                         XSecStr( x.CrossSection() );
  const string effcol = Scientific( x.SelectionEfficiency()*x.KFactor(), 2 );
  const Parameter err = GetWeightError( x );
  const string errcol = "+" + FloatingPoint( err.RelUpperError(), 2 )+ "/-" + FloatingPoint( err.RelLowerError(), 2 );

  const Parameter expyield = x.ExpectedYield()// double
                             * Prod( x.CrossSection().NormParam()// 1+- rel error
                                   , x.SelectionEfficiency().NormParam()
                                   , err.NormParam()
    );

  const string expcol =
    expyield.CentralValue() > 100 ?  FloatingPoint( expyield.CentralValue(), 0 ) :
    FloatingPoint( expyield.CentralValue(), 1 );

  return boost::str( samplelinefmt
    % namecol
    % AddMathBrace( xseccol )
    % AddMathBrace( effcol )
    % errcol
    % AddMathBrace( expcol )
    );
}

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

string
CountLine( const string& tag, const Parameter& x )
{
  return boost::str( samplelinefmt
    % tag.c_str()
    % ""
    % ""
    % ""
    % AddMathBrace( FloatingPoint( x, 0 ) )
    );
}

/*******************************************************************************
*   Helper functions for simplified summary
*******************************************************************************/

string
SimpleLine( const SampleTableMgr& x )
{
  Parameter expyield( 0, 0, 0 );

  for( const auto& smp : x.SampleList() ){
    expyield += smp.ExpectedYield()
                * smp.CrossSection().NormParam()
                * smp.SelectionEfficiency().NormParam()
                * GetWeightError( smp );
  }

  return boost::str( simplelinefmt
    % x.LatexName()
    % AddMathBrace( Scientific( x.AvgSelectionEfficiency().CentralValue(), 2 ) )
    % AddMathBrace( FloatingPoint( expyield.CentralValue(), 0 ) )
    );
}

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

string
SimpleCount( const string& tag, const Parameter& x )
{
  return boost::str( simplelinefmt
    % tag.c_str()
    % ""
    % AddMathBrace( FloatingPoint( x, 0 ) )
    );
}

/*******************************************************************************
*   Helper functions for Lumi summary files
*******************************************************************************/
string
LumiLine( const SampleMgr& x )
{
  double equiv = x.EffectiveLuminosity();

  const string namecol = x.LatexName();
  const string xsec    = x.Name().find( "Tstar" ) != string::npos ?
                         Scientific( x.CrossSection(), 2 ) :
                         XSecStr( x.CrossSection() );
  const string kfaccol = FloatingPoint( x.KFactor(), -1 );
  const string xseccol = str( boost::format( "%s(%s)" )%AddMathBrace( xsec )%x.GetCacheString( "XsecSource" ) );
  const string lumicol = Scientific( Parameter( equiv, 0, 0 ), 2 );
  const string gencol  = x.GetCacheString( "Generator" );

  return boost::str(
    lumilinefmt
    % namecol
    % xseccol
    % kfaccol
    % AddMathBrace( lumicol )
    % gencol
    );
}

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

Parameter
GetWeightError( const mgr::SampleMgr& sample )
{
  Parameter err(
    sample.GetCacheDouble( "EventWeightSum" ),
    sample.GetCacheDouble( "EventWeightUpSum" ) - sample.GetCacheDouble( "EventWeightSum" ),
    sample.GetCacheDouble( "EventWeightSum" )   - sample.GetCacheDouble( "EventWeightDownSum" )
    );

  err  = err.NormParam();
  err *= Parameter( 1, 0.046, 0.046 );
  err *= Parameter( 1, 0.03, 0.03 );
  return err;
}

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

string
AddMathBrace( const std::string& x )
{
  return "$" + x + "$";
}

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

string
XSecStr( const Parameter& x )
{
  if( x.CentralValue() > 100 ){
    return FloatingPoint( x, 0 );
  } else if( x.CentralValue() > 10 ){
    return FloatingPoint( x, 1 );
  } else {
    return Scientific( x, 2 );
  }
}


#include <facter/facts/solaris/memory_resolver.hpp>
#include <facter/logging/logging.hpp>
#include <facter/util/file.hpp>
#include <facter/util/solaris/k_stat.hpp>
#include <facter/execution/execution.hpp>
#include <boost/algorithm/string.hpp>
#include <sys/sysinfo.h>
#include <sys/sysconfig.h>
#include <sys/stat.h>
#include <sys/swap.h>

using namespace std;
using namespace facter::util;
using namespace facter::util::solaris;

#ifdef LOG_NAMESPACE
  #undef LOG_NAMESPACE
#endif
#define LOG_NAMESPACE "facts.solaris.memory"

namespace facter { namespace facts { namespace solaris {

    memory_resolver::data memory_resolver::collect_data(collection& facts)
    {
        data result;

        const long page_size = sysconf(_SC_PAGESIZE);
        const long max_dev_size = PATH_MAX;
        try {
            k_stat ks;
            auto ke = ks[make_pair("unix", "system_pages")][0];
            result.mem_total = ke.value<ulong_t>("physmem") * page_size;
            result.mem_free = ke.value<ulong_t>("pagesfree") * page_size;

            // Swap requires a little more effort. See
            // https://community.oracle.com/thread/1951228?start=0&tstart=0
            // http://www.brendangregg.com/K9Toolkit/swapinfo
            int num = 0;
            if ((num = swapctl(SC_GETNSWP,  0)) == -1) {
                LOG_DEBUG("swapctl failed: %1% (%2%): swap information is unavailable", strerror(errno), errno);
                return result;
            }
            if (num == 0) {
                // no swap devices configured
                return result;
            }

            // swap devices can be added online. So add one extra.
            num++;

            vector<char> buffer(num * sizeof(swapent_t) + sizeof(swaptbl_t));
            vector<vector<char>> str_table(num);

            swaptbl_t* swaps = reinterpret_cast<swaptbl_t*>(buffer.data());
            swaps->swt_n = num;

            for (int i = 0; i < num; i++) {
                str_table[i].resize(max_dev_size);
                swaps->swt_ent[i].ste_path = str_table[i].data();
            }

            if (swapctl(SC_LIST, swaps) == -1) {
                LOG_DEBUG("swapctl with SC_LIST failed: %1% (%2%): swap information is unavailable", strerror(errno), errno);
                return result;
            }

            for (int i = 0; i < num; i++) {
                result.swap_free += swaps->swt_ent[i].ste_free;
                result.swap_total += swaps->swt_ent[i].ste_pages;
            }

            result.swap_free *= page_size;
            result.swap_total *= page_size;
            return result;
        } catch (kstat_exception &ex) {
            LOG_DEBUG("memory facts unavailable (%1%)", ex.what());
            return {};
        }
    }
}}}  // namespace facter::facts::solaris


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
// codelets
#include "poplar/Target.hpp"
#include "poplibs_test/Check.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/codelets.hpp"
#include "poputil/VertexTemplates.hpp"
#include <boost/program_options.hpp>
#include <stdexcept>
#include <string.h>
#include <utility>

using namespace poplar;
using namespace poplar::program;
using namespace popops;
using namespace poputil;
using namespace poplibs_test::util;

using namespace poplibs_support;
namespace po = boost::program_options;
// Host model of the histogram.  Intentionally coded in a straightforward way
// that is different to the codelet implementation
std::vector<float> histogram(const std::vector<float> &data,
                             const unsigned unpaddedDataSize,
                             const unsigned offset,
                             const std::vector<float> &limits,
                             bool isAbsolute) {

  std::vector<float> histogram(limits.size() + 1, 0);
  for (unsigned i = offset; i < unpaddedDataSize + offset; i++) {
    auto dataElem = data[i];
    if (isAbsolute) {
      dataElem = std::fabs(dataElem);
    }
    for (unsigned j = 0; j < limits.size(); j++) {
      if (j == 0 && dataElem < limits[j]) {
        // Below the lower bound
        histogram[j]++;
      } else {
        if (j == limits.size() - 1 && dataElem >= limits[j]) {
          // Above the upper bound
          histogram[j + 1]++;
        }
        if (dataElem < limits[j] && dataElem >= limits[j - 1]) {
          // Between a bound and the one below it
          histogram[j]++;
        }
      }
    }
  }
  return histogram;
}

bool doTest(TestDevice &device, DeviceType deviceType, bool profile,
            unsigned padding, const unsigned rows,
            const std::vector<double> &data, const std::vector<float> &limits,
            bool isSupervisor, bool divideWorkByLimit,
            const poplar::Type &dataType, bool isAbsolute) {
  auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  const auto rawSize = target.getTypeSize(dataType);

  std::vector<char> rawData(data.size() * rawSize);
  std::vector<char> rawLimits(limits.size() * rawSize);

  copy(target, data.data(), data.size(), dataType, rawData.data());
  copy(target, limits.data(), limits.size(), dataType, rawLimits.data());

  auto cs = graph.addComputeSet("cs");

  const auto vertexClassSupervisor = templateVertex(
      "popops::HistogramSupervisor", dataType, isAbsolute, divideWorkByLimit);
  const auto vertexClass2D =
      templateVertex("popops::Histogram2D", dataType, isAbsolute);

  const auto histogramSize =
      (divideWorkByLimit ? 1 : target.getNumWorkerContexts()) *
      (limits.size() + 1);
  // We create one data tensor, with multiple vertices reading from it, each
  // with a different offset to ensure we check each input alignment.
  // This is kind of possible with the 2D vertex and multiple rows each of
  // an odd length, but not for the supervisor case.  Use this mechanism for
  // both vertex types
  auto vertexData = graph.addVariable(dataType, {data.size()});
  auto vertexLimits = graph.addVariable(dataType, {limits.size()});
  const auto unpaddedDataSize = data.size() - padding;
  graph.setTileMapping(vertexData, 0);
  graph.setTileMapping(vertexLimits, 0);
  graph.createHostWrite("data", vertexData);
  graph.createHostWrite("limits", vertexLimits);

  const auto numVertices = padding + 1;
  std::vector<std::string> readHandles(numVertices);

  for (unsigned offset = 0; offset < numVertices; offset++) {
    auto vertexHistogram = graph.addVariable(FLOAT, {histogramSize});

    auto v = graph.addVertex(cs, isSupervisor ? vertexClassSupervisor
                                              : vertexClass2D);
    graph.setTileMapping(v, 0);

    if (isSupervisor) {
      graph.connect(v["data"],
                    vertexData.slice({offset, offset + unpaddedDataSize}));
    } else {
      std::vector<Tensor> dataSlices(rows);
      auto remainder = unpaddedDataSize % rows;
      auto rowLength = unpaddedDataSize / rows;

      for (unsigned i = 0; i < rows - 1; i++) {
        dataSlices[i] = vertexData.slice(offset + rowLength * i,
                                         offset + rowLength * (i + 1));
      }
      // The last row will contain the remainder, if any
      dataSlices[rows - 1] =
          vertexData.slice(offset + rowLength * (rows - 1),
                           offset + rowLength * rows + remainder);
      graph.connect(v["data"], dataSlices);
    }
    graph.setInitialValue(v["histogramCount"], limits.size() + 1);
    graph.connect(v["limits"], vertexLimits);
    graph.connect(v["histogram"], vertexHistogram);
    graph.setTileMapping(vertexHistogram, 0);

    readHandles[offset] = "histogram_" + std::to_string(offset);
    graph.createHostRead(readHandles[offset], vertexHistogram);
  }
  OptionFlags engineOptions;
  if (profile) {
    engineOptions.set("debug.instrumentCompute", "true");
  }
  Sequence prog;
  prog.add(Execute(cs));
  Engine e(graph, prog, engineOptions);
  std::vector<std::vector<float>> resultHistogram(numVertices);

  device.bind([&](const Device &d) {
    e.load(d);

    e.writeTensor("data", rawData.data(), rawData.data() + rawData.size());
    e.writeTensor("limits", rawLimits.data(),
                  rawLimits.data() + rawLimits.size());

    e.run();
    for (unsigned i = 0; i < numVertices; i++) {
      resultHistogram[i].resize(histogramSize);
      e.readTensor(readHandles[i], resultHistogram[i].data(),
                   resultHistogram[i].data() + resultHistogram[i].size());
    }
  });

  // Cast half data back to get float data that represents half accuracy, or
  // just copy if float.  This matters where limits are equal to the histogram
  // entries in the numeric format of the codelet.
  std::vector<float> dataConvert(data.size());
  std::vector<float> limitsConvert(limits.size());
  copy(target, dataType, rawData.data(), dataConvert.data(),
       dataConvert.size());
  copy(target, dataType, rawLimits.data(), limitsConvert.data(),
       limitsConvert.size());

  bool success = true;
  // Generate host result, compare for each offset
  for (unsigned offset = 0; offset < numVertices; offset++) {
    std::vector<float> hostResult;
    hostResult = histogram(dataConvert, unpaddedDataSize, offset, limitsConvert,
                           isAbsolute);
    std::cout << "Padding:" << padding << " Data with pad:" << dataConvert
              << "\n";
    std::cout << "Limits:" << limitsConvert << "\n";

    std::cout << "    Hist:" << resultHistogram[offset]
              << "\nExpected:" << hostResult;
    std::cout << "\n";

    // Note - in the case of the supervisor vertex with work split by data
    // size then the output is larger than hostResult.size(), but the
    // first hostResult.size() entries hold the result
    for (unsigned i = 0; i < hostResult.size(); ++i) {
      if (hostResult[i] != resultHistogram[offset][i]) {
        success = false;
        break;
      }
    }
  }
  if (profile && deviceType != DeviceType::Cpu) {
    e.printProfileSummary(std::cout,
                          OptionFlags{{"showExecutionSteps", "true"}});
  }
  return success;
}

int main(int argc, char **argv) {
  DeviceType deviceType = DeviceType::IpuModel2;
  // Default input parameters.
  Type dataType = FLOAT;
  bool isAbsolute = false;
  bool isSupervisor = false;
  unsigned dataSize;
  unsigned limitSize;
  unsigned rows = 1;
  double dataMin = -65504;
  double dataRange = 65504 * 2;
  float limitsMin = -60000;
  float limitsRange = 60000 * 2;
  float limitsStep = 0.0;
  bool checkMisaligned = true;
  bool divideWorkByLimit = true;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Produce help message")
    ("device-type",
       po::value<DeviceType>(&deviceType)->default_value(deviceType),
       deviceTypeHelp)
    ("profile", "Output profiling report")
    ("type", po::value(&dataType)->default_value(dataType),
      "Data type of data and limits")
    ("data-size", po::value(&dataSize)->required(),
      "Number of data elements")
    ("data-min", po::value(&dataMin)->default_value(dataMin),
      "Minimum data value")
    ("data-range", po::value(&dataRange)->default_value(dataRange),
      "Data range")
    ("data-rows", po::value(&rows)->default_value(rows),
      "Data rows, for use with the 2D vertex")
    ("limits-size", po::value(&limitSize)->required(),
      "Number of limits, histogram results = limits + 1")
    ("limits-min", po::value(&limitsMin)->default_value(limitsMin),
      "Minimum limit")
    ("limits-range", po::value(&limitsRange)->default_value(limitsRange),
      "Limits range")
    ("limits-step", po::value(&limitsStep)->default_value(limitsStep),
      "Limits step, found from range unless this parameter is used")
    ("supervisor", po::value(&isSupervisor)->default_value(isSupervisor),
      "Test supervisor vertex")
    ("supervisor-by-limit", po::value(&divideWorkByLimit)->default_value(divideWorkByLimit),
      "Test supervisor vertex that divides work by limit, not input data")
    ("absolute", po::value(&isAbsolute)->default_value(isAbsolute),
      "Use absolute values of the data")
    ("check-misaligned", po::value(&checkMisaligned)->default_value(checkMisaligned),
      "Run the codelet multiple times with input data with each misalignment"
      " that is relevant to the data type")
    ;
  // clang-format on
  po::variables_map vm;
  bool profile = false;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    if (vm.count("profile")) {
      profile = true;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error parsing command line: " << e.what() << "\n";
    return 1;
  }
  // Create some padding so that we can run multiple times and check alignment
  const auto padding = checkMisaligned ? (dataType == HALF ? 4 : 2) - 1 : 0;

  // Random data within the range specified
  std::mt19937 randomEngine;
  std::vector<double> data(dataSize + padding);
  auto device = createTestDevice(deviceType);
  writeRandomValues(device.getTarget(), dataType, data.data(),
                    data.data() + data.size(), dataMin, dataMin + dataRange,
                    randomEngine);

  // Evenly spaced limits
  std::vector<float> limits(limitSize);
  if (limitsStep == 0.0) {
    limitsStep = limits.size() == 1
                     ? 1
                     : limitsRange / static_cast<float>(limits.size() - 1);
  }
  for (unsigned i = 0; i < limits.size(); i++) {
    limits[i] = limitsMin + static_cast<float>(i) * limitsStep;
  }
  auto success =
      doTest(device, deviceType, profile, padding, rows, data, limits,
             isSupervisor, divideWorkByLimit, dataType, isAbsolute);
  if (!success) {
    std::cerr << "Failure\n";
  }
  return !success;
}


// see brep_application/LICENSE.txt
//
//   Project Name:        Kratos
//   Last Modified by:    $Author: hbui $
//   Date:                $Date: 14 Feb 2017 $
//   Revision:            $Revision: 1.0 $
//
//

// External includes
#include <boost/python.hpp>

// Project includes
#include "custom_python/add_brep_and_level_set_to_python.h"
#include "custom_utilities/level_set/multipatch_z_level_set.h"
#include "custom_utilities/multipatch.h"

namespace Kratos
{

namespace Python
{

using namespace boost::python;

void IsogeometricApplication_AddBRepAndLevelSetToPython()
{
    /**************************************************************/
    /************* EXPORT INTERFACE FOR LEVEL SET *****************/
    /**************************************************************/

    class_<MultiPatchZLevelSet, MultiPatchZLevelSet::Pointer, bases<LevelSet, IsogeometricEcho>, boost::noncopyable>
    ( "MultiPatchZLevelSet", init<typename MultiPatch<2>::Pointer>() )
    .def("SetPredictionSampling", &MultiPatchZLevelSet::SetPredictionSampling)
    .def("SetProjectionTolerance", &MultiPatchZLevelSet::SetProjectionTolerance)
    .def("SetMaxIterations", &MultiPatchZLevelSet::SetMaxIterations)
    .def(self_ns::str(self))
    ;
}

}  // namespace Python.

}  // namespace Kratos.


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_COURSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_COURSE_HPP

#include <boost/geometry/algorithms/detail/azimuth.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

/// Calculate course (bearing) between two points.
///
/// NOTE: left for convenience and temporary backward compatibility
template <typename ReturnType, typename Point1, typename Point2>
inline ReturnType course(Point1 const& p1, Point2 const& p2)
{
    return azimuth<ReturnType>(p1, p2);
}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_COURSE_HPP


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/SimulationSetup/EstimationSetup/orbitDeterminationTestCases.h"
#include "Tudat/SimulationSetup/EstimationSetup/podProcessing.h"

namespace tudat
{
namespace unit_tests
{
BOOST_AUTO_TEST_SUITE( test_estimation_input_output )

//! This test checks whether the input/output of the estimation (weights, a priori covariance, unscaled covariance) are
//! correctly handed
BOOST_AUTO_TEST_CASE( test_EstimationInputAndOutput )
{
    int simulationType = 0;

    Eigen::VectorXd parameterPerturbation = getDefaultInitialParameterPerturbation( );

    // Define stringent a priori covariance
    Eigen::MatrixXd inverseAPrioriCovariance = 1.0E32 * Eigen::MatrixXd::Identity( 7, 7 );

    // Define moderate a priori covariance
    Eigen::MatrixXd moderateInverseAPriopriCovariance = Eigen::MatrixXd::Zero( 7, 7 );
    for( unsigned int i = 0; i < 7; i++ )
    {
        moderateInverseAPriopriCovariance( i, i ) = 1.0 / ( 1.0E-6 * parameterPerturbation( i ) * parameterPerturbation( i ) );
    }

    // Run estimation with strong a priori covariance
    std::pair< std::shared_ptr< PodOutput< double > >, Eigen::VectorXd > estimationOutputWithAprioriCovariance =
            executePlanetaryParameterEstimation< double, double >(
                simulationType, parameterPerturbation, inverseAPrioriCovariance );

    // Run estimation with effectively zero covariance
    std::pair< std::shared_ptr< PodOutput< double > >, Eigen::VectorXd > estimationOutputWithSmallAprioriCovariance =
            executePlanetaryParameterEstimation< double, double >(
                simulationType, parameterPerturbation, 1.0E-64 * inverseAPrioriCovariance );

    // Run estimation with moderate a priori covariance
    std::pair< std::shared_ptr< PodOutput< double > >, Eigen::VectorXd > estimationOutputWithModerateAprioriCovariance =
            executePlanetaryParameterEstimation< double, double >(
                simulationType, parameterPerturbation,  moderateInverseAPriopriCovariance );

    // Run estimation without a priori covariance
    std::pair< std::shared_ptr< PodOutput< double > >, Eigen::VectorXd > estimationOutputWithoutAprioriCovariance =
            executePlanetaryParameterEstimation< double, double >(
                simulationType, parameterPerturbation );

    // Run estimation without a priori covariance and increased weights
    double constantWeight = 100.0;
    std::pair< std::shared_ptr< PodOutput< double > >, Eigen::VectorXd > estimationOutputWithoutAprioriCovarianceAndWeakWeight =
            executePlanetaryParameterEstimation< double, double >(
                simulationType, parameterPerturbation, Eigen::MatrixXd::Zero( 7, 7 ), constantWeight);

    // Retrieve estimation errors and a priori covariances
    Eigen::MatrixXd tightConstraintInverseCovariance  =
            estimationOutputWithAprioriCovariance.first->getUnnormalizedInverseCovarianceMatrix( );
    Eigen::MatrixXd weakConstraintInverseCovariance  =
            estimationOutputWithSmallAprioriCovariance.first->getUnnormalizedInverseCovarianceMatrix( );
    Eigen::MatrixXd moderateConstraintInverseCovariance  =
            estimationOutputWithModerateAprioriCovariance.first->getUnnormalizedInverseCovarianceMatrix( );
    Eigen::MatrixXd noConstraintInverseCovariance  =
            estimationOutputWithoutAprioriCovariance.first->getUnnormalizedInverseCovarianceMatrix( );
    Eigen::MatrixXd noConstraintInverseCovarianceWithWeakWeight  =
            estimationOutputWithoutAprioriCovarianceAndWeakWeight.first->getUnnormalizedInverseCovarianceMatrix( );

    Eigen::VectorXd tightConstraintError  =
            estimationOutputWithAprioriCovariance.second;
    Eigen::VectorXd weakConstraintError  =
            estimationOutputWithSmallAprioriCovariance.second;
    Eigen::VectorXd moderateConstraintError  =
            estimationOutputWithModerateAprioriCovariance.second;
    Eigen::VectorXd noConstraintError  =
            estimationOutputWithoutAprioriCovariance.second;
    Eigen::VectorXd noConstraintWeakWeightError  =
            estimationOutputWithoutAprioriCovarianceAndWeakWeight.second;

    // Check if (effectively) unconstrained solutions converge at expected level
    for( unsigned int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( weakConstraintError( i ) ), 1.0E-2 );
        BOOST_CHECK_SMALL( std::fabs( weakConstraintError( i + 3 ) ), 1.0E-7 );

        BOOST_CHECK_SMALL( std::fabs( noConstraintError( i ) ), 1.0E-2 );
        BOOST_CHECK_SMALL( std::fabs( noConstraintError( i + 3 ) ), 1.0E-7 );

        BOOST_CHECK_SMALL( std::fabs( noConstraintWeakWeightError( i ) ), 1.0E-2 );
        BOOST_CHECK_SMALL( std::fabs( noConstraintWeakWeightError( i + 3 ) ), 1.0E-7 );
    }

    BOOST_CHECK_SMALL( std::fabs( weakConstraintError( 6 ) ), 500.0 );
    BOOST_CHECK_SMALL( std::fabs( noConstraintError( 6 ) ), 500.0 );
    BOOST_CHECK_SMALL( std::fabs( noConstraintWeakWeightError( 6 ) ), 500.0 );

    for( unsigned int i = 0; i < 7; i++ )
    {
        // Check if moderately constrained solution has intermediate accuracy
        BOOST_CHECK_EQUAL( std::fabs( moderateConstraintError( i ) ) > std::fabs( noConstraintError( i ) ), true );
        BOOST_CHECK_EQUAL( std::fabs( moderateConstraintError( i ) ) < std::fabs( tightConstraintError( i ) ), true );

        // Check if very tightly constrained solution has not differed from a priori error
        BOOST_CHECK_CLOSE_FRACTION( tightConstraintError( i ), parameterPerturbation( i ), 1.0E-8 );

        for( unsigned int j = 0; j < 7; j++ )
        {
            // Check if weights are correctly processed into covarince
            BOOST_CHECK_CLOSE_FRACTION( constantWeight * noConstraintInverseCovariance( i, j ),
                                        noConstraintInverseCovarianceWithWeakWeight( i, j ), 1.0E-8 );

            // Check if tight a priori constraints are processed correctly to a posteriori covariance
            if( i == j )
            {
                BOOST_CHECK_CLOSE_FRACTION(
                            tightConstraintInverseCovariance( i, j ), 1.0E32, 1.0E-10 );
            }
            else
            {
                BOOST_CHECK_SMALL( tightConstraintInverseCovariance( i, j ) / tightConstraintInverseCovariance( i, i ), 1.0E-10 );

            }
        }
    }
}

//! Test whether the covariance is correctly computed as a function of time
BOOST_AUTO_TEST_CASE( test_CovarianceAsFunctionOfTime )
{
    std::pair< std::shared_ptr< PodOutput< double > >, std::shared_ptr< PodInput< double, double > > > podData;

    // Simulate covariances directly by propagating to different final tomes
    std::map< int, Eigen::MatrixXd > manualCovarianes;
    for( unsigned int i = 1; i < 5; i++ )
    {
        executeEarthOrbiterParameterEstimation< double, double >(
                    podData, 1.0E7, i, 0, false );
        manualCovarianes[ i ] = podData.first->getUnnormalizedCovarianceMatrix( );
    }

    // Use final calculations to compute covariance as a function of time
    std::map< double, Eigen::MatrixXd > automaticCovariances = simulation_setup::calculateCovarianceMatrixAsFunctionOfTime(
                podData.second, podData.first, 86400.0 - 1.0 );

    // Check consistency
    int counter = 1;
    for( std::map< double, Eigen::MatrixXd >::const_iterator covarianceIterator = automaticCovariances.begin( );
         covarianceIterator != automaticCovariances.end( ); covarianceIterator++ )
    {
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( covarianceIterator->second, manualCovarianes.at( counter ), 1.0E-8 );
        counter++;
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#include <iostream>
#include <iomanip>
#include "PSMoveController.h"
#include "ServerLog.h"
#include "PSEyeVideoCapture.h"
#include "opencv2/opencv.hpp"
#include <algorithm>
#include <Eigen/Dense>

#ifndef HAVE_M_PI
#ifndef M_PI
#define M_PI    3.14159265358979323846264338327950288   /* pi */
#endif
#endif

const int HUE_RANGE = 25;
const int SAT_RANGE = 50;
const int VAL_RANGE = 32;

typedef cv::Vec< unsigned char, 3 > cvBGR;
typedef cv::Vec< unsigned char, 3 > cvRGB;
typedef cv::Vec< unsigned char, 3 > cvHSV;

cvHSV
bgr2hsv(cvBGR bgr_in)
{
    static cv::Mat img_bgr(1, 1, CV_8UC3, bgr_in);
    static cv::Mat img_hsv(1, 1, CV_8UC3);
    cv::cvtColor(img_bgr, img_hsv, CV_BGR2HSV);
    return img_hsv.at<cvHSV>(0, 0);
}

void hsv_range(cvHSV hsv_in, cvHSV& min_out, cvHSV& max_out)
{
    min_out.val[0] = MAX(hsv_in.val[0] - HUE_RANGE, 0);  //0-180
    min_out.val[1] = MAX(hsv_in.val[1] - SAT_RANGE, 0);  //0-255
    min_out.val[2] = MAX(hsv_in.val[2] - VAL_RANGE, 0);  //0-255

    max_out.val[0] = MIN(hsv_in.val[0] + HUE_RANGE, 180);
    max_out.val[1] = MIN(hsv_in.val[1] + SAT_RANGE, 255);
    max_out.val[2] = MIN(hsv_in.val[2] + VAL_RANGE, 255);
}

void onMouse(int evt, int x, int y, int flags, void* param) {
    if(evt == CV_EVENT_LBUTTONDOWN) {
        cv::Point *ptPtr = (cv::Point*)param;
        *ptPtr = cv::Point(x, y);
    }
}

int main()
{
    log_init("info");

    if (hid_init() == -1)
    {
        std::cerr << "Failed to initialize hidapi" << std::endl;
        return -1;
    }

    PSMoveController psmove;
    PSEyeVideoCapture cap(0); // open the default camera
    cv::namedWindow("bgr video");
    cv::namedWindow("hsv video");
    cv::namedWindow("result");

    cv::Point clickPoint;
    cv::setMouseCallback("hsv video", onMouse, (void*)&clickPoint);

	std::cout << "Opening PSMoveController..." << std::endl;
	if (psmove.open() && cap.isOpened())
	{
        const PSMoveControllerState *psmstate= nullptr;

        cap.set(cv::CAP_PROP_EXPOSURE, 16);
        cap.set(cv::CAP_PROP_GAIN, 64);

        psmove.poll();
        psmstate= static_cast<const PSMoveControllerState *>(psmove.getState());

		unsigned char r = 255;
		unsigned char g = 0;
		unsigned char b = 255;
        psmove.setLED(r, g, b);
        psmove.setRumbleIntensity(0);

        cv::Mat bgrFrame;
        cv::Mat hsvFrame;
        cv::Mat intGsFrame;
        cv::Mat gsFrame;
        cvHSV led_min, led_max;
        cvHSV psmoveHSVColour = bgr2hsv(cvBGR(b, g, r));

        // HACK FOR TESTING
        psmoveHSVColour = cvHSV(140, 25, 255);

        hsv_range(psmoveHSVColour, led_min, led_max);

		while (psmove.getIsBluetooth() && psmstate->Move != CommonControllerState::Button_DOWN)
		{
            psmove.poll();
            psmstate= static_cast<const PSMoveControllerState *>(psmove.getState());

            psmove.setLED(r, g, b); // TODO: Rate-limit LED update

            cap >> bgrFrame; // get a new frame from camera

            if (!bgrFrame.empty())
            {
                cv::cvtColor(bgrFrame, hsvFrame, CV_BGR2HSV);       // Convert frame to HSV colour space
                cv::inRange(hsvFrame, led_min, led_max, gsFrame);  // Filter based on led HSV range

//                cv::GaussianBlur(intGsFrame, gsFrame, cv::Size(3, 3), 0);
//                cv::blur(intGsFrame, gsFrame, cv::Size(3, 3));  // Fastest
//                cv::bilateralFilter(intGsFrame, gsFrame, 3, 6.0, 1.5);  // Preserves edges

                imshow("result", gsFrame);

//                if (clickPoint.x > 0)
//                {
//                    cvHSV hsv_bulb = hsvFrame.at<cvHSV>(clickPoint);
//                    std::cout << clickPoint << ";" << hsv_bulb << std::endl;
//                }

                std::vector<std::vector<cv::Point> > contours;
                cv::findContours(gsFrame, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

                cv::Scalar contCol = cv::Scalar( 255, 0, 0 );
                for (int i=0; i<contours.size(); ++i) {
                    cv::drawContours(bgrFrame, contours, i, contCol);
                }

                std::vector<cv::Point> biggest_contour;
                if (contours.size() > 0)
                {
                    double contArea = 0;
                    double newArea = 0;
                    for (auto it = contours.begin(); it != contours.end(); ++it) {
                        newArea = cv::contourArea(*it);
                        if (newArea > contArea)
                        {
                            contArea = newArea;
                            biggest_contour = *it;
                        }
                    }
                }

                //TODO: If our contour is suddenly much smaller than last frame,
                // but is next to an almost-as-big contour, then maybe these
                // 2 contours should be joined.
                // (i.e. if a finger is blocking the middle of the bulb)

                if (biggest_contour.size() > 6)
                {
                    // Remove any points in contour on edge of camera/ROI
                    std::vector<cv::Point>::iterator it = biggest_contour.begin();
                    while (it != biggest_contour.end()) {
                        if (it->x == 320 || it->x == -320 || it->y == 240 || it->y == -240) {
                            it = biggest_contour.erase(it);
                        }
                        else {
                            ++it;
                        }
                    }

                    // Get the convex hull
                    std::vector<cv::Point> hull;
                    cv::convexHull(biggest_contour, hull);

//                    // If our hull has lots of points, select the N consecutive
//                    // points that are furthest, because furthest are least
//                    // likely to be occluded (assuming player is near middle of FOV).
//                    int N = 10;
//                    if (hull.size() > N)
//                    {
//                        std::deque<float> dists;
//                        float max_dist = 0;
//                        float sum_dists = 0;
//                        int best_i = 0;
//                        for (int i=0; i < (hull.size()-N); i++) {
//                            float new_dist = hull[i].x*hull[i].x + hull[i].y*hull[i].y;
//                            sum_dists += new_dist;
//                            dists.push_back(new_dist);
//                            if (i >= N)
//                            {
//                                sum_dists -= dists.front();
//                                dists.pop_front();
//                                if (sum_dists > max_dist)
//                                {
//                                    best_i = i;
//                                    max_dist = sum_dists;
//                                }
//                            }
//                        }
//                        hull = std::vector<cv::Point>(&hull[best_i], &hull[best_i+N]);
//                    }

                    {
                        std::vector< std::vector<cv::Point> > contours;

                        contours.push_back(hull);
                        cv::drawContours(bgrFrame, contours, 0, cv::Scalar(255, 0, 255));
                    }

                    // Fit ellipse to reduced hull

                    // Subtract midpoint from each point.
                    std::for_each(hull.begin(), hull.end(), [](cv::Point& p) { p.x -= 320; p.y = 240 - p.y;});

                    if (hull.size() > 5)
                    {
                        // http://autotrace.sourceforge.net/WSCG98.pdf
                        std::vector<float> conic_params(6);

                        Eigen::MatrixXf D1(hull.size(), 3);
                        Eigen::MatrixXf D2(hull.size(), 3);
                        for (int ix=0; ix < hull.size(); ++ix) {
                            D1.row(ix)[0] = hull[ix].x * hull[ix].x;
                            D1.row(ix)[1] = hull[ix].x * hull[ix].y;
                            D1.row(ix)[2] = hull[ix].y * hull[ix].y;
                            D2.row(ix)[0] = hull[ix].x;
                            D2.row(ix)[1] = hull[ix].y;
                            D2.row(ix)[2] = 1;
                        }
                        // std::cout << "\n\n\n\n" << hull << "\n\n\n\n" << std::endl;
                        Eigen::Matrix3f S1 = D1.transpose() * D1;
                        Eigen::Matrix3f S2 = D1.transpose() * D2;
                        Eigen::Matrix3f S3 = D2.transpose() * D2;
                        Eigen::Matrix3f T = -S3.colPivHouseholderQr().solve(S2.transpose());
//                        Eigen::Matrix3f T = -S3.inverse() * S2.transpose();
                        Eigen::Matrix3f M = S2*T + S1;
                        Eigen::Matrix3f Mout;
                        Mout.block<1, 3>(0, 0) = M.block<1, 3>(2, 0) / 2;
                        Mout.block<1, 3>(1, 0) = -M.block<1, 3>(1, 0);
                        Mout.block<1, 3>(2, 0) = M.block<1, 3>(0, 0) / 2;
                        Eigen::EigenSolver<Eigen::Matrix3f> eigsolv(Mout);
                        for (int row_ix=0; row_ix<3; ++row_ix) {
                            Eigen::Vector3f evec = eigsolv.eigenvectors().col(row_ix).real();
                            //cond = 4 * evec(1, :) .* evec(3, :) - evec(2, :).^2; % evaluate a'Ca
                            if ((4.0 * evec[0] * evec[2]) - (evec[1] * evec[1]) > 0)
                            {
                                conic_params[0] = evec[0];
                                conic_params[1] = evec[1];
                                conic_params[2] = evec[2];
                                Eigen::Vector3f Tevec = T*evec;
                                conic_params[3] = Tevec[0];
                                conic_params[4] = Tevec[1];
                                conic_params[5] = Tevec[2];
                            }
                        }

                        cv::RotatedRect cvFitEllipse= cv::fitEllipse(hull);
                        cv::ellipse(
                            bgrFrame,
                            cv::Point(cvFitEllipse.center.x + 320, 240 - cvFitEllipse.center.y),
                            cv::Size(cvFitEllipse.size.width/2, cvFitEllipse.size.height/2),
                            cvFitEllipse.angle,
                            0, 360,
                            cv::Scalar(0, 255, 255));

                        // Convert to parametric params
                        float A = conic_params[0];
                        float B = conic_params[1] / 2;
                        float C = conic_params[2];
                        float D = conic_params[3] / 2;
                        float F = conic_params[4] / 2;
                        float G = conic_params[5];
                        float BB = B*B;
                        float AC = A*C;
                        float FF = F*F;
                        float off_d = BB - AC;
                        float h = (C*D - B*F) / off_d;
                        float k = (A*F - B*D) / off_d;
                        float semi_n = A*FF + C*D*D + G*BB - 2*B*D*F - AC*G;
                        float dAC = A-C;
                        float dACsq = dAC*dAC;
                        float semi_d_2 = sqrtf(dACsq + 4*BB);
                        float semi_d_3 = -A - C;
                        float a_sqrd = (2 * semi_n) / (off_d * (semi_d_3 + semi_d_2));
                        float b_sqrd = (2 * semi_n) / (off_d * (semi_d_3 - semi_d_2));

                        if (a_sqrd > FLT_EPSILON && b_sqrd > FLT_EPSILON)
                        {
                            // b and tau are only needed for drawing an ellipse.
                            float a = sqrt(a_sqrd);
                            float b = sqrt(b_sqrd);
                            double tau = atan2(2 * B, dAC) / 2;  //acot((A-C)/(2*B))/2;
                            if (A > C)
                            {
                                tau += M_PI/2;
                            }
//                            if (tau < 0)
//                            {
//                                float oldb = b;
//                                b = a;
//                                a = oldb;
//                                tau += M_PI;
//                            }
                            cv::ellipse(bgrFrame, cv::Point(h+320, 240-k), cv::Size(a, b), tau, 0, 360, cv::Scalar(0, 255, 0));

                            //Get sphere coordinates from parametric
                            float F_PX = 554.2563;
                            float R = 2.25;
                            float L_px = sqrt(h*h + k*k);
                            float m = L_px / F_PX;
                            float fl = F_PX / L_px;
                            float j = (L_px + a) / F_PX;
                            float l = (j - m) / (1 + j*m);
                            float D_cm = R * sqrt(1 + l*l) / l;
                            float z = D_cm * fl / sqrt(1 + fl*fl);
                            float L_cm = z * m;
                            float x = L_cm * h / L_px;
                            float y = L_cm * k / L_px;

                            std::cout << "X: " << x << " Y: " << y << " Z: " << z << std::endl;
                        }
                    }

                }

                imshow("bgr video", bgrFrame);
                imshow("hsv video", hsvFrame);
            }

			int myw = 4;
			std::cout << '\r' <<
				"# " << std::setw(myw) << std::left << psmstate->RawSequence <<
				" A(1): " <<
				std::setw(myw) << std::right << psmstate->RawAccel[0][0] << "," <<
                std::setw(myw) << std::right << psmstate->RawAccel[0][1] << "," <<
                std::setw(myw) << std::right << psmstate->RawAccel[0][2] <<
				"; A(2): " <<
                std::setw(myw) << std::right << psmstate->RawAccel[1][0] << "," <<
                std::setw(myw) << std::right << psmstate->RawAccel[1][1] << "," <<
                std::setw(myw) << std::right << psmstate->RawAccel[1][2] <<
				"; G(1): " <<
                std::setw(myw) << std::right << psmstate->RawGyro[0][0] << "," <<
                std::setw(myw) << std::right << psmstate->RawGyro[0][1] << "," <<
                std::setw(myw) << std::right << psmstate->RawGyro[0][2] <<
				"; G(2): " <<
                std::setw(myw) << std::right << psmstate->RawGyro[1][0] << "," <<
                std::setw(myw) << std::right << psmstate->RawGyro[1][1] << "," <<
                std::setw(myw) << std::right << psmstate->RawGyro[1][2] <<
				"; M: " <<
                std::setw(myw) << std::right << psmstate->RawMag[0] << "," <<
                std::setw(myw) << std::right << psmstate->RawMag[1] << "," <<
                std::setw(myw) << std::right << psmstate->RawMag[2] <<
				std::flush;

            cv::waitKey(16);
		}
        std::cout << std::endl;

        psmove.close();
	}

    // Tear-down hid api
    hid_exit();

	log_dispose();

    return 0;
}