/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2011 Artem Pavlenko
 *
 * This library 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 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_EXPRESSION_NODE_HPP
#define MAPNIK_EXPRESSION_NODE_HPP

// mapnik
#include <mapnik/value.hpp>
#include <mapnik/attribute.hpp>
#include <mapnik/expression_node_types.hpp>

// boost
#include <boost/regex.hpp>
#if defined(BOOST_REGEX_HAS_ICU)
#include <boost/regex/icu.hpp>
#endif
#include <boost/function.hpp>

namespace mapnik
{

template <typename Tag> struct make_op;
template <> struct make_op<tags::negate> { typedef std::negate<value_type> type;};
template <> struct make_op<tags::plus> { typedef std::plus<value_type> type;};
template <> struct make_op<tags::minus> { typedef std::minus<value_type> type;};
template <> struct make_op<tags::mult> { typedef std::multiplies<value_type> type;};
template <> struct make_op<tags::div> { typedef std::divides<value_type> type;};
template <> struct make_op<tags::mod> { typedef std::modulus<value_type> type;};
template <> struct make_op<tags::less> { typedef std::less<value_type> type;};
template <> struct make_op<tags::less_equal> { typedef std::less_equal<value_type> type;};
template <> struct make_op<tags::greater> { typedef std::greater<value_type> type;};
template <> struct make_op<tags::greater_equal> { typedef std::greater_equal<value_type> type;};
template <> struct make_op<tags::equal_to> { typedef std::equal_to<value_type> type;};
template <> struct make_op<tags::not_equal_to> { typedef std::not_equal_to<value_type> type;};
template <> struct make_op<tags::logical_not> { typedef std::logical_not<value_type> type;};
template <> struct make_op<tags::logical_and> { typedef std::logical_and<value_type> type;};
template <> struct make_op<tags::logical_or> { typedef std::logical_or<value_type> type;};

template <typename Tag>
struct unary_node
{
    unary_node (expr_node const& a)
        : expr(a) {}

    static const char* type()
    {
        return Tag::str();
    }

    expr_node expr;
};

template <typename Tag>
struct binary_node
{
    binary_node(expr_node const& a, expr_node const& b)
        : left(a),
          right(b) {}

    static const char* type()
    {
        return Tag::str();
    }
    expr_node left,right;
};

#if defined(BOOST_REGEX_HAS_ICU)

struct regex_match_node
{
    regex_match_node (expr_node const& a, UnicodeString const& ustr);
    expr_node expr;
    boost::u32regex pattern;
};

struct regex_replace_node
{
    regex_replace_node (expr_node const& a, UnicodeString const& ustr, UnicodeString const& f);
    expr_node expr;
    boost::u32regex pattern;
    UnicodeString format;
};

#else

struct regex_match_node
{
    regex_match_node (expr_node const& a, std::string const& str);
    expr_node expr;
    boost::regex pattern;
};

struct regex_replace_node
{
    regex_replace_node (expr_node const& a, std::string const& str, std::string const& f);
    expr_node expr;
    boost::regex pattern;
    std::string format;
};
#endif

struct function_call
{
    template<typename Fun>
    explicit function_call (expr_node const a, Fun f)
        : expr(a),
          call_(f) {}

    expr_node expr;
    boost::function<value_type(value_type)> call_;
};

// ops

inline expr_node& operator- (expr_node& expr)
{
    return expr = unary_node<tags::negate>(expr);
}

inline expr_node & operator += ( expr_node &left ,const expr_node &right)
{
    return left =  binary_node<tags::plus>(left,right);
}

inline expr_node & operator -= ( expr_node &left ,const expr_node &right)
{
    return left =  binary_node<tags::minus>(left,right);
}

inline expr_node & operator *= ( expr_node &left ,const expr_node &right)
{
    return left =  binary_node<tags::mult>(left,right);
}

inline expr_node & operator /= ( expr_node &left ,const expr_node &right)
{
    return left =  binary_node<tags::div>(left,right);
}

inline expr_node & operator %= ( expr_node &left ,const expr_node &right)
{
    return left = binary_node<tags::mod>(left,right);
}

inline expr_node & operator < ( expr_node &left, expr_node const& right)
{
    return left = binary_node<tags::less>(left,right);
}

inline expr_node & operator <= ( expr_node &left, expr_node const& right)
{
    return left = binary_node<tags::less_equal>(left,right);
}

inline expr_node & operator > ( expr_node &left, expr_node const& right)
{
    return left = binary_node<tags::greater>(left,right);
}

inline expr_node & operator >= ( expr_node &left, expr_node const& right)
{
    return left = binary_node<tags::greater_equal>(left,right);
}

inline expr_node & operator == ( expr_node &left, expr_node const& right)
{
    return left = binary_node<tags::equal_to>(left,right);
}

inline expr_node & operator != ( expr_node &left, expr_node const& right)
{
    return left = binary_node<tags::not_equal_to>(left,right);
}

inline expr_node & operator ! (expr_node & expr)
{
    return expr = unary_node<tags::logical_not>(expr);
}

inline expr_node & operator && ( expr_node &left, expr_node const& right)
{
    return left = binary_node<tags::logical_and>(left,right);
}

inline expr_node & operator || ( expr_node &left, expr_node const& right)
{
    return left = binary_node<tags::logical_or>(left,right);
}

}

#endif //MAPNIK_EXPRESSION_NODE_HPP


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  testRangeFactor.cpp
 *  @brief Unit tests for RangeFactor Class
 *  @author Stephen Williams
 *  @date Oct 2012
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp>

using namespace std;
using namespace gtsam;

// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(1));

typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;

/* ************************************************************************* */
LieVector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) {
  return factor.evaluateError(pose, point);
}

/* ************************************************************************* */
LieVector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) {
  return factor.evaluateError(pose, point);
}

/* ************************************************************************* */
TEST( RangeFactor, Constructor) {
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);

  RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
  RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
}

/* ************************************************************************* */
TEST( RangeFactor, ConstructorWithTransform) {
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));

  RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D);
  RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D);
}

/* ************************************************************************* */
TEST( RangeFactor, Equals ) {
  // Create two identical factors and make sure they're equal
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);

  RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model);
  RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model);
  CHECK(assert_equal(factor2D_1, factor2D_2));

  RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model);
  RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model);
  CHECK(assert_equal(factor3D_1, factor3D_2));
}

/* ************************************************************************* */
TEST( RangeFactor, EqualsWithTransform ) {
  // Create two identical factors and make sure they're equal
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));

  RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D);
  RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D);
  CHECK(assert_equal(factor2D_1, factor2D_2));

  RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D);
  RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D);
  CHECK(assert_equal(factor3D_1, factor3D_2));
}

/* ************************************************************************* */
TEST( RangeFactor, Error2D ) {
  // Create a factor
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  RangeFactor2D factor(poseKey, pointKey, measurement, model);

  // Set the linearization point
  Pose2 pose(1.0, 2.0, 0.57);
  Point2 point(-4.0, 11.0);

  // Use the factor to calculate the error
  Vector actualError(factor.evaluateError(pose, point));

  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
  Vector expectedError = (Vector(1) << 0.295630141);

  // Verify we get the expected error
  CHECK(assert_equal(expectedError, actualError, 1e-9));
}

/* ************************************************************************* */
TEST( RangeFactor, Error2DWithTransform ) {
  // Create a factor
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
  RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor);

  // Set the linearization point
  Rot2 R(0.57);
  Point2 t = Point2(1.0, 2.0) - R.rotate(body_P_sensor.translation());
  Pose2 pose(R, t);
  Point2 point(-4.0, 11.0);

  // Use the factor to calculate the error
  Vector actualError(factor.evaluateError(pose, point));

  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
  Vector expectedError = (Vector(1) << 0.295630141);

  // Verify we get the expected error
  CHECK(assert_equal(expectedError, actualError, 1e-9));
}

/* ************************************************************************* */
TEST( RangeFactor, Error3D ) {
  // Create a factor
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  RangeFactor3D factor(poseKey, pointKey, measurement, model);

  // Set the linearization point
  Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
  Point3 point(-2.0, 11.0, 1.0);

  // Use the factor to calculate the error
  Vector actualError(factor.evaluateError(pose, point));

  // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
  Vector expectedError = (Vector(1) << 0.295630141);

  // Verify we get the expected error
  CHECK(assert_equal(expectedError, actualError, 1e-9));
}

/* ************************************************************************* */
TEST( RangeFactor, Error3DWithTransform ) {
  // Create a factor
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
  RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);

  // Set the linearization point
  Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
  Point3 t = Point3(1.0, 2.0, -3.0) - R.rotate(body_P_sensor.translation());
  Pose3 pose(R, t);
  Point3 point(-2.0, 11.0, 1.0);

  // Use the factor to calculate the error
  Vector actualError(factor.evaluateError(pose, point));

  // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
  Vector expectedError = (Vector(1) << 0.295630141);

  // Verify we get the expected error
  CHECK(assert_equal(expectedError, actualError, 1e-9));
}

/* ************************************************************************* */
TEST( RangeFactor, Jacobian2D ) {
  // Create a factor
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  RangeFactor2D factor(poseKey, pointKey, measurement, model);

  // Set the linearization point
  Pose2 pose(1.0, 2.0, 0.57);
  Point2 point(-4.0, 11.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual;
  factor.evaluateError(pose, point, H1Actual, H2Actual);

  // Use numerical derivatives to calculate the Jacobians
  Matrix H1Expected, H2Expected;
  H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
  H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}

/* ************************************************************************* */
TEST( RangeFactor, Jacobian2DWithTransform ) {
  // Create a factor
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
  RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor);

  // Set the linearization point
  Rot2 R(0.57);
  Point2 t = Point2(1.0, 2.0) - R.rotate(body_P_sensor.translation());
  Pose2 pose(R, t);
  Point2 point(-4.0, 11.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual;
  factor.evaluateError(pose, point, H1Actual, H2Actual);

  // Use numerical derivatives to calculate the Jacobians
  Matrix H1Expected, H2Expected;
  H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
  H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}

/* ************************************************************************* */
TEST( RangeFactor, Jacobian3D ) {
  // Create a factor
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  RangeFactor3D factor(poseKey, pointKey, measurement, model);

  // Set the linearization point
  Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
  Point3 point(-2.0, 11.0, 1.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual;
  factor.evaluateError(pose, point, H1Actual, H2Actual);

  // Use numerical derivatives to calculate the Jacobians
  Matrix H1Expected, H2Expected;
  H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
  H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}

/* ************************************************************************* */
TEST( RangeFactor, Jacobian3DWithTransform ) {
  // Create a factor
  Key poseKey(1);
  Key pointKey(2);
  double measurement(10.0);
  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
  RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);

  // Set the linearization point
  Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
  Point3 t = Point3(1.0, 2.0, -3.0) - R.rotate(body_P_sensor.translation());
  Pose3 pose(R, t);
  Point3 point(-2.0, 11.0, 1.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual;
  factor.evaluateError(pose, point, H1Actual, H2Actual);

  // Use numerical derivatives to calculate the Jacobians
  Matrix H1Expected, H2Expected;
  H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
  H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


// Copyright  (C)  2009  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library 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 2.1 of the License, or (at your option) any later version.

// This library 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
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#ifndef KDL_ARTICULATEDBODYINERTIA_HPP
#define KDL_ARTICULATEDBODYINERTIA_HPP

#include "frames.hpp"

#include "rotationalinertia.hpp"
#include "rigidbodyinertia.hpp"

#include <Eigen/Core>

namespace KDL {

    /**
     *	\brief 6D Inertia of a articulated body
     *
     *	The inertia is defined in a certain reference point and a certain reference base.
     *	The reference point does not have to coincide with the origin of the reference frame.
     */
    class ArticulatedBodyInertia{
    public:

        /**
         * 	This constructor creates a zero articulated body inertia matrix,
         */
        ArticulatedBodyInertia(){
            *this=ArticulatedBodyInertia::Zero();
        }

        /**
         * 	This constructor creates a cartesian space articulated body inertia matrix,
         * 	the arguments is a rigid body inertia.
         */
        ArticulatedBodyInertia(const RigidBodyInertia& rbi);

        /**
         * 	This constructor creates a cartesian space inertia matrix,
         * 	the arguments are the mass, the vector from the reference point to cog and the rotational inertia in the cog.
         */
        explicit ArticulatedBodyInertia(double m, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());

        /**
         * Creates an inertia with zero mass, and zero RotationalInertia
         */
        static inline ArticulatedBodyInertia Zero(){
            return ArticulatedBodyInertia(Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero());
        };

        ~ArticulatedBodyInertia(){};

        friend ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I);
        friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
        friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
        friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
        friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
        friend Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t);
        friend ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I);
        friend ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I);

        /**
         * Reference point change with v the vector from the old to
         * the new point expressed in the current reference frame
         */
        ArticulatedBodyInertia RefPoint(const Vector& p);

        ArticulatedBodyInertia(const Eigen::Matrix3d& M,const Eigen::Matrix3d& H,const Eigen::Matrix3d& I);

        Eigen::Matrix3d M;
        Eigen::Matrix3d H;
        Eigen::Matrix3d I;
    };

    /**
     * Scalar product: I_new = double * I_old
     */
    ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I);
    /**
     * addition I: I_new = I_old1 + I_old2, make sure that I_old1
     * and I_old2 are expressed in the same reference frame/point,
     * otherwise the result is worth nothing
     */
    ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
    ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
    ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
    ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);

    /**
     * calculate spatial momentum: h = I*v
     * make sure that the twist v and the inertia are expressed in the same reference frame/point
     */
    Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t);

    /**
     * Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b.
     */
    ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I);
    /**
     * Reference frame orientation change Ia = R_a_b*Ib with R_a_b
     * the rotation of b expressed in a
     */
    ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I);

}
#endif


/// @file
///
/// Day 07: Recursive Circus
///

#include <algorithm>
#include <iostream>
#include <regex>
#include <stdexcept>

#include <boost/algorithm/string/replace.hpp>

#include "advent/days/07/recursive_circus.hh"
#include "advent/utils/container.hh"

namespace advent {

using namespace std::string_literals;

// Override
void Day07::set_up() {
	db = file_to_database("day_07_input.txt");
}

// Override.
void Day07::solve_part_one() {
	root = solve_part_one_for_input(db);

	std::cout << part_one()
			<< root
		<< std::endl;
}

// Override.
void Day07::solve_part_two() {
	std::cout << part_two()
			<< solve_part_two_for_input(db, root)
		<< std::endl;
}

// Override.
std::string Day07::part_one() const {
	return __COMPACT_PRETTY_FUNCTION__;
}

// Override.
std::string Day07::part_two() const {
	return __COMPACT_PRETTY_FUNCTION__;
}

///
/// Finds the name of the root of the loaded tree.
///
std::string Day07::solve_part_one_for_input(const DB& db) {
	for(const auto& look_for : db) {
		bool is_root = true;
		for (const auto& look_in : db) {
			auto children = look_in.second.children;
			if (children.size()) {
				if (std::find( children.begin(), children.end(), look_for.first)
						!= children.end()) {
					is_root = false;
					break;
				}
			}
		}
		if (is_root) {
			return look_for.first;
		}
	}
	return "";
}

///
/// Finds the correct weight for the sole badly weigted node.
///
int Day07::solve_part_two_for_input(const DB& db, const std::string& root) {
	bool found = false;
	auto result = find_correct_node_weight(db, root, found);
	assert(found);
	return result;
}

///
/// Converts string to integer.
///
int Day07::weight_to_int(const std::string& weight_str) {
	std::regex rx(R"(\((\w+)\))");
	std::smatch match;
	if (std::regex_search(weight_str.begin(), weight_str.end(), match, rx)) {
		return std::stoi(match[1]);
	}
	return -1;
}

///
/// Converts lines to a database representation.
///
DB Day07::lines_to_database(Lines<>& lines) {
	DB db;
	for (auto& line : lines) {
		line.erase(std::remove(line.begin(), line.end(), ','), line.end());
		auto split_line = split(line);
		if (split_line.size() < 2) {
			throw std::logic_error("line is too short");
		}

		std::vector<std::string> children{};
		if (split_line.size() > 3) {
			auto it = split_line.begin() + 2;

			if (*it != "->") {
				throw std::logic_error("3rd parameter should be ->");
			}

			while (++it != split_line.end()) {
				children.push_back(*it);
			}
		}

		db[split_line[0]] = DBEntry{weight_to_int(split_line[1]), children};
	}
	return db;
}

///
/// Read all lines from a file and coverts those lines to a database representation.
///
DB Day07::file_to_database(const std::string& file_name) {
	auto lines = read_lines_from_file<std::string>(file_name);
	return lines_to_database(lines);
}

///
/// Locates the node that has incorrect weight and calculates its proper value.
///
int Day07::find_correct_node_weight(const DB& db, const std::string& node, bool& found) {
	if (!contains(db, node)) {
		throw std::invalid_argument("node does not exist in the database");
	}

	auto& self = db.at(node);
	int weight = self.weight;

	std::multimap<int, std::string> children_weights;
	for (const auto& child : self.children) {
		auto child_weight = find_correct_node_weight(db, child, found);
		if (found) {
			return child_weight;
		}
		weight += child_weight;
		children_weights.insert(std::make_pair(child_weight, child));
	}

	auto bad_value = find_bad_value_and_compute_difference(children_weights);
	if (bad_value) {
		auto kid = db.at(bad_value->second);
		found = true;
		return kid.weight - bad_value->first;
	}

	return weight;
}

///
/// Finds mismatched value and computes the difference between it and the correct value.
///
std::optional<Value> Day07::find_bad_value_and_compute_difference(
		std::multimap<int, std::string>& weights) {
	int difference = 0;
	int last_good = 0;
	std::string node;

	if (weights.size() < 2) {
		return std::nullopt;
	}

	for (const auto& [weight, name] : weights) {
		if (weights.count(weight) == 1) {
			difference = weight;
			node = name;
			break;
		}
		last_good = weight;
	}

	if (!difference) {
		return std::nullopt;
	}

	if (!last_good) {
		for (const auto& weight : weights) {
			if (weight.first != difference) {
				last_good = weight.first;
				break;
			}
		}
	}

	return std::optional(std::make_pair(difference - last_good, node));
}

}


#ifndef PERIOR_TREE_NODE_HPP
#define PERIOR_TREE_NODE_HPP
#include "point.hpp"
#include "aabb.hpp"

#include <boost/container/static_vector.hpp>

namespace perior
{
namespace detail
{

template<typename T, std::size_t D, std::size_t Min, std::size_t Max>
struct rtree_node
{
    typedef T value_type;
    typedef value_type scalar_type;
    static constexpr std::size_t dim = D;
    typedef point<scalar_type, dim> point_type;
    typedef aabb<scalar_type, dim>  aabb_type;

    static constexpr std::size_t max_entry = Max;
    static constexpr std::size_t min_entry = Min;

    typedef boost::container::static_vector<std::size_t, Max> container_type;
    typedef typename container_type::iterator       iterator;
    typedef typename container_type::const_iterator const_iterator;

    rtree_node(const bool is_leaf_, const std::size_t parent_)
        : is_leaf(is_leaf_), parent(parent_)
    {}
    ~rtree_node(){}

    bool has_enough_storage() const noexcept
    {return this->entry.size() != max_entry;}

    bool has_enough_entry() const noexcept
    {return this->entry.size() >= min_entry;}

    bool           is_leaf;
    std::size_t    parent;
    container_type entry;
    aabb_type      box;
};

} // detail
} // perior
#endif//PERIOR_TREE_NODE_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_IEEE_FUNCTIONS_SIMD_COMMON_SIGNNZ_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_SIGNNZ_HPP_INCLUDED

#include <boost/simd/ieee/functions/signnz.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/shrai.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/simd/sdk/config.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::signnz_, tag::cpu_,
                         (A0)(X),
                         ((simd_<integer_<A0>,X>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return b_or(shrai(a0, (sizeof(A0)*8-2)), One<A0>());
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::signnz_, tag::cpu_,
                         (A0)(X),
                         ((simd_<unsigned_<A0>,X>))
                        )
  {
    typedef A0 result_type;
    inline result_type operator()(const A0&) const
    {
      return One<A0>();
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::signnz_, tag::cpu_,
                         (A0)(X),
                         ((simd_<floating_<A0>,X>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      #ifndef BOOST_SIMD_NO_NANS
      return if_else(is_nan(a0), a0, b_or(One<A0>(), b_and(Signmask<A0>(), a0)));
      #else
      return b_or(One<A0>(), b_and(Signmask<A0>(), a0));
      #endif
      ;
    }
  };
} } }
#endif


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include <QThread>
#include "Robot.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<sensor_msgs/JointState.h>
#include<geometry_msgs/WrenchStamped.h>
#include<rosgraph_msgs/Clock.h>
#include<dynamic_reconfigure/Config.h>
#include<gazebo_msgs/ApplyBodyWrenchRequest.h>
#include <gazebo_msgs/GetLinkState.h>
#include<gazebo_msgs/LinkStates.h>
#include <nav_msgs/Odometry.h>

using namespace  std;
using namespace  Eigen;

int32_t incEncoder[13];
double AbsEncoder[13];
int32_t diffs[12];

void abs_enc_receive(const  sensor_msgs::JointState &msg)
{
    for (int i = 0; i < 13; ++i) {
        AbsEncoder[i]=msg.position[i];
    }
}

void inc_enc_receive(const  sensor_msgs::JointState &msg)
{
    for (int i = 0; i < 13; ++i) {
        incEncoder[i]=msg.position[i];
    }
}

int main(int argc, char **argv)
{
    QMap<QString,int> MotorNames;
    QMap<int,int> HomeOrder;
    int MotorOffset[12];
    double GearRatio[12];
    ///init robot motors map
    MotorNames.insert("R1",0);
    MotorNames.insert("R2",1);
    MotorNames.insert("R3",2);
    MotorNames.insert("R4",3);
    MotorNames.insert("R5",9);
    MotorNames.insert("R6",8);

    MotorNames.insert("L1",5);
    MotorNames.insert("L2",4);
    MotorNames.insert("L3",6);
    MotorNames.insert("L4",7);
    MotorNames.insert("L5",10);
    MotorNames.insert("L6",11);
    MotorNames.insert("ALL",255);

    HomeOrder.insert(0,0);
    HomeOrder.insert(1,1);
    HomeOrder.insert(2,2);
    HomeOrder.insert(3,3);
    HomeOrder.insert(4,8);
    HomeOrder.insert(5,9);
    HomeOrder.insert(6,5);
    HomeOrder.insert(7,4);
    HomeOrder.insert(8,6);
    HomeOrder.insert(9,7);
    HomeOrder.insert(10,10);
    HomeOrder.insert(11,11);

     QMap<int,double> Direction;
    Direction.insert(0,1);
    Direction.insert(1,-1);
    Direction.insert(2,1);
    Direction.insert(3,-1);
    Direction.insert(4,1);
    Direction.insert(5,1);
    Direction.insert(6,-1);
    Direction.insert(7,1);
    Direction.insert(8,-1);
    Direction.insert(9,1);
    Direction.insert(10,1);
    Direction.insert(11,-1);

    // double GearRatio[12]={28.125,28.125,14.0625,22.5,28.125,28.125,14.0625,-22.5,33.75,33.75,33.75,33.75};
    GearRatio[MotorNames["R1"]]=56.25;
    GearRatio[MotorNames["R2"]]=56.25;
    GearRatio[MotorNames["R3"]]=28.125;
    GearRatio[MotorNames["R4"]]=45;
    GearRatio[MotorNames["R5"]]=67.5;
    GearRatio[MotorNames["R6"]]=67.5;
    GearRatio[MotorNames["L1"]]=56.25;
    GearRatio[MotorNames["L2"]]=56.25;
    GearRatio[MotorNames["L3"]]=28.125;
    GearRatio[MotorNames["L4"]]=45;
    GearRatio[MotorNames["L5"]]=67.5;
    GearRatio[MotorNames["L6"]]=67.5;

    MotorOffset[MotorNames["R1"]]=-42;
    MotorOffset[MotorNames["R2"]]=-77;
    MotorOffset[MotorNames["R3"]]=156;
    MotorOffset[MotorNames["R4"]]=0;//104;
    MotorOffset[MotorNames["R5"]]=16;
    MotorOffset[MotorNames["R6"]]=-149;
    MotorOffset[MotorNames["L1"]]=-127;
    MotorOffset[MotorNames["L2"]]=337;
    MotorOffset[MotorNames["L3"]]=-1;
    MotorOffset[MotorNames["L4"]]=203;
    MotorOffset[MotorNames["L5"]]=-970;
    MotorOffset[MotorNames["L6"]]=143;

    for (int i = 0; i < 12; ++i) {
        AbsEncoder[i+1]=MotorOffset[i];
    }

    vector<double> cntrl(13);
    int count = 0;
    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);

    ros::Subscriber abs_enc = nh.subscribe("surena/abs_joint_state", 100,abs_enc_receive );
    ros::Subscriber inc_enc = nh.subscribe("surena/inc_joint_state", 100,inc_enc_receive );

    ros::Rate loop_rate(200);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

    //************************ massage must be sent to
    //    for (int var = 0; var < 100; ++var) {

    //    msg.data.clear();
    //    for(int  k = 0;k < 12;k++)
    //    {
    //        msg.data.push_back(10);
    //    }

    //    chatter_pub.publish(msg);
    //    ros::spinOnce();
    //   loop_rate.sleep();
    // }
    //*************************

    while (inc_enc.getNumPublishers()==0) {
        ROS_INFO("Robot data is not available!");
        QThread::msleep(50);
    }

    for (int i = 0; i < 12; ++i) {
        diffs[i]=incEncoder[i+1];
    }

    double max=20.0;
    double kp=1000;//230400/2/3.14/2;
    int d=100;
    int start=0;
    while(ros::ok()){

        if(start<5){

            msg.data.clear();
            for(int  k = 0;k < 12;k++)
            {
                msg.data.push_back(0);
                //msg.data.push_back(100);
            }

            chatter_pub.publish(msg);
            ros::spinOnce();
            loop_rate.sleep();

            start++;
        }

        else{

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

                int j=HomeOrder[i];

                while (abs(-AbsEncoder[j+1])>M_PI/4096) {
                    //while (true) {

                    if(abs((-AbsEncoder[j+1])*kp*Direction[j])<=max){diffs[j]+= (-AbsEncoder[j+1])*kp*Direction[j];}
                    if(((-AbsEncoder[j+1])*kp*Direction[j])<-max){diffs[j]-=(max);}
                    if(((-AbsEncoder[j+1])*kp*Direction[j])>max){diffs[j]+=(max);}
                    ROS_INFO("abs:R1=%d,R2=%d,R3=%d,R4=%d,L2=%d,L1=%d,L3=%d,L4=%d,R6=%d,R5=%d,L5=%d,L6=%d",int(AbsEncoder[1]*4096/2/M_PI),int(AbsEncoder[2]*4096/2/M_PI),int(AbsEncoder[3]*4096/2/M_PI),int(AbsEncoder[4]*4096/2/M_PI),int(AbsEncoder[5]*4096/2/M_PI),int(AbsEncoder[6]*4096/2/M_PI),int(AbsEncoder[7]*4096/2/M_PI),int(AbsEncoder[8]*4096/2/M_PI),int(AbsEncoder[9]*4096/2/M_PI),int(AbsEncoder[10]*4096/2/M_PI),int(AbsEncoder[11]*4096/2/M_PI),int(AbsEncoder[12]*4096/2/M_PI));
                    ROS_INFO("ID=%d  ,ABS=%f   ,command=%d",j ,AbsEncoder[j+1] ,diffs[j]);

                                    msg.data.clear();
                                    for(int  k = 0;k < 12;k++)
                                    {
                                        msg.data.push_back(diffs[k]);
                                        //msg.data.push_back(100);
                                    }

                                    chatter_pub.publish(msg);
                                    ros::spinOnce();
                                    loop_rate.sleep();

                    //QThread::msleep(d);

                }

            }
        }

//        msg.data.clear();
//        for(int  k = 0;k < 12;k++)
//        {
//            msg.data.push_back(diffs[k]);
//            //msg.data.push_back(100);
//        }

//        chatter_pub.publish(msg);

//        ros::spinOnce();
//        loop_rate.sleep();

    }

    return 0;
}


/*
 * protonReconstruct.cpp
 *
 *  Created on: Dec 22, 2011
 *      Author: u051747
 */
#include <iostream>
#include "parameterparser.h"
#include "cuNDArray.h"
#include "hoCuNDArray.h"

#include "protonSubsetOperator.h"
#include "hoNDArray_fileio.h"
#include "check_CUDA.h"

//#include "hoCuGPBBSolver.h"
#include "hoCuPartialDerivativeOperator.h"
//#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"

#include "cuNDArray_math.h"
#include "cuNDArray_fileio.h"
#include "protonDROPSolver.h"
#include "cuSolverUtils.h"
#include "hdf5_utils.h"

#include "encodingOperatorContainer.h"
//#include "hoCuOperator.h"
#include "hoImageOperator.h"
#include "identityOperator.h"
#include <boost/program_options.hpp>
#include "vector_td_io.h"

#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "projectionSpaceOperator.h"
#include "hdf5.h"

using namespace std;
using namespace Gadgetron;
typedef float _real;

typedef solver<hoCuNDArray<_real>, hoCuNDArray<_real> > baseSolver;
namespace po = boost::program_options;
int main( int argc, char** argv)
{

	//
	// Parse command line
	//

	_real background =  0.00106;
	std::string dataName;
	std::string outputFile;
	vector_td<int,3> dimensions;
	vector_td<float,3> physical_dims;
	vector_td<float,3> origin;
	int iterations;
	int device;
	int subsets;
	bool use_hull,use_weights, use_non_negativity;
	float beta,gamma;
	float huber;
	float tv_weight;
	po::options_description desc("Allowed options");
	desc.add_options()
					("help", "produce help message")
					("data,D", po::value<std::string>(&dataName)->default_value("data.hdf5"), "HDF5 file containing projections and splines")
					("dimensions,d", po::value<vector_td<int,3> >(&dimensions)->default_value(vector_td<int,3>(512,512,1)), "Pixel dimensions of the image")
					("size,S", po::value<vector_td<float,3> >(&physical_dims)->default_value(vector_td<float,3>(20,20,5)), "Dimensions of the image")
					("center,c", po::value<vector_td<float,3> >(&origin)->default_value(vector_td<float,3>(0,0,0)), "Center of the reconstruction")
					("iterations,i", po::value<int>(&iterations)->default_value(10), "Dimensions of the image")
					("output,f", po::value<std::string>(&outputFile)->default_value("image.hdf5"), "Output filename")
					("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
					("TV,T",po::value<float>(&tv_weight)->default_value(0),"TV Weight ")
					("subsets,n", po::value<int>(&subsets)->default_value(10), "Number of subsets to use")
					("use_hull",po::value<bool>(&use_hull)->default_value(true),"Estimate convex hull of object")
					("use_weights",po::value<bool>(&use_weights)->default_value(false),"Use weights if available. ")
					("use_non_negativity",po::value<bool>(&use_non_negativity)->default_value(true),"Use non-negativity constraint. ")
					;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(bool)) std::cout << it->second.as<bool>();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}
	cudaSetDevice(device);
	//cudaDeviceReset();

	protonDROPSolver<hoCuNDArray > solver;
	solver.set_non_negativity_constraint(use_non_negativity);
	solver.set_max_iterations(iterations);
	solver.set_dump(false);
	solver.set_beta(1.9f);
	solver.set_non_negativity_constraint(use_non_negativity);
	solver.set_max_iterations(iterations);

	H5Eset_auto1(0,0);//Disable hdf5 error reporting
	std::vector<size_t> rhs_dims(&dimensions[0],&dimensions[3]); //Quick and dirty vector_td to vector
	std::cout << "Loading data" << std::endl;
	boost::shared_ptr<protonDataset<hoCuNDArray> >  data(new protonDataset<hoCuNDArray>(dataName,use_weights));
	std::cout << "Done" << std::endl;
	data = protonDataset<hoCuNDArray>::shuffle_dataset(data,subsets);

	data->preprocess(rhs_dims,physical_dims,false);

	boost::shared_ptr< protonSubsetOperator<hoCuNDArray> > E (new protonSubsetOperator<hoCuNDArray>(data->get_subsets(), physical_dims) );

	E->set_domain_dimensions(&rhs_dims);
	E->set_codomain_dimensions(data->get_projections()->get_dimensions().get());

  if (vm.count("TV")){
	  std::cout << "Total variation regularization in use" << std::endl;
	  boost::shared_ptr<hoCuTvOperator<float,3> > tv(new hoCuTvOperator<float,3>);
	  tv->set_weight(vm["TV"].as<float>());
	  solver.set_reg_op(tv);
  }

	solver.set_encoding_operator(E);
	solver.set_output_mode(baseSolver::OUTPUT_VERBOSE);

	boost::shared_ptr< hoCuNDArray<_real> > result;
	{
		GPUTimer timer("Time to solve");
		result = solver.solve(data->get_projections().get());
	}

	std::stringstream ss;
	for (int i = 0; i < argc; i++){
		ss << argv[i] << " ";
	}
	saveNDArray2HDF5<3>(result.get(),outputFile,physical_dims,origin,ss.str(), solver.get_max_iterations());
}


/**\file ampdispersion.c
 * \author Piyush Agram.
 *  */

#include "evd.hpp"
#include "gdal.h"
#include "gdal_priv.h"
#include "cpl_conv.h"
#include "cpl_port.h"
#include "fringe/fringe_common.hpp"
#include "fringe/EigenLapack.hpp"

#include <vector>
#include <armadillo>

int evd_process(evdOptions *opts)
{

    //Print user options to screen
    opts->print();

    int cols, rows, nbands;
    int blockysize, boxesperblock;
    int miniStackCount = opts->miniStackCount;

    //First thing to do is to read dimenions.
    GDALDataset* inDataset = NULL;          //Input stack
    GDALDataset* wtsDataset = NULL;         //Input weights
    GDALDataset* corrDataset = NULL;        //Output correlation dataset
    GDALDataset* compDataset = NULL;        //Compressed SLC dataset
    std::vector<GDALDataset*> outDataset;   //Vector of output SLCs

    //Vector of dates for internal bookkeeping
    std::vector<std::string> dates;

    //Clock variables
    double t_start, t_end;

    //Make sure C++11 is being used
    //unsigned int - 32 bytes
    checkLongSetting();

    //Register GDAL drivers
    GDALAllRegister();

    //Determine the number of uint32 bytes needed to store weights
    int numAround = (2*opts->Ny + 1) * (2*opts->Nx + 1);
    int nulong = ceil( (numAround*1.0)/ 32.0);
    std::cout << "Number of uint32 bytes for mask: " << nulong << "\n";

    //Open the stack dataset
    inDataset = reinterpret_cast<GDALDataset *>( GDALOpenShared( opts->inputDS.c_str(), GA_ReadOnly));
    if (inDataset == NULL)
    {
        std::cout << "Cannot open stack file { " <<  opts->inputDS << " } with GDAL for reading. \n";

        std::cout << "GDALOpen failed - " << opts->inputDS << "\n";
        std::cout << "Exiting with error code .... (102) \n";
        GDALDestroyDriverManager();
        return 102;
    }

    cols = inDataset->GetRasterXSize();
    rows = inDataset->GetRasterYSize();
    nbands = inDataset->GetRasterCount();

    std::cout << "Number of rows  = " << rows << "\n";
    std::cout << "Number of cols  = " << cols << "\n";
    std::cout << "Number of bands = " << nbands << "\n";

    //Resolve method and bandwidth
    if (opts->method.compare("STBAS") == 0)
    {
        //If bandwidth is not provided
        if (opts->bandWidth <= 0)
        {
            std::cout << "Requested STBAS but no bandwidth provided \n";
            GDALDestroyDriverManager();
            return 101;
        }
        else if (opts->bandWidth >= (nbands-1))
        {
            std::cout <<"Requested STBAS bandwidth "
                      << opts->bandWidth << " is larger than full bandwidth "
                      << (nbands-1) << "\n";
            GDALDestroyDriverManager();
            return 101;
        }
    }

    //Open weights file
    wtsDataset = reinterpret_cast<GDALDataset *>( GDALOpenShared( opts->wtsDS.c_str(), GA_ReadOnly));
    if (wtsDataset == NULL)
    {
        std::cout << "Could not open weights dataset {" << opts->wtsDS << "}\n";
        std::cout << "Exiting with non-zero error code ... 105 \n";
        GDALClose(inDataset);
        GDALDestroyDriverManager();
        return 105;
    }

    //Check for consistency between input and weight file
    {
        int code = 0;
        int cc = wtsDataset->GetRasterXSize();
        if (cc != cols)
        {
            std::cout << "Width mismatch between input dataset and weight dataset\n";
            code = 106;
        }

        int rr = wtsDataset->GetRasterYSize();
        if (rr != rows)
        {
            std::cout << "Length mismatch between input dataset and weight dataset \n";
            code = 107;
        }

        int bb = wtsDataset->GetRasterCount();
        if (bb != nulong)
        {
            std::cout << "Number of bands mismatch for weights and window size \n";
            code = 108;
        }

        {
            int inNx = ::atoi(wtsDataset->GetMetadataItem("HALFWINDOWX", "ENVI"));
            int inNy = ::atoi(wtsDataset->GetMetadataItem("HALFWINDOWY", "ENVI"));

            if (inNx != opts->Nx)
            {
                std::cout << "Half window size x of wts is different from input. \n";
                code = 109;
            }

            if (inNx == 0)
            {
                std::cout << "No non-zero metadata item called HALFWINDOWX \n";
                code = 109;
            }

            if (inNy != opts->Ny)
            {
                std::cout << "Half window size y of wts is different from input. \n";
                code = 110;
            }

            if (inNy == 0)
            {
                std::cout << "No non-zero metadata item called HALFWINDOWY \n";
                code = 110;
            }
        }

        if (code != 0)
        {
            std::cout <<"Exiting with error code ....("<<code<<")\n";
            GDALClose(inDataset);
            GDALClose(wtsDataset);
            GDALDestroyDriverManager();

            return code;
        }
    }

    //Determine blocksizes
    boxesperblock = int((opts->memsize * 1.0e6)/cols) / (opts->blocksize * (nbands*20+4+nulong)) ;
    blockysize = boxesperblock * opts->blocksize;
    if (blockysize < opts->blocksize)
    {
        blockysize = opts->blocksize;
        boxesperblock = 1;
    }

    if (blockysize > rows)
    {
        blockysize = rows;
        boxesperblock = 1;
    }

    std::cout << "Block size = " << blockysize << " lines \n";

    int totalblocks = ceil( rows / (1.0 * blockysize));
    std::cout << "Total number of blocks to process: " << totalblocks << "\n";

    //Start the clock
    t_start = getWallTime();

    //Temporary array for reading in complex data
    arma::cx_fmat cpxdata(cols*blockysize, nbands); //To store input data
    arma::cx_fmat evddata(cols*blockysize, nbands);  //To store output data
    arma::Mat<unsigned int> wts(nulong, cols*blockysize); //To store weights
    arma::fvec corr(cols*blockysize);    //To store correlation
    arma::cx_fvec comp(cols*blockysize); //To store compressed SLC

    //Start block-by-block processing
    int blockcount = 0;
    int nthreads = 0;

    //Resize vectors based on number of bands
    outDataset.resize(nbands);
    dates.resize(nbands);

    //Creation of output datasets
    {
        //Make sure that all the input stack layers have dates
        for(int b=1; b<=nbands; b++)
        {
            const char* date = inDataset->GetRasterBand(b)->GetMetadataItem("Date", "slc");
            if (strlen(date) != 8)
            {
                std::cout << "Band " << b << " does not appear to have Date information in slc metadata domain \n";
                GDALClose(inDataset);
                GDALClose(wtsDataset);
                GDALDestroyDriverManager();

                return 112;
            }
            dates[b-1] =  std::string(date);
        }

        //Check output folder
        VSIStatBufL sStat;

        //If folder already exists
        if ( VSIStatExL( opts->outputFolder.c_str(), &sStat, VSI_STAT_EXISTS_FLAG | VSI_STAT_NATURE_FLAG) == 0)
        {
            if ( VSI_ISDIR(sStat.st_mode) )
            {
                std::cout << "Output folder : " << opts->outputFolder << " already exists \n";
                std::cout << "Returning without processing. Clean up output folder and rerun.. \n";
                GDALClose(inDataset);
                GDALClose(wtsDataset);
                GDALDestroyDriverManager();

                return 113;
            }
            else
            {
                std::cout << opts->outputFolder << " already exists and appears to be a file on disk. \n";
                std::cout << "Returning without processing. Clean up output folder and rerun.. \n";
                GDALClose(inDataset);
                GDALClose(wtsDataset);
                GDALDestroyDriverManager();

                return 114;
            }
        }

        //Create output folder
        if (VSIMkdir( opts->outputFolder.c_str(), 0777) != 0)
        {
            std::cout << "Could not create output folder: " << opts->outputFolder << "\n";

            GDALClose(inDataset);
            GDALClose(wtsDataset);
            GDALDestroyDriverManager();
            return 115;
        }
        std::cout << "Created output folder: " << opts->outputFolder << "\n";

        //Creation of output dataset
        for(int b=1; b<=nbands; b++)
        {
            GDALDriver *poDriver = (GDALDriver*) GDALGetDriverByName("ENVI");
            char **mOptions = NULL;
            mOptions = CSLSetNameValue(mOptions, "INTERLEAVE", "BIP");
            mOptions = CSLSetNameValue(mOptions, "SUFFIX", "ADD");

            const char* fname = CPLStrdup(CPLFormFilename(opts->outputFolder.c_str(), dates[b-1].c_str(), ".slc"));
            std::cout << "Input: " << fname <<  " " << strlen(fname) <<  "\n";

            GDALDataset *temp = (GDALDataset*) poDriver->Create(fname, cols, rows, 1, GDT_CFloat32, mOptions);

            if (temp == NULL)
            {
                std::cout << "Could not create output SLC: " << fname << "\n";
                std::cout << "Exiting with non-zero error code ... 116 \n";

                GDALClose(inDataset);
                GDALClose(wtsDataset);

                for (int j=0; j < b-1; j++)
                {
                    GDALClose(outDataset[j]);
                }
                GDALDestroyDriverManager();

                return 116;
            }

            outDataset[b-1] = temp;

            std::cout << "Created : " << fname  <<  "  " << temp << "\n";

            CSLDestroy(mOptions);
        }

        //Temporal coherence
        {
            GDALDriver *poDriver = (GDALDriver*) GDALGetDriverByName("ENVI");
            char **mOptions = NULL;
            mOptions = CSLSetNameValue(mOptions, "INTERLEAVE", "BIP");
            mOptions = CSLSetNameValue(mOptions, "SUFFIX", "ADD");

            const char* fname = CPLStrdup(CPLFormFilename(opts->outputFolder.c_str(), "tcorr.bin", NULL));

            std::cout << "Corr: " << fname <<  " " << strlen(fname) << "\n";
            corrDataset = (GDALDataset*) poDriver->Create(fname, cols, rows, 1, GDT_Float32, mOptions);

            if (corrDataset == NULL)
            {
                std::cout << "Could not create temporal correlation file: " << fname << "\n";
                std::cout << "Exiting with non-zero error code ... 117 \n";

                GDALClose(inDataset);
                GDALClose(wtsDataset);

                for (int j=0; j<nbands; j++)
                {
                    GDALClose(outDataset[j]);
                }
                GDALDestroyDriverManager();
                return 117;
            }

            std::cout << "Created : " << fname << " " << corrDataset << "\n";

            CSLDestroy(mOptions);
        }

        //Compressed SLC
        {
            GDALDriver *poDriver = (GDALDriver*) GDALGetDriverByName("ENVI");
            char **mOptions = NULL;
            mOptions = CSLSetNameValue(mOptions, "INTERLEAVE", "BIP");
            mOptions = CSLSetNameValue(mOptions, "SUFFIX", "ADD");

            const char* fname = CPLStrdup(CPLFormFilename(opts->outputCompressedSlcFolder.c_str(), opts->compSlc.c_str(), NULL));

            std::cout << "Compressed SLC: " << fname <<  " " << strlen(fname) << "\n";
            compDataset = (GDALDataset*) poDriver->Create(fname, cols, rows, 1, GDT_CFloat32, mOptions);

            if (compDataset == NULL)
            {
                std::cout << "Could not create compressed SLC file: " << fname << "\n";
                std::cout << "Exiting with non-zero error code ... 117 \n";

                GDALClose(inDataset);
                GDALClose(wtsDataset);
                GDALClose(corrDataset);
                for (int j=0; j<nbands; j++)
                {
                    GDALClose(outDataset[j]);
                }
                GDALDestroyDriverManager();
                return 117;
            }

            std::cout << "Created : " << fname << " " << compDataset << "\n";
            CSLDestroy(mOptions);
        }

    }

    nthreads = numberOfThreads();

    //Useful variables
    int Ny = opts->Ny;
    int Nx = opts->Nx;
    int Wy = 2*Ny+1;
    int Wx = 2*Nx+1;

    const Ulongmask bitmask = {Ny,Nx};

    //Setup the Eigen Value workers
    EVWorker *workers = new EVWorker[nthreads];
    for(int ii=0; ii < nthreads; ii++)
    {
        workers[ii].prepare(nbands);
    }

    //Setup arrays for computing coherence matrix
    arma::cx_cube Numer(nbands, nbands, nthreads);
    arma::cx_cube Amp1(nbands, nbands, nthreads);
    arma::cx_cube Amp2(nbands,nbands, nthreads);
    arma::cx_cube Covar(nbands, nbands, nthreads);

    //Block-by-block processing
    int yoff = 0;

    while( yoff < rows)
    {
        //Increment block counter
        blockcount++;

        int inysize = blockysize;

        //Number of lines to read for the block
        if ((yoff+inysize) > rows)
            inysize = rows - yoff;

//        std::cout << "Block " << blockcount << ": " << inysize << " lines \n";

        //Block logic
        int firstlinetowrite;
        int linestowrite;
        int rollback;

        //For first block include the top half window.
        if (blockcount == 1)
        {
            firstlinetowrite = 0;
            linestowrite = inysize - Ny;
            rollback = Ny;

            if( (yoff+blockysize) >= rows) //There is only one block, write the whole thing
            {
                linestowrite = inysize;
                rollback = 0;
            }

        }
        else if( (yoff+blockysize) >= rows) //Last block. Write bottom half window.
        {
            firstlinetowrite = Ny;
            linestowrite = inysize-Ny;
            rollback = 0;
        }
        else
        {
            firstlinetowrite = Ny;
            linestowrite = inysize-2*Ny;
            rollback = 0;
        }

        //Init to zeros for each block
        cpxdata.zeros();
        evddata.zeros();
        wts.zeros();
        Numer.zeros();
        Amp1.zeros();
        Amp2.zeros();
        corr.zeros();
        comp.zeros();

        //Read in data from all SLCs
        for(int bb=0; bb < nbands; bb++)
        {
            int status = inDataset->GetRasterBand(bb+1)->RasterIO( GF_Read,
                        0, yoff,
                        cols, inysize,
                        (void*) (cpxdata.colptr(bb)),
                        cols, inysize, GDT_CFloat32,
                        sizeof(std::complex<float>),
                        sizeof(std::complex<float>)*cols, NULL);

            if (status != 0)
            {
                std::cout << "Error reading data from band " << bb+1 << " at line "
                    << yoff << "\n";
                std::cout << "Exiting with error code .... (118) \n";
                GDALClose(inDataset);
                GDALClose(wtsDataset);
                for(int jj=0; jj<nbands; jj++) GDALClose(outDataset[jj]);
                GDALClose(corrDataset);
                GDALClose(compDataset);
                GDALDestroyDriverManager();
                return 118;
            }
        }

        {
            //Read in the weights dataset
            int status =  wtsDataset->RasterIO(GF_Read,
                    0, yoff,
                    cols, inysize,
                    (void*) (wts.memptr()),
                    cols, inysize, GDT_UInt32,
                    nulong, NULL,
                    4*nulong, 4*nulong*cols,
                    4, NULL);
            if (status != 0)
            {
                std::cout << "Error reading weights at line " << yoff << "\n";
                std::cout << "Exiting with error code .... (119) \n";
                GDALClose(inDataset);
                GDALClose(wtsDataset);
                GDALClose(corrDataset);
                GDALClose(compDataset);
                for (int jj=0; jj<nbands; jj++) GDALClose(outDataset[jj]);
                GDALDestroyDriverManager();
                return 119;
            }
        }

        //Copy method to constant string
        const std::string method = opts->method;
        const int BW = opts->bandWidth;
        const bool isstbas = (method.compare("STBAS") == 0);
        const bool ismle = (method.compare("MLE") == 0);

    #pragma omp parallel for\
        default(shared)
        for(int pp=(firstlinetowrite*cols); pp < ((firstlinetowrite+linestowrite)*cols); pp++)
        {
            unsigned int *cenptr = wts.colptr(pp);
            if (bitmask.getbit(cenptr,0,0) == 0) continue;

            //Get thread number
            int threadnum = omp_get_thread_num();

            Covar.slice(threadnum).zeros();
            Numer.slice(threadnum).zeros();
            Amp1.slice(threadnum).zeros();
            Amp2.slice(threadnum).zeros();

            int cenii = pp/cols;
            int cenjj = pp%cols;

            int xmin = std::max(cenjj-Nx, 0);
            int xmax = std::min(cols-1, cenjj+Nx);

            int ymin = std::max(cenii-Ny, 0);
            int ymax = std::min(inysize-1, cenii+Ny);

            //Gather data
            int npix = 0;
            for (int ii=ymin; ii <=ymax; ii++)
            {
                for (int jj=xmin; jj <= xmax; jj++)
                {
                    /*std::cout << "ii = " << ii - cenii
                              << ", jj = " << jj - cenjj
                              << ", flag = " << bitmask.getbit(cenptr, ii-cenii, jj-cenjj) << "\n";*/

                    if (bitmask.getbit(cenptr, ii-cenii, jj-cenjj) != 0)
                    {
                        npix++;
                        arma::cx_frowvec ptr = cpxdata.row(ii*cols + jj);

                        //Compute correlation for top half of the matrix only
                        for(int ti=0; ti < nbands; ti++)
                        {
                            for(int tj=ti+1; tj<nbands; tj++)
                            {
                                Numer.at(ti,tj,threadnum) += ptr[ti] * std::conj(ptr[tj]);
                                Amp1.at(ti,tj,threadnum)  += std::complex<double>(std::pow(std::abs(ptr[ti]),2),0.);
                                Amp2.at(ti,tj,threadnum)  += std::complex<double>(std::pow(std::abs(ptr[tj]),2),0.);
                            }
                        }
                    }
                }
            }

            if (npix < 2) continue;     //Minimum number of neighbors

            //Create coherence matrix - common to all methods
            for (int ti=0; ti <nbands; ti++)
            {
                for(int tj=ti+1; tj< nbands; tj++)
                {
                    std::complex<double> val1 = Numer.at(ti,tj,threadnum);
                    double a1 = Amp1.at(ti,tj,threadnum).real();
                    double a2 = Amp2.at(ti,tj,threadnum).real();

                    std::complex<double> res = val1 / std::sqrt(a1*a2);
                    Covar.at(ti,tj,threadnum) = res;
                    Covar.at(tj,ti,threadnum) = std::conj(res);
                }
                Covar.at(ti,ti,threadnum) = std::complex<double>(1.0, 0.0);
            }

            if (ismle)
            {
                //Store absolute value in Amp1
                for (int ti=0; ti<nbands; ti++)
                {
                    for(int tj=ti+1; tj<nbands; tj++)
                    {
                        double coh = std::abs( Covar.at(ti,tj,threadnum));
                        Amp1.at(ti,tj,threadnum) = std::complex<double>(coh, 0.0);
                        Amp1.at(tj,ti,threadnum) = std::complex<double>(coh, 0.0);
                    }
                    Amp1.at(ti,ti,threadnum) = std::complex<double>(1.0, 0.0);
                }

                bool valid = true;
                //This is check to see if coherence is numerically well behaved
                {
                    //Copy covar to amp2
                    Amp2.slice(threadnum) = Covar.slice(threadnum);

                    //Check if covariance positive semi definite
                    int status = workers[threadnum].smallestEigen(Amp2.slice_memptr(threadnum), false);
                    if (status != 0)
                    {
                        corr[pp] = -1;
                        valid = false;
                    }
                    else
                    {
                        if (workers[threadnum].eigval[0] < 1.0e-6)
                        {
                            corr[pp] = -2;
                            valid = false;
                        }
                    }
                }
                if (!valid) continue;   //Skip rest of computation

                //This is check to see if coherence is numerically well behaved
                {
                    //Copy coherence to Amp2
                    Amp2.slice(threadnum) = Amp1.slice(threadnum);

                    //Check if coherence is positive semi-definite
                    int status = workers[threadnum].smallestEigen(Amp2.slice_memptr(threadnum), false);

                    if (status !=0)
                    {
                        corr[pp] = -3;
                        valid = false;
                    }
                    else
                    {
                        if (workers[threadnum].eigval[0] < 1.0e-6)
                        {
                            corr[pp] = -4;
                            valid = false;
                        }
                    }
                }
                if (!valid) continue;   //Skip rest of computation

                //MLE stuff
                //We have access to eigenvector as well as Covar here
                //This is where the iterative MLE solver goes
                //MLE solver should put solution back into evddata

                //Compute inverse of the coherence matrix here
                {
                    //copy coherence to Amp2
                    Amp2.slice(threadnum) = Amp1.slice(threadnum);

                    //Perform inverse operation
                    int status = workers[threadnum].positiveDefiniteInverse(Amp2.slice_memptr(threadnum));
                    if (status != 0)
                    {
                        corr[pp] = -5;
                        valid = false;
                    }
                }
                if (!valid) continue;   //Skip rest of computation

                //Perform Hadamard product
                Amp2.slice(threadnum) %= Covar.slice(threadnum);

                //Obtain the smallest eigen vector
                {
                    int status = workers[threadnum].smallestEigen( Amp2.slice_memptr(threadnum), true);
                    if (status != 0)
                    {
                        corr[pp] = -6;
                        valid = false;
                    }
                    else
                    {
                        if (workers[threadnum].eigval[0] < 1.0e-6)
                        {
                            corr[pp] = -7;
                            valid = false;
                        }
                    }
                }
                if (!valid) continue;   //Skip updating evddata

            } //End of MLE
            else
            {
                //This is for STBAS / EVD - which both use the same concept
                //Largest eigen vector of the correlation matrix

                //If STBAS, apply bandwidth limits
                if (isstbas)
                {
                    //Blank out covariance entries outside the bandwidth
                    for (int ti=0; ti <nbands; ti++)
                    {
                        for(int tj=ti+BW+1; tj< nbands; tj++)
                        {
                            Covar.at(tj,ti,threadnum) = std::complex<double>(0., 0.);
                            Covar.at(ti,tj,threadnum) = std::complex<double>(0., 0.);
                        }
                    }
                }

                //Copy covar into amp1 for computation
                //Need to preserve covar for temporal coherence computation
                Amp1.slice(threadnum) = Covar.slice(threadnum);

                //Obtain the largest eigen vector
                bool valid = true;
                {
                    int status = workers[threadnum].largestEigen( Amp1.slice_memptr(threadnum), true);
                    if (status != 0)
                    {
                        corr[pp] = -6;
                        valid = false;
                    }
                    else
                    {
                        if (workers[threadnum].eigval[0] < 1.0e-6)
                        {
                            corr[pp] = -7;
                            valid = false;
                        }
                    }
                }
                if (!valid) continue;   //Skip updating evddata

            } //End of EVD or STBAS

            //If we have gotten this far, "eigvec" should contain the solution
            //For MLE, its from smallestEigen and its largetstEigen for others
            //Ensure first date of acquired SLCs is set to zero
            {
                std::complex<double> cJ(0.0, 1.0);
                double ph0 = std::arg(workers[threadnum].eigvec[miniStackCount-1]);
                for(int ii=0; ii < nbands; ii++)
                {
                    double res = std::arg(workers[threadnum].eigvec[ii]);
                    evddata.at(pp, ii) = std::exp(cJ * (res - ph0));
                }

                //Ensure that the reference index is exactly 1 - i.e, phase 0
                evddata.at(pp, miniStackCount-1) = std::complex<float>(1.0, 0.0);
            }

            //compress acquired SLCs of the stack.
            //This is common to all methods
            //For sequential approach previous compressed SLCs are excluded in the current compression
            {
                std::complex<double> sumcomp = 0.0;
                for(int ii=miniStackCount-1; ii < nbands; ii++)
                {
                    sumcomp += cpxdata.at(pp, ii) * std::conj( evddata.at(pp,ii));
                }
                comp.at(pp) = sumcomp / (1.0 * (nbands - miniStackCount + 1));
            }

            //This part might need some updating to iterate starting with the eigen value solution
            //Numerical simulations dont seem to show any impact - but all papers suggest this

            //Temporal coherence estimation
            //This is common to all methods
            {
                std::complex<double> tempcorr(0.0,0.0);
                std::complex<double> cJ(0.0,1.0);
                int counter = 0;
                for(int ti=0; ti<nbands; ti++)
                {
                    int ulim = isstbas ? (ti+BW+1) : nbands;
                    for(int tj=ti+1; tj<ulim; tj++)
                    {
                        int ind = ti + nbands * tj;

                        tempcorr += std::exp(cJ * (std::arg(Covar.at(ti,tj,threadnum)) - std::arg( evddata.at(pp,ti)) + std::arg( evddata.at(pp,tj))));
                        counter++;
                    }
                }
                corr.at(pp) = std::abs(tempcorr) /(counter * 1.0);
            }

        }

        //Write the data to output bands
        for(int ii=0; ii<nbands; ii++)
        {
            int status = outDataset[ii]->GetRasterBand(1)->RasterIO(GF_Write, 0, yoff+firstlinetowrite,
                        cols, linestowrite,
                        (void*) (evddata.colptr(ii) + firstlinetowrite*cols),
                        cols, linestowrite, GDT_CFloat32,
                        sizeof(std::complex<float>), sizeof(std::complex<float>)*cols, NULL);

            if (status != 0)
            {
                std::cout << "Error writing EVD data at line " << yoff << " for band " << ii+1 << "\n";
                std::cout << "Exiting with error code .... (120) \n";
                GDALClose(inDataset);
                GDALClose(wtsDataset);
                GDALClose(corrDataset);
                GDALClose(compDataset);
                for(int jj=0; jj < nbands; jj++) GDALClose(outDataset[jj]);
                GDALDestroyDriverManager();
                return 120;
            }
        }

        //Write the temporal correlation data
        {
            int status = corrDataset->GetRasterBand(1)->RasterIO(GF_Write, 0, yoff+firstlinetowrite,
                    cols, linestowrite,
                    (void*) (corr.memptr()+firstlinetowrite*cols),
                    cols, linestowrite, GDT_Float32,
                    sizeof(float), sizeof(float)*cols, NULL);

            if (status != 0)
            {
                std::cout << "Error writing temporal correlation data at line " << yoff << "\n";
                std::cout << "Exiting with error code .... (121) \n";
                GDALClose(inDataset);
                GDALClose(wtsDataset);
                GDALClose(corrDataset);
                GDALClose(compDataset);
                for(int jj=0; jj<nbands; jj++) GDALClose(outDataset[jj]);
                GDALDestroyDriverManager();
                return 121;
            }
        }

        //Write the compressed SLC data
        {
            int status = compDataset->GetRasterBand(1)->RasterIO(GF_Write, 0, yoff+firstlinetowrite,
                    cols, linestowrite,
                    (void*) (comp.memptr()+firstlinetowrite*cols),
                    cols, linestowrite, GDT_CFloat32,
                    sizeof(std::complex<float>), sizeof(std::complex<float>)*cols, NULL);

            if (status != 0)
            {
                std::cout << "Error writing temporal correlation data at line " << yoff << "\n";
                std::cout << "Exiting with error code .... (121) \n";
                GDALClose(inDataset);
                GDALClose(wtsDataset);
                GDALClose(corrDataset);
                GDALClose(compDataset);
                for(int jj=0; jj<nbands; jj++) GDALClose(outDataset[jj]);
                GDALDestroyDriverManager();
                return 121;
            }
        }

        //Update the counter
        if ((yoff+blockysize) < rows)
        {
            yoff += linestowrite - rollback;
        }
        else
        {
            yoff=rows;
        }

        GDALTermProgress( yoff/(1.0*rows), NULL, NULL);

    }

    //Free the workers
    delete [] workers;

    //Close the datasets
    GDALClose(inDataset);
    GDALClose(wtsDataset);
    for (int b=0; b<nbands;b++)
    {
        GDALClose(outDataset[b]);
    }
    GDALClose(corrDataset);
    GDALClose(compDataset);
    return(0);

};

/* Main driver*/
int main(int  argc, const char *argv[] ) {

    //Options
    evdOptions opts;
    int status;

    //Parse command line options
    status = opts.initFromCmdLine(argc, argv);

    if (status != 0)
    {
        std::cout << "Error parsing command line inputs \n";
        std::cout << "Exiting with non-zero return code .... \n";
        return (101);
    }

    //Execute
    status = evd_process(&opts);

    if (status != 0)
    {
        std::cout << "Error processing calamp \n";
        std::cout << "Exiting with non-zero return code ....\n";
        return (status);
    }

    return(0);

};


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
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 or http://www.mitk.org for details.

===================================================================*/
#ifndef __itkFiniteDiffOdfMaximaExtractionFilter_cpp
#define __itkFiniteDiffOdfMaximaExtractionFilter_cpp

#include "itkFiniteDiffOdfMaximaExtractionFilter.h"
#include <itkImageRegionConstIterator.h>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkImageRegionIterator.h>
#include <vnl/vnl_vector.h>
#include <itkOrientationDistributionFunction.h>
#include <itkContinuousIndex.h>

#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>

#include <boost/math/special_functions.hpp>
#include <boost/progress.hpp>

using namespace boost::math;

namespace itk {

static bool CompareVectors(const vnl_vector_fixed< double, 3 >& v1, const vnl_vector_fixed< double, 3 >& v2)
{
    return (v1.magnitude()>v2.magnitude());
}

template< class PixelType, int ShOrder, int NrOdfDirections >
FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::FiniteDiffOdfMaximaExtractionFilter()
    : m_NormalizationMethod(MAX_VEC_NORM)
    , m_MaxNumPeaks(2)
    , m_PeakThreshold(0.4)
    , m_AbsolutePeakThreshold(0)
    , m_ClusteringThreshold(0.9)
    , m_AngularThreshold(0.7)
    , m_NumCoeffs((ShOrder*ShOrder + ShOrder + 2)/2 + ShOrder)
    , m_Toolkit(FSL)
{
    this->SetNumberOfRequiredInputs(1);
}

template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::FindCandidatePeaks(OdfType& odf, double thr, std::vector< DirectionType >& container)
{
    double gfa = odf.GetGeneralizedFractionalAnisotropy();
    //Find the peaks using a finite difference method
    bool flag = true;
    vnl_vector_fixed< bool, NrOdfDirections > used; used.fill(false);
    //Find the peaks
    for (int i=0; i<NrOdfDirections; i++)
    {
        if (used[i])
            continue;

        double val = odf.GetElement(i);
        if (val>thr && val*gfa>m_AbsolutePeakThreshold)  // limit to one hemisphere ???
        {
            flag = true;
            std::vector< int > neighbours = odf.GetNeighbors(i);
            for (unsigned int j=0; j<neighbours.size(); j++)
                if (val<=odf.GetElement(neighbours.at(j)))
                {
                    flag = false;
                    break;
                }
            if (flag)   // point is a peak
            {
                container.push_back(odf.GetDirection(i).normalize());
                used[i] = true;
                for (unsigned int j=0; j<neighbours.size(); j++)
                    used[neighbours.at(j)] = true;
            }
        }
    }
}

template< class PixelType, int ShOrder, int NrOdfDirections >
std::vector< vnl_vector_fixed< double, 3 > >  FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>::MeanShiftClustering(std::vector< vnl_vector_fixed< double, 3 > >& inDirs)
{
    std::vector< DirectionType > outDirs;
    if (inDirs.empty())
        return inDirs;

    DirectionType oldMean, currentMean, workingMean;
    std::vector< int > touched;

    // initialize
    touched.resize(inDirs.size(), 0);
    bool free = true;
    currentMean = inDirs[0];  // initialize first seed
    while (free)
    {
        oldMean.fill(0.0);

        // start mean-shift clustering
        float angle = 0.0;
        int counter = 0;
        while ((currentMean-oldMean).magnitude()>0.0001)
        {
            counter = 0;
            oldMean = currentMean;
            workingMean = oldMean;
            workingMean.normalize();
            currentMean.fill(0.0);
            for (unsigned int i=0; i<inDirs.size(); i++)
            {
                angle = dot_product(workingMean, inDirs[i]);
                if (angle>=m_ClusteringThreshold)
                {
                    currentMean += inDirs[i];
                    touched[i] = 1;
                    counter++;
                }
                else if (-angle>=m_ClusteringThreshold)
                {
                    currentMean -= inDirs[i];
                    touched[i] = 1;
                    counter++;
                }
            }
        }

        // found stable mean
        if (counter>0)
        {
            float mag = currentMean.magnitude();
            if (mag>0)
            {
                currentMean /= mag;
                outDirs.push_back(currentMean);
            }
        }

        // find next unused seed
        free = false;
        for (unsigned int i=0; i<touched.size(); i++)
            if (touched[i]==0)
            {
                currentMean = inDirs[i];
                free = true;
            }
    }

    if (inDirs.size()==outDirs.size())
    {
        return outDirs;
    }
    else
        return MeanShiftClustering(outDirs);
}

template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::BeforeThreadedGenerateData()
{
    typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );
    itk::Vector<double,3> spacing = ShCoeffImage->GetSpacing();
    double minSpacing = spacing[0];
    if (spacing[1]<minSpacing)
        minSpacing = spacing[1];
    if (spacing[2]<minSpacing)
        minSpacing = spacing[2];

    mitk::Point3D origin = ShCoeffImage->GetOrigin();
    itk::Matrix<double, 3, 3> direction = ShCoeffImage->GetDirection();
    ImageRegion<3> imageRegion = ShCoeffImage->GetLargestPossibleRegion();

    if (m_MaskImage.IsNotNull())
    {
        origin = m_MaskImage->GetOrigin();
        direction = m_MaskImage->GetDirection();
        imageRegion = m_MaskImage->GetLargestPossibleRegion();
    }

    m_DirectionImageContainer = ItkDirectionImageContainer::New();
    for (unsigned int i=0; i<m_MaxNumPeaks; i++)
    {
        itk::Vector< float, 3 > nullVec; nullVec.Fill(0.0);
        ItkDirectionImage::Pointer img = ItkDirectionImage::New();
        img->SetSpacing( spacing );
        img->SetOrigin( origin );
        img->SetDirection( direction );
        img->SetRegions( imageRegion );
        img->Allocate();
        img->FillBuffer(nullVec);
        m_DirectionImageContainer->InsertElement(m_DirectionImageContainer->Size(), img);
    }
    if (m_MaskImage.IsNull())
    {
        m_MaskImage = ItkUcharImgType::New();
        m_MaskImage->SetSpacing( spacing );
        m_MaskImage->SetOrigin( origin );
        m_MaskImage->SetDirection( direction );
        m_MaskImage->SetRegions( imageRegion );
        m_MaskImage->Allocate();
        m_MaskImage->FillBuffer(1);
    }
    m_NumDirectionsImage = ItkUcharImgType::New();
    m_NumDirectionsImage->SetSpacing( spacing );
    m_NumDirectionsImage->SetOrigin( origin );
    m_NumDirectionsImage->SetDirection( direction );
    m_NumDirectionsImage->SetRegions( imageRegion );
    m_NumDirectionsImage->Allocate();
    m_NumDirectionsImage->FillBuffer(0);

    this->SetNumberOfIndexedOutputs(m_MaxNumPeaks);

    // calculate SH basis
    OdfType odf;
    vnl_matrix_fixed<double, 3, NrOdfDirections>* directions = odf.GetDirections();
    vnl_matrix< double > sphCoords;
    std::vector< DirectionType > dirs;
    for (int i=0; i<NrOdfDirections; i++)
        dirs.push_back(directions->get_column(i));
    Cart2Sph(dirs, sphCoords);                          // convert candidate peaks to spherical angles
    m_ShBasis = CalcShBasis(sphCoords);                // evaluate spherical harmonics at each peak

    MITK_INFO << "Starting finite differences maximum extraction";
    MITK_INFO << "ODF sampling points: " << NrOdfDirections;
    MITK_INFO << "SH order: " << ShOrder;
    MITK_INFO << "Maximum peaks: " << m_MaxNumPeaks;
    MITK_INFO << "Relative threshold: " << m_PeakThreshold;
    MITK_INFO << "Absolute threshold: " << m_AbsolutePeakThreshold;
    MITK_INFO << "Clustering threshold: " << m_ClusteringThreshold;
    MITK_INFO << "Angular threshold: " << m_AngularThreshold;
}

template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::AfterThreadedGenerateData()
{
    MITK_INFO << "Generating vector field";
    vtkSmartPointer<vtkCellArray> m_VtkCellArray = vtkSmartPointer<vtkCellArray>::New();
    vtkSmartPointer<vtkPoints>    m_VtkPoints = vtkSmartPointer<vtkPoints>::New();

    typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );
    ImageRegionConstIterator< CoefficientImageType > cit(ShCoeffImage, ShCoeffImage->GetLargestPossibleRegion() );

    mitk::Vector3D spacing = ShCoeffImage->GetSpacing();
    double minSpacing = spacing[0];
    if (spacing[1]<minSpacing)
        minSpacing = spacing[1];
    if (spacing[2]<minSpacing)
        minSpacing = spacing[2];

    int maxProgress = ShCoeffImage->GetLargestPossibleRegion().GetSize()[0]*ShCoeffImage->GetLargestPossibleRegion().GetSize()[1]*ShCoeffImage->GetLargestPossibleRegion().GetSize()[2];
    boost::progress_display disp(maxProgress);

    while( !cit.IsAtEnd() )
    {
        ++disp;

        typename CoefficientImageType::IndexType index = cit.GetIndex();
        if (m_MaskImage->GetPixel(index)==0)
        {
            ++cit;
            continue;
        }

        for (unsigned int i=0; i<m_DirectionImageContainer->Size(); i++)
        {
            ItkDirectionImage::Pointer img = m_DirectionImageContainer->GetElement(i);
            itk::Vector< float, 3 > pixel = img->GetPixel(index);
            DirectionType dir; dir[0] = pixel[0]; dir[1] = pixel[1]; dir[2] = pixel[2];

            vtkSmartPointer<vtkPolyLine> container = vtkSmartPointer<vtkPolyLine>::New();
            itk::ContinuousIndex<double, 3> center;
            center[0] = index[0];
            center[1] = index[1];
            center[2] = index[2];
            itk::Point<double> worldCenter;
            m_MaskImage->TransformContinuousIndexToPhysicalPoint( center, worldCenter );

            itk::Point<double> worldStart;
            worldStart[0] = worldCenter[0]-dir[0]/2 * minSpacing;
            worldStart[1] = worldCenter[1]-dir[1]/2 * minSpacing;
            worldStart[2] = worldCenter[2]-dir[2]/2 * minSpacing;
            vtkIdType id = m_VtkPoints->InsertNextPoint(worldStart.GetDataPointer());
            container->GetPointIds()->InsertNextId(id);
            itk::Point<double> worldEnd;
            worldEnd[0] = worldCenter[0]+dir[0]/2 * minSpacing;
            worldEnd[1] = worldCenter[1]+dir[1]/2 * minSpacing;
            worldEnd[2] = worldCenter[2]+dir[2]/2 * minSpacing;
            id = m_VtkPoints->InsertNextPoint(worldEnd.GetDataPointer());
            container->GetPointIds()->InsertNextId(id);
            m_VtkCellArray->InsertNextCell(container);
        }
        ++cit;
    }

    vtkSmartPointer<vtkPolyData> directionsPolyData = vtkSmartPointer<vtkPolyData>::New();
    directionsPolyData->SetPoints(m_VtkPoints);
    directionsPolyData->SetLines(m_VtkCellArray);
    m_OutputFiberBundle = mitk::FiberBundleX::New(directionsPolyData);

    for (unsigned int i=0; i<m_DirectionImageContainer->Size(); i++)
    {
        ItkDirectionImage::Pointer img = m_DirectionImageContainer->GetElement(i);
        this->SetNthOutput(i, img);
    }
}

template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::ThreadedGenerateData( const OutputImageRegionType& outputRegionForThread, ThreadIdType threadID )
{
    typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );

    ImageRegionConstIterator< CoefficientImageType > cit(ShCoeffImage, outputRegionForThread );

    OdfType odf;
    while( !cit.IsAtEnd() )
    {
        typename CoefficientImageType::IndexType index = cit.GetIndex();
        if (m_MaskImage->GetPixel(index)==0)
        {
            ++cit;
            continue;
        }

        CoefficientPixelType c = cit.Get();

        // calculate ODF
        double max = 0;
        odf.Fill(0.0);
        for (int i=0; i<NrOdfDirections; i++)
        {
            for (int j=0; j<m_NumCoeffs; j++)
                odf[i] += c[j]*m_ShBasis(i,j);
            if (odf[i]>max)
                max = odf[i];
        }
        if (max<0.0001)
        {
            ++cit;
            continue;
        }

        std::vector< DirectionType > candidates, peaks, temp;
        peaks.clear();
        max *= m_PeakThreshold;                         // relative threshold
        FindCandidatePeaks(odf, max, candidates);       // find all local maxima
        candidates = MeanShiftClustering(candidates);   // cluster maxima

        vnl_matrix< double > shBasis, sphCoords;
        Cart2Sph(candidates, sphCoords);                // convert candidate peaks to spherical angles
        shBasis = CalcShBasis(sphCoords);            // evaluate spherical harmonics at each peak
        max = 0.0;
        for (unsigned int i=0; i<candidates.size(); i++)         // scale peaks according to ODF value
        {
            double val = 0;
            for (int j=0; j<m_NumCoeffs; j++)
                val += c[j]*shBasis(i,j);
            if (val>max)
                max = val;
            peaks.push_back(candidates[i]*val);
        }
        std::sort( peaks.begin(), peaks.end(), CompareVectors );  // sort peaks

        // kick out directions to close to a larger direction (too far away to cluster but too close to keep)
        unsigned int m = peaks.size();
        if ( m>m_DirectionImageContainer->Size() )
            m = m_DirectionImageContainer->Size();
        for (unsigned int i=0; i<m; i++)
        {
            DirectionType v1 = peaks.at(i);
            double val = v1.magnitude();
            if (val<max*m_PeakThreshold || val<m_AbsolutePeakThreshold)
                break;

            bool flag = true;
            for (unsigned int j=0; j<peaks.size(); j++)
                if (i!=j)
                {
                    DirectionType v2 = peaks.at(j);
                    double val2 = v2.magnitude();
                    double angle = fabs(dot_product(v1,v2)/(val*val2));
                    if (angle>m_AngularThreshold && val<val2)
                    {
                        flag = false;
                        break;
                    }
                }

            if (flag)
                temp.push_back(v1);
        }
        peaks = temp;

        // fill output image
        unsigned int num = peaks.size();
        if ( num>m_DirectionImageContainer->Size() )
            num = m_DirectionImageContainer->Size();
        for (unsigned int i=0; i<num; i++)
        {
            vnl_vector<double> dir = peaks.at(i);

            ItkDirectionImage::Pointer img = m_DirectionImageContainer->GetElement(i);

            switch (m_NormalizationMethod)
            {
            case NO_NORM:
                break;
            case SINGLE_VEC_NORM:
                dir.normalize();
                break;
            case MAX_VEC_NORM:
                dir /= max;
                break;
            }
//            dir[0] = -dir[0];
//            dir[2] = -dir[2];

            dir = m_MaskImage->GetDirection()*dir;
            itk::Vector< float, 3 > pixel;
            pixel.SetElement(0, dir[0]);
            pixel.SetElement(1, dir[1]);
            pixel.SetElement(2, dir[2]);
            img->SetPixel(index, pixel);
        }
        m_NumDirectionsImage->SetPixel(index, num);
        ++cit;
    }
    MITK_INFO << "Thread " << threadID << " finished extraction";
}

// convert cartesian to spherical coordinates
template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::Cart2Sph(const std::vector< DirectionType >& dir, vnl_matrix<double>& sphCoords)
{
    sphCoords.set_size(dir.size(), 2);

    for (unsigned int i=0; i<dir.size(); i++)
    {
        double mag = dir[i].magnitude();

        if( mag<0.0001 )
        {
            sphCoords(i,0) = M_PI/2; // theta
            sphCoords(i,1) = M_PI/2; // phi
        }
        else
        {
            sphCoords(i,0) = acos(dir[i](2)/mag); // theta
            sphCoords(i,1) = atan2(dir[i](1), dir[i](0)); // phi
        }
    }
}

// generate spherical harmonic values of the desired order for each input direction
template< class PixelType, int ShOrder, int NrOdfDirections >
vnl_matrix<double> FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::CalcShBasis(vnl_matrix<double>& sphCoords)
{
    int M = sphCoords.rows();
    int j, m; double mag, plm;
    vnl_matrix<double> shBasis;
    shBasis.set_size(M, m_NumCoeffs);

    for (int p=0; p<M; p++)
    {
        j=0;
        for (int l=0; l<=ShOrder; l=l+2)
            for (m=-l; m<=l; m++)
            {
                switch (m_Toolkit)
                {
                case FSL:
                plm = legendre_p<double>(l,abs(m),cos(sphCoords(p,0)));
                mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;

                if (m<0)
                    shBasis(p,j) = sqrt(2.0)*mag*cos(fabs((double)m)*sphCoords(p,1));
                else if (m==0)
                    shBasis(p,j) = mag;
                else
                    shBasis(p,j) = pow(-1.0, m)*sqrt(2.0)*mag*sin(m*sphCoords(p,1));
                    break;
                case MRTRIX:

                plm = legendre_p<double>(l,abs(m),-cos(sphCoords(p,0)));
                mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;
                if (m>0)
                    shBasis(p,j) = mag*cos(m*sphCoords(p,1));
                else if (m==0)
                    shBasis(p,j) = mag;
                else
                    shBasis(p,j) = mag*sin(-m*sphCoords(p,1));
                    break;
                }

                j++;
            }
    }
    return shBasis;
}

}

#endif // __itkFiniteDiffOdfMaximaExtractionFilter_cpp


//mcalign.cpp
/*
 * Align projections by mass-centers
 * moving all centers to the center of the picture
 * (C) 2006 olegabr. All rights reserved.
*/

#include <Magick++.h>

#include <iostream>
using std::cout; using std::endl;

#include <cmath>
using std::sqrt;

#include <boost/lexical_cast.hpp>
using boost::lexical_cast;

#include <tomo3d.h>
#include <object3d.h>
#include <shadow2d.h>
#include <filtered_shadow2d.h>

namespace {
	void usage()
	{
		std::cout << "mcalign start_angle end_angle increment src_images_directory dst_images_directory\n";
	}

	void mcalign(shadow2d& sh, point2d pntCenter)
	{
		shadow2d::iterator p = sh.begin();
		shadow2d::iterator e = sh.end();

		projection2d pr(p->get_size(), p->get_point_size(), p->get_angle());
		for (; p != e; ++p)
		{
			deviation_t d(pntCenter - p->get_mass_center());
			pr.assign_and_deviate(*p, d);
			pr.swap(*p);
		}
	}
}

int main(int argc, char* argv[])
{
	if (argc < 6)
	{
		usage();
		return 0;
	}
	try {
		Magick::InitializeMagick(argv[0]);

		angle_t
			start    (lexical_cast<double>(argv[1])),
			end      (lexical_cast<double>(argv[2])),
			increment(lexical_cast<double>(argv[3]));
		angle_series_t series(start, end, increment);

		shadow2d shadow(series);
		cout << "Loading projections..." << endl;
		shadow.load_from_image(argv[4]);

		size2d_t sz = shadow.get_size();
		mcalign(shadow, point2d(sz.width / 2, sz.height / 2));

		shadow.save_to_image(argv[5]);
	}
	catch (boost::bad_lexical_cast &ex) {
		cout << ex.what() << endl;
	}
	catch (Magick::Exception &ex) {
		cout << ex.what() << endl;
	}
	catch (std::exception &e) {
		cout << e.what() << endl;
	}
	catch (...) {
		cout << "Unknown exception occured." << endl;
	}

	return 0;
}