#ifndef _HAND_EYE_CALIBRATION_VREP_PLUGIN_HPP_
#define _HAND_EYE_CALIBRATION_VREP_PLUGIN_HPP_

#include <iostream>
#include <memory>

#include <boost/exception/all.hpp>
#include <spdlog/spdlog.h>

#include "grl/vrep/Eigen.hpp"
#include "grl/vrep/Vrep.hpp"
#include "camodocal/calib/HandEyeCalibration.h"

#include "v_repLib.h"

#include "grl/vector_ostream.hpp"

namespace grl {

/// Creates a complete vrep plugin object
/// usage:
/// @code
///    auto handEyeCalib = std::make_shared<grl::HandEyeCalibrationVrepPlugin>();
///    handEyeCalib->construct();
///    // move objects to new position
/// @endcode
///
/// @todo this implementation is a bit hacky, redesign it
/// @todo separate out grl specific code from general kuka control code
/// @todo Template on robot driver and create a driver that just reads/writes to/from the simulation, then pass the two templates so the simulation and the real driver can be selected.
class HandEyeCalibrationVrepPlugin : public std::enable_shared_from_this<HandEyeCalibrationVrepPlugin> {
public:

  static const bool debug = false;

    enum ParamIndex {
        RobotBaseName,             ///<  V-REP handle string for the base of the first known transform, often a robot base
        RobotTipName,              ///<  V-REP handle string for the tip of the first known transform, often a robot tip
        OpticalTrackerBaseName,    ///<  V-REP handle string for the tip of the first known transform, often a robot tip
        OpticalTrackerDetectedObjectName
    };

    /// @todo allow default params
    typedef std::tuple<
        std::string,
        std::string,
        std::string,
        std::string
        > Params;

    static const Params defaultParams(){
        return std::make_tuple(
                    "Robotiiwa"               , // RobotBaseHandle,
                    "RobotFlangeTip"          , // RobotTipHandle,
                    "OpticalTrackerBase#0"    , // OpticalTrackerBaseName,
                    "Fiducial#22"               // FiducialArmTip
                );
    }

/// @todo allow KukaFRIThreadSeparator parameters to be updated
HandEyeCalibrationVrepPlugin (Params params = defaultParams())
      :
      isFirstFrame(true),
      frameCount(0),
      params_(params)
{
/// @todo figure out how to re-enable when .so isn't loaded
 // initHandles();
}

/// construct() function completes initialization of the plugin
/// @todo move this into the actual constructor, but need to correctly handle or attach vrep shared libraries for unit tests.
void construct(){
  logger_ = spdlog::get("console");
  initHandles();

  // set the current transformEstimate to the initial estimate already in vrep
  if(allHandlesSet) transformEstimate = getObjectTransform(opticalTrackerDetectedObjectName,robotTip);
}

void addFrame() {
   logger_->info( "Adding hand eye calibration frame #", ++frameCount);

    auto robotTipInRobotBase    = getObjectTransform(robotTip,robotBase);
    auto fiducialInOpticalTrackerBase = getObjectTransform(opticalTrackerDetectedObjectName,opticalTrackerBase);

    if(isFirstFrame){
      firstRobotTipInRobotBaseInverse             = robotTipInRobotBase.inverse();
      firstFiducialInOpticalTrackerBaseInverse    = fiducialInOpticalTrackerBase.inverse();
      isFirstFrame = false;
    }

    auto robotTipInFirstTipBase      = firstRobotTipInRobotBaseInverse * robotTipInRobotBase;        // A_0_Inv * A_i
    auto fiducialInFirstFiducialBase = firstFiducialInOpticalTrackerBaseInverse * fiducialInOpticalTrackerBase;  // B_0_Inv * B_i

    rvecsArm.push_back(     eigenRotToEigenVector3dAngleAxis(robotTipInFirstTipBase.rotation()        ));
    tvecsArm.push_back(                                      robotTipInFirstTipBase.translation()     );

    rvecsFiducial.push_back(eigenRotToEigenVector3dAngleAxis(fiducialInFirstFiducialBase.rotation()   ));
    tvecsFiducial.push_back(                                 fiducialInFirstFiducialBase.translation());

   if(debug){

     logger_->info( "\nrobotTipInRobotBase:\n", poseString(robotTipInRobotBase));
     logger_->info( "\nfiducialInOpticalTrackerBase:\n", poseString(fiducialInOpticalTrackerBase));

     logger_->info( "\nrobotTipInFirstTipBase:\n", poseString(robotTipInFirstTipBase));
     logger_->info( "\nfiducialInFirstFiducialBase:\n", poseString(fiducialInFirstFiducialBase));

     // print simulation transfrom from tip to fiducial
     Eigen::Affine3d RobotTipToFiducial = getObjectTransform(opticalTrackerDetectedObjectName,robotTip);
     logger_->info( poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): "));

     BOOST_VERIFY(robotTipInFirstTipBase.translation().norm() - fiducialInFirstFiducialBase.translation().norm() < 0.1);
   }
}

/// @brief run solver to estimate the unknown transform and set the simulation to the estimated value
///
/// after calling addFrame a number of times in different positions and orientations
///
/// @todo probably want to run at least part of this in a separate thread.
/// @todo evaluate if applyEstimate should not be called by this
void estimateHandEyeScrew(){

   logger_->info(  "Running Hand Eye Screw Estimate with the following numbers of entries in each category:  rvecsFiducial",rvecsFiducial.size(),
   " tvecsFiducial: ", tvecsFiducial.size(), " rvecsArm: ", rvecsArm.size(), " tvecsArm: ", tvecsArm.size());

   BOOST_VERIFY(allHandlesSet);

  handEyeCalib.estimateHandEyeScrew(
      rvecsArm,
      tvecsArm,
      rvecsFiducial,
      tvecsFiducial,
      transformEstimate.matrix()
  );

    // get fiducial in optical tracker base frame
    int ret = simGetObjectPosition(opticalTrackerDetectedObjectName, opticalTrackerBase, detectedObjectPosition.begin());
    if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("HandEyeCalibrationVrepPlugin: Could not get position"));
    ret = simGetObjectQuaternion(opticalTrackerDetectedObjectName, opticalTrackerBase, detectedObjectQuaternion.begin());
    if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("HandEyeCalibrationVrepPlugin: Could not get quaternion"));

   if(debug){
     // print simulation transfrom from tip to fiducial
     Eigen::Affine3d RobotTipToFiducial = getObjectTransform(opticalTrackerDetectedObjectName,robotTip);
     logger_->info(  "\n", poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): "));
   }

   logger_->info( "\n", poseString(transformEstimate,"estimated RobotTipToFiducial:"));

   applyEstimate();

   // print results
   Eigen::Quaterniond eigenQuat(transformEstimate.rotation());
   logger_->info( "Hand Eye Screw Estimate quat wxyz\n: ", eigenQuat.w(), " ", eigenQuat.x(), " ", eigenQuat.y(), " ", eigenQuat.z(), " ", " translation xyz: ", transformEstimate.translation().x(), " ", transformEstimate.translation().y(), " ", transformEstimate.translation().z(), " ");

    logger_->info( "Optical Tracker Base Measured quat wxyz\n: ", detectedObjectQuaternion[0], " ", detectedObjectQuaternion[1], " ", detectedObjectQuaternion[2], " ", detectedObjectQuaternion[3], " ", " translation xyz: ", detectedObjectPosition[0], " ", detectedObjectPosition[1], " ", detectedObjectPosition[2], " ");

}

/// @brief  Will apply the stored estimate to the v-rep simulation value
///
/// A default transform is saved when construct() was called
/// so if no estimate has been found that will be used.
void applyEstimate(){

   // set transform between end effector and fiducial
   setObjectTransform(opticalTrackerDetectedObjectName, robotTip, transformEstimate);
}

Eigen::Affine3d getEstimate(){
   return transformEstimate;
}

/// @brief Correct modified transform for measured optical tracker data
///
/// If real time transform measurements are happening applyEstimate will move the measured object
/// relative to the sensor and this puts the transform measurement back so the object
/// appears in its real position. However, if running in simulation only
/// the sensor will be in the correct position, so you don't want to call this.
void restoreSensorPosition(){
   // set real optical tracker position and orientation relative to the robot fiducial
   simSetObjectPosition  (opticalTrackerBase, opticalTrackerDetectedObjectName, detectedObjectPosition.begin());
   simSetObjectQuaternion(opticalTrackerBase, opticalTrackerDetectedObjectName, detectedObjectQuaternion.begin());
}

private:

/// @todo create a function that calls simGetObjectHandle and throws an exception when it fails
/// @todo throw an exception if any of the handles is -1
void initHandles() {

	robotTip                         = grl::vrep::getHandleFromParam<RobotTipName>                    (params_);					//Obtain RobotTip handle
	robotBase                        = grl::vrep::getHandleFromParam<RobotBaseName>                   (params_);
	opticalTrackerBase               = grl::vrep::getHandleFromParam<OpticalTrackerBaseName>          (params_);
	opticalTrackerDetectedObjectName = grl::vrep::getHandleFromParam<OpticalTrackerDetectedObjectName>(params_);

	allHandlesSet  = true;
}

camodocal::HandEyeCalibration handEyeCalib;
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > rvecsArm;
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > tvecsArm;
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > rvecsFiducial;
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > tvecsFiducial;

bool isFirstFrame;
std::size_t frameCount;
Eigen::Affine3d firstRobotTipInRobotBaseInverse;
Eigen::Affine3d firstFiducialInOpticalTrackerBaseInverse;

///
std::array<float,3> detectedObjectPosition;
std::array<float,4> detectedObjectQuaternion;
/// estimate transform between end effector and fiducial
Eigen::Affine3d transformEstimate;

int robotTip = -1;					//Obtain RobotTip handle
int robotBase = -1;
int opticalTrackerDetectedObjectName = -1;
int opticalTrackerBase = -1;
//int ImplantCutPath = -1;
//int BallJointPath = -1;
//int bone = -1;

bool allHandlesSet = false;

private:
Params params_;
std::shared_ptr<spdlog::logger> logger_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

}

#endif


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <iostream>

#include "auxiliary_functions.h"
#include "house_robber.h"

BOOST_AUTO_TEST_SUITE(insert_intervals)

BOOST_AUTO_TEST_CASE(test_1) {
	std::vector<int> nums = { 1,2,3,1 };
	Solution sol;

	int haul = sol.rob(nums);

	BOOST_TEST(4 == haul);
}

BOOST_AUTO_TEST_CASE(test_2) {
	std::vector<int> nums = { 2,7,9,3,1 };
	Solution sol;

	int haul = sol.rob(nums);

	BOOST_TEST(12 == haul);
}

BOOST_AUTO_TEST_CASE(test_3) {
	std::vector<int> nums = { };
	Solution sol;

	int haul = sol.rob(nums);

	BOOST_TEST(0 == haul);
}

BOOST_AUTO_TEST_CASE(test_4) {
	std::vector<int> nums = { 2,1,1,3,1,7 };
	Solution sol;

	int haul = sol.rob(nums);

	BOOST_TEST(12 == haul);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file geometry.hpp
 * @author Ondrej Prochazka <ondrej.prochazka@citationtech.net>
 *
 * Analytical geometry functions
 */

#ifndef MATH_GEOMETRY_HPP
#define MATH_GEOMETRY_HPP

#include "geometry_core.hpp"

#include <algorithm>
#include <array>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace math {

namespace ublas = boost::numeric::ublas;

typedef std::array<math::Point2d, 3> Triangle2d;
typedef std::array<math::Point3d, 3> Triangle3d;

typedef std::vector<Triangle2d> Triangles2d;
typedef std::vector<Triangle3d> Triangles3d;

typedef Points2d Polygon; // single CCW ring, not closed
typedef std::vector<Polygon> MultiPolygon; // multiple rings, holes CW

/**
 * Find the point where two lines get closest in 3D space.
 * The lines are defined parametrically in euclidian coordinates.
 * Return value is the euclidian metrics of the line distance, plus
 * the two parameter values for each of the lines.
 */

float lineDistance(
    const ublas::vector<float> & p1, const ublas::vector<float> & u1,
    const ublas::vector<float> & p2, const ublas::vector<float> & u2,
    float & r1, float & r2 );

/** normalize vector */
template <class T>
inline ublas::vector<typename T::value_type> normalize( const T & v ) {
    return v / ublas::norm_2( v );
}

/** normalize vector */
template <class T>
inline Point2_<T> normalize( const Point2_<T> & v ) {
    return v / ublas::norm_2( v );
}

/** normalize vector */
template <class T>
inline Point3_<T> normalize( const Point3_<T> & v ) {
    return v / ublas::norm_2( v );
}

/** length of the vector */
template <typename T>
inline double length( const T & v ) {
    return ublas::norm_2(v);
}

/** angle between two 3D vectors (in radians)
 * See Kahan W.: How Futile are Mindless Assessments of Roundoff in
 * Floating-Point Computation?, page 46
 * (https://people.eecs.berkeley.edu/~wkahan/Mindless.pdf)
 *  **/
template <class T>
inline T angle(const Point3_<T>& v1, const Point3_<T>& v2)
{
    return 2
           * std::atan2(length(v1 * length(v2) - length(v1) * v2),
                        length(v1 * length(v2) + length(v1) * v2));
}

/** homogeneous coordinates (from euclidian) */
template <class T>
inline ublas::vector<T, ublas::bounded_array<T, 4> >
homogeneous( const Point3_<T> & src )
{
    ublas::vector<T, ublas::bounded_array<T, 4> > dst(4);
    dst(0) = src(0); dst(1) = src(1); dst(2) = src(2);
    dst(3) = T(1);
    return dst;
}

/** homogeneous coordinates (from euclidian) */
template <class T>
inline ublas::vector<typename T::value_type> homogeneous( const T & src ) {

    ublas::vector<typename T::value_type> ret(
        ublas::unit_vector<typename T::value_type>(
            src.size() + 1, src.size() ) );
    ublas::subrange( ret, 0, src.size() ) = src;
    return ret;
}

/** euclidian coordinates (from homogenous) */
template <class T>
inline Point3_<T>
euclidian( const ublas::vector<T, ublas::bounded_array<T, 4> > & src )
{
    auto div(src(3));
    return Point3_<T>(src(0) / div, src(1) / div, src(2) / div);
}

/** euclidian coordinates (from homogenous) */
template <class T>
inline ublas::vector<typename T::value_type> euclidian( const T & src ) {
    return ublas::subrange( src, 0, src.size() - 1 ) / src( src.size() - 1 );
}

/** cross product, in euclidian 3D */

template<typename T, typename U>
inline ublas::vector<typename T::value_type> crossProduct(
    const T & u,
    const U & v ) {

    assert( u.size() == 3 && v.size() == 3 );
    ublas::vector<typename T::value_type> retval( 3 );
    retval(0) = u(1) * v(2) - v(1) * u(2);
    retval(1) = -u(0) * v(2) + v(0) * u(2);
    retval(2) = u(0) * v(1) - v(0) * u(1);
    return retval;
}

template <typename T>
inline Point3_<T> crossProduct(const Point3_<T> & u, const Point3_<T> & v )
{
    return Point3_<T>(u(1) * v(2) - v(1) * u(2)
                     , -u(0) * v(2) + v(0) * u(2)
                     , u(0) * v(1) - v(0) * u(1));
}

template<typename T>
inline Point3_<T> crossProduct(const Point3_<T> & u, const ublas::vector<T> & v )
{
    assert(v.size() == 3);
    return Point3_<T>(u(1) * v(2) - v(1) * u(2)
                     , -u(0) * v(2) + v(0) * u(2)
                     , u(0) * v(1) - v(0) * u(1));
}

template<typename T>
inline Point3_<T> crossProduct(const ublas::vector<T> & u, const Point3_<T> & v )
{
    assert(u.size() == 3);
    return Point3_<T>(u(1) * v(2) - v(1) * u(2)
                     , -u(0) * v(2) + v(0) * u(2)
                     , u(0) * v(1) - v(0) * u(1));
}

/** 2D version of cross product; result is not an vector but crossproduct's
 *  z-component.
 */
template <typename T>
inline T crossProduct(const Point2_<T> & u, const Point2_<T> & v )
{
    return u(0) * v(1) - v(0) * u(1);
}

//! return area of a triangle in 2D plane
template <typename T>
inline double triangleArea(const Point2_<T>& a, const Point2_<T>& b,
                           const Point2_<T>& c)
{
    return std::abs(crossProduct(Point2(b - a), Point2(c - a))) * 0.5;
}

//! return area of a triangle in 3D space
template <typename T>
inline double triangleArea(const Point3_<T>& a, const Point3_<T>& b,
                           const Point3_<T>& c)
{
    return norm_2(crossProduct(b - a, c - a)) * 0.5;
}

#ifdef MATH_HAS_OPENCV
    //! return area of a triangle in 2D plane
    template <typename T>
    inline double triangleArea(const cv::Point_<T>& a, const cv::Point_<T>& b,
                               const cv::Point_<T>& c)
    {
        return triangleArea(Point2_<T>(a), Point2_<T>(b), Point2_<T>(c));
    }

    //! return area of a triangle in 3D space
    template <typename T>
    inline double triangleArea(const cv::Point3_<T>& a, const cv::Point3_<T>& b,
                               const cv::Point3_<T>& c)
    {
        return triangleArea(Point3_<T>(a), Point3_<T>(b), Point3_<T>(c));
    }
#endif // MATH_HAS_OPENCV

/** Returns a positive number if the sequence of points {a, b, c} turns
 *  counter-clockwise in the XY plane (negative number otherwise).
 *  Zero (within numerical tolerance) means collinear.
 */
template <typename T>
inline T ccw(const Point2_<T> &a, const Point2_<T> &b, const Point2_<T> &c)
{
    return (b(0) - a(0))*(c(1) - a(1)) - (b(1) - a(1))*(c(0) - a(0));
}

/** Parametric line, in euclidian 2D
 */

template <typename T>
struct Line2_
{
    T p, u;

    Line2_(const T p, const T u) : p(p), u(u) {};
};

typedef Line2_<Point2> Line2;

/** Returns a point where two lines `l1` and `l2` intersect.
 */
template <typename T>
inline T lineIntersection(const Line2_<T>& l1, const Line2_<T>& l2)
{
    T pd = l2.p - l1.p; // diff of origins
    auto num = -l2.u(1) * pd(0) + l2.u(0) * pd(1);
    auto den = l2.u(0) * l1.u(1) - l1.u(0) * l2.u(1);
    return l1.p + (num / den) * l1.u;
}

/**
 * Line segment represented by start and end points
 */
template <typename T>
struct Segment2_
{
    T p1, p2;

    Segment2_(const T p1, const T p2) : p1(p1), p2(p2) {};
};

typedef Segment2_<Point2> Segment2;

/**
 * Parametric line, in euclidian 3D
 */

struct Line3 {
    Point3 p, u;

    Line3(const Point3 p = Point3(), const Point3 u = Point3() )
        : p( p ), u( u ) {}

    /** Returns line's point at given parameter (t)
     */
    Point3 point(double t) const { return p + u * t; }
};

template <typename E, typename T>
inline std::basic_ostream<E, T> & operator << (
        std::basic_ostream<E,T> & os,
        const Line3 & line ) {

    os << line.p << " + t * " << line.u;
    return os;
}

Point3 midpoint( const Line3 & line1, const Line3 & line2
               , double minAngleCos = 0.9962 );

double pointLineDistance(const Point3 &p, const Line3 &line);

/**
 * Parametric plane, in euclidian 3D
 * Legacy representation of plane by 3 points.
 */
namespace legacy {
struct Plane3 {

    Point3 p, u, v;

    Plane3(
        const ublas::vector<double> p = ublas::zero_vector<double>( 3 ),
        const ublas::vector<double> u = ublas::zero_vector<double>( 3 ),
        const ublas::vector<double> v = ublas::zero_vector<double>( 3 ) )
        : p( p ), u( u ), v( v ) {}

    template <class Matrix>
    Plane3 transform( const Matrix & trafo ) const {

        Plane3 tplane;

        tplane.p = euclidian( prod( trafo, homogeneous( p ) ) );
        tplane.u = euclidian( prod( trafo, homogeneous( p + u ) ) ) - tplane.p;
        tplane.v = euclidian( prod( trafo, homogeneous( p + v ) ) ) - tplane.p;

        return tplane;

    }
};

template <typename E, typename T>
inline std::basic_ostream<E, T> & operator << (
        std::basic_ostream<E,T> & os,
        const Plane3 & plane ) {

    os << plane.p << " + t * " << plane.u << " + s * " << plane.v;
    return os;
}
}  // namespace legacy

/** line and plane intersection */

Point3 intersection( const Line3 & line, const legacy::Plane3 & plane );

/** line and plane intersection
 *  instead of point returns 3 coefficients:
 *      * lines t-parameter
 *      * planes t-parameter
 *      * planes s-parameter
 */
Point3 intersectionParams(const Line3 &line, const legacy::Plane3 &plane);

/**
* Plane represented in general form
* Representation as (a,b,c,d) in equation: ax+by+cz+d=0
*/
struct Plane3
{
    math::Point3 n_;  // vector with (a,b,c) i.e. normal vector
    double d_;

    // Plane from parameters
    Plane3(double a, double b, double c, double d) : n_(a, b, c), d_(d) { }

    // Plane from point and normal
    Plane3(const math::Point3d& pt, const math::Point3d& n)
        : n_(n),
          d_(-ublas::inner_prod(pt, n)) {};

    // Plane from three points
    Plane3(const math::Point3d& p1,
           const math::Point3d& p2,
           const math::Point3d& p3)
        : Plane3(p1, crossProduct(p2 - p1, p3 - p1)) {};

    // Plane from legacy representation (point-normal packed as Line3)
    Plane3(const math::Line3& l) : Plane3(l.p, l.u) {};

    // Returns a plane with opposite orientation
    Plane3 opposite() const {
        Plane3 pl2(*this);
        pl2.n_ *= -1;
        pl2.d_ *= -1;
        return pl2;
    }
};

/**
 * Returns a distance of point to a plane (perpendicular)
*/
double pointPlaneDistance(const Point3 &p, const Plane3 &plane);

/**
 * Returns an orthogonal projection of a point to a plane
 */
Point3 pointPlaneProjection(const Point3 &p, const Plane3 &plane);

/**
 * Returns a point of intersection between plane and line
 */
Point3 linePlaneIntersection(const Line3 &l, const Plane3 &plane);

/**
 * Returns a line of intersection between two planes
 */
Line3 planeIntersection(const Plane3 &p1, const Plane3 &p2);

/**
 * Returns a point of intersection between three planes
 */
Point3 planeIntersection(const Plane3 &p1, const Plane3 &p2, const Plane3 &p3);

/**
 * Returns a measure of triangular polyface regularity. Value of 1.0
 * indicates an equilateral triangle, value of 0.0 indicates a triangle
 * with at least one degenerate edge.
 */
double polygonRegularity(
    const Point3 & v0, const Point3 & v1, const Point3 & v2  );

/**
 * Returns a measure of quad polyface regularity. Value of 1.0
 * indicates a square, value of 0.0 indicates a triangle
 * with at least one degenerate edge.
 */
double polygonRegularity(
    const Point3 & v0, const Point3 & v1, const Point3 & v2, const Point3 & v3 );

/**
 * Returns whether the triangle and rectagle collide.
 */
bool triangleRectangleCollision( math::Point2 triangle[3]
                               , math::Point2 ll, math::Point2 ur);

/**
 * Convert cartesian coordinates (r) to barycentric with respect to the (a,b,c)
 * triangle. See http://en.wikipedia.org/wiki/Barycentric_coordinate_system
 */
math::Point3 barycentricCoords(const math::Point2 &r, const math::Point2 &a
                               , const math::Point2 &b, const math::Point2 &c);
inline
math::Point3 barycentricCoords(const math::Point2 &r, const Triangle2d &tri) {
    return barycentricCoords(r, tri[0], tri[1], tri[2]);
}

namespace detail {
    template <typename T, typename Q> struct ExtentsTypeTraits;

    template <typename T> struct ExtentsTypeTraits<T, Point2_<T>> {
        typedef Extents2_<T> type;
    };

    template <typename T> struct ExtentsTypeTraits<T, Point3_<T>> {
        typedef Extents3_<T> type;
    };

#ifdef MATH_HAS_OPENCV
    template <typename T> struct ExtentsTypeTraits<T, cv::Point_<T>> {
        typedef Extents2_<T> type;
    };

    template <typename T> struct ExtentsTypeTraits<T, cv::Point3_<T>> {
        typedef Extents3_<T> type;
    };
#endif
} // namespace detail

template <typename Iterator>
inline auto computeExtents(Iterator begin, Iterator end)
    -> typename detail::ExtentsTypeTraits
    <typename std::iterator_traits<Iterator>::value_type::value_type
    , typename std::iterator_traits<Iterator>::value_type>::type
{
    typedef typename detail::ExtentsTypeTraits
        <typename std::iterator_traits<Iterator>::value_type::value_type
         , typename std::iterator_traits<Iterator>::value_type>::type Extents;
    typedef typename Extents::point_type point_type;

    if (begin == end) {
        return Extents(InvalidExtents{});
    }

    Extents extents(*begin++);
    std::for_each(begin, end
                  , [&extents](const point_type &p) {
                      update(extents, p);
                  });
    return extents;
}

/** Simplified version for STL container.
 */
template <typename Container>
inline auto computeExtents(const Container &c)
    -> typename detail::ExtentsTypeTraits
    <typename Container::value_type::value_type
     , typename Container::value_type>::type
{
    return computeExtents(c.begin(), c.end());
}

// prefered natural aliases for above

template <typename Iterator>
inline auto extents(Iterator begin, Iterator end)
    -> typename detail::ExtentsTypeTraits
    <typename std::iterator_traits<Iterator>::value_type::value_type
    , typename std::iterator_traits<Iterator>::value_type>::type
{
    return computeExtents<Iterator>(begin,end);
}

template <typename Container>
inline auto extents(const Container &c)
    -> typename detail::ExtentsTypeTraits
    <typename Container::value_type::value_type
     , typename Container::value_type>::type
{
    return computeExtents<Container>(c);
}

} // namespace math

#endif // MATH_GEOMETRY_HPP


//
// Copyright (c) 2015 CNRS
//

#ifndef __pinocchio_force_set_hpp__
#define __pinocchio_force_set_hpp__

#include "pinocchio/spatial/fwd.hpp"
#include <Eigen/Geometry>

namespace pinocchio
{
  template<typename _Scalar, int _Options>
  class ForceSetTpl
  {
  public:
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef SE3Tpl<Scalar,Options> SE3;

    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;

  public:
    // Constructors
    ForceSetTpl(const int & ncols ) : size(ncols),m_f(3,ncols), m_n(3,ncols)
    { m_f.fill(NAN); m_n.fill(NAN); }
    ForceSetTpl(const Matrix3x & linear, const Matrix3x & angular)
      : size((int)linear.cols()),m_f(linear), m_n(angular)
    {  assert( linear.cols() == angular.cols() ); }

    Matrix6x matrix() const
    {
      Matrix6x F(6,size); F << m_f, m_n;
      // F.template topRows<3>() = m_f;
      // F.template bottomRows<3>()  = m_n;
      return F;
    }
    operator Matrix6x () const { return matrix(); }

    // Getters
    const Matrix3x & linear() const { return m_f; }
    const Matrix3x & angular() const { return m_n; }

    /// af = aXb.act(bf)
    ForceSetTpl se3Action(const SE3 & m) const
    {
      Matrix3x Rf (m.rotation()*linear());
      return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
      // TODO check if nothing better than explicitely calling skew
    }
    /// bf = aXb.actInv(af)
    ForceSetTpl se3ActionInverse(const SE3 & m) const
    {
      return ForceSetTpl(m.rotation().transpose()*linear(),
		      m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
      // TODO check if nothing better than explicitely calling skew
    }

    friend std::ostream & operator << (std::ostream & os, const ForceSetTpl & phi)
    {
      os
	<< "F =\n" << phi.linear() << std::endl
	<< "Tau =\n" << phi.angular() << std::endl;
      return os;
    }

    /* --- BLOCK ------------------------------------------------------------ */
    struct Block
    {
      ForceSetTpl & ref;
      int idx,len;
      Block( ForceSetTpl & ref, const int & idx, const int & len )
	: ref(ref), idx(idx), len(len) {}

      Eigen::Block<ForceSetTpl::Matrix3x> linear() { return ref.m_f.block(0,idx,3,len); }
      Eigen::Block<ForceSetTpl::Matrix3x> angular() { return ref.m_n.block(0,idx,3,len); }
      Eigen::Block<const ForceSetTpl::Matrix3x> linear()  const
      { return ((const ForceSetTpl::Matrix3x &)(ref.m_f)).block(0,idx,3,len); }
      Eigen::Block<const ForceSetTpl::Matrix3x> angular() const
      { return ((const ForceSetTpl::Matrix3x &)(ref.m_n)).block(0,idx,3,len); }

      ForceSetTpl::Matrix6x matrix() const
      {
	ForceSetTpl::Matrix6x res(6,len); res << linear(),angular();
	return res;
      }

      Block& operator= (const ForceSetTpl & copy)
      {
	assert(copy.size == len);
	linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.m_f;
	angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.m_n;
	return *this;
      }

      Block& operator= (const ForceSetTpl::Block & copy)
      {
	assert(copy.len == len);
	linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
	angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
	return *this;
      }

      template <typename D>
      Block& operator= (const Eigen::MatrixBase<D> & m)
      {
        eigen_assert(D::RowsAtCompileTime == 6);
        assert(m.cols() == len);
        linear() = m.template topRows<3>();
        angular() = m.template bottomRows<3>();
        return *this;
      }

      /// af = aXb.act(bf)
      ForceSetTpl se3Action(const SE3 & m) const
      {
	// const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
	// const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
	Matrix3x Rf ((m.rotation()*linear()));
	return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
	// TODO check if nothing better than explicitely calling skew
      }
      /// bf = aXb.actInv(af)
      ForceSetTpl se3ActionInverse(const SE3 & m) const
      {
	// const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
	// const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
	return ForceSetTpl(m.rotation().transpose()*linear(),
			   m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
	// TODO check if nothing better than explicitely calling skew
      }

    };

    Block block(const int & idx, const int & len) { return Block(*this,idx,len); }

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */

  public: //
  private:
    int size;
    Matrix3x m_f,m_n;
  };

  typedef ForceSetTpl<double,0> ForceSet;

  namespace internal
  {
    template<>
    struct SE3GroupAction<ForceSet::Block>    { typedef ForceSet ReturnType; };
  }

} // namespace pinocchio

#endif // ifndef __pinocchio_force_set_hpp__


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::random::simulate_batches.hpp                  //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_RANDOM_SIMULATE_BATCHES_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_RANDOM_SIMULATE_BATCHES_HPP_ER_2009

#include <stdexcept>
#include <ostream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/statistics/survival/data/data/mngr/default_parameter_records.hpp>
#include <boost/statistics/survival/data/data/mngr/default_covariates_prior_model.hpp>
#include <boost/typeof/typeof.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{

    template<typename T,typename X,typename D,typename M,typename N,typename U>
    void simulate_batches(
        std::ostream& ofs_pr_mngrs,
//        std::ofstream& ofs_pr_mngrs,
        default_covariates_prior_model_mngr<X,D,M>& xpm_mngr, //TODO const
        N n_batches,
        N n_records,
        const T& t,
        const T& delta_t,
        U& urng
    ){
        typedef BOOST_TYPEOF_TPL(
            boost::sample(
                (xpm_mngr.prior_model_wrapper()).prior(),
                urng
            )
        ) par_;

        typedef boost::archive::text_oarchive           oa_;
        typedef default_parameter_records_mngr<T,par_>  pr_mngr_;
        pr_mngr_ pr_mngr;

        oa_ oa_pr_mngrs(ofs_pr_mngrs);
        for(unsigned i = 0; i<n_batches; i++){
            //records.clear();
            par_ par  = boost::sample( //TODO think about the unscope issue
                (xpm_mngr.prior_model_wrapper()).prior(),
                urng
            );

            pr_mngr.clear_records();
            pr_mngr.set_parameter( par );
            typedef default_covariates_model_mngr<X,M>      xm_mngr_;
            xm_mngr_& xm_mngr = static_cast<xm_mngr_&>(xpm_mngr); // TODO const
            pr_mngr.back_generate(
                n_records,
                xm_mngr,
                t,
                delta_t,
                urng
            );

            oa_pr_mngrs << pr_mngr;
        }
    }

}// data
}// survival
}// statistics
}// boost

#endif


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "MDIntegrator.h"
#include <Utils/Geometry/GeometryUtilities.h>
#include <Eigen/Geometry>
#include <random>

namespace Scine {
namespace Utils {

static constexpr double femtosecondsToAtomicUnits = 41.3413746; // 1 fs in atomic time units
static constexpr double massConversionFactor = 1822.88848619;   // 1 u in electron masses
static constexpr double atomicUnitsToKelvin = 3.1577464e5;
static constexpr double kelvinToAtomicUnits = 1.0 / atomicUnitsToKelvin;
static constexpr double boltzmannConstant = 3.1668114e-6; // E_h / K

MDIntegrator::MDIntegrator() : targetTemperature_(298.15 * kelvinToAtomicUnits) {
  setTimeStepInFemtoseconds(1.0);
  setRelaxationTimeFactor(10.0);
}

void MDIntegrator::setElementTypes(const Utils::ElementTypeCollection& elements) {
  nAtoms_ = static_cast<int>(elements.size());
  masses_ = Utils::Geometry::getMasses(elements);
  resetVelocities();
  resetAccelerations();
}

void MDIntegrator::calculateAccelerationsFromGradients(const Utils::GradientCollection& gradients) {
  assert(gradients.size() == accelerations_.size());
  for (int i = 0; i < nAtoms_; ++i)
    accelerations_.row(i) = (-1.0 / masses_[i]) * gradients.row(i);
}

void MDIntegrator::rescaleVelocitiesForTemperatureBath() {
  if (!temperatureBath_)
    return;
  double currentTemperature = getCurrentTemperature();
  double factor = std::sqrt(1. + timeStep_ / temperatureRelaxationTime_ * (targetTemperature_ / currentTemperature - 1));
  velocities_ *= factor;
}

void MDIntegrator::resetVelocities() {
  velocities_.resize(nAtoms_, 3);
  velocities_.setZero();
  sampleVelocitiesFromBoltzmannDistribution();
}

void MDIntegrator::resetAccelerations() {
  accelerations_.resize(nAtoms_, 3);
  accelerations_.setZero();
}

void MDIntegrator::setVelocities(const Utils::DisplacementCollection& velocities) {
  velocities_ = velocities;
}

Utils::DisplacementCollection MDIntegrator::getVelocities() const {
  return velocities_;
}

void MDIntegrator::setTimeStepInFemtoseconds(double fs) {
  timeStep_ = fs * femtosecondsToAtomicUnits / massConversionFactor;
  temperatureRelaxationTime_ = timeStep_ * relaxationTimeFactor_;
}

void MDIntegrator::setRelaxationTimeFactor(double factor) {
  relaxationTimeFactor_ = factor;
  temperatureRelaxationTime_ = timeStep_ * relaxationTimeFactor_;
}

void MDIntegrator::setTargetTemperatureInKelvin(double T) {
  targetTemperature_ = T * kelvinToAtomicUnits;
  temperatureBath_ = true;
}

double MDIntegrator::getCurrentTemperature() {
  int numberDegreesOfFreedom = 3 * nAtoms_; // TODO: Subtract translation and remove center of mass motion
  double currentTemperature =
      (velocities_.rowwise().squaredNorm().array() * Eigen::Map<const Eigen::ArrayXd>(masses_.data(), masses_.size())).sum() /
      numberDegreesOfFreedom;
  return currentTemperature;
}

void MDIntegrator::sampleVelocitiesFromBoltzmannDistribution() {
  std::mt19937 gen(seed_);
  double numerator = std::sqrt(targetTemperature_ * boltzmannConstant * atomicUnitsToKelvin);
  int index = 0;
  for (double mass : masses_) {
    std::normal_distribution<> d(0., numerator * std::sqrt(1. / mass));
    velocities_.row(index) = Position{d(gen), d(gen), d(gen)};
    ++index;
  }
}

void MDIntegrator::setSeed(int seed) {
  seed_ = seed;
}

void MDIntegrator::removeCenterOfMassLinearMomentum(const Eigen::MatrixX3d& positions) {
  const Eigen::VectorXd& masses = Eigen::Map<const Eigen::VectorXd>(masses_.data(), masses_.size());
  double totalMass = masses.sum();
  auto centerOfMass = Utils::Geometry::getCenterOfMass(positions, masses_);

  // Remove total linear momentum
  Eigen::MatrixX3d linearMomentumVector = velocities_.array().colwise() * masses.array();
  Eigen::RowVector3d centerOfMassVelocity = linearMomentumVector.colwise().sum() / totalMass;

  velocities_.rowwise() -= centerOfMassVelocity;
}

void MDIntegrator::removeCenterOfMassAngularMomentum(const Eigen::MatrixX3d& positions) {
  const Eigen::VectorXd& masses = Eigen::Map<const Eigen::VectorXd>(masses_.data(), masses_.size());
  double totalMass = masses.sum();
  auto centerOfMass = Utils::Geometry::getCenterOfMass(positions, masses_);
  auto inertiaTensor = Utils::Geometry::calculateInertiaTensor(positions, masses_, centerOfMass);
  Eigen::MatrixX3d positionsRelativeToCOM = positions.rowwise() - centerOfMass;

  // Remove total angular momentum
  Eigen::MatrixX3d linearMomentumVector = velocities_.array().colwise() * masses.array();
  Eigen::RowVector3d totalAngularMomentum = Eigen::RowVector3d::Zero(1, 3);
  for (int i = 0; i < linearMomentumVector.rows(); ++i) {
    Eigen::RowVector3d particleAngularMomentum = positionsRelativeToCOM.row(i).cross(linearMomentumVector.row(i));
    totalAngularMomentum += particleAngularMomentum;
  }

  Eigen::RowVector3d angularVelocityVector =
      inertiaTensor.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(totalAngularMomentum.transpose());

  for (int i = 0; i < positionsRelativeToCOM.rows(); ++i) {
    velocities_.row(i) -= angularVelocityVector.cross(positionsRelativeToCOM.row(i));
  }
}
} // namespace Utils
} // namespace Scine


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_FEATURES_MATCHING_GENERAL_CORNER_DETECTOR_HPP
#define PIC_FEATURES_MATCHING_GENERAL_CORNER_DETECTOR_HPP

#include "../image.hpp"
#include "../util/string.hpp"
#include "../util/string.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief The GeneralCornerDetector class
 */
class GeneralCornerDetector
{
protected:
    Image *lum;
    bool bLum;

    static bool scD (Eigen::Vector3f i, Eigen::Vector3f  j)
    {
        return (i[2] > j [2]);
    }

    static bool scA (Eigen::Vector3f i, Eigen::Vector3f  j)
    {
        return (i[2] < j [2]);
    }

public:
    /**
     * @brief GeneralCornerDetector
     */
    GeneralCornerDetector()
    {
        lum = NULL;
        bLum = false;
    }

    ~GeneralCornerDetector()
    {
    }

    /**
     * @brief execute
     * @param img
     * @param corners
     */
    virtual void execute(Image *img, std::vector< Eigen::Vector2f > *corners)
    {

    }

    /**
     * @brief getCornersImage
     * @param corners
     * @param imgOut
     * @param bColor
     * @return
     */
    Image *getCornersImage(std::vector< Eigen::Vector2f > *corners,
                              Image *imgOut, unsigned int width, unsigned int height, bool bColor)
    {
        if(corners == NULL) {
            return imgOut;
        }

        if(imgOut == NULL) {
            if((width < 1) || (height < 1)){
                return NULL;
            }

            imgOut = new Image(width, height, 1);
        }

        imgOut->setZero();

        for(unsigned int i = 0; i < corners->size(); i++) {
            int x = int((*corners)[i][0]);
            int y = int((*corners)[i][1]);

            if(bColor) {
                (*imgOut)(x, y)[0] = 1.0f;
            } else {
                (*imgOut)(x, y)[0] = (*corners)[i][2];
            }
        }

        return imgOut;
    }

    /**
     * @brief sortCornersAndTransfer
     * @param corners
     * @param cornersOut
     * @param bDescend
     */
    static void sortCornersAndTransfer(std::vector< Eigen::Vector3f > *corners, std::vector< Eigen::Vector2f > *cornersOut, bool bDescend = true)
    {
        sortCorners(corners, bDescend);

        for(size_t i = 0; i < corners->size(); i++) {
            Eigen::Vector3f tmp = corners->at(i);

            Eigen::Vector2f p;
            p[0] = tmp[0];
            p[1] = tmp[1];

            cornersOut->push_back(p);
        }
    }

    /**
     * @brief sortCorners
     * @param corners
     */
    static void sortCorners(std::vector< Eigen::Vector3f > *corners, bool bDescend = true)
    {
        if(bDescend) {
            std::sort(corners->begin(), corners->end(), scD);
        } else {
            std::sort(corners->begin(), corners->end(), scA);
        }
    }

    /**
     * @brief exportToString
     * @param corners
     * @return
     */
    static std::string exportToString(std::vector< Eigen::Vector2f > *corners)
    {
        std::string out = "[";
        auto n = corners->size();

        for(auto i = 0; i < (n - 1); i++) {
            out += fromNumberToString(corners->at(i)[0]) + " ";
            out += fromNumberToString(corners->at(i)[1]) + "; ";
        }
        out += fromNumberToString(corners->at(n - 1)[0]) + " ";
        out += fromNumberToString(corners->at(n - 1)[1]) + "]";
        return out;
    }

    /**
     * @brief removeClosestCorners
     */
    static void removeClosestCorners(std::vector< Eigen::Vector2f > *corners,
                                     std::vector< Eigen::Vector2f > *out,
                                     float threshold,
                                     int max_limit)
    {
        int n = MIN(int(corners->size()), max_limit);
        bool *processed = new bool [n];
        memset(processed, 0, sizeof(bool) * n);

        for(int i = 0; i < n; i++) {
            //find the closest
            if(!processed[i]) {
                processed[i] = true;

                std::vector< int > indices;
                for(int j = 0; j < n; j++) {
                    if(j != i) {
                        continue;
                    }

                    if(!processed[j]) {
                        float dx = (*corners)[j][0] - (*corners)[i][0];
                        float dy = (*corners)[j][1] - (*corners)[i][1];
                        float dist = sqrtf(dx * dx + dy * dy);

                        if(dist < threshold) {
                            //processed[j] = true;
                            indices.push_back(j);
                        }
                    }
                }

                auto n_i = indices.size();
                Eigen::Vector2f point;

                if(n_i > 0) {
                    point[0] = (*corners)[i][0];
                    point[1] = (*corners)[i][1];
                    int point_c = 1;

                    for(auto j = 0; j < indices.size(); j++) {
                        auto k = indices[j];
                        if(!processed[k]) {
                            processed[k] = true;
                            point[0] += (*corners)[k][0];
                            point[1] += (*corners)[k][1];
                            point_c++;
                        }
                    }

                    point[0] /= float(point_c);
                    point[1] /= float(point_c);
                } else {
                    point[0] = (*corners)[i][0];
                    point[1] = (*corners)[i][1];
                }
                out->push_back(point);
            }
        }

        delete[] processed;
    }

    /**
     * @brief test
     * @param gcd
     */
    static void test(GeneralCornerDetector *gcd)
    {
        if(gcd == NULL){
            return;
        }

        Image full_image(1, 512, 512, 3);
        full_image.setZero();

        Image quad(1, 128, 128, 3);
        quad = 1.0f;

        full_image.copySubImage(&quad, 192, 192);

        std::vector< Eigen::Vector2f > corners;
        gcd->execute(&full_image, &corners);

        printf("\n Corner Detector Test:\n");

        for(unsigned int i = 0; i < corners.size(); i++) {
            printf("X: %f Y: %f\n", corners[i][0], corners[i][1]);
        }

        printf("\n");

        Image *img_corners = gcd->getCornersImage(&corners, NULL, 512, 512, true);
        img_corners->Write("general_corner_test_image.hdr");
    }
};

#endif

} // end namespace pic

#endif /* PIC_FEATURES_MATCHING_GENERAL_CORNER_DETECTOR_HPP */


#include "GRU.hpp"
#include "ActFunc.hpp"
#include "Utils.hpp"
#include <Eigen/SVD>

GRU::GRU(const int inputDim, const int hiddenDim){
  this->Wxr = MatD(hiddenDim, inputDim);
  this->Whr = MatD(hiddenDim, hiddenDim);
  this->br = VecD::Zero(hiddenDim);

  this->Wxz = MatD(hiddenDim, inputDim);
  this->Whz = MatD(hiddenDim, hiddenDim);
  this->bz = VecD::Zero(hiddenDim);

  this->Wxu = MatD(hiddenDim, inputDim);
  this->Whu = MatD(hiddenDim, hiddenDim);
  this->bu = VecD::Zero(hiddenDim);
}

void GRU::init(Rand& rnd, const Real scale){
  rnd.uniform(this->Wxr, scale);
  rnd.uniform(this->Whr, scale);

  rnd.uniform(this->Wxz, scale);
  rnd.uniform(this->Whz, scale);

  rnd.uniform(this->Wxu, scale);
  rnd.uniform(this->Whu, scale);

  this->Whr = Eigen::JacobiSVD<MatD>(this->Whr, Eigen::ComputeFullV|Eigen::ComputeFullU).matrixU();
  this->Whz = Eigen::JacobiSVD<MatD>(this->Whz, Eigen::ComputeFullV|Eigen::ComputeFullU).matrixU();
  this->Whu = Eigen::JacobiSVD<MatD>(this->Whu, Eigen::ComputeFullV|Eigen::ComputeFullU).matrixU();
}

void GRU::forward(const VecD& xt, const GRU::State* prev, GRU::State* cur){
  cur->r = this->br + this->Wxr*xt + this->Whr*prev->h;
  cur->z = this->bz + this->Wxz*xt + this->Whz*prev->h;

  ActFunc::logistic(cur->r);
  ActFunc::logistic(cur->z);

  cur->rh = cur->r.array()*prev->h.array();
  cur->u = this->bu + this->Wxu*xt + this->Whu*cur->rh;
  ActFunc::tanh(cur->u);
  cur->h = (1.0-cur->z.array())*prev->h.array() + cur->z.array()*cur->u.array();
}

void GRU::backward(GRU::State* prev, GRU::State* cur, GRU::Grad& grad, const VecD& xt){
  VecD delr, delz, delu, delrh;

  delz = ActFunc::logisticPrime(cur->z).array()*cur->delh.array()*(cur->u-prev->h).array();
  delu = ActFunc::tanhPrime(cur->u).array()*cur->delh.array()*cur->z.array();
  delrh = this->Whu.transpose()*delu;
  delr = ActFunc::logisticPrime(cur->r).array()*delrh.array()*prev->h.array();

  cur->delx =
    this->Wxr.transpose()*delr+
    this->Wxz.transpose()*delz+
    this->Wxu.transpose()*delu;

  prev->delh.noalias() +=
    this->Whr.transpose()*delr+
    this->Whz.transpose()*delz;
  prev->delh.array() +=
    delrh.array()*cur->r.array()+
    cur->delh.array()*(1.0-cur->z.array());

  grad.Wxr.noalias() += delr*xt.transpose();
  grad.Whr.noalias() += delr*prev->h.transpose();

  grad.Wxz.noalias() += delz*xt.transpose();
  grad.Whz.noalias() += delz*prev->h.transpose();

  grad.Wxu.noalias() += delu*xt.transpose();
  grad.Whu.noalias() += delu*cur->rh.transpose();

  grad.br += delr;
  grad.bz += delz;
  grad.bu += delu;
}

void GRU::sgd(const GRU::Grad& grad, const Real learningRate){
  this->Wxr -= learningRate*grad.Wxr;
  this->Whr -= learningRate*grad.Whr;
  this->br -= learningRate*grad.br;

  this->Wxz -= learningRate*grad.Wxz;
  this->Whz -= learningRate*grad.Whz;
  this->bz -= learningRate*grad.bz;

  this->Wxu -= learningRate*grad.Wxu;
  this->Whu -= learningRate*grad.Whu;
  this->bu -= learningRate*grad.bu;
}

void GRU::save(std::ofstream& ofs){
  Utils::save(ofs, this->Wxr); Utils::save(ofs, this->Whr); Utils::save(ofs, this->br);
  Utils::save(ofs, this->Wxz); Utils::save(ofs, this->Whz); Utils::save(ofs, this->bz);
  Utils::save(ofs, this->Wxu); Utils::save(ofs, this->Whu); Utils::save(ofs, this->bu);
}

void GRU::load(std::ifstream& ifs){
  Utils::load(ifs, this->Wxr); Utils::load(ifs, this->Whr); Utils::load(ifs, this->br);
  Utils::load(ifs, this->Wxz); Utils::load(ifs, this->Whz); Utils::load(ifs, this->bz);
  Utils::load(ifs, this->Wxu); Utils::load(ifs, this->Whu); Utils::load(ifs, this->bu);
}

void GRU::State::clear(){
  this->h = VecD();
  this->u = VecD();
  this->r = VecD();
  this->z = VecD();
  this->rh = VecD();
  this->delh = VecD();
  this->delx = VecD();
}

GRU::Grad::Grad(const GRU& gru){
  this->Wxr = MatD::Zero(gru.Wxr.rows(), gru.Wxr.cols());
  this->Whr = MatD::Zero(gru.Whr.rows(), gru.Whr.cols());
  this->br = VecD::Zero(gru.br.rows());

  this->Wxz = MatD::Zero(gru.Wxz.rows(), gru.Wxz.cols());
  this->Whz = MatD::Zero(gru.Whz.rows(), gru.Whz.cols());
  this->bz = VecD::Zero(gru.bz.rows());

  this->Wxu = MatD::Zero(gru.Wxu.rows(), gru.Wxu.cols());
  this->Whu = MatD::Zero(gru.Whu.rows(), gru.Whu.cols());
  this->bu = VecD::Zero(gru.bu.rows());
};

void GRU::Grad::init(){
  this->Wxr.setZero(); this->Whr.setZero(); this->br.setZero();
  this->Wxz.setZero(); this->Whz.setZero(); this->bz.setZero();
  this->Wxu.setZero(); this->Whu.setZero(); this->bu.setZero();
}

Real GRU::Grad::norm(){
  return
    this->Wxr.squaredNorm()+this->Whr.squaredNorm()+this->br.squaredNorm()+
    this->Wxz.squaredNorm()+this->Whz.squaredNorm()+this->bz.squaredNorm()+
    this->Wxu.squaredNorm()+this->Whu.squaredNorm()+this->bu.squaredNorm();
}

void GRU::Grad::operator += (const GRU::Grad& grad){
  this->Wxr += grad.Wxr; this->Whr += grad.Whr; this->br += grad.br;
  this->Wxz += grad.Wxz; this->Whz += grad.Whz; this->bz += grad.bz;
  this->Wxu += grad.Wxu; this->Whu += grad.Whu; this->bu += grad.bu;
}


#include "ReconstructionImpl.h"
#include "omp.h"
#include "cgls.hpp"
#include "sirt.hpp"
#include "mlem.hpp"
#include "ui_calls.hpp"
#include "utils.hpp"
#include "voxels.hpp"
#include "blas.hpp"
#include <boost/filesystem.hpp>

CCPi::ReconstructionImpl::ReconstructionImpl(void)
{
	deviceId = CCPi::dev_Nikon_XTek;
	numberOfProcessors = omp_get_max_threads();
	algorithmId = CCPi::alg_CGLS;
	bHyperThreads = true;
	numberOfIterations = 20;
	resolution = 1;
	regularise = 0.01;
}

CCPi::ReconstructionImpl::~ReconstructionImpl(void)
{
	if(voxels!=NULL)
		delete voxels;
}

void CCPi::ReconstructionImpl::setDeviceId(devices id)
{
	deviceId = id;
}

void CCPi::ReconstructionImpl::setAlgorithmId(algorithms id)
{
	algorithmId = id;
}

void CCPi::ReconstructionImpl::setNumberOfProcessors(int noProcs)
{
	numberOfProcessors = noProcs;
}

void CCPi::ReconstructionImpl::enableHyperThreads()
{
	bHyperThreads = true;
}
void CCPi::ReconstructionImpl::disableHyperThreads()
{
	bHyperThreads = false;
}

void CCPi::ReconstructionImpl::setResolution(int resolution)
{
	this->resolution = resolution;
}
void CCPi::ReconstructionImpl::setNumberOfIterations(int iterations)
{
	numberOfIterations = iterations;
}

void CCPi::ReconstructionImpl::setRegularisation(double value)
{
	regularise = value;
}

void CCPi::ReconstructionImpl::enableBeamHardening()
{
	bBeamHardening = true;
}

void CCPi::ReconstructionImpl::disableBeamHardening()
{
	bBeamHardening = false;
}

void CCPi::ReconstructionImpl::setBeamHardening(bool value)
{
	bBeamHardening = value;
}

void CCPi::ReconstructionImpl::setFilename(std::string name)
{
	filename = name;
}

bool CCPi::ReconstructionImpl::run()
{
	if (!bHyperThreads) {
		int num_processors = numberOfProcessors / 2;
		omp_set_num_threads(num_processors);
	}else{
		omp_set_num_threads(numberOfProcessors);
	}
	bool noerror = true;

	CCPi::instrument *device = 0;

	switch (deviceId) {
	case CCPi::dev_Nikon_XTek:
		device = new CCPi::Nikon_XTek;
		break;
	default:
		noerror = false;
		break;
	}

	CCPi::reconstruction_alg *recon_algorithm = 0;
	switch (algorithmId) {
	case CCPi::alg_CGLS:
	  recon_algorithm = new CCPi::cgls_3d(numberOfIterations);
	  break;
	case CCPi::alg_SIRT:
	  recon_algorithm = new CCPi::sirt(numberOfIterations);
	  break;
	case CCPi::alg_MLEM:
	  recon_algorithm = new CCPi::mlem(numberOfIterations);
	  break;
	case CCPi::alg_CGLS_Tikhonov:
	  recon_algorithm = new CCPi::cgls_tikhonov(numberOfIterations, regularise);
	  break;
	case CCPi::alg_CGLS_TVreg:
	  recon_algorithm = new CCPi::cgls_tv_reg(numberOfIterations, regularise);
	  break;
	default:
		noerror = false;
	}

	if (noerror) {
		noerror = false;
		bool phantom = false;
		real rotation_centre = -1.0;
		boost::filesystem::path p(filename);
		if (device->setup_experimental_geometry(p.parent_path().generic_string(), p.filename().generic_string(), rotation_centre, resolution, phantom)) {
		  int nx_voxels = 0;
		  int ny_voxels = 0;
		  int maxz_voxels = 0;
		  int nz_voxels = 0;
		  int block_size = 0;
		  int block_step = 0;
		  calculate_block_sizes(nx_voxels, ny_voxels, nz_voxels, maxz_voxels, block_size, block_step, 1,
		    0, resolution, device, recon_algorithm->supports_blocks());
		  int z_data_size = block_size * resolution;
		  int z_data_step = block_step * resolution;
		  device->set_v_block(z_data_size);
		  int block_offset = 0;
		  int z_data_offset = block_offset * resolution;
				if (device->finish_voxel_geometry(voxel_origin, voxel_size, nx_voxels, ny_voxels, nz_voxels)) {
					if (device->read_scans(p.parent_path().generic_string(), 0, z_data_size, true, phantom)) {
						voxels = new voxel_data(boost::extents[nx_voxels][ny_voxels][nz_voxels], boost::c_storage_order());
						init_data(*voxels, nx_voxels, ny_voxels, nz_voxels);
						if (bBeamHardening)
							device->apply_beam_hardening();
						noerror = recon_algorithm->reconstruct(device, *voxels, voxel_origin, voxel_size);
					}
				}
		}
	}
	if (!noerror)
		delete voxels;
	return noerror;
}

bool CCPi::ReconstructionImpl::saveResults(std::string outputFilename, CCPi::output_format outputFormat)
{
	bool clamp_output = true;
	const voxel_data::size_type *s = voxels->shape();
	clamp_min(*voxels, 0.0, s[0], s[1], s[2]);
	CCPi::write_results(outputFilename, *voxels, voxel_origin, voxel_size, 0, (int)s[2], outputFormat, clamp_output);
	return true;
}


//
//  GlExtras.cpp
//  Kepler
//
//  Created by Robert Hodgin on 4/7/11.
//  Copyright 2013 Smithsonian Institution. All rights reserved.
//

#include <boost/foreach.hpp>
#include "BloomGl.h"
#include "cinder/Quaternion.h"

using namespace ci;

namespace bloom { namespace gl {

//	void drawBillboard( const Vec3f &pos, const Vec2f &scale, float rotationDegrees, const Vec3f &bbRight, const Vec3f &bbUp )
//	{
//		glEnableClientState( GL_VERTEX_ARRAY );
//		Vec3f verts[4];
//		glVertexPointer( 3, GL_FLOAT, 0, &verts[0].x );
//		glEnableClientState( GL_TEXTURE_COORD_ARRAY );
//		GLfloat texCoords[8] = { 0, 0, 0, 1, 1, 0, 1, 1 };
//		glTexCoordPointer( 2, GL_FLOAT, 0, texCoords );
//
//		float sinA = math<float>::sin( toRadians( rotationDegrees ) );
//		float cosA = math<float>::cos( toRadians( rotationDegrees ) );
//
//		verts[0] = pos + bbRight * ( -0.5f * scale.x * cosA - 0.5f * sinA * scale.y ) + bbUp * ( -0.5f * scale.x * sinA + 0.5f * cosA * scale.y );
//		verts[1] = pos + bbRight * ( -0.5f * scale.x * cosA - -0.5f * sinA * scale.y ) + bbUp * ( -0.5f * scale.x * sinA + -0.5f * cosA * scale.y );
//		verts[2] = pos + bbRight * ( 0.5f * scale.x * cosA - 0.5f * sinA * scale.y ) + bbUp * ( 0.5f * scale.x * sinA + 0.5f * cosA * scale.y );
//		verts[3] = pos + bbRight * ( 0.5f * scale.x * cosA - -0.5f * sinA * scale.y ) + bbUp * ( 0.5f * scale.x * sinA + -0.5f * cosA * scale.y );
//
//		glDrawArrays( GL_TRIANGLE_STRIP, 0, 4 );
//
//		glDisableClientState( GL_VERTEX_ARRAY );
//		glDisableClientState( GL_TEXTURE_COORD_ARRAY );
//	}

	void drawBillboard( const Vec3f &pos, const Vec2f &scale, float rotInRadians, const Vec3f &bbRight, const Vec3f &bbUp )
	{
		glEnableClientState( GL_VERTEX_ARRAY );
		Vec3f verts[4];
		glVertexPointer( 3, GL_FLOAT, 0, &verts[0].x );
		glEnableClientState( GL_TEXTURE_COORD_ARRAY );
		GLfloat texCoords[8] = { 0, 0, 0, 1, 1, 0, 1, 1 };
		glTexCoordPointer( 2, GL_FLOAT, 0, texCoords );

		float sinA = math<float>::sin( rotInRadians );
		float cosA = math<float>::cos( rotInRadians );

		float scaleXCosA = 0.5f * scale.x * cosA;
		float scaleXSinA = 0.5f * scale.x * sinA;
		float scaleYSinA = 0.5f * scale.y * sinA;
		float scaleYCosA = 0.5f * scale.y * cosA;
		verts[0] = pos + bbRight * ( -scaleXCosA - scaleYSinA ) + bbUp * ( -scaleXSinA + scaleYCosA );
		verts[1] = pos + bbRight * ( -scaleXCosA + scaleYSinA ) + bbUp * ( -scaleXSinA - scaleYCosA );
		verts[2] = pos + bbRight * (  scaleXCosA - scaleYSinA ) + bbUp * (  scaleXSinA + scaleYCosA );
		verts[3] = pos + bbRight * (  scaleXCosA + scaleYSinA ) + bbUp * (  scaleXSinA - scaleYCosA );

		glDrawArrays( GL_TRIANGLE_STRIP, 0, 4 );

		glDisableClientState( GL_VERTEX_ARRAY );
		glDisableClientState( GL_TEXTURE_COORD_ARRAY );
	}

	void drawSphericalBillboard( const Vec3f &camEye, const Vec3f &objPos, const Vec2f &scale, float rotInRadians )
	{
		glPushMatrix();
		glTranslatef( objPos.x, objPos.y, objPos.z );

		Vec3f lookAt = Vec3f::zAxis();
		Vec3f upAux;
		float angleCosine;

		Vec3f objToCam = ( camEye - objPos ).normalized();
		Vec3f objToCamProj = Vec3f( objToCam.x, 0.0f, objToCam.z );
		objToCamProj.normalize();

		upAux = lookAt.cross( objToCamProj );

// Cylindrical billboarding
		angleCosine = constrain( lookAt.dot( objToCamProj ), -1.0f, 1.0f );
		glRotatef( toDegrees( acos(angleCosine) ), upAux.x, upAux.y, upAux.z );

// Spherical billboarding
		angleCosine = constrain( objToCamProj.dot( objToCam ), -1.0f, 1.0f );
		if( objToCam.y < 0 )	glRotatef( toDegrees( acos(angleCosine) ), 1.0f, 0.0f, 0.0f );
		else					glRotatef( toDegrees( acos(angleCosine) ),-1.0f, 0.0f, 0.0f );

		Vec3f verts[4];
		GLfloat texCoords[8] = { 0, 0, 0, 1, 1, 0, 1, 1 };

		glEnableClientState( GL_VERTEX_ARRAY );
		glEnableClientState( GL_TEXTURE_COORD_ARRAY );
		glVertexPointer( 3, GL_FLOAT, 0, &verts[0].x );
		glTexCoordPointer( 2, GL_FLOAT, 0, texCoords );

		float sinA = math<float>::sin( rotInRadians );
		float cosA = math<float>::cos( rotInRadians );

		float scaleXCosA = 0.5f * scale.x * cosA;
		float scaleXSinA = 0.5f * scale.x * sinA;
		float scaleYSinA = 0.5f * scale.y * sinA;
		float scaleYCosA = 0.5f * scale.y * cosA;

		verts[0] = Vec3f( ( -scaleXCosA - scaleYSinA ), ( -scaleXSinA + scaleYCosA ), 0.0f );
		verts[1] = Vec3f( ( -scaleXCosA + scaleYSinA ), ( -scaleXSinA - scaleYCosA ), 0.0f );
		verts[2] = Vec3f( (  scaleXCosA - scaleYSinA ), (  scaleXSinA + scaleYCosA ), 0.0f );
		verts[3] = Vec3f( (  scaleXCosA + scaleYSinA ), (  scaleXSinA - scaleYCosA ), 0.0f );

		glDrawArrays( GL_TRIANGLE_STRIP, 0, 4 );

		glDisableClientState( GL_VERTEX_ARRAY );
		glDisableClientState( GL_TEXTURE_COORD_ARRAY );

//		glDisable( GL_TEXTURE_2D );
//		ci::gl::color( Color( 1.0f, 1.0f, 1.0f ) );
//		ci::gl::drawLine( Vec3f::zero(), objToCam );
//		glEnable( GL_TEXTURE_2D );

		glPopMatrix();
	}

    void drawSphericalRotatedBillboard( const ci::Vec3f &pos, const ci::Vec3f &lookAt, const ci::Vec3f &turnAt, const ci::Vec2f &scale )
    {
        glPushMatrix();

        // hacked together from three.js's Matrix4.lookAt...

		Vec3f z = ( pos - lookAt ).normalized();

		if ( z.length() == 0 ) {
			z.z = 1;
		}

        Vec3f up = turnAt - pos;

		Vec3f x = up.cross(z).normalized();

		if ( x.length() == 0 ) {
			z.x += 0.0001;
			x = up.cross(z).normalized();
		}

        Vec3f y = z.cross(x).normalized();

        float m[16];
        m[ 0] = x.x; m[ 4] = y.x; m[ 8] = z.x; m[12] = pos.x;
        m[ 1] = x.y; m[ 5] = y.y; m[ 9] = z.y; m[13] = pos.y;
        m[ 2] = x.z; m[ 6] = y.z; m[10] = z.z; m[14] = pos.z;
        m[ 3] = 0;   m[ 7] = 0;   m[11] = 0;   m[15] = 1;

        glMultMatrixf(m);

        ///////////////// and now we just get to draw a square
        // ... might be worth pre-multiplying the verts to avoid the push/mult/pop entirely?
        // ... or setting these up as a VBO since they're always the same
        // ... or batching everything that shares a texture into one billboard array

        ci::Vec2f verts[4];
		GLfloat texCoords[8] = { 0, 0, 0, 1, 1, 0, 1, 1 };

		glEnableClientState( GL_VERTEX_ARRAY );
		glEnableClientState( GL_TEXTURE_COORD_ARRAY );
		glVertexPointer( 2, GL_FLOAT, 0, &verts[0].x );
		glTexCoordPointer( 2, GL_FLOAT, 0, texCoords );

		verts[0] = ci::Vec2f(-0.5f,-0.5f) * scale;
		verts[1] = ci::Vec2f(-0.5f, 0.5f) * scale;
		verts[2] = ci::Vec2f( 0.5f,-0.5f) * scale;
		verts[3] = ci::Vec2f( 0.5f, 0.5f) * scale;

		glDrawArrays( GL_TRIANGLE_STRIP, 0, 4 );

		glDisableClientState( GL_VERTEX_ARRAY );
		glDisableClientState( GL_TEXTURE_COORD_ARRAY );

        glPopMatrix();
    }

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

    // FIXME: whither ordered_map?
    boost::unordered_map<GLuint, BatchRef> batchByTex;
    std::vector<BatchRef> batches;

    void beginBatch()
    {
        batchByTex.clear();
        batches.clear();
    }

    void batchRect( const ci::gl::Texture &texture, const ci::Rectf &srcRect, const ci::Rectf &dstRect )
    {
        GLuint texId = texture.getId();
        boost::unordered_map<GLuint, BatchRef>::iterator iter = batchByTex.find( texId );
        BatchRef batch;
        if (iter != batchByTex.end()) {
            batch = iter->second;
        }
        else {
            batch = BatchRef(new Batch());
            batch->texture = texture;
            batches.push_back(batch);
            batchByTex[texId] = batch;
        }
        int verts = batch->vertices.size();
        batch->vertices.resize(verts + 6);
        batch->vertices[verts].vertex  = ci::Vec2f(dstRect.x1, dstRect.y1);
        batch->vertices[verts].texture = ci::Vec2f(srcRect.x1, srcRect.y1);
        verts++;
        batch->vertices[verts].vertex  = ci::Vec2f(dstRect.x2, dstRect.y1);
        batch->vertices[verts].texture = ci::Vec2f(srcRect.x2, srcRect.y1);
        verts++;
        batch->vertices[verts].vertex  = ci::Vec2f(dstRect.x2, dstRect.y2);
        batch->vertices[verts].texture = ci::Vec2f(srcRect.x2, srcRect.y2);
        verts++;
        batch->vertices[verts].vertex  = ci::Vec2f(dstRect.x1, dstRect.y1);
        batch->vertices[verts].texture = ci::Vec2f(srcRect.x1, srcRect.y1);
        verts++;
        batch->vertices[verts].vertex  = ci::Vec2f(dstRect.x1, dstRect.y2);
        batch->vertices[verts].texture = ci::Vec2f(srcRect.x1, srcRect.y2);
        verts++;
        batch->vertices[verts].vertex  = ci::Vec2f(dstRect.x2, dstRect.y2);
        batch->vertices[verts].texture = ci::Vec2f(srcRect.x2, srcRect.y2);
        //verts++;
    }

    void batchRect( const ci::gl::Texture &texture, const ci::Area &srcArea, const ci::Rectf &dstRect )
    {
        batchRect( texture, texture.getAreaTexCoords( srcArea ), dstRect );
    }

    void batchRect( const ci::gl::Texture &texture, const ci::Vec2f &pos )
    {
        batchRect( texture, texture.getCleanBounds(), ci::Rectf(pos.x, pos.y, pos.x + texture.getWidth(), pos.y + texture.getHeight()) );
    }

    void endBatch()
    {
        glEnableClientState(GL_VERTEX_ARRAY);
        glEnableClientState(GL_TEXTURE_COORD_ARRAY);
        BOOST_FOREACH(BatchRef batch, batches) {
            batch->texture.enableAndBind();
            glVertexPointer(2, GL_FLOAT, sizeof(VertexData), &batch->vertices[0].vertex);
            glTexCoordPointer(2, GL_FLOAT, sizeof(VertexData), &batch->vertices[0].texture);
            glDrawArrays(GL_TRIANGLES, 0, batch->vertices.size());
            batch->texture.disable();
        }
        glDisableClientState(GL_VERTEX_ARRAY);
        glDisableClientState(GL_TEXTURE_COORD_ARRAY);
    }

} }