#include "slot.h"

#include <cassert>
#include <cmath>

#include <boost/algorithm/clamp.hpp>
#include <boost/optional.hpp>

#include "../shader_core/config.h"
#include "../shader_core/rect.h"
#include "../shader_core/vector.h"

static bool set_curve(csg::Curve& curve, const csg::Curve& new_value)
{
	if (new_value.min() != curve.min() || new_value.max() != curve.max()) {
		return false;
	}
	curve = new_value;
	return true;
}

bool csg::ColorSlotValue::operator==(const ColorSlotValue& other) const
{
	return value.similar(other.value, FLOAT_COMPARE_DIFF);
}

void csg::EnumSlotValue::set(const size_t new_value)
{
	if (new_value <= max_value) {
		value = new_value;
	}
}

const char* csg::EnumSlotValue::internal_name() const
{
	boost::optional<NodeEnumOptionInfo> opt_info{ NodeEnumOptionInfo::from(meta_enum, value) };
	if (opt_info) {
		return opt_info->internal_name();
	}
	return "NAME ERROR";
}

bool csg::EnumSlotValue::operator==(const EnumSlotValue& other) const
{
	return (value == other.value && meta_enum == other.meta_enum);
}

void csg::FloatSlotValue::set(const float new_value)
{
	value = boost::algorithm::clamp(new_value, min, max);
}

bool csg::FloatSlotValue::operator==(const FloatSlotValue& other) const
{
	return (fabs(value - other.value) < FLOAT_COMPARE_DIFF) && min == other.min && max == other.max && _precision == other._precision;
}

void csg::IntSlotValue::set(const int new_value)
{
	value = boost::algorithm::clamp(new_value, min, max);
}

bool csg::IntSlotValue::operator==(const IntSlotValue& other) const
{
	return value == other.value && min == other.min && max == other.max;
}

void csg::VectorSlotValue::set(const csc::Float3 new_value)
{
	const float x{ boost::algorithm::clamp(new_value.x, min.x, max.x) };
	const float y{ boost::algorithm::clamp(new_value.y, min.y, max.y) };
	const float z{ boost::algorithm::clamp(new_value.z, min.z, max.z) };
	value = csc::Float3{ x, y, z };
}

bool csg::VectorSlotValue::operator==(const VectorSlotValue& other) const
{
	return value.similar(other.value, FLOAT_COMPARE_DIFF);
}

csg::RGBCurveSlotValue::RGBCurveSlotValue() :
	curve_all{ csc::Float2{ 0.0f, 0.0f }, csc::Float2{ 1.0f, 1.0f } },
	curve_r{ csc::Float2{ 0.0f, 0.0f }, csc::Float2{ 1.0f, 1.0f } },
	curve_g{ csc::Float2{ 0.0f, 0.0f }, csc::Float2{ 1.0f, 1.0f } },
	curve_b{ csc::Float2{ 0.0f, 0.0f }, csc::Float2{ 1.0f, 1.0f } }
{

}

bool csg::RGBCurveSlotValue::set_all(const Curve& value)
{
	return set_curve(curve_all, value);
}

bool csg::RGBCurveSlotValue::set_r(const Curve& value)
{
	return set_curve(curve_r, value);
}

bool csg::RGBCurveSlotValue::set_g(const Curve& value)
{
	return set_curve(curve_g, value);
}

bool csg::RGBCurveSlotValue::set_b(const Curve& value)
{
	return set_curve(curve_b, value);
}

bool csg::RGBCurveSlotValue::operator==(const RGBCurveSlotValue& other) const
{
	return (
		curve_all.similar(other.curve_all , FLOAT_COMPARE_DIFF) &&
		curve_r.similar(other.curve_r, FLOAT_COMPARE_DIFF) &&
		curve_g.similar(other.curve_g, FLOAT_COMPARE_DIFF) &&
		curve_b.similar(other.curve_b, FLOAT_COMPARE_DIFF)
		);
}

csg::VectorCurveSlotValue::VectorCurveSlotValue(const csc::Float2 min, const csc::Float2 max) :
	curve_x{ min, max },
	curve_y{ min, max },
	curve_z{ min, max },
	min{ min },
	max{ max }
{

}

bool csg::VectorCurveSlotValue::set_x(const Curve& value)
{
	return set_curve(curve_x, value);
}

bool csg::VectorCurveSlotValue::set_y(const Curve& value)
{
	return set_curve(curve_y, value);
}

bool csg::VectorCurveSlotValue::set_z(const Curve& value)
{
	return set_curve(curve_z, value);
}

bool csg::VectorCurveSlotValue::set_bounds(const csc::FloatRect bounds_rect)
{
	const bool valid_x{ curve_x.bounds_valid(bounds_rect) };
	const bool valid_y{ curve_y.bounds_valid(bounds_rect) };
	const bool valid_z{ curve_z.bounds_valid(bounds_rect) };
	if (valid_x && valid_y && valid_z) {
		curve_x.set_bounds(bounds_rect);
		curve_y.set_bounds(bounds_rect);
		curve_z.set_bounds(bounds_rect);
		min = bounds_rect.begin();
		max = bounds_rect.end();
		return true;
	}
	else {
		return false;
	}
}

bool csg::VectorCurveSlotValue::operator==(const VectorCurveSlotValue& other) const
{
	return (
		curve_x.similar(other.curve_x, FLOAT_COMPARE_DIFF) &&
		curve_y.similar(other.curve_y, FLOAT_COMPARE_DIFF) &&
		curve_z.similar(other.curve_z, FLOAT_COMPARE_DIFF) &&
		min.similar(other.min, FLOAT_COMPARE_DIFF) &&
		max.similar(other.max, FLOAT_COMPARE_DIFF)
	);
}

bool csg::ColorRampSlotValue::operator==(const ColorRampSlotValue& other) const
{
	return ramp.similar(other.ramp, FLOAT_COMPARE_DIFF);
}

bool csg::ImageSlotValue::operator==(const ImageSlotValue& other) const
{
    return strcmp(filePath, other.filePath) ==0;
}

csg::SlotValue& csg::SlotValue::operator=(const SlotValue& other)
{
	_type = other._type;
	value_union = other.value_union;

	if (other.curve_rgb_value) {
		curve_rgb_value = std::make_unique<RGBCurveSlotValue>(*other.curve_rgb_value);
	}
	else {
		curve_vector_value = std::unique_ptr<VectorCurveSlotValue>();
	}

	if (other.curve_vector_value) {
		curve_vector_value = std::make_unique<VectorCurveSlotValue>(*other.curve_vector_value);
	}
	else {
		curve_vector_value = std::unique_ptr<VectorCurveSlotValue>();
	}

	if (other.color_ramp_value) {
		color_ramp_value = std::make_unique<ColorRampSlotValue>(*other.color_ramp_value);
	}
	else {
		color_ramp_value = std::unique_ptr<ColorRampSlotValue>();
	}

    if (other.image_path_value) {
        image_path_value = std::make_unique<ImageSlotValue>(*other.image_path_value);
    }
    else {
        image_path_value = std::unique_ptr<ImageSlotValue>();
    }

	return *this;
}

template <> boost::optional<csg::BoolSlotValue> csg::SlotValue::as() const {
	return (type() != SlotType::BOOL) ? boost::none : boost::optional<csg::BoolSlotValue>{ value_union.bool_value };
}
template <> boost::optional<csg::ColorSlotValue> csg::SlotValue::as() const {
	return (type() != SlotType::COLOR) ? boost::none : boost::optional<csg::ColorSlotValue>{ value_union.color_value };
}
template <> boost::optional<csg::EnumSlotValue> csg::SlotValue::as() const {
	return (type() != SlotType::ENUM) ? boost::none : boost::optional<csg::EnumSlotValue>{ value_union.enum_value };
}
template <> boost::optional<csg::FloatSlotValue> csg::SlotValue::as() const {
	return (type() != SlotType::FLOAT) ? boost::none : boost::optional<csg::FloatSlotValue>{ value_union.float_value };
}
template <> boost::optional<csg::IntSlotValue> csg::SlotValue::as() const {
	return (type() != SlotType::INT) ? boost::none : boost::optional<csg::IntSlotValue>{ value_union.int_value };
}
template <> boost::optional<csg::VectorSlotValue> csg::SlotValue::as() const {
	return (type() != SlotType::VECTOR) ? boost::none : boost::optional<csg::VectorSlotValue>{ value_union.vector_value };
}
template <> boost::optional<csg::RGBCurveSlotValue> csg::SlotValue::as() const {
	return (type() != SlotType::CURVE_RGB || static_cast<bool>(curve_rgb_value) == false) ? boost::none : boost::optional<csg::RGBCurveSlotValue>{ *curve_rgb_value };
}
template <> boost::optional<csg::VectorCurveSlotValue> csg::SlotValue::as() const {
	return (type() != SlotType::CURVE_VECTOR || static_cast<bool>(curve_vector_value) == false) ? boost::none : boost::optional<csg::VectorCurveSlotValue>{ *curve_vector_value };
}
template <> boost::optional<csg::ColorRampSlotValue> csg::SlotValue::as() const {
	return (type() != SlotType::COLOR_RAMP || static_cast<bool>(color_ramp_value) == false) ? boost::none : boost::optional<csg::ColorRampSlotValue>{ *color_ramp_value };
}

template <> boost::optional<csg::ImageSlotValue> csg::SlotValue::as() const {
    return (type() != SlotType::IMAGE || static_cast<bool>(image_path_value) == false) ? boost::none : boost::optional<csg::ImageSlotValue>{ *image_path_value };
}

bool csg::SlotValue::operator==(const SlotValue& other) const
{
	if (_type != other._type) {
		return false;
	}

	switch (_type) {
		case SlotType::BOOL:
		{
			if (value_union.bool_value != other.value_union.bool_value) {
				return false;
			}
			break;
		}
		case SlotType::COLOR:
		{
			if (value_union.color_value != other.value_union.color_value) {
				return false;
			}
			break;
		}
		case SlotType::ENUM:
		{
			if (value_union.enum_value != other.value_union.enum_value) {
				return false;
			}
			break;
		}
		case SlotType::FLOAT:
		{
			if (value_union.float_value != other.value_union.float_value) {
				return false;
			}
			break;
		}
		case SlotType::INT:
		{
			if (value_union.int_value != other.value_union.int_value) {
				return false;
			}
			break;
		}
		case SlotType::VECTOR:
		{
			if (value_union.vector_value != other.value_union.vector_value) {
				return false;
			}
			break;
		}
		case SlotType::CURVE_RGB:
		{
			assert(curve_rgb_value.get() != nullptr);
			assert(other.curve_rgb_value.get() != nullptr);
			if (*curve_rgb_value != *other.curve_rgb_value) {
				return false;
			}
			break;
		}
		case SlotType::CURVE_VECTOR:
		{
			assert(curve_vector_value.get() != nullptr);
			assert(other.curve_vector_value.get() != nullptr);
			if (*curve_vector_value != *other.curve_vector_value) {
				return false;
			}
			break;
		}
		case SlotType::COLOR_RAMP:
		{
			assert(color_ramp_value.get() != nullptr);
			assert(other.color_ramp_value.get() != nullptr);
			if (*color_ramp_value != *other.color_ramp_value) {
				return false;
			}
			break;
		}
        case SlotType::IMAGE:
        {
            assert(image_path_value.get() != nullptr);
            assert(other.image_path_value.get() != nullptr);
            if (*image_path_value != *other.image_path_value) {
                return false;
            }
            break;
        }
		default:
			// This should only happen when a new unsupported type is added
			assert(false);
	}

	return true;
}

bool csg::Slot::operator==(const Slot& other) const
{
	if (_name != other._name) {
		return false;
	}

	if (_dir != other._dir) {
		return false;
	}

	if (_type != other._type) {
		return false;
	}

	if (value.has_value() != other.value.has_value()) {
		return false;
	}

	if (value.has_value()) {
		if (*value != *(other.value)) {
			return false;
		}
	}

	return true;
}


//
// Copyright (c) 2018 CNRS, NYU, MPI Tübingen
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid 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 GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#ifndef __tsid_python_contact_6d_hpp__
#define __tsid_python_contact_6d_hpp__

#include <pinocchio/fwd.hpp>
#include <boost/python.hpp>
#include <string>
#include <eigenpy/eigenpy.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include "tsid/contacts/contact-6d.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/math/constraint-inequality.hpp"
#include "tsid/math/constraint-equality.hpp"
#include "tsid/math/constraint-base.hpp"
#include "tsid/tasks/task-se3-equality.hpp"

namespace tsid
{
  namespace python
  {
    namespace bp = boost::python;

    template<typename Contact6d>
    struct Contact6DPythonVisitor
    : public boost::python::def_visitor< Contact6DPythonVisitor<Contact6d> >
    {

      template<class PyClass>

      void visit(PyClass& cl) const
      {
        cl
        .def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::MatrixXd, Eigen::VectorXd, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactPoint"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce")), "Default Constructor"))
        .def(bp::init<std::string, robots::RobotWrapper &, std::string, Eigen::MatrixXd, Eigen::VectorXd, double, double, double, double> ((bp::arg("name"), bp::arg("robot"), bp::arg("framename"), bp::arg("contactPoint"), bp::arg("contactNormal"), bp::arg("frictionCoeff"), bp::arg("minForce"), bp::arg("maxForce"), bp::arg("wForceReg")), "Deprecated Constructor"))
        .add_property("n_motion", &Contact6d::n_motion, "return number of motion")
        .add_property("n_force", &Contact6d::n_force, "return number of force")
        .add_property("name", &Contact6DPythonVisitor::name, "return name")
        .def("computeMotionTask", &Contact6DPythonVisitor::computeMotionTask, bp::args("t", "q", "v", "data"))
        .def("computeForceTask", &Contact6DPythonVisitor::computeForceTask, bp::args("t", "q", "v", "data"))
        .def("computeForceRegularizationTask", &Contact6DPythonVisitor::computeForceRegularizationTask, bp::args("t", "q", "v", "data"))

        .add_property("getForceGeneratorMatrix", bp::make_function(&Contact6DPythonVisitor::getForceGeneratorMatrix, bp::return_value_policy<bp::copy_const_reference>()))

        .def("getNormalForce", &Contact6DPythonVisitor::getNormalForce, bp::arg("vec"))
        .add_property("getMinNormalForce", &Contact6d::getMinNormalForce)
        .add_property("getMaxNormalForce", &Contact6d::getMaxNormalForce)

        .add_property("Kp", bp::make_function(&Contact6DPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
        .add_property("Kd", bp::make_function(&Contact6DPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
        .def("setKp", &Contact6DPythonVisitor::setKp, bp::arg("Kp"))
        .def("setKd", &Contact6DPythonVisitor::setKd, bp::arg("Kd"))

        .def("setContactPoints", &Contact6DPythonVisitor::setContactPoints, bp::args("vec"))
        .def("setContactNormal", &Contact6DPythonVisitor::setContactNormal, bp::args("vec"))
        .def("setFrictionCoefficient", &Contact6DPythonVisitor::setFrictionCoefficient, bp::args("friction_coeff"))
        .def("setMinNormalForce", &Contact6DPythonVisitor::setMinNormalForce, bp::args("min_force"))
        .def("setMaxNormalForce", &Contact6DPythonVisitor::setMaxNormalForce, bp::args("max_force"))
        .def("setReference", &Contact6DPythonVisitor::setReference, bp::args("SE3"))
        .def("setForceReference", &Contact6DPythonVisitor::setForceReference, bp::args("f_vec"))
        .def("setRegularizationTaskWeightVector", &Contact6DPythonVisitor::setRegularizationTaskWeightVector, bp::args("w_vec"))
        ;
      }
      static std::string name(Contact6d & self){
        std::string name = self.name();
        return name;
      }

      static math::ConstraintEquality computeMotionTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, pinocchio::Data & data){
        self.computeMotionTask(t, q, v, data);
        math::ConstraintEquality cons(self.getMotionConstraint().name(), self.getMotionConstraint().matrix(), self.getMotionConstraint().vector());
        return cons;
      }
      static math::ConstraintInequality computeForceTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
        self.computeForceTask(t, q, v, data);
        math::ConstraintInequality cons(self.getForceConstraint().name(), self.getForceConstraint().matrix(), self.getForceConstraint().lowerBound(), self.getForceConstraint().upperBound());
        return cons;
      }
      static math::ConstraintEquality computeForceRegularizationTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
        self.computeForceRegularizationTask(t, q, v, data);
        math::ConstraintEquality cons(self.getForceRegularizationTask().name(), self.getForceRegularizationTask().matrix(), self.getForceRegularizationTask().vector());
        return cons;
      }

      static const Eigen::MatrixXd & getForceGeneratorMatrix(Contact6d & self){
        return self.getForceGeneratorMatrix();
      }
      static const Eigen::VectorXd & Kp (Contact6d & self){
        return self.Kp();
      }
      static const Eigen::VectorXd & Kd (Contact6d & self){
        return self.Kd();
      }
      static void setKp (Contact6d & self, const::Eigen::VectorXd Kp){
        return self.Kp(Kp);
      }
      static void setKd (Contact6d & self, const::Eigen::VectorXd Kd){
        return self.Kd(Kd);
      }
      static bool setContactPoints (Contact6d & self, const::Eigen::MatrixXd contactpoints){
        return self.setContactPoints(contactpoints);
      }
      static bool setContactNormal (Contact6d & self, const::Eigen::VectorXd contactNormal){
        return self.setContactNormal(contactNormal);
      }
      static bool setFrictionCoefficient (Contact6d & self, const double frictionCoefficient){
        return self.setFrictionCoefficient(frictionCoefficient);
      }
      static bool setMinNormalForce (Contact6d & self, const double minNormalForce){
        return self.setMinNormalForce(minNormalForce);
      }
      static bool setMaxNormalForce (Contact6d & self, const double maxNormalForce){
        return self.setMaxNormalForce(maxNormalForce);
      }
      static void setReference(Contact6d & self, const pinocchio::SE3 & ref){
        self.setReference(ref);
      }
      static void setForceReference(Contact6d & self, const::Eigen::VectorXd f_ref){
        self.setForceReference(f_ref);
      }
      static void setRegularizationTaskWeightVector(Contact6d & self, const::Eigen::VectorXd w){
        self.setRegularizationTaskWeightVector(w);
      }
      static double getNormalForce(Contact6d & self, Eigen::VectorXd f){
        return self.getNormalForce(f);
      }

      static void expose(const std::string & class_name)
      {
        std::string doc = "Contact6d info.";
        bp::class_<Contact6d>(class_name.c_str(),
                          doc.c_str(),
                          bp::no_init)
        .def(Contact6DPythonVisitor<Contact6d>());
      }
    };
  }
}

#endif // ifndef __tsid_python_contact_hpp__


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

#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
#include <map>
#include <algorithm>
#include <stdint.h>

#include "gflags/gflags.h"
#include "glog/logging.h"
#include <boost/shared_ptr.hpp>

#include "caffe/proto/caffe.pb.h"
#include "caffe/blob.hpp"
#include "caffe/util/benchmark.hpp"
#include "caffe/util/io.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/layer.hpp"
#include "caffe/layers/roi_align_layer.hpp"
#include "caffe/filler.hpp"

#include "gtest/gtest.h"
#include "caffe/test/test_caffe_main.hpp"
#include "caffe/test/test_gradient_check_util.hpp"

using namespace std;
using namespace caffe;

int main(int nargc, char** args) {

  // set caffe::GPU
  caffe::Caffe::SetDevice(0);
  caffe::Caffe::set_mode(caffe::Caffe::GPU);

  // define input & output blobs
  Blob<float>* input_0 = new Blob<float>(1, 2, 12, 12);
  Blob<float>* input_1 = new Blob<float>(1, 1, 2, 7);
  Blob<float>* output = new Blob<float>(2, 2, 6, 6);
  vector<Blob<float>*> bottom_vec;
  vector<Blob<float>*> top_vec;

  // set the bottom blob data
  FillerParameter filler_param;
  filler_param.set_std(0.5);
  GaussianFiller<float> filler(filler_param);
  filler.Fill(input_0);

  // set the ROI data
  float* roi_data = input_1->mutable_cpu_data();
  caffe_set(input_1->count(), (float)0, roi_data);
  roi_data[0] = 0;
  roi_data[3] = 0.1;
  roi_data[4] = 0.17;
  roi_data[5] = 0.4;
  roi_data[6] = 0.85;
  roi_data[7+0] = 0;
  roi_data[7+3] = 0.38;
  roi_data[7+4] = 0.02;
  roi_data[7+5] = 0.79;
  roi_data[7+6] = 0.9;

  // set the bottom and top data-pointer
  bottom_vec.push_back(input_0);
  bottom_vec.push_back(input_1);
  top_vec.push_back(output);

  // set the param of Align layer
  int roiResizedWidth = output->width();
  int roiResizedHeight = output->height();
  int inter_times = 1;

  // create reorg layer
  LayerParameter layer_param;
  layer_param.set_name("test_Align");
  layer_param.set_type("RoiAlign");
  RoiAlignParameter* roi_align_param = layer_param.mutable_roi_align_param();
  roi_align_param->set_roi_resized_width(roiResizedWidth);
  roi_align_param->set_roi_resized_height(roiResizedHeight);
  roi_align_param->set_inter_times(inter_times);

  // RoiAlignLayer<float> roi_align_layer(layer_param);
  boost::shared_ptr<Layer<float> > roi_align_layer = LayerRegistry<float>::CreateLayer(layer_param);
  // LOG(INFO) << "Step6...";
  roi_align_layer->SetUp(bottom_vec, top_vec);
  //
  // LOG(INFO) << "Start Running...";

  // Runing the layer
  // roi_align_layer->Reshape(bottom_vec, top_vec);
  // roi_align_layer->Forward(bottom_vec, top_vec);

  // RoiAlignLayer<float> roi_align_layer(layer_param);
  /**
   * first one: stepsize
   * second one: threshold
   */
  GradientChecker<float> checker(1e-4, 1e-2);
  checker.CheckGradientExhaustive(roi_align_layer.get(), bottom_vec, top_vec);
  // checker.CheckGradientExhaustive(&roi_align_layer, bottom_vec, top_vec);
  /**
   * [top_id, top_data_id, blob_id, feat_id] :
   * top_id -> top.size()[...]
   * top_data_id -> top[...].count()[...]
   * blob_id -> bottom.size()[...]
   * feat_id -> bottom[...].count()[...]
   * Note: the ROIData, with blob_id == 1 should be ignored.
   * So error with blob_id == 1 could be ignored cuz we do not backpropagate the gradient to roi input.
   */
  LOG(INFO) << "Gradient Check Finished.";
  return 0;
}


/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "mitkMakeMaskImagesFromStereoVideo.h"

#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

#include <cv.h>
//#include <opencv2/highgui/highgui.hpp>
#include <highgui.h>

#include <mitkCameraCalibrationFacade.h>
#include <mitkOpenCVFileIOUtils.h>
#include <mitkOpenCVMaths.h>
#include <mitkOpenCVPointTypes.h>

#include <niftkFileHelper.h>
#include <niftkFileIOUtils.h>

namespace mitk
{

//-----------------------------------------------------------------------------
MakeMaskImagesFromStereoVideo::MakeMaskImagesFromStereoVideo()
:
m_VideoIn("")
, m_Directory("")
, m_TrackerIndex(0)
, m_ReferenceIndex(-1)
, m_InitOK(false)
, m_ProjectOK(false)
, m_VideoWidth(1920)
, m_VideoHeight(540)
, m_Capture(NULL)
, m_AllowableTimingError (20e6) // 20 milliseconds
, m_AskOverWrite(false)
, m_HaltOnVideoReadFail(true)
, m_FlipVideo(false)
, m_StartFrame(12) //by default we select different frames than mitkPickPointsOnStereoVideo
, m_EndFrame(0)
, m_Frequency(150)
{
}

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

//-----------------------------------------------------------------------------
void MakeMaskImagesFromStereoVideo::Initialise(std::string directory)
{
  m_InitOK = false;
  m_Directory = directory;
  m_OutDirectory = m_Directory + niftk::GetFileSeparator() +  "VideoMasks";
  niftk::LoadTimeStampData(m_TimeStampsFile, m_TimeStampsSet);
  if (m_TimeStampsSet.size() > 0)
  {
    m_Frequency = 1;
  }
  m_InitOK = true;
  return;

}

//-----------------------------------------------------------------------------
void MakeMaskImagesFromStereoVideo::Project(mitk::VideoTrackerMatching::Pointer trackerMatcher)
{
  if ( ! m_InitOK )
  {
    MITK_WARN << "Called project before initialise.";
    return;
  }

  if ( m_Capture == NULL )
  {
    m_VideoIn = niftk::FindVideoFile(m_Directory, niftk::Basename (niftk::Basename ( trackerMatcher->GetFrameMap() )));
    if ( m_VideoIn == "" )
    {
      m_InitOK = false;
      return;
    }
    try
    {
      m_Capture = mitk::InitialiseVideoCapture(m_VideoIn, (! m_HaltOnVideoReadFail ));
    }
    catch (std::exception& e)
    {
      MITK_ERROR << "Caught exception " << e.what();
      exit(1);
    }
    try
    {
      niftk::CreateDirAndParents ( m_OutDirectory );
    }
    catch (std::exception& e)
    {
      MITK_ERROR << "Caught exception " << e.what();
      exit(1);
    }
  }

  m_ProjectOK = false;

  int framenumber = 0 ;
  int key = 0;
  int annotationLineThickness = 1;
  cvNamedWindow ("Left Channel", CV_WINDOW_AUTOSIZE);
  cvNamedWindow ("Right Channel", CV_WINDOW_AUTOSIZE);

  cv::Mat blankMat = cvCreateMat(10,100,CV_32FC3);
  IplImage blankImage(blankMat);
  cvShowImage("Left Channel" , &blankImage);
  cvShowImage("Right Channel" , &blankImage);
  unsigned long long startTime;
  trackerMatcher->GetVideoFrame(0, &startTime);
  while ( framenumber < trackerMatcher->GetNumberOfFrames() && key != 'q')
  {
    if ( ( m_StartFrame < m_EndFrame ) && ( framenumber < m_StartFrame || framenumber > m_EndFrame ) )
    {
      cv::Mat videoImage;
      m_Capture->read(videoImage);
      MITK_INFO << "Skipping frame " << framenumber;
      framenumber ++;
    }
    else
    {
      long long timingError;
      cv::Mat WorldToLeftCamera = trackerMatcher->GetCameraTrackingMatrix(framenumber, &timingError, m_TrackerIndex, NULL, m_ReferenceIndex).inv();

      cv::Mat tempMat;

      cv::Mat leftVideoImage;
      cv::Mat rightVideoImage;
      m_Capture->read(tempMat);
      if ( m_FlipVideo )
      {
        int flipMode = 0 ; // flip around the x axis
        cv::flip(tempMat,leftVideoImage,flipMode);
      }
      else
      {
        leftVideoImage = tempMat.clone();
      }

      m_Capture->read(tempMat);
      if ( m_FlipVideo )
      {
        int flipMode = 0 ; // flip around the x axis
        cv::flip(tempMat,rightVideoImage,flipMode);
      }
      else
      {
        rightVideoImage = tempMat.clone();
      }

      if ( std::abs(timingError) <  m_AllowableTimingError )
      {
        key = cvWaitKey (20);

        if ( (framenumber-m_StartFrame) %m_Frequency == 0 )
        {
          unsigned long long timeStamp;
          trackerMatcher->GetVideoFrame(framenumber, &timeStamp);

          if (m_TimeStampsSet.size() == 0
              || (m_TimeStampsSet.size() != 0 && m_TimeStampsSet.find(timeStamp) != m_TimeStampsSet.end())
              )
          {
            MITK_INFO << "Picking contours on frame pair " << framenumber << ", " << framenumber+1 << " [ " << (timeStamp - startTime)/1e9 << " s ], c to make mask image, n for next frame, q to quit";

            std::string leftOutName = m_OutDirectory + niftk::GetFileSeparator() + boost::lexical_cast<std::string>(timeStamp);
            trackerMatcher->GetVideoFrame(framenumber+1, &timeStamp);
            std::string rightOutName = m_OutDirectory + niftk::GetFileSeparator() + boost::lexical_cast<std::string>(timeStamp);
            bool overWriteLeft = true;
            bool overWriteRight = true;
            key = 0;
            if ( boost::filesystem::exists (leftOutName + "_leftContour.xml") )
            {
              if ( m_AskOverWrite )
              {
                MITK_INFO << leftOutName + "_leftContour.xml" << " exists, overwrite (y/n)";
                key = 0;
                while ( ! ( key == 'n' || key == 'y' ) )
                {
                  key = cvWaitKey(20);
                }
                if ( key == 'n' )
                {
                  overWriteLeft = false;
                }
                else
                {
                  overWriteLeft = true;
                }
              }
              else
              {
                MITK_INFO << leftOutName + "_leftContour.xml" << " exists, skipping.";
                overWriteLeft = false;
              }
            }

            key = 0;
            if ( boost::filesystem::exists (rightOutName + "_rightContour.xml") )
            {
              if ( m_AskOverWrite )
              {
                MITK_INFO << rightOutName  + "_rightContour.xml" << " exists, overwrite (y/n)";
                while ( ! ( key == 'n' || key == 'y' ) )
                {
                  key = cv::waitKey(20);
                }
                if ( key == 'n' )
                {
                  overWriteRight = false;
                }
                else
                {
                  overWriteRight = true;
                }
              }
              else
              {
                MITK_INFO << rightOutName + "_rightContour.xml" << " exists, skipping.";
                overWriteRight = false;
              }
            }

            PickedPointList::Pointer leftPickedPoints = PickedPointList::New();
            PickedPointList::Pointer rightPickedPoints = PickedPointList::New();
            leftPickedPoints->SetFrameNumber (framenumber);
            leftPickedPoints->SetChannel ("left");
            leftPickedPoints->SetTimeStamp(timeStamp);
            leftPickedPoints->SetInLineMode (true);
            leftPickedPoints->SetInOrderedMode (false);
            rightPickedPoints->SetFrameNumber (framenumber + 1);
            rightPickedPoints->SetChannel ("right");
            rightPickedPoints->SetTimeStamp(timeStamp);
            rightPickedPoints->SetInLineMode (true);
            rightPickedPoints->SetInOrderedMode (false);

            cv::Mat leftAnnotatedVideoImage = leftVideoImage.clone();
            cv::Mat rightAnnotatedVideoImage = rightVideoImage.clone();
            cv::Mat leftMaskImage;
            cv::Mat rightMaskImage;
            bool showMasks = false;
            key = 0;
            if ( overWriteLeft  ||  overWriteRight  )
            {
              while ( key != 'n' && key != 'q' )
              {
                key = cv::waitKey(20);
                if ( key == 'c' )
                {
                  if ( showMasks )
                  {
                    MITK_INFO << "Exiting contour mode";
                  }
                  else
                  {
                    MITK_INFO << "Attempting to make contour images";
                    leftMaskImage = leftPickedPoints->CreateMaskImage ( leftAnnotatedVideoImage );
                    rightMaskImage = rightPickedPoints->CreateMaskImage ( rightAnnotatedVideoImage );
                    cv::imwrite(leftOutName + "_leftMask.png" ,leftMaskImage);
                    cv::imwrite(rightOutName + "_rightMask.png" ,rightMaskImage);
                    cv::imwrite(leftOutName + "_left.png" ,leftVideoImage);
                    cv::imwrite(rightOutName + "_right.png" ,rightVideoImage);
                  }
                  showMasks = ! showMasks;
                }
                if ( overWriteLeft )
                {
                  cvSetMouseCallback("Left Channel",PointPickingCallBackFunc, leftPickedPoints);
                  if ( leftPickedPoints->GetIsModified() )
                  {
                    leftAnnotatedVideoImage = leftVideoImage.clone();
                    leftPickedPoints->AnnotateImage(leftAnnotatedVideoImage, annotationLineThickness);

                    std::ofstream leftPointOut ((leftOutName+ "_leftContour.xml").c_str());
                    leftPickedPoints->PutOut( leftPointOut );
                    leftPointOut.close();
                  }
                  if ( showMasks )
                  {
                    IplImage image(leftMaskImage);
                    cvShowImage("Left Channel" , &image);
                  }
                  else
                  {
                    IplImage image(leftAnnotatedVideoImage);
                    cvShowImage("Left Channel" , &image);
                  }
                }

                if ( overWriteRight )
                {
                  cvSetMouseCallback("Right Channel",PointPickingCallBackFunc, rightPickedPoints);
                  if ( rightPickedPoints->GetIsModified() )
                  {
                    rightAnnotatedVideoImage = rightVideoImage.clone();
                    rightPickedPoints->AnnotateImage(rightAnnotatedVideoImage, annotationLineThickness);

                    std::ofstream rightPointOut ((rightOutName + "_rightContour.xml").c_str());
                    rightPickedPoints->PutOut (rightPointOut);
                    rightPointOut.close();
                  }
                  if ( showMasks )
                  {
                    IplImage rimage(rightMaskImage);
                    cvShowImage("Right Channel" , &rimage);
                  }
                  else
                  {
                    IplImage rimage(rightAnnotatedVideoImage);
                    cvShowImage("Right Channel" , &rimage);
                  }
                }
              }
            }
            cvShowImage("Left Channel" , &blankImage);
            cvShowImage("Right Channel" , &blankImage);

          } // end if timestamp was specified, and it matches.

        } // end if (sampling every 150 frames or so).
      }
      else
      {
        MITK_INFO << "Skipping frame " << framenumber << " high timing error " << timingError;
      }
      framenumber += 2;
    }
  }
  m_ProjectOK = true;
}

} // end namespace


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

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

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

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

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

// Original copyright notice:

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

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

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP

#include <string>
#include <vector>

#include <boost/geometry/formulas/eccentricity_sqr.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/pj_ellps.hpp>
#include <boost/geometry/srs/projections/impl/pj_param.hpp>
#include <boost/geometry/srs/projections/proj4.hpp>

namespace boost { namespace geometry { namespace projections {

namespace detail {

/* set ellipsoid parameters a and es */
template <typename T>
inline T SIXTH() { return .1666666666666666667; } /* 1/6 */
template <typename T>
inline T RA4() { return .04722222222222222222; } /* 17/360 */
template <typename T>
inline T RA6() { return .02215608465608465608; } /* 67/3024 */
template <typename T>
inline T RV4() { return .06944444444444444444; } /* 5/72 */
template <typename T>
inline T RV6() { return .04243827160493827160; } /* 55/1296 */

/* initialize geographic shape parameters */
template <typename BGParams, typename T>
inline void pj_ell_set(BGParams const& /*bg_params*/, std::vector<pvalue<T> >& parameters, T &a, T &es)
{
    T b = 0.0;
    T e = 0.0;
    std::string name;

    /* check for varying forms of ellipsoid input */
    a = es = 0.;

    /* R takes precedence */
    if (pj_param(parameters, "tR").i)
        a = pj_param(parameters, "dR").f;
    else { /* probable elliptical figure */

        /* check if ellps present and temporarily append its values to pl */
        name = pj_param(parameters, "sellps").s;
        if (! name.empty())
        {
            const int n = sizeof(pj_ellps) / sizeof(pj_ellps[0]);
            int index = -1;
            for (int i = 0; i < n && index == -1; i++)
            {
                if(pj_ellps[i].id == name)
                {
                    index = i;
                }
            }

            if (index == -1) {
                BOOST_THROW_EXCEPTION( projection_exception(-9) );
            }

            parameters.push_back(pj_mkparam<T>(pj_ellps[index].major));
            parameters.push_back(pj_mkparam<T>(pj_ellps[index].ell));
        }
        a = pj_param(parameters, "da").f;
        if (pj_param(parameters, "tes").i) /* eccentricity squared */
            es = pj_param(parameters, "des").f;
        else if (pj_param(parameters, "te").i) { /* eccentricity */
            e = pj_param(parameters, "de").f;
            es = e * e;
        } else if (pj_param(parameters, "trf").i) { /* recip flattening */
            es = pj_param(parameters, "drf").f;
            if (!es) {
                BOOST_THROW_EXCEPTION( projection_exception(-10) );
            }
            es = 1./ es;
            es = es * (2. - es);
        } else if (pj_param(parameters, "tf").i) { /* flattening */
            es = pj_param(parameters, "df").f;
            es = es * (2. - es);
        } else if (pj_param(parameters, "tb").i) { /* minor axis */
            b = pj_param(parameters, "db").f;
            es = 1. - (b * b) / (a * a);
        }     /* else es == 0. and sphere of radius a */
        if (!b)
            b = a * sqrt(1. - es);
        /* following options turn ellipsoid into equivalent sphere */
        if (pj_param(parameters, "bR_A").i) { /* sphere--area of ellipsoid */
            a *= 1. - es * (SIXTH<T>() + es * (RA4<T>() + es * RA6<T>()));
            es = 0.;
        } else if (pj_param(parameters, "bR_V").i) { /* sphere--vol. of ellipsoid */
            a *= 1. - es * (SIXTH<T>() + es * (RV4<T>() + es * RV6<T>()));
            es = 0.;
        } else if (pj_param(parameters, "bR_a").i) { /* sphere--arithmetic mean */
            a = .5 * (a + b);
            es = 0.;
        } else if (pj_param(parameters, "bR_g").i) { /* sphere--geometric mean */
            a = sqrt(a * b);
            es = 0.;
        } else if (pj_param(parameters, "bR_h").i) { /* sphere--harmonic mean */
            a = 2. * a * b / (a + b);
            es = 0.;
        } else {
            int i = pj_param(parameters, "tR_lat_a").i;
            if (i || /* sphere--arith. */
                pj_param(parameters, "tR_lat_g").i) { /* or geom. mean at latitude */
                T tmp;

                tmp = sin(pj_param(parameters, i ? "rR_lat_a" : "rR_lat_g").f);
                if (geometry::math::abs(tmp) > geometry::math::half_pi<T>()) {
                    BOOST_THROW_EXCEPTION( projection_exception(-11) );
                }
                tmp = 1. - es * tmp * tmp;
                a *= i ? .5 * (1. - es + tmp) / ( tmp * sqrt(tmp)) :
                    sqrt(1. - es) / tmp;
                es = 0.;
            }
        }
    }

    /* some remaining checks */
    if (es < 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(-12) );
    }
    if (a <= 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(-13) );
    }
}

template <BOOST_GEOMETRY_PROJECTIONS_DETAIL_TYPENAME_PX, typename T>
inline void pj_ell_set(srs::static_proj4<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX> const& bg_params,
                       std::vector<pvalue<T> >& /*parameters*/, T &a, T &es)
{
    typedef srs::static_proj4<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX> static_parameters_type;
    typedef typename srs::par4::detail::pick_ellps
        <
            static_parameters_type
        > pick_ellps;

    typename pick_ellps::model_type model = pick_ellps::model(bg_params);

    a = geometry::get_radius<0>(model);
    T b = geometry::get_radius<2>(model);
    es = 0.;
    if (a != b)
    {
        es = formula::eccentricity_sqr<T>(model);

        // Ignore all other parameters passed in string, at least for now
    }

    /* some remaining checks */
    if (es < 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(-12) );
    }
    if (a <= 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(-13) );
    }
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP


//==============================================================================
//         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_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_SSE2_CUMPROD_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_SSE2_CUMPROD_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/toolbox/swar/functions/cumprod.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/functions/bitwise_or.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is type8_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::cumprod_, boost::simd::tag::sse2_,
                         (A0),
                         ((simd_<type8_<A0>,boost::simd::tag::sse_>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename dispatch::meta::as_integer<A0>::type sint;
      A0 a = a0;
      sint o = simd::bitwise_cast<sint>(One<A0>());
      sint tmp = simd::bitwise_cast<sint>(a0);
      sint tmp1 = _mm_or_si128(_mm_slli_si128(tmp,1), _mm_srli_si128(o,15));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,2), _mm_srli_si128(o,14));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,3), _mm_srli_si128(o,13));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,4), _mm_srli_si128(o,12));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,5), _mm_srli_si128(o,11));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,6), _mm_srli_si128(o,10));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,7), _mm_srli_si128(o,9));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,8), _mm_srli_si128(o,8));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,9), _mm_srli_si128(o,7));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,10), _mm_srli_si128(o,6));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,11), _mm_srli_si128(o,5));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,12), _mm_srli_si128(o,4));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,13), _mm_srli_si128(o,3));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,14), _mm_srli_si128(o,2));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,15), _mm_srli_si128(o,1));
      return a*simd::bitwise_cast<A0>(tmp1);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is type64_
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::cumprod_, boost::simd::tag::sse2_,
                         (A0),
                         ((simd_<type64_<A0>,boost::simd::tag::sse_>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename dispatch::meta::as_integer<A0>::type sint;
      sint o = simd::bitwise_cast<sint>(One<A0>());
      sint tmp = simd::bitwise_cast<sint>(a0);
      sint tmp1 = _mm_or_si128(_mm_slli_si128(tmp,8), _mm_srli_si128(o,8));
      return a0*simd::bitwise_cast<A0>(tmp1);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is type16_
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::cumprod_, boost::simd::tag::sse2_,
                         (A0),
                         ((simd_<type16_<A0>,boost::simd::tag::sse_>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename dispatch::meta::as_integer<A0>::type sint;
      A0 a = a0;
      sint o = simd::bitwise_cast<sint>(One<A0>());
      sint tmp = simd::bitwise_cast<sint>(a0);
      sint tmp1 = _mm_or_si128(_mm_slli_si128(tmp,2), _mm_srli_si128(o,14));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,4), _mm_srli_si128(o,12));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp, 6), _mm_srli_si128(o,10));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,8), _mm_srli_si128(o,8));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,10), _mm_srli_si128(o,6));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,12), _mm_srli_si128(o,4));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp,14), _mm_srli_si128(o,2));
      return a*simd::bitwise_cast<A0>(tmp1);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is type32_
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::cumprod_, boost::simd::tag::sse2_,
                         (A0),
                         ((simd_<type32_<A0>,boost::simd::tag::sse_>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename dispatch::meta::as_integer<A0>::type sint;
      A0 a = a0;
      sint o = simd::bitwise_cast<sint>(One<A0>());
      sint tmp = simd::bitwise_cast<sint>(a0);
      sint tmp1 = _mm_or_si128(_mm_slli_si128(tmp, 4), _mm_srli_si128(o,12));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp, 8), _mm_srli_si128(o,8));
      a = a*simd::bitwise_cast<A0>(tmp1);
      tmp1 = _mm_or_si128(_mm_slli_si128(tmp, 12), _mm_srli_si128(o,4));
      return a*simd::bitwise_cast<A0>(tmp1);
    }
  };

} } }
#endif
#endif


// -*- C++ -*-
//
// Copyright Sylvain Bougerel 2009 - 2012.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file COPYING or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_TEST_DYN_LINK
#define SPATIAL_ENABLE_ASSERT // detect interal issues that should not occur

#include <boost/test/unit_test.hpp>
#include <utility> // std::make_pair
#include "../../src/point_multimap.hpp"
#include "spatial_test_types.hpp"

using namespace spatial;

BOOST_AUTO_TEST_CASE( test_point_map_constructors )
{
  point_multimap<2, int2, int> points;
  point_multimap<0, int2, int> runtime_points;
}

BOOST_AUTO_TEST_CASE( test_point_map_copy_assignment )
{
  point_multimap<2, int2, int> points;
  points.insert(std::make_pair(zeros, 0));
  points.insert(std::make_pair(ones, 1));
  points.insert(std::make_pair(twos, 2));
  point_multimap<2, int2, int> copy(points);
  BOOST_CHECK_EQUAL(points.size(), copy.size());
  BOOST_CHECK(*points.begin() == *copy.begin());
  points = copy;
  BOOST_CHECK_EQUAL(points.size(), copy.size());
  BOOST_CHECK(*points.begin() == *copy.begin());
}

BOOST_AUTO_TEST_CASE( test_zero_point_map_copy_assignment )
{
  point_multimap<0, int2, int> points;
  points.insert(std::make_pair(zeros, 0));
  points.insert(std::make_pair(ones, 1));
  points.insert(std::make_pair(twos, 2));
  point_multimap<0, int2, int> copy(points);
  BOOST_CHECK_EQUAL(points.size(), copy.size());
  BOOST_CHECK(*points.begin() == *copy.begin());
  points = copy;
  BOOST_CHECK_EQUAL(points.size(), copy.size());
  BOOST_CHECK(*points.begin() == *copy.begin());
}


/*
Copyright 2017 InitialDLab

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.
*/
/*
 * Rtree Experiements
 *
 * by Lu Wang 2014
 */

#include <iostream>
#include <vector>
#include <string>
#include <memory>
#include <cstdlib>

#include <boost/geometry/geometries/box.hpp>
#include <boost/timer/timer.hpp>
#include <boost/lexical_cast.hpp>

#include "rtree/rtree.h"
#include "rtree/tests/integrity_checker.h"

#include "level_sampling/level_sampling.h"

#include "mongo_types.h"
#include "hilbert/hilbert.h"

//#define USE_OSM

#ifdef USE_OSM
// osm
std::string const DATASET = "osm";
int const LEVEL_SAMPLING_MIN_MEM_LEVEL = 12;
#else
// geo
std::string const DATASET = "geo";
int const LEVEL_SAMPLING_MIN_MEM_LEVEL = 4;
#endif

/*
 * cache system disk cache
 */
const char * clear_cache_cmd = "/uusoc/scratch/magpie17/robertc/clearCache > /dev/null";
void clear_cache()
{
    auto unused = system(clear_cache_cmd);
    // TODO clear block manager cache when implemented
}

namespace bg = boost::geometry;
namespace bt = boost::timer;

struct Exp
{
    // how many sample queries to perform
    using entry = mongo_types::entry;
    using sample_entry = mongo_types::sample_entry;
    using point = mongo_types::point3d;
    using box = mongo_types::box3d;

    using rtree_t = rtree::rtree<
        entry,
        sample_entry,
        box,
        256, // node sample count
        16  // max fanout
    >;

    using ls_t = level_sampling::level_sampling<
        entry,
        sample_entry,
        box,
        16 // max fanout
    >;

    Exp(std::string const& rtree_name, std::string const& ls_name)
        : rtree_name(rtree_name)
        , ls_name(ls_name)
    { }

    void build_rtree (bool load_mem_nodes = false) {
        bt::cpu_timer timer;
        rtree_ptr.reset(new rtree_t(rtree_name, false, load_mem_nodes));
        std::cout << "tree size: " << rtree_ptr->size() << std::endl;

        auto stats = rtree_ptr->get_stats();
        std::cout << "leaf node count: " << stats.leaf_nodes << std::endl;
        std::cout << "internal node count: " << stats.internal_nodes << std::endl;
        std::cout << "tree building time(ms): " << ' ' << timer.elapsed().wall/1000000.0 << std::endl;

        std::cout << "bbox: ";
        dump_box(std::cout, rtree_ptr->bbox());
        std::cout << std::endl;
    }

    void build_ls (void) {
        bt::cpu_timer timer;
        ls_ptr.reset(new ls_t(ls_name, LEVEL_SAMPLING_MIN_MEM_LEVEL));
        std::cout << "min mem level: " << ls_ptr->min_mem_level << std::endl;
        std::cout << "max level: " << ls_ptr->max_level << std::endl;
    }

    void run() {
        build_rtree(true);
        auto stats = rtree_ptr->count_nodes();
        std::cerr << "internal nodes: " << stats.internal_nodes << std::endl;
        std::cerr << "leaf nodes: " << stats.leaf_nodes << std::endl;
        std::cerr << "io internal nodes: " << stats.io_internal_nodes << std::endl;
        std::cerr << "io leaf nodes: " << stats.io_leaf_nodes << std::endl;
    }

    std::unique_ptr<rtree_t> rtree_ptr;
    std::unique_ptr<ls_t> ls_ptr;
    std::string rtree_name, ls_name;

    std::vector<box> queries;
};

int main(int argc, char ** argv)
{
    //std::string filename = argv[1];
    std::string path = "/uusoc/scratch/magpie21/luwang/data/built/" + DATASET;

    //Exp(filename+"-rtree", filename+"-ls").run();
    Exp(path + "_tree", path + "_level").run();

    return 0;
}


#ifndef VISION_HPP_
#define VISION_HPP_
//dimensões dos quadrados do tabuleiro
#define CALIBRATION_SQUARE_DIMENSION 0.02629f
// número de intersecções do tabuleiro
#define CHESSBOARD_DIMENSION cv::Size(6,9)

#include "Geometry/Geometry.h"
#include "opencv2/opencv.hpp"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/video.hpp>
#include <opencv2/video/tracking.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/bind.hpp>
#include <iostream>
#include "tag.hpp"

namespace vision
{
	// Todas as cores que serão encontradas pela visão:
	enum Color
	{
		Main,
		Green,
		Ball,
		Adv
	};

	// Limites Min e Max dos tresholds:
	enum Limit
	{
		Min,
		Max
	};

	static const int MAX_ADV = 3;

	class Vision {
	public:

		static const unsigned int MAX_COLORS = Color::Adv - Color::Main + 1;
//		Numero da tag é definido pela sua posicao no std::array retornado
		struct RecognizedTag {
			cv::Point position = {ROBOT_RADIUS/2, 0};
			double orientation = 0;
			cv::Point front_point = {ROBOT_RADIUS, 0};
			cv::Point rear_point = {0, 0};
		};

		struct Ball {
			cv::Point position = {0, 0};
			bool isFound = false;
		};

	private:

		// image size
		int width;
		int height;

		// Frames
		cv::Mat in_frame, lab_frame;
		std::vector<cv::Mat> threshold_frame;
		cv::Mat splitFrame;

		// Robots
		std::vector<cv::Point> advRobots;

		// Ball
		Ball ball;

		// TAGS
		std::vector<std::vector<Tag>> tags;

		// HSV/CIELAB Calibration Parameters
		int cieL[MAX_COLORS][2];
		int cieA[MAX_COLORS][2];
		int cieB[MAX_COLORS][2];
		int dilate[MAX_COLORS];
		int erode[MAX_COLORS];
		int blur[MAX_COLORS];
		int areaMin[MAX_COLORS];

		// threads
		boost::thread_group threshold_threads;

		void preProcessing();
		void posProcessing(unsigned long color);
		void segmentAndSearch(unsigned long color);
		void searchTags(unsigned long color);
		void findTags();
		std::map<unsigned int, RecognizedTag> pick_a_tag();
		int in_sphere(cv::Point secondary, Tag &main_tag, double &orientation);

	public:

		Vision(int w, int h);
		~Vision();

		std::map<unsigned int, RecognizedTag> run(cv::Mat raw_frame);
		double calcDistance(cv::Point p1, cv::Point p2) const;
		void saveCameraCalibPicture(std::string in_name, std::string directory);

		//Cam calib
		std::vector<cv::Mat> savedCamCalibFrames;
		cv::Mat cameraMatrix;
		cv::Mat distanceCoeficents;
		bool flag_cam_calibrated = false;
		void getChessBoardCorners(std::vector<cv::Mat> images, std::vector<std::vector<cv::Point3f>>& pts3d,std::vector<std::vector<cv::Point2f>>& pts2d) const;
		std::vector<cv::Mat> getCamCalibFrames() const { return savedCamCalibFrames; };
		cv::Mat getcameraMatrix() const { return cameraMatrix; };
		cv::Mat getdistanceCoeficents() const { return distanceCoeficents; };
		void setFlagCamCalibrated(bool value);
		void saveCamCalibFrame();
		void popCamCalibFrames();
		void collectImagesForCalibration();
		void cameraCalibration();
		std::vector<cv::Point3f> createKnownBoardPosition(cv::Size boardSize, float squareEdgeLenght);
		bool foundChessBoardCorners() const;
		void switchMainWithAdv();

		Ball& getBall() { return ball; };
		std::vector<cv::Point>& get_adv_robots() { return advRobots; };
		cv::Mat getSplitFrame();
		cv::Mat getThreshold(unsigned long index);

		int getCIE_L(unsigned long index0, int index1) const { return cieL[index0][index1]; };
		int getCIE_A(unsigned long index0, int index1) const { return cieA[index0][index1]; };
		int getCIE_B(unsigned long index0, int index1) const { return cieB[index0][index1]; };
		int getErode(unsigned long index) const { return erode[index]; };
		int getDilate(unsigned long index) const { return dilate[index]; };
		int getBlur(unsigned long index) const { return blur[index]; };
		int getAmin(unsigned long index) const { return areaMin[index]; };

		void setFrameSize(int inWidth, int inHeight);

		void setCIE_L(unsigned long index0, int index1, int inValue);
		void setCIE_A(unsigned long index0, int index1, int inValue);
		void setCIE_B(unsigned long index0, int index1, int inValue);
		void setErode(unsigned long index, int inValue);
		void setDilate(unsigned long index, int inValue);
		void setBlur(unsigned long index, int inValue);
		void setAmin(unsigned long index, int inValue);
	};
}

#endif /* VISION_HPP_ */


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

  @copyright 2016 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_META_CARDINAL_OF_HPP_INCLUDED
#define BOOST_SIMD_META_CARDINAL_OF_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/core/ignore_unused.hpp>
#include <boost/config.hpp>
#include <boost/simd/detail/nsm.hpp>

namespace boost { namespace simd
{
  namespace tt = nsm::type_traits;
  /*!
    @ingroup group-api

    Computes the register width required to store an instance of any type @c T.

    @par Models:

    @intconst

    @par Semantic:

    For any given cv-qualified type @c T,

    @code
    using r = cardinal_of<T>::type;
    @endcode

    is equivalent to:

    @code
    using r = sd::integral_constant<std::size_t,sizeof(T)/sizeof(boost::dispatch::scalar_of_t<T>)>;
    @endcode

    @tparam T Type which register size is to be computed.

    @see boost::simd::cardinal
  **/
  template<typename T>
  struct  cardinal_of
#if !defined(DOXYGEN_ONLY)
        : tt::integral_constant<std::size_t,1ULL>
#endif
  {};

#if !defined(DOXYGEN_ONLY)
  template<typename T>  struct cardinal_of<T&>                : cardinal_of<T> {};
  template<typename T>  struct cardinal_of<T&&>               : cardinal_of<T> {};
  template<typename T>  struct cardinal_of<T const>           : cardinal_of<T> {};
  template<typename T>  struct cardinal_of<volatile T>        : cardinal_of<T> {};
  template<typename T>  struct cardinal_of<volatile T const>  : cardinal_of<T> {};
#endif

  /*!
    @ingroup group-api
    Eager short-cut for cardinal_of
  **/
  template<typename T>  using cardinal_of_t = typename cardinal_of<T>::type;

  /*!
    @ingroup group-api

    Computes the register size of any given value v.

    @par Semantic:

    For any instance @c v of type @c T,

    @code
    auto r = cardinal(v)
    @endcode

    is equivalent to:

    @code
    auto r = cardinal_of<T>::type{};
    @endcode

    @param v Value which register size is to be computed
    @return An @intconst containing v register size.

    @see boost::simd::cardinal_of
  **/
  template<typename T> BOOST_FORCEINLINE cardinal_of_t<T> cardinal(T&& v) BOOST_NOEXCEPT
  {
    boost::ignore_unused(v);
    return {};
  }
} }

#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_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_INBTRUE_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_INBTRUE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/reduction/functions/inbtrue.hpp>
#include <boost/simd/include/functions/simd/genmask.hpp>
#include <boost/simd/include/functions/simd/popcnt.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  //============================================================================
  // Implementation when type A0 is arithmetic_
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::inbtrue_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<arithmetic_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef std::size_t                                                 result_type;
    typedef typename meta::make_dependent<boost::simd::int8_t,A0>::type base;
    typedef simd::native<base,boost::simd::tag::sse_>                   i8type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      i8type tmp = bitwise_cast<i8type>(genmask(a0));
      return  boost::simd::popcnt(_mm_movemask_epi8(tmp))
            * boost::simd::meta::cardinal_of<A0>::value >> 4;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::inbtrue_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<logical_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef std::size_t                                                 result_type;
    typedef typename meta::make_dependent<boost::simd::int8_t,A0>::type base;
    typedef simd::native<base,boost::simd::tag::sse_>                   i8type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      i8type tmp = bitwise_cast<i8type>(genmask(a0));
      return  boost::simd::popcnt(_mm_movemask_epi8(tmp))
            * boost::simd::meta::cardinal_of<A0>::value >> 4;
    }
  };

  //============================================================================
  // Implementation when type A0 is double
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::inbtrue_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef std::size_t result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      int32_t  r = _mm_movemask_pd(genmask(a0));
      return   (r&1)+(r>>1);
    }
  };

  //============================================================================
  // Implementation when type A0 is float
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::inbtrue_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef std::size_t                                     result_type;
    typedef typename meta::make_dependent<int32_t,A0>::type type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      type r = _mm_movemask_ps(genmask(a0));
      return  (r&1)+((r>>1)&1)+((r>>2)&1)+(r>>3);
    }
  };
} } }

#endif
#endif


#define BOOST_TEST_MODULE BlindingTest
#include <boost/test/unit_test.hpp>
#include <fc/array.hpp>
#include <fc/crypto/base58.hpp>
#include <fc/crypto/hex.hpp>
#include <fc/crypto/elliptic.hpp>
#include <fc/crypto/openssl.hpp>
#include <fc/exception/exception.hpp>

// See https://github.com/bitcoin/bips/blob/master/bip-0032.mediawiki#Test_Vectors

static fc::string TEST1_SEED = "000102030405060708090a0b0c0d0e0f";
static fc::string TEST1_M_PUB = "xpub661MyMwAqRbcFtXgS5sYJABqqG9YLmC4Q1Rdap9gSE8NqtwybGhePY2gZ29ESFjqJoCu1Rupje8YtGqsefD265TMg7usUDFdp6W1EGMcet8";
static fc::string TEST1_M_PRIV = "xprv9s21ZrQH143K3QTDL4LXw2F7HEK3wJUD2nW2nRk4stbPy6cq3jPPqjiChkVvvNKmPGJxWUtg6LnF5kejMRNNU3TGtRBeJgk33yuGBxrMPHi";
static fc::string TEST1_M_0H_PUB = "xpub68Gmy5EdvgibQVfPdqkBBCHxA5htiqg55crXYuXoQRKfDBFA1WEjWgP6LHhwBZeNK1VTsfTFUHCdrfp1bgwQ9xv5ski8PX9rL2dZXvgGDnw";
static fc::string TEST1_M_0H_PRIV = "xprv9uHRZZhk6KAJC1avXpDAp4MDc3sQKNxDiPvvkX8Br5ngLNv1TxvUxt4cV1rGL5hj6KCesnDYUhd7oWgT11eZG7XnxHrnYeSvkzY7d2bhkJ7";
static fc::string TEST1_M_0H_1_PUB = "xpub6ASuArnXKPbfEwhqN6e3mwBcDTgzisQN1wXN9BJcM47sSikHjJf3UFHKkNAWbWMiGj7Wf5uMash7SyYq527Hqck2AxYysAA7xmALppuCkwQ";
static fc::string TEST1_M_0H_1_PRIV = "xprv9wTYmMFdV23N2TdNG573QoEsfRrWKQgWeibmLntzniatZvR9BmLnvSxqu53Kw1UmYPxLgboyZQaXwTCg8MSY3H2EU4pWcQDnRnrVA1xe8fs";
static fc::string TEST1_M_0H_1_2H_PUB = "xpub6D4BDPcP2GT577Vvch3R8wDkScZWzQzMMUm3PWbmWvVJrZwQY4VUNgqFJPMM3No2dFDFGTsxxpG5uJh7n7epu4trkrX7x7DogT5Uv6fcLW5";
static fc::string TEST1_M_0H_1_2H_PRIV = "xprv9z4pot5VBttmtdRTWfWQmoH1taj2axGVzFqSb8C9xaxKymcFzXBDptWmT7FwuEzG3ryjH4ktypQSAewRiNMjANTtpgP4mLTj34bhnZX7UiM";
static fc::string TEST1_M_0H_1_2H_2_PUB = "xpub6FHa3pjLCk84BayeJxFW2SP4XRrFd1JYnxeLeU8EqN3vDfZmbqBqaGJAyiLjTAwm6ZLRQUMv1ZACTj37sR62cfN7fe5JnJ7dh8zL4fiyLHV";
static fc::string TEST1_M_0H_1_2H_2_PRIV = "xprvA2JDeKCSNNZky6uBCviVfJSKyQ1mDYahRjijr5idH2WwLsEd4Hsb2Tyh8RfQMuPh7f7RtyzTtdrbdqqsunu5Mm3wDvUAKRHSC34sJ7in334";
static fc::string TEST1_M_0H_1_2H_2_1g_PUB = "xpub6H1LXWLaKsWFhvm6RVpEL9P4KfRZSW7abD2ttkWP3SSQvnyA8FSVqNTEcYFgJS2UaFcxupHiYkro49S8yGasTvXEYBVPamhGW6cFJodrTHy";
static fc::string TEST1_M_0H_1_2H_2_1g_PRIV = "xprvA41z7zogVVwxVSgdKUHDy1SKmdb533PjDz7J6N6mV6uS3ze1ai8FHa8kmHScGpWmj4WggLyQjgPie1rFSruoUihUZREPSL39UNdE3BBDu76";

static fc::string TEST2_SEED = "fffcf9f6f3f0edeae7e4e1dedbd8d5d2cfccc9c6c3c0bdbab7b4b1aeaba8a5a29f9c999693908d8a8784817e7b7875726f6c696663605d5a5754514e4b484542";
static fc::string TEST2_M_PUB = "xpub661MyMwAqRbcFW31YEwpkMuc5THy2PSt5bDMsktWQcFF8syAmRUapSCGu8ED9W6oDMSgv6Zz8idoc4a6mr8BDzTJY47LJhkJ8UB7WEGuduB";
static fc::string TEST2_M_PRIV = "xprv9s21ZrQH143K31xYSDQpPDxsXRTUcvj2iNHm5NUtrGiGG5e2DtALGdso3pGz6ssrdK4PFmM8NSpSBHNqPqm55Qn3LqFtT2emdEXVYsCzC2U";
static fc::string TEST2_M_0_PUB = "xpub69H7F5d8KSRgmmdJg2KhpAK8SR3DjMwAdkxj3ZuxV27CprR9LgpeyGmXUbC6wb7ERfvrnKZjXoUmmDznezpbZb7ap6r1D3tgFxHmwMkQTPH";
static fc::string TEST2_M_0_PRIV = "xprv9vHkqa6EV4sPZHYqZznhT2NPtPCjKuDKGY38FBWLvgaDx45zo9WQRUT3dKYnjwih2yJD9mkrocEZXo1ex8G81dwSM1fwqWpWkeS3v86pgKt";
static fc::string TEST2_M_0_m1_PUB = "xpub6ASAVgeehLbnwdqV6UKMHVzgqAG8Gr6riv3Fxxpj8ksbH9ebxaEyBLZ85ySDhKiLDBrQSARLq1uNRts8RuJiHjaDMBU4Zn9h8LZNnBC5y4a";
static fc::string TEST2_M_0_m1_PRIV = "xprv9wSp6B7kry3Vj9m1zSnLvN3xH8RdsPP1Mh7fAaR7aRLcQMKTR2vidYEeEg2mUCTAwCd6vnxVrcjfy2kRgVsFawNzmjuHc2YmYRmagcEPdU9";
static fc::string TEST2_M_0_m1_1_PUB = "xpub6DF8uhdarytz3FWdA8TvFSvvAh8dP3283MY7p2V4SeE2wyWmG5mg5EwVvmdMVCQcoNJxGoWaU9DCWh89LojfZ537wTfunKau47EL2dhHKon";
static fc::string TEST2_M_0_m1_1_PRIV = "xprv9zFnWC6h2cLgpmSA46vutJzBcfJ8yaJGg8cX1e5StJh45BBciYTRXSd25UEPVuesF9yog62tGAQtHjXajPPdbRCHuWS6T8XA2ECKADdw4Ef";
static fc::string TEST2_M_0_m1_1_m2_PUB = "xpub6ERApfZwUNrhLCkDtcHTcxd75RbzS1ed54G1LkBUHQVHQKqhMkhgbmJbZRkrgZw4koxb5JaHWkY4ALHY2grBGRjaDMzQLcgJvLJuZZvRcEL";
static fc::string TEST2_M_0_m1_1_m2_PRIV = "xprvA1RpRA33e1JQ7ifknakTFpgNXPmW2YvmhqLQYMmrj4xJXXWYpDPS3xz7iAxn8L39njGVyuoseXzU6rcxFLJ8HFsTjSyQbLYnMpCqE2VbFWc";
static fc::string TEST2_M_0_m1_1_m2_2_PUB = "xpub6FnCn6nSzZAw5Tw7cgR9bi15UV96gLZhjDstkXXxvCLsUXBGXPdSnLFbdpq8p9HmGsApME5hQTZ3emM2rnY5agb9rXpVGyy3bdW6EEgAtqt";
static fc::string TEST2_M_0_m1_1_m2_2_PRIV = "xprvA2nrNbFZABcdryreWet9Ea4LvTJcGsqrMzxHx98MMrotbir7yrKCEXw7nadnHM8Dq38EGfSh6dqA9QWTyefMLEcBYJUuekgW4BYPJcr9E7j";

static fc::string BLIND_K_X = "08be5c5076f8cbd1283cf98e74bff873032b9bc79a0769962bf3900f33c2df6e";
static fc::string BLIND_T_X = "80deff382af8a8e4a5f297588e44d5bf858f30a524f74b13efcefeb54f4b3f47";
static fc::string BLINDED_HASH = "7196e80cdafdfdfb7496323ad24bf47dda8447febd7426e444facc04940c7309";
static fc::string BLIND_SIG = "40d6a477d849cc860df8ad159481f2ffc5b4dc3131b86a799d7d10460824dd53";
static fc::string UNBLINDED = "700092a72a05e33509f9b068aa1d7c5336d8b5692b4157da199d7ec1e10fd7c0";

BOOST_AUTO_TEST_CASE(test_extended_keys_1)
{
    char seed[16];
    fc::from_hex( TEST1_SEED, seed, sizeof(seed) );
    fc::ecc::extended_private_key master = fc::ecc::extended_private_key::generate_master( seed, sizeof(seed) );
    BOOST_CHECK_EQUAL( master.str(), TEST1_M_PRIV );
    BOOST_CHECK_EQUAL( master.get_extended_public_key().str(), TEST1_M_PUB );

    BOOST_CHECK_EQUAL( fc::ecc::extended_private_key::from_base58(TEST1_M_PRIV).str(), TEST1_M_PRIV );
    BOOST_CHECK_EQUAL( fc::ecc::extended_public_key::from_base58(TEST1_M_PUB).str(), TEST1_M_PUB );
    BOOST_CHECK_EQUAL( fc::ecc::extended_private_key::from_base58(TEST1_M_0H_PRIV).str(), TEST1_M_0H_PRIV );
    BOOST_CHECK_EQUAL( fc::ecc::extended_public_key::from_base58(TEST1_M_0H_PUB).str(), TEST1_M_0H_PUB );

    fc::ecc::extended_private_key m_0 = master.derive_child(0x80000000);
    BOOST_CHECK_EQUAL( m_0.str(), TEST1_M_0H_PRIV );
    BOOST_CHECK_EQUAL( m_0.get_extended_public_key().str(), TEST1_M_0H_PUB );

    fc::ecc::extended_private_key m_0_1 = m_0.derive_child(1);
    BOOST_CHECK_EQUAL( m_0_1.str(), TEST1_M_0H_1_PRIV );
    BOOST_CHECK_EQUAL( m_0_1.get_extended_public_key().str(), TEST1_M_0H_1_PUB );
    BOOST_CHECK_EQUAL( m_0.get_extended_public_key().derive_child(1).str(), TEST1_M_0H_1_PUB );

    fc::ecc::extended_private_key m_0_1_2 = m_0_1.derive_child(0x80000002);
    BOOST_CHECK_EQUAL( m_0_1_2.str(), TEST1_M_0H_1_2H_PRIV );
    BOOST_CHECK_EQUAL( m_0_1_2.get_extended_public_key().str(), TEST1_M_0H_1_2H_PUB );

    fc::ecc::extended_private_key m_0_1_2_2 = m_0_1_2.derive_child(2);
    BOOST_CHECK_EQUAL( m_0_1_2_2.str(), TEST1_M_0H_1_2H_2_PRIV );
    BOOST_CHECK_EQUAL( m_0_1_2_2.get_extended_public_key().str(), TEST1_M_0H_1_2H_2_PUB );
    BOOST_CHECK_EQUAL( m_0_1_2.get_extended_public_key().derive_child(2).str(), TEST1_M_0H_1_2H_2_PUB );

    fc::ecc::extended_private_key m_0_1_2_2_1g = m_0_1_2_2.derive_child(1000000000);
    BOOST_CHECK_EQUAL( m_0_1_2_2_1g.str(), TEST1_M_0H_1_2H_2_1g_PRIV );
    BOOST_CHECK_EQUAL( m_0_1_2_2_1g.get_extended_public_key().str(), TEST1_M_0H_1_2H_2_1g_PUB );
    BOOST_CHECK_EQUAL( m_0_1_2_2.get_extended_public_key().derive_child(1000000000).str(), TEST1_M_0H_1_2H_2_1g_PUB );
}

BOOST_AUTO_TEST_CASE(test_extended_keys_2)
{
    char seed[64];
    fc::from_hex( TEST2_SEED, seed, sizeof(seed) );
    fc::ecc::extended_private_key master = fc::ecc::extended_private_key::generate_master( seed, sizeof(seed) );
    BOOST_CHECK_EQUAL( master.str(), TEST2_M_PRIV );
    BOOST_CHECK_EQUAL( master.get_extended_public_key().str(), TEST2_M_PUB );

    fc::ecc::extended_private_key m_0 = master.derive_child(0);
    BOOST_CHECK_EQUAL( m_0.str(), TEST2_M_0_PRIV );
    BOOST_CHECK_EQUAL( m_0.get_extended_public_key().str(), TEST2_M_0_PUB );
    BOOST_CHECK_EQUAL( master.get_extended_public_key().derive_child(0).str(), TEST2_M_0_PUB );

    fc::ecc::extended_private_key m_0_m1 = m_0.derive_child(-1);
    BOOST_CHECK_EQUAL( m_0_m1.str(), TEST2_M_0_m1_PRIV );
    BOOST_CHECK_EQUAL( m_0_m1.get_extended_public_key().str(), TEST2_M_0_m1_PUB );

    fc::ecc::extended_private_key m_0_m1_1 = m_0_m1.derive_child(1);
    BOOST_CHECK_EQUAL( m_0_m1_1.str(), TEST2_M_0_m1_1_PRIV );
    BOOST_CHECK_EQUAL( m_0_m1_1.get_extended_public_key().str(), TEST2_M_0_m1_1_PUB );
    BOOST_CHECK_EQUAL( m_0_m1.get_extended_public_key().derive_child(1).str(), TEST2_M_0_m1_1_PUB );

    fc::ecc::extended_private_key m_0_m1_1_m2 = m_0_m1_1.derive_child(-2);
    BOOST_CHECK_EQUAL( m_0_m1_1_m2.str(), TEST2_M_0_m1_1_m2_PRIV );
    BOOST_CHECK_EQUAL( m_0_m1_1_m2.get_extended_public_key().str(), TEST2_M_0_m1_1_m2_PUB );

    fc::ecc::extended_private_key m_0_m1_1_m2_2 = m_0_m1_1_m2.derive_child(2);
    BOOST_CHECK_EQUAL( m_0_m1_1_m2_2.str(), TEST2_M_0_m1_1_m2_2_PRIV );
    BOOST_CHECK_EQUAL( m_0_m1_1_m2_2.get_extended_public_key().str(), TEST2_M_0_m1_1_m2_2_PUB );
    BOOST_CHECK_EQUAL( m_0_m1_1_m2.get_extended_public_key().derive_child(2).str(), TEST2_M_0_m1_1_m2_2_PUB );
}

//static void print(const unsigned char* data, int len) {
//    for (int i = 0; i < len; i++) {
//        printf("%02x", *data++);
//    }
//}

BOOST_AUTO_TEST_CASE(test_blinding_1)
{
    char buffer[7] = "test_";
    fc::ecc::extended_private_key alice = fc::ecc::extended_private_key::generate_master( "master" );
    fc::ecc::extended_private_key bob = fc::ecc::extended_private_key::generate_master( "puppet" );

    for ( int i = 0; i < 30; i++ )
    {
        buffer[5] = '0' + i;
        fc::ecc::extended_public_key bob_pub = bob.get_extended_public_key();
        fc::sha256 hash = fc::sha256::hash( buffer, sizeof(buffer) );
        fc::ecc::public_key t = alice.blind_public_key( bob_pub, i );
        fc::ecc::blinded_hash blinded = alice.blind_hash( hash, i );
        fc::ecc::blind_signature blind_sig = bob.blind_sign( blinded, i );
        try {
            fc::ecc::compact_signature sig = alice.unblind_signature( bob_pub, blind_sig, hash, i );
            fc::ecc::public_key validate( sig, hash );
//            printf("Validated: "); print((unsigned char*) validate.serialize().begin(), 33);
//            printf("\nT: "); print((unsigned char*) t.serialize().begin(), 33); printf("\n");
            BOOST_CHECK( validate.serialize() == t.serialize() );
        } catch (const fc::exception& e) {
            printf( "Test %d: %s\n", i, e.to_string().c_str() );
        }
        alice = alice.derive_child( i );
        bob = bob.derive_child( i | 0x80000000 );
    }
}

BOOST_AUTO_TEST_CASE(test_blinding_2)
{
    char message[7] = "test_0";
    fc::ecc::extended_private_key alice = fc::ecc::extended_private_key::generate_master( "master" );
    fc::ecc::extended_private_key bob = fc::ecc::extended_private_key::generate_master( "puppet" );
    fc::ecc::extended_public_key bob_pub = bob.get_extended_public_key();
    fc::sha256 hash = fc::sha256::hash( message, sizeof(message) );

    fc::ecc::public_key t = alice.blind_public_key( bob_pub, 0 );
    fc::ecc::public_key_data pub = t.serialize();
    char buffer[32];
    fc::from_hex( BLIND_T_X, buffer, sizeof(buffer) );
    BOOST_CHECK( !memcmp( pub.begin() + 1, buffer, sizeof(buffer) ) );

    fc::ecc::blinded_hash blinded = alice.blind_hash( hash, 0 );
    fc::from_hex( BLINDED_HASH, buffer, sizeof(buffer) );
    BOOST_CHECK( !memcmp( blinded.data(), buffer, sizeof(buffer) ) );

    fc::ecc::blind_signature blind_sig = bob.blind_sign( blinded, 0 );
    fc::from_hex( BLIND_SIG, buffer, sizeof(buffer) );
    BOOST_CHECK( !memcmp( blind_sig.data(), buffer, sizeof(buffer) ) );

    fc::ecc::compact_signature sig = alice.unblind_signature( bob_pub, blind_sig, hash, 0 );
    fc::from_hex( BLIND_K_X, buffer, sizeof(buffer) );
    BOOST_CHECK( !memcmp( sig.begin() + 1, buffer, sizeof(buffer) ) );
    fc::from_hex( UNBLINDED, buffer, sizeof(buffer) );
    BOOST_CHECK( !memcmp( sig.begin() + 33, buffer, sizeof(buffer) ) );
}

static void to_bignum(const char* data32, fc::ssl_bignum& out) {
    unsigned char dummy[33]; dummy[0] = 0;
    memcpy(dummy, data32, 32);
    BN_bin2bn((unsigned char*) data32, 32, out);
}

//static void print(const fc::sha256 hash) {
//    print((unsigned char*) hash.data(), hash.data_size());
//}
//
//static void print(const BIGNUM* bn) {
//    unsigned char buffer[64];
//    int len = BN_num_bytes(bn);
//    if (len > sizeof(buffer)) {
//        printf("BN too long - %d bytes?!", len);
//        return;
//    }
//    BN_bn2bin(bn, buffer);
//    print(buffer, len);
//}
//
//static void print(const fc::ec_group& curve, const fc::ec_point& p, fc::bn_ctx& ctx) {
//    fc::ssl_bignum x;
//    fc::ssl_bignum y;
//    EC_POINT_get_affine_coordinates_GFp(curve, p, x, y, ctx);
//    printf("(");
//    print(x);
//    printf(", ");
//    print(y);
//    printf(")");
//}

namespace fc {
SSL_TYPE(ec_key,       EC_KEY,       EC_KEY_free)
}

BOOST_AUTO_TEST_CASE(openssl_blinding)
{
    // Needed this "test" for producing data for debugging my libsecp256k1 implementation

    char buffer[7] = "test_0";
    fc::ecc::extended_private_key alice = fc::ecc::extended_private_key::generate_master( "master" );
    fc::ecc::extended_private_key bob = fc::ecc::extended_private_key::generate_master( "puppet" );
    fc::ec_group curve(EC_GROUP_new_by_curve_name(NID_secp256k1));
    fc::bn_ctx ctx(BN_CTX_new());
    fc::ssl_bignum n;
    EC_GROUP_get_order(curve, n, ctx);
    fc::ssl_bignum n_half;
    BN_rshift1(n_half, n);
    fc::ssl_bignum zero; BN_zero(zero);

    fc::sha256 hash_ = fc::sha256::hash( buffer, sizeof(buffer) );
    fc::ssl_bignum hash; to_bignum(hash_.data(), hash);
    fc::ssl_bignum a; to_bignum(alice.derive_hardened_child(0).get_secret().data(), a);
    fc::ssl_bignum b; to_bignum(alice.derive_hardened_child(1).get_secret().data(), b);
    fc::ssl_bignum c; to_bignum(alice.derive_hardened_child(2).get_secret().data(), c);
    fc::ssl_bignum d; to_bignum(alice.derive_hardened_child(3).get_secret().data(), d);

    fc::ec_point P(EC_POINT_new(curve));
    fc::ecc::public_key_data Pd = bob.get_extended_public_key().derive_child(0).serialize();
    fc::ssl_bignum Px; to_bignum(Pd.begin() + 1, Px);
    EC_POINT_set_compressed_coordinates_GFp(curve, P, Px, (*Pd.begin()) & 1, ctx);

    fc::ec_point Q(EC_POINT_new(curve));
    fc::ecc::public_key_data Qd = bob.get_extended_public_key().derive_child(1).serialize();
    fc::ssl_bignum Qx; to_bignum(Qd.begin() + 1, Qx);
    EC_POINT_set_compressed_coordinates_GFp(curve, Q, Qx, (*Qd.begin()) & 1, ctx);

    // Alice computes K = (c·a)^-1·P and public key T = (a·Kx)^-1·(b·G + Q + d·c^-1·P).
    fc::ec_point K(EC_POINT_new(curve));
    fc::ssl_bignum tmp;
    BN_mod_mul(tmp, a, c, n, ctx);
    BN_mod_inverse(tmp, tmp, n, ctx);
    EC_POINT_mul(curve, K, zero, P, tmp, ctx);

    fc::ec_point T(EC_POINT_new(curve));
    BN_mod_inverse(tmp, c, n, ctx);
    BN_mod_mul(tmp, d, tmp, n, ctx);
    EC_POINT_mul(curve, T, b, P, tmp, ctx);
    EC_POINT_add(curve, T, T, Q, ctx);
    fc::ssl_bignum Kx;
    fc::ssl_bignum Ky;
    EC_POINT_get_affine_coordinates_GFp(curve, K, Kx, Ky, ctx);
    BN_mod_mul(tmp, a, Kx, n, ctx);
    BN_mod_inverse(tmp, tmp, n, ctx);
    EC_POINT_mul(curve, T, zero, T, tmp, ctx);

    fc::ssl_bignum blinded;
    BN_mod_mul(blinded, a, hash, n, ctx);
    BN_mod_add(blinded, blinded, b, n, ctx);

    fc::ssl_bignum p; to_bignum(bob.derive_normal_child(0).get_secret().data(), p);
    fc::ssl_bignum q; to_bignum(bob.derive_normal_child(1).get_secret().data(), q);
    BN_mod_inverse(p, p, n, ctx);
    BN_mod_mul(q, q, p, n, ctx);
    fc::ssl_bignum blind_sig;
    BN_mod_mul(blind_sig, p, blinded, n, ctx);
    BN_mod_add(blind_sig, blind_sig, q, n, ctx);

    fc::ecdsa_sig sig(ECDSA_SIG_new());
    BN_copy(sig->r, Kx);
    BN_mod_mul(sig->s, c, blind_sig, n, ctx);
    BN_mod_add(sig->s, sig->s, d, n, ctx);

    if (BN_cmp(sig->s, n_half) > 0) {
        BN_sub(sig->s, n, sig->s);
    }

    fc::ec_key verify(EC_KEY_new());
    EC_KEY_set_public_key(verify, T);
    BOOST_CHECK( ECDSA_do_verify( (unsigned char*) hash_.data(), hash_.data_size(), sig, verify ) );
//        printf("a: "); print(a);
//        printf("\nb: "); print(b);
//        printf("\nc: "); print(c);
//        printf("\nd: "); print(d);
//        printf("\nP: "); print(curve, P, ctx);
//        printf("\nQ: "); print(curve, Q, ctx);
//        printf("\nK: "); print(curve, K, ctx);
//        printf("\nT: "); print(curve, T, ctx);
//        printf("\np: "); print(p);
//        printf("\nq: "); print(q);
//        printf("\nhash: "); print(hash_);
//        printf("\nblinded: "); print(blinded);
//        printf("\nblind_sig: "); print(blind_sig);
//        printf("\nunblinded: "); print(sig->s);
//        printf("\n");
}


#ifndef STAN_MATH_TORSTEN_PKMODEL_PRED_HPP
#define STAN_MATH_TORSTEN_PKMODEL_PRED_HPP

#include <Eigen/Dense>
#include <vector>

namespace torsten{

/**
 * Every Torsten function calls Pred.
 *
 * Predicts the amount in each compartment for each event,
 * given the event schedule and the parameters of the model.
 *
 * Proceeds in two steps. First, computes all the events that
 * are not included in the original data set but during which
 * amounts in the system get updated. Secondly, predicts
 * the amounts in each compartment sequentially by going
 * through the augmented schedule of events. The returned pred
 * Matrix only contains the amounts in the event originally
 * specified by the users.
 *
 * This function is valid for all models. What changes from one
 * model to the other are the Pred1 and PredSS functions, which
 * calculate the amount at an individual event.
 *
 * @tparam T_time type of scalar for time
 * @tparam T_amt type of scalar for amount
 * @tparam T_rate type of scalar for rate
 * @tparam T_ii type of scalar for interdose interval
 * @tparam T_parameters type of scalar for the ODE parameters
 * @tparam T_biovar type of scalar for bio-variability parameters
 * @tparam T_tlag type of scalar for lag times parameters
 * @param[in] time times of events
 * @param[in] amt amount at each event
 * @param[in] rate rate at each event
 * @param[in] ii inter-dose interval at each event
 * @param[in] evid event identity:
 *                    (0) observation
 *                    (1) dosing
 *                    (2) other
 *                    (3) reset
 *                    (4) reset AND dosing
 * @param[in] cmt compartment number at each event (starts at 1)
 * @param[in] addl additional dosing at each event
 * @param[in] ss steady state approximation at each event
 * (0: no, 1: yes)
 * @param[in] pMatrix parameters at each event
 * @param[in] addParm additional parameters at each event
 * @parem[in] model basic info for ODE model and evolution operators
 * @param[in] SystemODE matrix describing linear ODE system that
 * defines compartment model. Used for matrix exponential solutions.
 * Included because it may get updated in modelParameters.
 * @return a matrix with predicted amount in each compartment
 * at each event.
 */
template<typename T_time,
        typename T_amt,
        typename T_rate,
        typename T_ii,
        typename T_parameters,
        typename T_biovar,
        typename T_tlag,
        typename F_one,
        typename F_SS>
Eigen::Matrix<typename boost::math::tools::promote_args<T_time, T_amt, T_rate,
  T_ii, typename boost::math::tools::promote_args<T_parameters, T_biovar,
  T_tlag>::type >::type, Eigen::Dynamic, Eigen::Dynamic>
Pred(const std::vector<T_time>& time,
     const std::vector<T_amt>& amt,
     const std::vector<T_rate>& rate,
     const std::vector<T_ii>& ii,
     const std::vector<int>& evid,
     const std::vector<int>& cmt,
     const std::vector<int>& addl,
     const std::vector<int>& ss,
     const std::vector<std::vector<T_parameters> >& pMatrix,
     const std::vector<std::vector<T_biovar> >& biovar,
     const std::vector<std::vector<T_tlag> >& tlag,
     const int& nCmt,
     const std::vector<Eigen::Matrix<T_parameters,
       Eigen::Dynamic, Eigen::Dynamic> >& system,
     const F_one& Pred1,
     const F_SS& PredSS) {
  using Eigen::Matrix;
  using Eigen::Dynamic;
  using boost::math::tools::promote_args;
  using std::vector;
  using::stan::math::multiply;

  typedef typename promote_args<T_time, T_amt, T_rate, T_ii,
    typename promote_args<T_parameters, T_biovar, T_tlag>::type >::type scalar;
  typedef typename promote_args<T_time, T_tlag>::type T_tau;
  typedef typename promote_args<T_rate, T_biovar>::type T_rate2;

  // BOOK-KEEPING: UPDATE DATA SETS
  EventHistory<T_tau, T_amt, T_rate, T_ii>
    events(time, amt, rate, ii, evid, cmt, addl, ss);

  ModelParameterHistory<T_tau, T_parameters, T_biovar, T_tlag>
    parameters(time, pMatrix, biovar, tlag, system);
  RateHistory<T_tau, T_rate> rates;

  events.Sort();
  parameters.Sort();
  int nKeep = events.get_size();

  events.AddlDoseEvents();
  parameters.CompleteParameterHistory(events);

  events.AddLagTimes(parameters, nCmt);
  rates.MakeRates(events, nCmt);
  parameters.CompleteParameterHistory(events);

  Matrix<scalar, 1, Dynamic> zeros = Matrix<scalar, 1, Dynamic>::Zero(nCmt);
  Matrix<scalar, 1, Dynamic> init = zeros;

  // COMPUTE PREDICTIONS
  Matrix<scalar, Dynamic, Dynamic>
    pred = Matrix<scalar, Dynamic, Dynamic>::Zero(nKeep, nCmt);

  scalar Scalar = 1;  // trick to promote variables to scalar

  T_tau dt, tprev = events.get_time(0);
  Matrix<scalar, Dynamic, 1> pred1;
  Event<T_tau, T_amt, T_rate, T_ii> event;
  ModelParameters<T_tau, T_parameters, T_biovar, T_tlag> parameter;
  int iRate = 0, ikeep = 0;

  for (int i = 0; i < events.get_size(); i++) {
    event = events.GetEvent(i);

    // Use index iRate instead of i to find rate at matching time, given there
    // is one rate per time, not per event.
    if (rates.get_time(iRate) != events.get_time(i)) iRate++;
    Rate<T_tau, T_rate2> rate2;
    rate2.copy(rates.GetRate(iRate));

    for (int j = 0; j < nCmt; j++)
      rate2.rate[j] *= parameters.GetValueBio(i, j);

    parameter = parameters.GetModelParameters(i);

    if ((event.get_evid() == 3) || (event.get_evid() == 4)) {  // reset events
      dt = 0;
      init = zeros;
    } else {
      dt = event.get_time() - tprev;
      pred1 = Pred1(dt, parameter, init, rate2.get_rate());
      init = pred1;
    }

    if (((event.get_evid() == 1 || event.get_evid() == 4)
      && (event.get_ss() == 1 || event.get_ss() == 2)) ||
      event.get_ss() == 3) {  // steady state event
      pred1 = multiply(PredSS(parameter,
                              parameters.GetValueBio(i, event.get_cmt() - 1)
                                * event.get_amt(),
                              event.get_rate(), event.get_ii(),
                              event.get_cmt()),
                       Scalar);

      // the object PredSS returns doesn't always have a scalar type. For
      // instance, PredSS does not depend on tlag, but pred does. So if
      // tlag were a var, the code must promote PredSS to match the type
      // of pred1. This is done by multiplying predSS by a Scalar.

      if (event.get_ss() == 2) init += pred1;  // steady state without reset
      else
        init = pred1;  // steady state with reset (ss = 1)
    }

    if (((event.get_evid() == 1) || (event.get_evid() == 4)) &&
      (event.get_rate() == 0)) {  // bolus dose
      init(0, event.get_cmt() - 1)
        += parameters.GetValueBio(i, event.get_cmt() - 1) * event.get_amt();
    }

    if (event.get_keep()) {
      pred.row(ikeep) = init;
      ikeep++;
    }
  tprev = event.get_time();
  }

  return pred;
}

}

#endif