// Standard headers
#include <cstdlib>
#include <cmath>
#include <random>
#include <vector>
#include <algorithm>
#include <iostream>
#include <thread>
#include <functional>
#include <limits>
#include <signal.h>
#include "std_msgs/Bool.h"
#include "cmath"
#include <iterator>
#include <boost/range/combine.hpp>

// My headers
#include "common_mav.h"
#include "graph.h"
#include "airsim_ros_pkgs/get_trajectory.h"
#include "cinematography/drone_state.h"
#include "airsim_ros_pkgs/MultiDOFarray.h"
#include "cinematography/artistic_spec.h"
#include "visualization_msgs/Marker.h"
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/PoseArray.h>
#include <trajectory_msgs/MultiDOFJointTrajectoryPoint.h>

// Octomap specific headers
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include <octomap_msgs/GetOctomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap_server/OctomapServer.h>

// Trajectory smoothening headers
#include <mav_trajectory_generation/polynomial_optimization_linear.h>
#include <mav_trajectory_generation/trajectory.h>
#include <mav_trajectory_generation/trajectory_sampling.h>

// OMPL specific headers
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/planners/prm/PRM.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/config.h>

#define DEG_TO_RAD(d)   d*M_PI/180
#define RAD_TO_DEG(r)   r*180/M_PI

using namespace std;
using namespace octomap;
using piecewise_trajectory = std::vector<graph::node>;
using smooth_trajectory = mav_trajectory_generation::Trajectory;
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;

ros::Publisher traj_pub;
ros::Publisher rviz_pub;
ros::Publisher rviz_actor_pub;
ros::Publisher rviz_drone_pub;

namespace ob = ompl::base;
namespace og = ompl::geometric;

geometry_msgs::Point position;
geometry_msgs::Twist twist;
geometry_msgs::Twist acceleration;

// TODO: change the bound to something meaningful
double x__low_bound__global = -200, x__high_bound__global = 200;
double y__low_bound__global = -200 , y__high_bound__global = 200;
double z__low_bound__global = 0, z__high_bound__global = 40;
double sampling_interval__global = 0.5;
double v_max__global = 3, a_max__global = 5;
float g_planning_budget = 4;
std::string motion_planning_core_str;

octomap::OcTree * octree = nullptr;
airsim_ros_pkgs::MultiDOFarray traj_topic;
bool g_requested_trajectory = false;
bool path_found = false;

double drone_height__global = 0.6;
double drone_radius__global = 2;

// Define default artistic constraints
double viewport_heading = M_PI / 3;
double viewport_pitch = M_PI / 4;
double viewport_distance = 5;

std::function<piecewise_trajectory (geometry_msgs::Point, geometry_msgs::Point, int, int , int, octomap::OcTree *)> motion_planning_core;

void setup();
bool collision(octomap::OcTree * octree, const graph::node& n1, const graph::node& n2, graph::node * end_ptr = nullptr);
bool out_of_bounds(const graph::node& pos);
// *** F:DN Optimize and smoothen a piecewise path without causing any new collisions.
smooth_trajectory smoothen_the_shortest_path(piecewise_trajectory& piecewise_path, octomap::OcTree* octree, Eigen::Vector3d initial_velocity, Eigen::Vector3d initial_acceleration);

// ***F:DN Build the response to the service from the smooth_path
airsim_ros_pkgs::MultiDOFarray create_traj_msg(smooth_trajectory& smooth_path);

// ***F:DN Use the PRM sampling method to find a piecewise path
piecewise_trajectory PRM(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree);

// ***F:DN Use the RRT sampling method to find a piecewise path
piecewise_trajectory RRT(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree);

// ***F:DN Use the RRT sampling method from OMPL to find a piecewise path
piecewise_trajectory OMPL_RRT(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree);

// ***F:DN Use bi-directonal RRT from OMPL to find a piecewise path
piecewise_trajectory OMPL_RRTConnect(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree);

// ***F:DN Use the PRM sampling method from OMPL to find a piecewise path
piecewise_trajectory OMPL_PRM(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree);

piecewise_trajectory OMPL_RRTstar(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree);

#ifdef INFLATE
  bool collision(octomap::OcTree * octree, const graph::node& n1, const graph::node& n2, graph::node * end_ptr)
  {
      // First, check if anything goes underground
      if (n1.z <= 0 || n2.z <= 0)
          return true;

      double dx = n2.x - n1.x;
      double dy = n2.y - n1.y;
      double dz = n2.z - n1.z;

      double distance = std::sqrt(dx*dx + dy*dy + dz*dz);

      octomap::point3d start(n1.x, n1.y, n1.z);
      octomap::point3d direction(dx, dy, dz);
      octomap::point3d end;

      bool collided = octree->castRay(start, direction, end, true, distance);

      if (end_ptr != nullptr && collided) {
          end_ptr->x = end.x();
          end_ptr->y = end.y();
          end_ptr->z = end.z();
      }

      return collided;
  }
#else
  bool collision(octomap::OcTree * octree, const graph::node& n1, const graph::node& n2, graph::node * end_ptr)
  {

      // First, check if anything goes underground
      if (n1.z <= 0 || n2.z <= 0)
          return true;

      const double pi = 3.14159265359;

      // The drone is modeled as a cylinder.
      // Angles are in radians and lengths are in meters.

      double height = drone_height__global;
      double radius = drone_radius__global;

      const double angle_step = pi/4;
      const double radius_step = radius/3;
      const double height_step = height/2;

      double dx = n2.x - n1.x;
      double dy = n2.y - n1.y;
      double dz = n2.z - n1.z;

      double distance = std::sqrt(dx*dx + dy*dy + dz*dz);

      octomap::point3d direction(dx, dy, dz);
      octomap::point3d end;

      for (double h = -height/2; h <= height/2; h += height_step) {
        for (double r = 0; r <= radius; r += radius_step) {
          for (double a = 0; a <= pi*2; a += angle_step) {
            octomap::point3d start(n1.x + r*std::cos(a), n1.y + r*std::sin(a), n1.z + h);

            if (octree->castRay(start, direction, end, true, distance)) {

                        if (end_ptr != nullptr) {
                            end_ptr->x = end.x();
                            end_ptr->y = end.y();
                            end_ptr->z = end.z();
                        }
              return true;
              }
          }
        }
      }

      return false;
  }
#endif

bool out_of_bounds(const graph::node& pos) {
    return (pos.x < x__low_bound__global
            || pos.x > x__high_bound__global
            || pos.y < y__low_bound__global
            || pos.y > y__high_bound__global
            || pos.z < z__low_bound__global
            || pos.z > z__high_bound__global);
}

bool occupied(octomap::OcTree * octree, double x, double y, double z){
  const double OCC_THRESH = 0.5;
  octomap::OcTreeNode * otn = octree->search(x, y, z);
  // if(otn == nullptr){
  //   return false;
  // }
  // else{
  //   return otn->getOccupancy() >= OCC_THRESH;
  // }
    // Debug
    // cout << x << "  " << y << " " << z << "   ";
    // if(otn != nullptr){
    //   cout << otn->getOccupancy() << endl;
    // }
    // else{
    //   cout << "null pointer" << endl;
    // }

  // if it is nullptr, return false, meanning it is not occupied
  // if it is not null ptr, check the probability, return occupied if
  // the prob is bigger than the threashould.
  return otn != nullptr && otn->getOccupancy() >= OCC_THRESH;
}

bool OMPLStateValidityChecker(const ompl::base::State * state)
{
    const auto *pos = state->as<ob::RealVectorStateSpace::StateType>();

    double x = pos->values[0];
    double y = pos->values[1];
    double z = pos->values[2];

    return !occupied(octree, x, y, z);
}

class OMPLMotionValidator : public ompl::base::MotionValidator
{
	public:
	    OMPLMotionValidator(const ompl::base::SpaceInformationPtr &si)
	        : ompl::base::MotionValidator(si)
	    {
	    }

	    bool checkMotion(const ompl::base::State *s1,
	            const ompl::base::State *s2) const override
	    {
	        const auto *pos1 = s1->as<ob::RealVectorStateSpace::StateType>();
	        const auto *pos2 = s2->as<ob::RealVectorStateSpace::StateType>();

	        double x1 = pos1->values[0], x2 = pos2->values[0];
	        double y1 = pos1->values[1], y2 = pos2->values[1];
	        double z1 = pos1->values[2], z2 = pos2->values[2];

	        graph::node end;
	        return !collision(octree, {x1,y1,z1}, {x2,y2,z2}, &end);
	    }

	    bool checkMotion(const ompl::base::State *s1,
	            const ompl::base::State *s2,
	            std::pair<ompl::base::State*, double>& lastValid) const override
	    {
	        namespace ob = ompl::base;

	        const auto *pos1 = s1->as<ob::RealVectorStateSpace::StateType>();
	        const auto *pos2 = s2->as<ob::RealVectorStateSpace::StateType>();

	        double x1 = pos1->values[0], x2 = pos2->values[0];
	        double y1 = pos1->values[1], y2 = pos2->values[1];
	        double z1 = pos1->values[2], z2 = pos2->values[2];

	        graph::node end;
	        bool valid = !collision(octree, {x1,y1,z1}, {x2,y2,z2}, &end);

	        if (!valid) {
	            auto *end_pos = lastValid.first->as<ob::RealVectorStateSpace::StateType>();
	            end_pos->values[0] = end.x;
	            end_pos->values[1] = end.y;
	            end_pos->values[2] = end.z;

	            double dx = x2-x1, dy = y2-y1, dz = z2-z1;
	            double end_dx = end.x-x1, end_dy = end.y-y1, end_dz = end.z-z1;

	            if (dx != 0)
	                lastValid.second = end_dx / dx;
	            else if (dy != 0)
	                lastValid.second = end_dy / dy;
	            else if (dz != 0)
	                lastValid.second = end_dz / dz;
	            else
	                lastValid.second = 0;
	        }

	        return valid;
	    }

};

void postprocess(piecewise_trajectory& path)
{
    // We use a greedy approach to shorten the path here.
    // We connect non-adjacent nodes in the path that do not have collisions.
    for (auto it = path.begin(); it != path.end()-1; ) {
        bool shortened = false;
        for (auto it2 = path.end()-1; it2 != it+1 && !shortened; --it2) {
            if (!collision(octree, *it, *it2)) {
                it = path.erase(it+1, it2);
                shortened = true;
            }
        }

        if (!shortened)
            ++it;
    }
}

// function for generating octree from octomap msgs
void generate_octomap(const octomap_msgs::Octomap& msg)
{
    //cout << "we are generating octomap from octomap binary" << endl;
    if (octree != nullptr) {
        delete octree;
    }
    octomap::AbstractOcTree * tree = octomap_msgs::msgToMap(msg);
     octree = dynamic_cast<octomap::OcTree*> (tree);

    if (octree == nullptr) {
        ROS_ERROR("Octree could not be pulled.");
    }
}

void print_rviz_traj(piecewise_trajectory path, std::string name, bool actor) {
    visualization_msgs::Marker rviz_path = visualization_msgs::Marker();
    rviz_path.header.seq = 0;
    rviz_path.header.stamp = ros::Time::now();
    rviz_path.header.frame_id = "world_ned";
    rviz_path.ns = name;
    rviz_path.id = 0;
    rviz_path.type = visualization_msgs::Marker::POINTS;
    rviz_path.action = visualization_msgs::Marker::ADD;
    rviz_path.pose.orientation.w = 1.0;
    rviz_path.scale.x = 0.2;
    rviz_path.scale.y = 0.2;
    rviz_path.color.g = 1.0f;
    rviz_path.color.a = 1.0;
    for(graph::node n : path) {
        geometry_msgs::Point p;
        p.x = n.x;
        p.y = n.y;
        p.z = n.z;
        rviz_path.points.push_back(p);
    }
    if (actor)
        rviz_actor_pub.publish(rviz_path);
    else
        rviz_drone_pub.publish(rviz_path);
}

void print_rviz_traj(trajectory_msgs::MultiDOFJointTrajectory path, std::string name, bool actor) {
    geometry_msgs::PoseArray rviz_path = geometry_msgs::PoseArray();
    rviz_path.header.seq = 0;
    rviz_path.header.stamp = ros::Time::now();
    rviz_path.header.frame_id = "world_ned";
    for(trajectory_msgs::MultiDOFJointTrajectoryPoint n : path.points) {
        geometry_msgs::Pose p;
        p.orientation = n.transforms.data()->rotation;
        p.position.x = n.transforms.data()->translation.x;
        p.position.y = n.transforms.data()->translation.y;
        p.position.z = n.transforms.data()->translation.z;
        rviz_path.poses.push_back(p);
    }

    if (actor)
        rviz_actor_pub.publish(rviz_path);
    else
        rviz_drone_pub.publish(rviz_path);
}

void print_rviz_traj(airsim_ros_pkgs::MultiDOFarray path, std::string name, bool actor) {
    geometry_msgs::PoseArray rviz_path = geometry_msgs::PoseArray();
    rviz_path.header.seq = 0;
    rviz_path.header.stamp = ros::Time::now();
    rviz_path.header.frame_id = "world_ned";
    for(airsim_ros_pkgs::MultiDOF n : path.points) {
        geometry_msgs::Pose p;
        p.orientation = tf2::toMsg(tf2::Quaternion(0, 0, sin(n.yaw / 2), cos(n.yaw / 2)));
        p.position.x = n.x;
        p.position.y = n.y;
        p.position.z = n.z;
        rviz_path.poses.push_back(p);
    }

    if (actor)
        rviz_actor_pub.publish(rviz_path);
    else
        rviz_drone_pub.publish(rviz_path);
}

// Calculate an ideal drone trajectory using a given actor trajectory (both in NED and radians)
airsim_ros_pkgs::MultiDOFarray calc_ideal_drone_traj(const airsim_ros_pkgs::MultiDOFarray& actor_traj) {
    airsim_ros_pkgs::MultiDOFarray drone_traj;
    drone_traj.points.reserve(actor_traj.points.size());

    float horiz_dist = cos(viewport_pitch) * viewport_distance;
    float height = sin(viewport_pitch) * viewport_distance;

    // For each point in the actor's trajectory...
    for (airsim_ros_pkgs::MultiDOF point : actor_traj.points) {
        airsim_ros_pkgs::MultiDOF n;

        // Center p on the actor to get the drone's ideal coordinates
        n.x = point.x + cos(viewport_heading + point.yaw) * horiz_dist;
        n.y = point.y + sin(viewport_heading + point.yaw) * horiz_dist;
        n.z = point.z - height;
        n.yaw = point.yaw + M_PI + viewport_heading;
        n.duration = point.duration;

        if (n.yaw > M_PI) {
            n.yaw -= 2*M_PI;
        }
        drone_traj.points.push_back(n);
    }
    return drone_traj;
}

void face_actor(airsim_ros_pkgs::MultiDOFarray& drone_traj, const airsim_ros_pkgs::MultiDOFarray& actor_traj) {
    if (drone_traj.points.size() != actor_traj.points.size()) {
        ROS_ERROR("Cannot face actor. Two trajectories don't match in number of points");
    }

    std::vector<airsim_ros_pkgs::MultiDOF>::iterator dit = drone_traj.points.begin();
    std::vector<airsim_ros_pkgs::MultiDOF>::const_iterator ait = actor_traj.points.begin();

    double d_time = dit->duration, a_time = 0;
    for(; ait < actor_traj.points.end(); ait++) {
        a_time = ait->duration;

        // There will be more drone points than actor points. Match each drone point to the most recent actor point.
        while(d_time < a_time) {
//            // Convert point's position to tf2 for math
//            tf2::Vector3 d_pos, a_pos;
//            tf2::convert(dit->transforms.data()->translation, d_pos);
//            tf2::convert(ait->transforms.data()->translation, a_pos);
//
//            // Calculate the rotation from forward facing to facing the actor
//            tf2::Vector3 diff = a_pos - d_pos;
//            tf2::Vector3 forward = tf2::Vector3(0,1,0);
//            tf2::Quaternion orient = tf2::shortestArcQuatNormalize2(forward, diff);
//
//            // Rebuild the transform and put it back in the drone trajectory
////            geometry_msgs::Transform t;
////            t.translation = tf2::toMsg(d_pos);
//            dit->transforms[0].rotation = tf2::toMsg(orient);
//            //dit->transforms.push_back(t);

            // Get the vector difference (just x and y, since we only care about the yaw of the drone)
            tf2::Vector3 diff = tf2::Vector3(ait->x, ait->y, 0) - tf2::Vector3(dit->x, dit->y, 0);

            // Get the angle
            double angle = atan(diff.y() / diff.x());

            // Double check which cartesian quadrant you're in and add/subtract a 180 offset if necessary
            if (diff.x() < 0) {
                if (diff.y() < 0) {
                    angle -= M_PI;
                } else {
                    angle += M_PI;
                }
            }

            // Get the next drone point
            dit++;
            d_time = dit->duration;
        }

    }

    return;
}

//======================VVV==Cost functions==VVV====================================

double traj_smoothness(const airsim_ros_pkgs::MultiDOFarray& drone_traj) {
    return 1;
}

double shot_quality(const airsim_ros_pkgs::MultiDOFarray& drone_traj, airsim_ros_pkgs::MultiDOFarray& ideal_traj) {
    return 1;
}

double obstacle_avoidance(const airsim_ros_pkgs::MultiDOFarray& drone_traj) {   // TODO: Add 2nd argument for TSDF
    return 1;
}

double occlusion_avoidance(const airsim_ros_pkgs::MultiDOFarray& drone_traj, airsim_ros_pkgs::MultiDOFarray& actor_traj) {   // TODO: Add 3rd argument for TSDF
    return 1;
}

double traj_cost_function(const airsim_ros_pkgs::MultiDOFarray& drone_traj, airsim_ros_pkgs::MultiDOFarray& actor_traj, airsim_ros_pkgs::MultiDOFarray& ideal_traj) {      // TODO: Add 4th argument for TSDF
    double LAMBDA_1, LAMBDA_2, LAMBDA_3 = 1;    // TODO: Have these specified as ROS parameters

    return traj_smoothness(drone_traj) + LAMBDA_1 * obstacle_avoidance(drone_traj) + LAMBDA_2 * occlusion_avoidance(drone_traj, actor_traj) + LAMBDA_3 * shot_quality(drone_traj, ideal_traj);
}

// TODO: Implement gradient equivalents of all the above, and hessian approximations (A_smooth + delta_1 * A_shot)

void optimize_trajectory(const airsim_ros_pkgs::MultiDOFarray& drone_traj, airsim_ros_pkgs::MultiDOFarray& actor_traj) {
    airsim_ros_pkgs::MultiDOFarray ideal_traj = calc_ideal_drone_traj(actor_traj);     // ξ_shot when calculating shot quality

    int MAX_ITERATIONS;     // TODO: Make this a ROS parameter
    for(int i = 0; i < 1000; i++) {

    }
}

//======================^^^==Cost functions==^^^====================================

smooth_trajectory smoothen_the_shortest_path(piecewise_trajectory& piecewise_path, octomap::OcTree* octree, Eigen::Vector3d initial_velocity, Eigen::Vector3d initial_acceleration)
{
    // Variables for visualization for debugging purposes
    double distance = 0.5;

    // Setup optimizer
    mav_trajectory_generation::Vertex::Vector vertices;
    const int dimension = 3;
    const int derivative_to_optimize = mav_trajectory_generation::derivative_order::SNAP;

    // Convert roadmap path to optimizer's path format
    mav_trajectory_generation::Vertex start_v(dimension), end_v(dimension);
    start_v.addConstraint(mav_trajectory_generation::derivative_order::VELOCITY, initial_velocity);
    start_v.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(piecewise_path.front().x, piecewise_path.front().y, piecewise_path.front().z));

    end_v.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(piecewise_path.back().x, piecewise_path.back().y, piecewise_path.back().z));
    end_v.addConstraint(mav_trajectory_generation::derivative_order::VELOCITY, Eigen::Vector3d(0,0,0));

    vertices.push_back(start_v);
    for (auto it = piecewise_path.begin()+1; it+1 != piecewise_path.end(); ++it) {
      mav_trajectory_generation::Vertex v(dimension);
      v.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(it->x, it->y, it->z));
      vertices.push_back(v);
    }
    vertices.push_back(end_v);

    const int N = 10;
    mav_trajectory_generation::PolynomialOptimization<N> opt(dimension);

    // Optimize until no collisions are present
    bool col;
    do {
      col = false;
      // Estimate the time the drone should take flying between each node
      auto segment_times = estimateSegmentTimes(vertices, v_max__global, a_max__global);

      std::vector<double>  times;
      for (auto el :segment_times) {
          times.push_back(.8*el);
      }

      // Optimize and create a smooth path from the vertices
      opt.setupFromVertices(vertices, times, derivative_to_optimize);
      opt.solveLinear();

      // Return all the smooth segments in the path
      // (Each segment goes from one of the original nodes to the next one in the path)
      mav_trajectory_generation::Segment::Vector segments;
      opt.getSegments(&segments);

      // Loop through the vector of segments looking for collisions
      for (int i = 0; !col && i < segments.size(); ++i) {
        const double time_step = 0.1;
        double segment_len = segments[i].getTime();

        auto segment_start = *(piecewise_path.begin() + i);
        auto segment_end = *(piecewise_path.begin() + i + 1);

        // Step through each individual segment, at increments of "time_step" seconds, looking for a collision
        for (double t = 0; t < segment_len - time_step; t += time_step) {
            auto pos1 = segments[i].evaluate(t);
            auto pos2 = segments[i].evaluate(t + time_step);

            graph::node n1 = {pos1.x(), pos1.y(), pos1.z()};
            graph::node n2 = {pos2.x(), pos2.y(), pos2.z()};

            // Check for a collision between two near points on the segment
            if (out_of_bounds(n1) || out_of_bounds(n2) || collision(octree, n1, n2)) {
                // Add a new vertex in the middle of the segment we are currently on
                mav_trajectory_generation::Vertex middle(dimension);

                double middle_x = (segment_start.x + segment_end.x) / 2;
                double middle_y = (segment_start.y + segment_end.y) / 2;
                double middle_z = (segment_start.z + segment_end.z) / 2;

                middle.addConstraint(mav_trajectory_generation::derivative_order::POSITION, Eigen::Vector3d(middle_x, middle_y, middle_z));

                vertices.insert(vertices.begin()+i+1, middle);

                // Add a new node to the piecewise path where the vertex is
                graph::node middle_node = {middle_x, middle_y, middle_z};
                piecewise_path.insert(piecewise_path.begin()+i+1, middle_node);

                col = true;

                break;
            }
        }
      }
  } while (col);

  // Return the collision-free smooth trajectory
  mav_trajectory_generation::Trajectory traj;
  opt.getTrajectory(&traj);

  return traj;
}

//bool get_trajectory_fun(airsim_ros_pkgs::get_trajectory::Request &req, airsim_ros_pkgs::get_trajectory::Response &res)
// Get actor's predicted trajectory (in NED and radians)
void get_actor_trajectory(const airsim_ros_pkgs::MultiDOFarray& actor_traj)
{
    if (actor_traj.points.size() == 0) {
        ROS_ERROR("Received empty actor path");
        return;
    }

    print_rviz_traj(actor_traj, "actor_traj", true);

    airsim_ros_pkgs::MultiDOFarray ideal_path;
    smooth_trajectory smooth_path;

    geometry_msgs::Point start;
    start.x += twist.linear.x*g_planning_budget;
    start.y += twist.linear.y*g_planning_budget;
    start.z += twist.linear.z*g_planning_budget;

    ideal_path = calc_ideal_drone_traj(actor_traj);

    print_rviz_traj(ideal_path, "drone_traj", false);

    // TODO: Put in artificial load here to simulate optimizing

    for(int i = 1; i < ideal_path.points.size(); i++) {
        ideal_path.points[i-1].vx = (ideal_path.points[i].x - ideal_path.points[i-1].x) / ideal_path.points[i-1].duration;
        ideal_path.points[i-1].vy = (ideal_path.points[i].y - ideal_path.points[i-1].y) / ideal_path.points[i-1].duration;
        ideal_path.points[i-1].vz = (ideal_path.points[i].z - ideal_path.points[i-1].z) / ideal_path.points[i-1].duration;
    }

    // Publish the trajectory (for debugging purposes)
    ideal_path.header.stamp = ros::Time::now();
    path_found = true;

    ROS_INFO("Publishing drone trajectory");

    traj_pub.publish(ideal_path);
}

airsim_ros_pkgs::MultiDOFarray create_traj_msg(smooth_trajectory& smooth_path)
{
    const double safe_radius = 1.0;
    airsim_ros_pkgs::MultiDOFarray multiDOFtrajectory = airsim_ros_pkgs::MultiDOFarray();

    multiDOFtrajectory.header.seq = 0;
    multiDOFtrajectory.header.stamp = ros::Time::now(); //DEBUGGING. Should be 0
    multiDOFtrajectory.header.frame_id = "world_enu";

    // Sample trajectory
    mav_msgs::EigenTrajectoryPoint::Vector states;

    mav_trajectory_generation::sampleWholeTrajectory(smooth_path, sampling_interval__global, &states);

    // Get starting position
    graph::node start = {states[0].position_W.x(), states[0].position_W.y(), states[0].position_W.z()};

//    // Convert sampled trajectory points to MultiDOFJointTrajectory response
//    multiDOFtrajectory.joint_names.push_back("base");

    int state_index = 0;

    for (const auto& s : states) {
        airsim_ros_pkgs::MultiDOF point;

        geometry_msgs::Transform pos;
        graph::node current;

        point.x = current.x = s.position_W.x();
        point.y = current.y = s.position_W.y();
        point.z = current.z = s.position_W.z();

        geometry_msgs::Twist vel;
        point.vx = s.velocity_W.x();
        point.vy = s.velocity_W.y();
        point.vz = s.velocity_W.z();

        geometry_msgs::Twist acc;
        point.ax = s.acceleration_W.x();
        point.ay = s.acceleration_W.y();
        point.az = s.acceleration_W.z();

        ros::Duration dur(float(s.time_from_start_ns) / 1e9);

        // if (res.unknown != -1 &&
        //         !known(octree, current.x, current.y, current.z)
        //         && dist(start, current) > safe_radius) {
        //     ROS_WARN("Trajectory enters unknown space.");
        //     res.unknown = state_index;
        // }

        multiDOFtrajectory.points.push_back(point);

        state_index++;
    }

    return multiDOFtrajectory;
}

void get_drone_state(const cinematography::drone_state::ConstPtr& state) {
    position = state->position;
    twist = state->twist;
    acceleration = state->acceleration;

    x__low_bound__global = std::min(x__low_bound__global, position.x);
    x__high_bound__global = std::max(x__high_bound__global, position.x);
    y__low_bound__global = std::min(y__low_bound__global, position.y);
    y__high_bound__global = std::max(y__high_bound__global, position.y);
    z__low_bound__global = std::min(z__low_bound__global, position.z);
    z__high_bound__global = std::max(z__high_bound__global, position.z);
}

void get_artistic_specs(const cinematography::artistic_spec::ConstPtr& as) {
    viewport_heading = as->heading;
    viewport_pitch = as->pitch;
    viewport_distance = as->distance;
}

// set for package delivery
void setup(){
  // set up our planner
  ros::param::get("/motion_planner/motion_planning_core", motion_planning_core_str);
  if (motion_planning_core_str == "OMPL-RRT")
      motion_planning_core = OMPL_RRT;
  else if (motion_planning_core_str == "OMPL-RRTConnect")
      motion_planning_core = OMPL_RRTConnect;
  else if (motion_planning_core_str == "OMPL-PRM")
      motion_planning_core = OMPL_PRM;
  else if (motion_planning_core_str == "OMPL_RRTstar")
      motion_planning_core = OMPL_RRTstar;

  ros::param::get("/planning_budget", g_planning_budget);
  ros::param::get("/motion_planner/sampling_interval", sampling_interval__global);
  ros::param::get("/motion_planner/planner_drone_radius", drone_radius__global);
  ros::param::get("/motion_planner/planner_drone_height", drone_height__global);
  ros::param::get("/motion_planner/v_max", v_max__global);
  ros::param::get("/motion_planner/a_max", a_max__global);
}

int main(int argc, char ** argv)
{
    ros::init(argc, argv, "motion_planner");
    ros::NodeHandle nh("/motion_planner");

    motion_planning_core = OMPL_RRTConnect;
    setup();

    //ros::ServiceServer service = nh.advertiseService("get_trajectory_srv", get_trajectory_fun);
    ros::Subscriber actor_traj_sub = nh.subscribe("/actor_traj", 1, get_actor_trajectory);
    ros::Subscriber octomap_sub = nh.subscribe("/octomap_binary", 1, generate_octomap);
    ros::Subscriber drone_state_sub = nh.subscribe("/drone_state", 1, get_drone_state);
    ros::Subscriber art_spec_sub = nh.subscribe("artistic_specs", 1, get_artistic_specs);
    traj_pub = nh.advertise<airsim_ros_pkgs::MultiDOFarray>("/multidoftraj", 1);
    rviz_pub = nh.advertise<visualization_msgs::Marker>("/scanning_visualization_marker", 1);
    rviz_actor_pub = nh.advertise<geometry_msgs::PoseArray>("/rviz_actor_traj", 1);
    rviz_drone_pub = nh.advertise<geometry_msgs::PoseArray>("/rviz_drone_traj", 1);

    // Sleep
    ros::spin();
    return 0;
}

template<class PlannerType>
piecewise_trajectory OMPL_plan(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree)
{
#ifndef INFLATE

    piecewise_trajectory result;

    auto space(std::make_shared<ob::RealVectorStateSpace>(3));

    // Set bounds
    ob::RealVectorBounds bounds(3);
    bounds.setLow(0, x__low_bound__global);
    bounds.setHigh(0, x__high_bound__global);
    bounds.setLow(1, y__low_bound__global);
    bounds.setHigh(1, y__high_bound__global);
    bounds.setLow(2, z__low_bound__global);
    bounds.setHigh(2, z__high_bound__global);

    space->setBounds(bounds);

    og::SimpleSetup ss(space);

    // Setup collision checker
    ob::SpaceInformationPtr si = ss.getSpaceInformation();
    si->setStateValidityChecker(OMPLStateValidityChecker);
    si->setMotionValidator(std::make_shared<OMPLMotionValidator>(si));
    si->setup();

    // Set planner
    //ob::PlannerPtr planner(new og::RRTstar(si));
    ob::PlannerPtr planner(new PlannerType(si));
    ss.setPlanner(planner);

    ob::ScopedState<> start_state(space);
    start_state[0] = start.x;
    start_state[1] = start.y;
    start_state[2] = start.z;

    ob::ScopedState<> goal_state(space);
    goal_state[0] = goal.x;
    goal_state[1] = goal.y;
    goal_state[2] = goal.z;

    ss.setStartAndGoalStates(start_state, goal_state);

    ss.setup();

    // Solve for path
    ob::PlannerStatus solved = ss.solve(g_planning_budget);

    if (solved)
    {
        //ROS_INFO("Solution found!");
        ss.simplifySolution();

        for (auto state : ss.getSolutionPath().getStates()) {
            const auto *pos = state->as<ob::RealVectorStateSpace::StateType>();

            double x = pos->values[0];
            double y = pos->values[1];
            double z = pos->values[2];

            result.push_back({x, y, z});
        }
    }
    else
        ROS_ERROR("Path not found!");

    return result;
#else
    ROS_ERROR("OMPL-based planners cannot be compiled together with inflation!");
#endif
}

piecewise_trajectory OMPL_RRT(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree)
{
    return OMPL_plan<ompl::geometric::RRT>(start, goal, width, length, n_pts_per_dir, octree);
}

piecewise_trajectory OMPL_RRTConnect(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree)
{
    return OMPL_plan<ompl::geometric::RRTConnect>(start, goal, width, length, n_pts_per_dir, octree);
}

piecewise_trajectory OMPL_PRM(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree)
{
    return OMPL_plan<ompl::geometric::PRM>(start, goal, width, length, n_pts_per_dir, octree);
}

piecewise_trajectory OMPL_RRTstar(geometry_msgs::Point start, geometry_msgs::Point goal, int width, int length, int n_pts_per_dir, octomap::OcTree * octree)
{
    return OMPL_plan<ompl::geometric::RRTstar>(start, goal, width, length, n_pts_per_dir, octree);
}


//=================================================================================================
// Copyright (c) 2011, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#include <hector_pose_estimation/measurements/baro.h>
#include <hector_pose_estimation/filter/set_filter.h>

#include <boost/bind.hpp>

namespace hector_pose_estimation {

template class Measurement_<BaroModel>;

BaroModel::BaroModel()
{
  stddev_ = 1.0;
  qnh_ = 1013.25;
  parameters().add("qnh", qnh_);
}

BaroModel::~BaroModel() {}

void BaroModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
{
  y_pred(0) = qnh_ * pow(1.0 - (0.0065 * (state.getPosition().z() + getElevation())) / 288.15, 5.255);
}

void BaroModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
{
  if (state.position()) {
    state.position()->cols(C)(0,Z) = qnh_ * 5.255 * pow(1.0 - (0.0065 * (state.getPosition().z() + getElevation())) / 288.15, 4.255) * (-0.0065 / 288.15);
  }
}

double BaroModel::getAltitude(const BaroUpdate& update)
{
  return 288.15 / 0.0065 * (1.0 - pow(update.getVector()(0) / qnh_, 1.0/5.255));
}

BaroUpdate::BaroUpdate() : qnh_(0) {}
BaroUpdate::BaroUpdate(double pressure) : qnh_(0) { *this = pressure; }
BaroUpdate::BaroUpdate(double pressure, double qnh) : qnh_(qnh) { *this = pressure; }

Baro::Baro(const std::string &name)
  : Measurement_<BaroModel>(name)
  , HeightBaroCommon(this)
{
  parameters().add("auto_elevation", auto_elevation_);
}

void Baro::onReset()
{
  HeightBaroCommon::onReset();
}

bool Baro::prepareUpdate(State &state, const Update &update) {
  if (update.qnh() != 0) setQnh(update.qnh());
  // Note: boost::bind is not real-time safe!
  setElevation(resetElevation(state, boost::bind(&BaroModel::getAltitude, getModel(), update)));
  return true;
}

} // namespace hector_pose_estimation


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

#include "nmap.hpp"
#include "gdal.h"
#include "gdal_priv.h"
#include "cpl_conv.h"
#include <armadillo>

//Detect if compiling with nvcc
#ifdef BUILD_WITH_CUDA
    #define RUN_CUDA_NMAP 1
#else
    #define RUN_CUDA_NMAP 0
#endif

int nmap_process(nmapOptions *opts)
{

    //Flag for test
    bool useKS = false;

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

    if (opts->method.compare("KS2") == 0)
    {
        std::cout << "Using Kolmogorov-Smirnov 2-sample test\n";
        useKS = true;
    }
    else if (opts->method.compare("AD2") == 0)
    {
        std::cout << "Using Anderson-Darling 2-sample test \n";
    }
    else
    {
        std::cout << "Statistics method can be KS2 or AD2\n";
        std::cout << "Unknown method: " << opts->method << "\n";
        std::cout << "Returning with non-zero error code \n";
        return 1;
    }

    int cols, rows, nbands;
    int blockysize, boxesperblock;

    //First thing to do is to read dimenions.
    GDALDataset* inDataset = NULL;
    GDALDataset* ncountDataset = NULL;
    GDALDataset* wtsDataset = NULL;
    GDALDataset* mskDataset = NULL;

    //Workers
    KS2sample *ksworkers;
    AD2unique *adworkers;

    //Clock variables
    double t_start, t_end;

    //Register GDAL drivers
    GDALAllRegister();

    //Determine the number of uint32 bytes needed to store weights
    int nulong = ceil( ((2*opts->Ny+1)*(2*opts->Nx+1))/ 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";

    checkLongSetting();

    //Check GPU settings
    bool useGPU = !(opts->noGPU);

    if(RUN_CUDA_NMAP)
    {
        if(useGPU)
        {
            std::cout << "User has requested GPU and a device seems to be available. \n";
            std::cout << "Will execute GPU version. \n";
        }
        else
        {
            std::cout << "User had requested no GPU but device is available. \n";
            std::cout << "Executing CPU version. \n";
        }
    }
    else
    {
        std::cout << "NO GPU Available - executing CPU version \n";
        useGPU = false;
    }

    //See if mask is available
    if (!opts->maskDS.empty())
    {
        if (strcasecmp(opts->maskDS.c_str(), "None") != 0)
        {
            mskDataset = reinterpret_cast<GDALDataset *>( GDALOpenShared( opts->maskDS.c_str(), GA_ReadOnly));
            if (mskDataset == NULL)
            {
                std::cout << "Cannot open mask file { " << opts->maskDS << " } with GDAL for reading. \n";
                std::cout << "Exiting with error code .... (102) \n";
                GDALDestroyDriverManager();
                return 102;
            }

            int cc = mskDataset->GetRasterXSize();
            int rr = mskDataset->GetRasterYSize();
            int bb = mskDataset->GetRasterCount();
            int code = 0;

            if (cc != cols)
            {
                std::cout << "Mask file width does not match stack size width \n";
                code = 104;
            }

            if (rr != rows)
            {
                std::cout << "Mask file length does not match stack size length \n";
                code = 105;
            }

            if (bb != 1)
            {
                std::cout << "Mask file has more than one band \n";
                code = 106;
            }

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

    //Determine blocksizes
    boxesperblock = int((opts->memsize * 1.0e6)/cols) / (opts->blocksize * 4 * (nbands+2+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_fvec cpxdata(cols*blockysize);             //To read in one SLC
    arma::Col<unsigned char> zeromask(cols*blockysize); //Mask for one SLC

    //Actual arrays
    arma::fmat amp(nbands,cols*blockysize);             //Amplitudes of all bands
    arma::Mat<int> count(cols,blockysize);              //Count for all pixels
    arma::Mat<unsigned int> wts(nulong, cols*blockysize);   //Weights for all pixels

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

    //Store the normalizing constants
    arma::vec alpha(nbands);

    //Populate norms
    for (int bb=0; bb < nbands; bb++)
    {
        const char *bandconst = inDataset->GetRasterBand(bb+1)->GetMetadataItem( "amplitudeConstant", "slc");
        double bconst = 0.0;

        if (bandconst != NULL)
        {
             bconst = CPLScanDouble(bandconst, 20);
        }

        if(bconst <= 0.0)
        {
            bconst = 1.0;
            std::cout << "No calibration constant found for band " << bb+1 <<". Setting to 1.0. \n";
        }
        alpha[bb] = bconst;
    }

    double normval = alpha[0];
    for (int bb=0; bb < nbands; bb++)
    {
            alpha[bb] /= normval;
    }
    alpha[0] = 1.0;

    for(int bb=0; bb < nbands; bb++)
        std::cout << "Band " << bb+1 << ": " << alpha[bb] << "\n";

    //Creation of count dataset
    GDALDriver *poDriver = (GDALDriver*) GDALGetDriverByName("ENVI");
    char **mOptions = NULL;
    mOptions = CSLSetNameValue(mOptions, "INTERLEAVE", "BIP");
    mOptions = CSLSetNameValue(mOptions, "SUFFIX", "ADD");

    ncountDataset = (GDALDataset*) poDriver->Create(opts->ncountDS.c_str(), cols, rows, 1, GDT_Int16, mOptions);
    if (ncountDataset == NULL)
    {
        std::cout << "Could not create count dataset {" << opts->ncountDS << "} \n";
        std::cout << "Exiting with non-zero error code ... 104 \n";

        GDALClose(inDataset);
        GDALDestroyDriverManager();
        return 104;
    }

    wtsDataset = (GDALDataset*) poDriver->Create(opts->wtsDS.c_str(), cols, rows, nulong, GDT_UInt32, mOptions);
    if (wtsDataset == NULL)
    {
        std::cout << "Could not create weights dataset {" << opts->wtsDS << "}\n";
        std::cout << "Exiting with non-zero error code ... 105 \n";
        GDALClose(inDataset);
        GDALClose(ncountDataset);
        GDALDestroyDriverManager();
        return 105;
    }

    CSLDestroy(mOptions);

    nthreads = numberOfThreads();
    //Setup workers
    if (useKS)
    {
        ksworkers = new KS2sample[nthreads];
        for(int tt=0; tt < nthreads; tt++)
        {
            ksworkers[tt].length = nbands;
        }
    }
    else
    {
        adworkers = new AD2unique[nthreads];
        for(int tt=0; tt<nthreads; tt++)
        {
            adworkers[tt].init(nbands);
        }
    }

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

    const Ulongmask bitmask = {Ny,Nx};

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

#ifdef BUILD_NMAP_WITH_CUDA
    //Lock GPU if required
    if (useGPU) lockGPU();
#endif

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

        //Init to zeros
        amp.zeros();
        count.zeros();
        wts.zeros();
        zeromask.ones();

        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";

        //Read in the mask
        if (mskDataset != NULL)
        {
            status = mskDataset->GetRasterBand(1)->RasterIO( GF_Read,
                    0, yoff,
                    cols, inysize,
                    (void*) (zeromask.memptr()),
                    cols, inysize, GDT_Byte,
                    sizeof(unsigned char),
                    sizeof(unsigned char)*cols, NULL);

            if (status != 0)
            {
                std::cout << "Error reading mask band at line "<< yoff << "\n";
                std::cout << "Exiting with error code .... (108) \n";
                GDALClose(inDataset);
                GDALClose(ncountDataset);
                GDALClose(wtsDataset);
                GDALDestroyDriverManager();
                return 108;
            }
        }

        for(int bb=0; bb < nbands; bb++)
        {
            //Read in the data
            status = inDataset->GetRasterBand(bb+1)->RasterIO( GF_Read,
                        0, yoff,
                        cols, inysize,
                        (void*) (cpxdata.memptr()),
                        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 .... (108) \n";
                GDALClose(inDataset);
                GDALClose(ncountDataset);
                GDALDestroyDriverManager();
                return 108;
            }

//            std::cout << "Done reading band : " << bb+1 << "\n";

            //Read in normalized amplitude to amplitude array
    #pragma omp parallel for\
            default(shared)
            for(int jj=0;jj<(inysize*cols);jj++)
            {
                float val;
                val = std::abs(cpxdata(jj)) / alpha(bb);
                zeromask[jj] = (zeromask[jj] != 0) && (val != 0.) && (!std::isnan(val));
                if (zeromask[jj] != 0)
                {
                    amp(bb,jj) = val;
                }
            }
        }

        //Standard CPU-based processing
        if (!useGPU)
        {

            //Sort all the amp arrays because the Goodness-of-fit tests need this
    #pragma omp parallel for\
            default(shared)
            for(int jj=0; jj<(inysize*cols); jj++)
            {
                if(zeromask[jj] == 0) continue;

                float *ptr = amp.colptr(jj);
                std::sort(ptr, ptr+nbands);
            }

            //Fast computation
            //We only loop over half 2 quadrants
            //By symmetry similarity between pixel p and q
            //is same as similarity between q and p
            for(int ii=0; ii<=Ny; ii++)
            {
                int ymax = inysize-ii;
                int start = (ii==0)? 0:-Nx;

                for(int jj=start; jj<=Nx; jj++)
                {
                    int xmin = std::max(-jj,0);
                    int xmax = std::min(cols-jj, cols);

//                  std::cout << "dy = " << ii <<", dx = " << jj << "\n";
    #pragma omp parallel for\
            default(shared)
                    for(int pp=0; pp < (ymax*cols); pp++)
                    {

                        //Check zeromask
                        if (zeromask[pp] == 0) continue;

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

                        if ((cenjj < xmin) || (cenjj>=xmax)) continue;

                        int refii = (cenii+ii);
                        int refjj = (cenjj+jj);
                        int qq = refii*cols + refjj;

                        //Also check zeromask here
                        if (zeromask[qq]==0) continue;

                        float* cenpix = amp.colptr(pp);
                        float* refpix = amp.colptr(qq);

                        //Always include the center pixel
                        if (pp == qq)
                        {
                            count(pp) += 1;
                            bitmask.setbit( wts.colptr(pp), ii, jj, true);
                        }
                        else
                        {
                            int threadnum = omp_get_thread_num();
                            double prob;

                            if (useKS)
                            {
                                prob = ksworkers[threadnum].test(cenpix, refpix);
                            }
                            else
                            {
                                prob = adworkers[threadnum].test(cenpix, refpix);
                            }
//                          std::cout << cenii << "   " << cenjj << "  " << prob << "   " << prob1 << "\n";

                            //Check if similar
                            if (prob >= thresh)
                            {
                                count(pp) +=1;
                                count(qq) += 1;

                                bitmask.setbit( wts.colptr(pp), ii, jj, true);
                                bitmask.setbit( wts.colptr(qq), -ii, -jj, true);
                            }
                        }
                    }
                }
            }
        }
#ifdef BUILD_NMAP_WITH_CUDA
        else    //Use GPU for processing
        {
            nmapProcessBlock(amp.memptr(), zeromask.memptr(),
                        cols, inysize, nbands,
                        count.memptr(), wts.memptr(),
                        nulong, thresh,
                        Nx, Ny);

        }
#endif
        //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;
        }

        //Print debugs
/*        std::cout << "Block number: " << blockcount << "\n";
        std::cout << "yoff: " << yoff << "\n";
        std::cout << "firstlinetowrite: " << firstlinetowrite << "\n";
        std::cout << "linestowrite: " << linestowrite << "\n";
        std::cout << "rollback: " << rollback << "\n";

        std::cout << "Gdal block \n";
        std::cout << "First line : " << yoff + firstlinetowrite << "\n";
        std::cout << "Number lines: " << linestowrite << "\n";
        std::cout << "Next read start: " << yoff + linestowrite - rollback << "\n"; */

        //Write stuff to output files

        //Write to count file
        status = ncountDataset->GetRasterBand(1)->RasterIO(GF_Write, 0, yoff+firstlinetowrite,
                    cols, linestowrite,
                    (void*) (count.colptr(firstlinetowrite)),
                    cols, linestowrite, GDT_Int32,
                    sizeof(int), sizeof(int)*cols, NULL);

        status = wtsDataset->RasterIO(GF_Write,0,yoff+firstlinetowrite,
                            cols, linestowrite,
                            (void*)(wts.colptr(firstlinetowrite*cols)),
                            cols, linestowrite, GDT_UInt32,
                            nulong, NULL,
                            sizeof(unsigned int)*nulong,
                            sizeof(unsigned int)*cols*nulong,
                            sizeof(unsigned int), NULL);

        if (status != 0)
        {
            std::cout << "Error writing wts data at line " << yoff << "\n";
            std::cout << "Exiting with error code .... (111) \n";
            GDALClose(inDataset);
            GDALClose(ncountDataset);
            GDALClose(wtsDataset);
            GDALDestroyDriverManager();
            return 111;
        }

//        std::cout << "Weights at (10,10): " << count(10,10) << " "
//                  << wts(3,10*cols+10)<< "\n";
//        bitmask.print( wts.colptr(10*cols + 10));

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

        status = GDALTermProgress( yoff/(1.0*rows), NULL, NULL);
    }

#ifdef BUILD_NMAP_WITH_CUDA
    //Release GPU if required
    if (useGPU) unlockGPU();
#endif

    t_end = getWallTime();

    std::cout << "nmap processing time: " << (t_end-t_start)/60.0 << " mins \n";

    //Annotate the output datasets with window sizes for future reference
    status = ncountDataset->SetMetadataItem("HALFWINDOWX", std::to_string(Nx).c_str(), "ENVI");
    status = ncountDataset->SetMetadataItem("HALFWINDOWY", std::to_string(Ny).c_str(), "ENVI");
    status = wtsDataset->SetMetadataItem("HALFWINDOWX", std::to_string(Nx).c_str(), "ENVI");
    status = wtsDataset->SetMetadataItem("HALFWINDOWY", std::to_string(Ny).c_str(), "ENVI");

    //Clear up workers
    if (useKS)
    {
        delete [] ksworkers;
    }
    else
    {
        delete [] adworkers;
    }

    //Close the datasets
    GDALClose(inDataset);
    GDALClose(ncountDataset);
    GDALClose(wtsDataset);

    if (mskDataset != NULL)
        GDALClose(mskDataset);

    return(0);

};

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

    //Options
    nmapOptions 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 = nmap_process(&opts);

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

    return(0);

};


/*
//@HEADER
// ************************************************************************
//
// solvers_linear_traits.hpp
//                     		  Pressio
//                             Copyright 2019
//    National Technology & Engineering Solutions of Sandia, LLC (NTESS)
//
// Under the terms of Contract DE-NA0003525 with NTESS, the
// U.S. Government retains certain rights in this software.
//
// Pressio is licensed under BSD-3-Clause terms of use:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
// IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact Francesco Rizzi (fnrizzi@sandia.gov)
//
// ************************************************************************
//@HEADER
*/

#ifndef SOLVERS_LINEAR_IMPL_SOLVERS_LINEAR_TRAITS_HPP_
#define SOLVERS_LINEAR_IMPL_SOLVERS_LINEAR_TRAITS_HPP_

#ifdef PRESSIO_ENABLE_TPL_EIGEN
#include <Eigen/Core>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Householder>
#include <Eigen/QR>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>
#include <Eigen/OrderingMethods>
#endif

namespace pressio{ namespace linearsolvers{

template <typename T>
struct Traits {
  static constexpr bool direct        = false;
  static constexpr bool iterative     = false;
#ifdef PRESSIO_ENABLE_TPL_EIGEN
  static constexpr bool eigen_enabled = false;
#endif
#ifdef PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = false;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::iterative::CG>
{
  static constexpr bool direct        = false;
  static constexpr bool iterative     = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <
    typename MatrixT,
    typename PrecT = Eigen::DiagonalPreconditioner<typename MatrixT::Scalar>
    >
  using eigen_solver_type = Eigen::ConjugateGradient<MatrixT, Eigen::Lower, PrecT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::iterative::Bicgstab>
{
  static constexpr bool direct        = false;
  static constexpr bool iterative     = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <
    typename MatrixT,
    typename PrecT = Eigen::DiagonalPreconditioner<typename MatrixT::Scalar>
    >
  using eigen_solver_type = Eigen::BiCGSTAB<MatrixT, PrecT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::iterative::LSCG>
{
  static constexpr bool direct        = false;
  static constexpr bool iterative     = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <
    typename MatrixT,
    typename PrecT = Eigen::DiagonalPreconditioner<typename MatrixT::Scalar>
  >
  using eigen_solver_type = Eigen::LeastSquaresConjugateGradient<MatrixT, PrecT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::ColPivHouseholderQR>
{

  static constexpr bool iterative = false;
  static constexpr bool direct = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  /* if matrix is dense, use Eigen::ColPivHouseholderQR.
   * if the native matrix is sparse, then use Eigen::SparseQR.
   * to use SparseQR, the matrix has to be:
   * (a) sparse
   * (b) column-major
   * (c) compressed mode to use COLAMDOrdering
  */
  template <typename MatrixT>
  using eigen_solver_type =
    typename std::conditional<
      pressio::is_sparse_matrix_eigen<MatrixT>::value &&
      MatrixT::IsRowMajor==0,
      Eigen::SparseQR<MatrixT, Eigen::COLAMDOrdering<typename MatrixT::StorageIndex>>,
      Eigen::ColPivHouseholderQR<MatrixT>
      >::type;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::HouseholderQR>
{
  static constexpr bool iterative = false;
  static constexpr bool direct = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <typename MatrixT>
  using eigen_solver_type = Eigen::HouseholderQR<MatrixT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::PartialPivLU>
{
  static constexpr bool iterative = false;
  static constexpr bool direct = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <typename MatrixT>
  using eigen_solver_type = Eigen::PartialPivLU<MatrixT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::potrsL>
{
  static constexpr bool iterative = false;
  static constexpr bool direct = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <typename MatrixT>
  using eigen_solver_type = Eigen::LLT<MatrixT, Eigen::Lower>;
  static constexpr bool eigen_enabled = true;
#endif

#if defined PRESSIO_ENABLE_TPL_TRILINOS or defined PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::potrsU>
{
  static constexpr bool direct = true;
  static constexpr bool eigen_enabled = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <typename MatrixT>
  using eigen_solver_type = Eigen::LLT<MatrixT, Eigen::Upper>;
#endif

#if defined PRESSIO_ENABLE_TPL_TRILINOS or defined PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::getrs>
{
  static constexpr bool direct = true;
#ifdef PRESSIO_ENABLE_TPL_EIGEN
  static constexpr bool eigen_enabled = false;
#endif

#if defined PRESSIO_ENABLE_TPL_TRILINOS or defined PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::geqrf>
{
  static constexpr bool direct = true;
#ifdef PRESSIO_ENABLE_TPL_EIGEN
  static constexpr bool eigen_enabled = false;
#endif
#if defined PRESSIO_ENABLE_TPL_TRILINOS or defined PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = true;
#endif
};

}}
#endif  // SOLVERS_LINEAR_IMPL_SOLVERS_LINEAR_TRAITS_HPP_


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing mod capabilities

    Computes the remainder of division.
    The return value is x-n*y, where n is the value x/y,
    truncated to \f$-\infty\f$.

    @par semantic:
    For any given value @c x, @c y of type @c T:

    @code
    T r = mod(x, y);
    @endcode

    The code is similar to:

    @code
    T r = x-div(x, y, floor)*y;
    @endcode

    @see remainder, rem,  modulo

  **/
  const boost::dispatch::functor<tag::mod_> mod = {};
} }
#endif

#include <boost/simd/function/scalar/mod.hpp>
#include <boost/simd/function/simd/mod.hpp>

#endif


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2014 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <iostream>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "simplex_tree_iostream_operator"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

//  ^
// /!\ Nothing else from Simplex_tree shall be included to test includes are well defined.
#include "gudhi/Simplex_tree.h"

using namespace Gudhi;

struct MyOptions : Simplex_tree_options_full_featured {
  // Not doing persistence, so we don't need those
  static const bool store_key = false;
  static const bool store_filtration = false;
  // I have few vertices
  typedef short Vertex_handle;
};

typedef boost::mpl::list<Simplex_tree<>,
                         Simplex_tree<Simplex_tree_options_fast_persistence>
                        > list_of_tested_variants;

BOOST_AUTO_TEST_CASE_TEMPLATE(iostream_operator, Stree_type, list_of_tested_variants) {
  std::cout << "********************************************************************" << std::endl;
  std::cout << "SIMPLEX TREE IOSTREAM OPERATOR" << std::endl;

  Stree_type st;

  st.insert_simplex_and_subfaces({0, 1, 6, 7}, 4.0);
  st.insert_simplex_and_subfaces({3, 4, 5}, 3.0);
  st.insert_simplex_and_subfaces({3, 0}, 2.0);
  st.insert_simplex_and_subfaces({2, 1, 0}, 3.0);

  st.initialize_filtration();
  // Display the Simplex_tree
  std::cout << "The ORIGINAL complex contains " << st.num_simplices() << " simplices - dimension = "
            << st.dimension() << std::endl;
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, with [filtration value]:" << std::endl;
  for (auto f_simplex : st.filtration_simplex_range()) {
    std::cout << "   " << "[" << st.filtration(f_simplex) << "] ";
    for (auto vertex : st.simplex_vertex_range(f_simplex)) {
      std::cout << (int) vertex << " ";
    }
    std::cout << std::endl;
  }

  // st:
  //    1   6
  //    o---o
  //   /X\7/
  //  o---o---o---o
  //  2   0   3\X/4
  //            o
  //            5
  std::string iostream_file("simplex_tree_for_iostream_operator_unit_test.txt");
  std::ofstream simplex_tree_ostream(iostream_file.c_str());
  simplex_tree_ostream << st;
  simplex_tree_ostream.close();

  Stree_type read_st;
  std::ifstream simplex_tree_istream(iostream_file.c_str());
  simplex_tree_istream >> read_st;

  // Display the Simplex_tree
  std::cout << "The READ complex contains " << read_st.num_simplices() << " simplices - dimension = "
            << read_st.dimension() << std::endl;
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, with [filtration value]:" << std::endl;
  for (auto f_simplex : read_st.filtration_simplex_range()) {
    std::cout << "   " << "[" << read_st.filtration(f_simplex) << "] ";
    for (auto vertex : read_st.simplex_vertex_range(f_simplex)) {
      std::cout << (int) vertex << " ";
    }
    std::cout << std::endl;
  }

  BOOST_CHECK(st == read_st);
}

BOOST_AUTO_TEST_CASE(mini_iostream_operator) {
  std::cout << "********************************************************************" << std::endl;
  std::cout << "MINI SIMPLEX TREE IOSTREAM OPERATOR" << std::endl;

  Simplex_tree<MyOptions> st;

  st.insert_simplex_and_subfaces({0, 1, 6, 7});
  st.insert_simplex_and_subfaces({3, 4, 5});
  st.insert_simplex_and_subfaces({3, 0});
  st.insert_simplex_and_subfaces({2, 1, 0});

  st.initialize_filtration();
  // Display the Simplex_tree
  std::cout << "The ORIGINAL complex contains " << st.num_simplices() << " simplices - dimension = "
            << st.dimension() << std::endl;
  for (auto f_simplex : st.filtration_simplex_range()) {
    std::cout << "   " << "[" << st.filtration(f_simplex) << "] ";
    for (auto vertex : st.simplex_vertex_range(f_simplex)) {
      std::cout << (int) vertex << " ";
    }
    std::cout << std::endl;
  }

  // st:
  //    1   6
  //    o---o
  //   /X\7/
  //  o---o---o---o
  //  2   0   3\X/4
  //            o
  //            5
  std::string iostream_file("simplex_tree_for_iostream_operator_unit_test.txt");
  std::ofstream simplex_tree_ostream(iostream_file.c_str());
  simplex_tree_ostream << st;
  simplex_tree_ostream.close();

  Simplex_tree<MyOptions> read_st;
  std::ifstream simplex_tree_istream(iostream_file.c_str());
  simplex_tree_istream >> read_st;

  // Display the Simplex_tree
  std::cout << "The READ complex contains " << read_st.num_simplices() << " simplices - dimension = "
            << read_st.dimension() << std::endl;
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, with [filtration value]:" << std::endl;
  for (auto f_simplex : read_st.filtration_simplex_range()) {
    std::cout << "   " << "[" << read_st.filtration(f_simplex) << "] ";
    for (auto vertex : read_st.simplex_vertex_range(f_simplex)) {
      std::cout << (int) vertex << " ";
    }
    std::cout << std::endl;
  }

  BOOST_CHECK(st == read_st);
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2006 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <boost/random.hpp>
#include <boost/time_series/delta_series.hpp>
#include <boost/time_series/dense_series.hpp>
#include <boost/time_series/sparse_series.hpp>
#include <boost/time_series/heaviside_series.hpp>
#include <boost/time_series/inverse_heaviside_series.hpp>
#include <boost/time_series/characteristic_series.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/constant_series.hpp>
#include <boost/time_series/ordered_inserter.hpp>
#include <boost/time_series/numeric/numeric.hpp>
#include <boost/time_series/numeric/shift.hpp>
#include <boost/time_series/numeric/period_sums.hpp>
#include <boost/time_series/numeric/adjacent_difference.hpp>
#include <boost/time_series/numeric/coarse_grain.hpp>

namespace rrs = boost::range_run_storage;
namespace seq = boost::sequence;

int main()
{
    using namespace boost::time_series;

    boost::mt19937 rng;

    // the discretization is a template parameter
    // it is passed to the constructor mainly to check
    // whether two series are compatible. If not specified,
    // the discretization defaults to int(1), so that the
    // discretization is a runtime quantity.
    typedef monthly discretization;

    int number_of_years = 5; // the simulation horizon
    int start_time = -2; // start two months in the past

    int end_time = number_of_years * days_per_year() / discretization();

    // create the interest rates series
    dense_series<double, discretization> interest_rate(start = start_time, stop = end_time);

    // sample interest rates by a uniform distribution
    // this is a stupid model for interest rates but simple

    boost::variate_generator<boost::mt19937&, boost::uniform_real<> >
        rates_generator(rng, boost::uniform_real<>(0.02, 0.1));

    // syntax here works with Boost.random. Note: Only dense_series can be
    // filled this way.
    std::pair<double *, std::size_t> buffer = get_raw_buffer(interest_rate);
    std::generate(
         buffer.first
       , buffer.first + buffer.second
       , rates_generator
    );

    // now let's set up a financial instrument
    // the client gets the loan on January 10 of the first year;
    // we assume that index 0 stands for amount at the end of the first month

    int loan_date = 10;
    int loan_quarter = loan_date / days_per_quarter();
    int loan_duration = 3 * days_per_year(); // 3 years

    // the outstanding is now 1000000 for the duration of the loan and 0 otherwise
    constant_series<double, discretization> amount(1000000.);
    characteristic_unit_series<double, discretization> outstanding_range(
        loan_date / discretization()
      , loan_duration / discretization()
    );

    // outstanding amout at sampling time
    characteristic_series<double, discretization> outstanding = amount * outstanding_range;

    // quarterly interest payments at end of quarter
    // let's prepare a series consiting of 1s in the months where he pays
    // interest

    sparse_series<double, discretization> interest_payment_dates;
    ordered_inserter<sparse_series<double, discretization> > in1(interest_payment_dates);
    for(int i = days_per_quarter(); i <= loan_duration; i += days_per_quarter())
    {
        in1(1., (i + days_per_quarter() * loan_quarter) / discretization());
    }
    in1.commit();

    // let's extract the interest rates: they might be the ones from the end of
    // the last month of the previous quarter, thus we need to shift
    // afterwards rate contains the rates applicable to that interest payment

    sparse_series<double, discretization> rate =
        interest_payment_dates * shift(interest_rate, days_per_quarter()/discretization());

    // let us next calculate the amortization, assuming an annual schedule
    // and amortization of 2% per year

    sparse_series<double, discretization> amortization;
    ordered_inserter<sparse_series<double, discretization> > in2(amortization);
    for(int i = days_per_year(); i < loan_duration; i += days_per_year())
    {
        in2(0.02 * amount[0], i / discretization());
    }
    in2.commit();

    // now we can adjust the outstanding: subtract the amortization

    outstanding -= amortization;

    // finally we want to get the payback at the last date
    // as the adjacent difference at the end of the loan

    int loan_ends = (loan_date + loan_duration) / discretization();

    delta_series<double, discretization> payback(
        start = loan_ends - 1
      , value = outstanding[loan_ends] - outstanding[loan_ends - 1]
    );

    // next we need to calculate the interest payments

    // first integrate the outstanding over each quarter
    // starting at the end of the previous month (-1), not at 0 or -2
    sparse_series<double, discretization> integrated_amount =
        period_sums(outstanding, -1, days_per_quarter() / discretization());

    // we need to define where we store that sum: in the 1st or last entry
    // of the period. Le us assume it is the first, then we now need to shift
    // it by 3 months

    integrated_amount = shift(integrated_amount, days_per_quarter() / discretization());

    // next we might need to adjust the first and last partial month, but
    // we skip this here for now

    sparse_series<double, discretization> interest_payment = integrated_amount * rate;

    // Now we know how much the bank gets:

    sparse_series<double, discretization> cash_flow =
        adjacent_difference(outstanding) + interest_payment;

    // we can also calculate the total payments per year:

    sparse_series<double, discretization> annual_payments =
        period_sums(amortization + interest_payment + payback, 0, days_per_year() / discretization());

    // we can also go to coarser grids to save storage if needed
    // we might have different coarse graining functions:
    // coarse graining by sampling or integrating over periods
    // here we did the period sums before, so sampling is fine

    sparse_series<double, yearly> coarse_grained_paymets =
        coarse_grain(annual_payments, yearly());

    // now, what happens in case of bankruptcy

    boost::variate_generator<boost::mt19937&, boost::uniform_int<> > bankruptcy(
        rng, boost::uniform_int<>(0, 3600));

    // the client goes bankrupt some time in the next 10 years
    int tau = bankruptcy();
    int tau_coarse = tau / discretization();

    sparse_series<double, discretization> real_cashflow =
        inverse_heaviside_unit_series<double, discretization>(tau_coarse - 1) * cash_flow;

    if(tau < loan_date + loan_duration)
    {
        // he goes bankrupt during the loan period
        // we can still recover e.g. 80% of the outstanding
        real_cashflow += delta_series<double, discretization>(tau_coarse, 0.8 * outstanding[tau_coarse]);
    }

    return 0;
}


// a sorting example that uses the worst-case distribution for spreadsort.
//
//  Copyright Steven Ross 2009.
//
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/ for updates, documentation, and revision history.

#include <boost/algorithm/sorting/spread_sort.hpp>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <algorithm>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <iostream>
using namespace boost;
using namespace std;

#define DATA_TYPE boost::uint64_t

#define ALR_THRESHOLD 20

const unsigned max_count = ALR_THRESHOLD - 1;
const unsigned bit_shift = detail::rough_log_2_size(max_count) - detail::LOG_MEAN_BIN_SIZE;
const unsigned radix_threshold = detail::rough_log_2_size(max_count) + 1;

const DATA_TYPE typed_one = 1;

void
fill_vector(vector<DATA_TYPE> & input, const DATA_TYPE base_value, unsigned remaining_bits
            , const vector<unsigned> & indices, int index)
{
  if(index < 0) {
    for(unsigned u = 0; u < max_count; ++u)
      input.push_back((base_value << remaining_bits) + (rand() % (1 << remaining_bits)));
  }
  else {
    unsigned shift = indices[index];
    fill_vector(input, (base_value << shift) + ((1 << shift) - 1),
                remaining_bits - shift, indices, index - 1);
    fill_vector(input, base_value << shift, remaining_bits - shift, indices,
                index - 1);
  }
}

//Pass in an argument to test std::sort
int main(int argc, const char ** argv) {
  vector<DATA_TYPE> input;
  vector<unsigned> offsets;
  unsigned total_length = sizeof(uint64_t) * 8;
  unsigned bit_length = total_length;
  unsigned bit_offset = bit_shift;
  bit_length -= radix_threshold;
  for(; bit_length >= ++bit_offset; bit_length -= bit_offset)
    offsets.push_back(bit_offset);
  for(int ii = (1 << bit_length) - 1; ii >= 0; --ii)
    fill_vector(input, ii, total_length - bit_length, offsets, offsets.size() - 1);
  //Run multiple loops, if requested
  for(unsigned u = 0; u < 2; ++u) {
    vector<DATA_TYPE> array = input;
    clock_t start, end;
    double elapsed;
    start = clock();
    if(u)
      std::sort(array.begin(), array.end());
    else
      spread_sort(array.begin(), array.end());
    end = clock();
    elapsed = ((double) (end - start));
    if(u)
      printf("std::sort elapsed time %f\n", elapsed / CLOCKS_PER_SEC);
    else
      printf("spreadsort elapsed time %f\n", elapsed / CLOCKS_PER_SEC);
    array.clear();
  }
  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// tree_view::node.hpp                                                       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_TREE_VIEW_NODE_HPP_ER_2009
#define BOOST_TREE_VIEW_NODE_HPP_ER_2009
#include <stdexcept>
#include <ostream>
#include <boost/operators.hpp>
#include <boost/tree_view/stage.hpp>
#include <boost/tree_view/dynamic_stage.hpp>

namespace boost{
namespace tree_view{

// Data structure reprensentation:
// i : position in physical storage
// k : position in stage
// j : stage
//
// Example for n = 3 :
//
// j:   0   1   2
//
// i:   0->(            k:  0->(
//          1->(                0->(
//              4                   0
//              5                   1
//              6                   2
//          )                   )
//          2->(                1->(
//              7                   3
//              8                   4
//              9                   5
//          )                   )
//          3->(                2->(
//              10                  6
//              11                  7
//              12                  8
//          )                   )
//      )

    // Data structure representing a node in a tree
    //
    // n : number of branches
    // m : number of stages
    template<unsigned n,unsigned m = BOOST_SWITCH_LIMIT>
    struct node :
        incrementable<
            node<n,m>,
            decrementable<
                node<n,m>,
                less_than_comparable<
                    node<n,m>,
                    equality_comparable<
                        node<n,m>
                    >
                >
            >
        >
    {
        typedef std::size_t size_type;
        typedef node<n,m> this_;
        typedef dynamic_stage<n> dyna_stage_;
        unsigned stage;
        unsigned position_in_stage;

        node();
        node(unsigned j,unsigned k);

        // ++ and -- are along breadth-first
        this_& operator++();
        this_& operator--();
        bool operator==(const this_&)const;
        bool operator<(const this_&)const;

        static std::string header;

    };

}//tree_view

    template<unsigned n,unsigned m> std::ostream&
    operator<<(std::ostream& out, const tree_view::node<n,m>& that);

    template<unsigned n,unsigned m>
    bool
    is_root(const tree_view::node<n,m>& that);

    template<unsigned n,unsigned m>
    typename tree_view::node<n,m>::size_type
    position(const tree_view::node<n,m>& that);

    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    parent(const tree_view::node<n,m>& child);

    // first_child(a) is first node in the next stage, with parent a.
    // To visit all the nodes sharing the same parent:
    // node<n,m> node = first_child(a);
    // while(parent(node++)!=a){...}
    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    first_child(const tree_view::node<n,m>& parent);

    // -> first, back and last of each stage
    // [first,...,back,last)
    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    first(const tree_view::node<n,m>& root,unsigned stage);

    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    back(const tree_view::node<n,m>& root,unsigned stage);

    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    last(const tree_view::node<n,m>& root,unsigned stage);
    // <-

    // prior and next by breadth first.
    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    prior(const tree_view::node<n,m>& node);

    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    next(const tree_view::node<n,m>& node);

namespace tree_view{

    // Implementation

    // j: stage
    // k: position relative to other nodes at the same stage in the tree_view
    // i: position in the vector
    template<unsigned n,unsigned m>
    std::string node<n,m>::header = "(j,k,i)";

    template<unsigned n,unsigned m>
    node<n,m>::node():stage(0),position_in_stage(0){}

    template<unsigned n,unsigned m>
    node<n,m>::node(unsigned j,unsigned k)
    :stage(j),position_in_stage(k){
        const char* msg = "node(%1%,!(%2%<%3%)),  ";
        unsigned l = dyna_stage_::number_nodes(this->stage);
        if(! (k <  l) ){
            format f(msg);
            f % j % k % l;
            throw std::out_of_range(
                f.str()
            );
        }
    }

    template<unsigned n,unsigned m>
    node<n,m>&
    node<n,m>::operator++(){
        unsigned mm = dyna_stage_::position_last(this->stage);
        if( 1 + position(*this) < mm){
            ++(this->position_in_stage);
        }else{
            ++stage;
            (this->position_in_stage) = 0;
        }
        return (*this);
    }

    template<unsigned n,unsigned m>
    node<n,m>&
    node<n,m>::operator--(){
        const char* msg = "--node<n,m> at root";
        if(this->stage == 0){
            throw std::out_of_range(
                msg
            );
        }
        unsigned mm = dyna_stage_::position_first( this->stage );
        if( position(*this)-1 >= mm){
            --(this->position_in_stage);
        }else{
            --stage;
            (this->position_in_stage)
                = dyna_stage_::number_nodes( this->stage) - 1;
        }
        return (*this);
    }

    template<unsigned n,unsigned m>
    bool node<n,m>::operator==(const this_& that)const{
        return position(*this) == position(that);
    }

    template<unsigned n,unsigned m>
    bool node<n,m>::operator<(const this_& that)const{
        return position(*this) < position(that);
    }

}

    template<unsigned n,unsigned m>
    std::ostream& operator<<(
        std::ostream& out,
        const tree_view::node<n,m>& that
    ){
        out
            << '(' << that.stage
            << ',' << that.position_in_stage
            << ',' << position(that)
            << ')';
        return out;
    }

    template<unsigned n,unsigned m>
    bool
    is_root(const tree_view::node<n,m>& that){
        typedef tree_view::node<n,m> node_;
        static const node_ root_node;
        return that == root_node;
    }

    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    first_child(const tree_view::node<n,m>& that){
        typedef tree_view::node<n,m> node_;
        node_ node = that;
        ++node.stage;
        (node.position_in_stage) *= n;
        return node;
    }

    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    parent(const tree_view::node<n,m>& that){
        typedef tree_view::node<n,m> node_;
        const char* msg = "node<n,m>::parent() root has no parent";
        if( that.stage == 0 ){
            throw std::out_of_range(
                msg
            );
        }
        node_ node = that;
        --node.stage;
        node.position_in_stage /= n;
        return node;
    }

    template<unsigned n,unsigned m>
    typename tree_view::node<n,m>::size_type
    position(const tree_view::node<n,m>& that){
        typedef tree_view::node<n,m> node_;
        typedef typename node_::dyna_stage_ dyna_stage_;
        return dyna_stage_::position_first(that.stage)
            +(that.position_in_stage);
    }

    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    first(const tree_view::node<n,m>& root,unsigned stage){
        typedef tree_view::node<n,m> node_;
        typedef typename node_::dyna_stage_ dyna_stage_;
        return node_(stage,0);
    }

    template<unsigned n,unsigned m> tree_view::node<n,m>
    back(const tree_view::node<n,m>& root,unsigned stage){
        typedef tree_view::node<n,m> node_;
        typedef typename node_::dyna_stage_ dyna_stage_;
        unsigned k = dyna_stage_::number_nodes( stage ) - 1;
        return node_(stage,k);
    }

    template<unsigned n,unsigned m> tree_view::node<n,m>
    last(const tree_view::node<n,m>& root,unsigned stage){
        return first(root,stage+1);
    }

    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    prior(const tree_view::node<n,m>& node){
        typedef tree_view::node<n,m> node_;
        node_ new_node = node;
        return --new_node;
    }

    template<unsigned n,unsigned m>
    tree_view::node<n,m>
    next(const tree_view::node<n,m>& node){
        typedef tree_view::node<n,m> node_;
        node_ new_node = node;
        return ++new_node;
    }

}// boost

#endif


/**
 *  testIvec.cpp
 *
 *  Test interval vector class
 *
 *  Created by Yinan Li on August 19, 2016.
 *
 *  Hybrid Systems Group, University of Waterloo.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE IntervalVectorClass

#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_log.hpp>
#include <cmath>
#include "src/interval_vector.h"

/*
  test case 1: constructors and printing
*/
BOOST_AUTO_TEST_CASE(test_initialization)
{
    rocs::ivec a(3);  // the constructor ivec(int n)
    std::cout << a << std::endl;
    a.setval(0, rocs::interval(-1, 1));
    a[1] = rocs::interval(-0.1, 0.01);
    a[2] = rocs::interval(0, 2);
    std::cout << a << std::endl;
    rocs::ivec b{rocs::interval(-1, 1),
		 rocs::interval(-0.1, 0.01),
		 rocs::interval(0, 2)};  // the initialize_list constructor
    rocs::ivec c(b);  // copy constructor
    rocs::ivec d = b;  // assignment constructor
    rocs::ivec e;
    std::cout << e << std::endl;

    BOOST_CHECK_EQUAL(a, b);
    BOOST_CHECK_EQUAL(a, c);
    BOOST_CHECK_EQUAL(a, d);
    BOOST_CHECK(e.isempty());
}

/*
  test case 2: property access functions
*/
BOOST_AUTO_TEST_CASE(test_property_access_functions)
{
    rocs::ivec b{rocs::interval(-1, 1),
		 rocs::interval(-0.1, 0.01),
		 rocs::interval(0, 2)};
    rocs::ivec f{rocs::interval(1, 0),
		 rocs::interval(rocs::NINF, rocs::PINF)};

    /* test isempty() */
    BOOST_CHECK(!b.isempty());
    BOOST_CHECK(f.isempty());

    /* test setters and getters */
    BOOST_CHECK_EQUAL(3, b.getdim());
    BOOST_CHECK_CLOSE(2., b.maxwidth(), 1e-9);
    BOOST_CHECK_EQUAL(0, b.maxdim());

    std::vector<double> infexp{-1., -0.1, 0.};
    std::vector<double> supexp{1., 0.01, 2.};
    std::vector<double> rexp{1., 0.055, 1.};

    // BOOST_CHECK_EQUAL(infexp, b.getinf());
    // BOOST_CHECK_EQUAL(supexp, b.getsup());
    // BOOST_CHECK_EQUAL(rexp, b.radius());
    BOOST_CHECK(infexp == b.getinf());
    BOOST_CHECK(supexp == b.getsup());
    BOOST_CHECK(rexp == b.radius());
    std::vector<double> brad = b.radius();
    for(int i = 0; i < 3; ++i)
	std::cout << brad[i] << ',';
    std::cout << '\n';

}

/*
  test case 3: test set operations
*/
BOOST_AUTO_TEST_CASE(test_set_operations)
{
    rocs::ivec a{rocs::interval(-1, 1),
		 rocs::interval(-0.1, 0.01),
		 rocs::interval(0, 2)};
    rocs::ivec x{rocs::interval(1.11, 2.3),
		 rocs::interval(-0.5, 0),
		 rocs::interval(0, 2)};
    rocs::ivec y{rocs::interval(-1, 1),
		 rocs::interval(-0.1, -0.08),
		 rocs::interval(0.1, 1.15)};

    rocs::ivec z = y;
    z[1] = rocs::interval(2, 1);
    BOOST_CHECK(a.isout(x));
    BOOST_CHECK(a.isin(y));
    BOOST_CHECK(a.isout(z));

    std::vector<double> s1{0, 0, 0};
    std::vector<double> s2{1, 1, 1};
    BOOST_CHECK(a.isin(s1));
    BOOST_CHECK(a.isout(s2));

    rocs::ivec b{rocs::interval(0.3, 0.5),
		 rocs::interval(0.1, 2),
		 rocs::interval(-3, -1)};
    rocs::ivec c{rocs::interval(4, 7),
		 rocs::interval(3, 4)};
    rocs::ivec d{rocs::interval(0.4, 0.8),
		 rocs::interval(0.5, 1),
		 rocs::interval(-3.2, -2.9)};
    rocs::ivec aorb{rocs::interval(-1, 1),
		    rocs::interval(-0.1, 2),
		    rocs::interval(-3, 2)};
    rocs::ivec bandd{rocs::interval(0.4, 0.5),
		     rocs::interval(0.5, 1),
		     rocs::interval(-3, -2.9)};

    BOOST_CHECK(intersect(a, b).isempty());
    BOOST_CHECK(hull(a, b) == aorb);
    BOOST_CHECK(intersect(b, d) == bandd);
}

/*
  test case 4: test bisections
*/
BOOST_AUTO_TEST_CASE(test_bisections)
{
    rocs::ivec a{rocs::interval(-1, 1),
		 rocs::interval(-0.1, 0.01),
		 rocs::interval(0, 2)};

    rocs::ivec left = lowerhalf(a, 2);
    rocs::ivec right = upperhalf(a, 2);

    rocs::ivec refl{rocs::interval(-1, 1),
		    rocs::interval(-0.1, 0.01),
		    rocs::interval(0, 1)};
    rocs::ivec refr{rocs::interval(-1, 1),
		    rocs::interval(-0.1, 0.01),
		    rocs::interval(1, 2)};

    BOOST_CHECK_EQUAL(left, refl);
    BOOST_CHECK_EQUAL(right, refr);
}

/*
  test case 4: test operator overloads
*/
BOOST_AUTO_TEST_CASE(test_addsub_overloads)
{
    rocs::ivec a{rocs::interval(-1, 1),
		 rocs::interval(-0.1, 0.01),
		 rocs::interval(0, 2)};
    rocs::ivec b{rocs::interval(0.3, 0.5),
		 rocs::interval(0.1, 2),
		 rocs::interval(-3, -1)};
    rocs::ivec c{rocs::interval(4, 7),
		 rocs::interval(3, 4)};
    rocs::ivec d = {rocs::interval(-1, 1),
		    rocs::interval(-0.1, 0.01),
		    rocs::interval(0, 2)};

    double val = 0.04;
    rocs::ivec abadd{rocs::interval(-0.7, 1.5),
		     rocs::interval(0., 2.01),
		     rocs::interval(-3., 1.)};
    rocs::ivec absub{rocs::interval(-1.5, 0.7),
		     rocs::interval(-2.1, -0.09),
		     rocs::interval(1., 5.)};
    rocs::ivec avadd{rocs::interval(-0.96, 1.04),
		     rocs::interval(-0.06, 0.05),
		     rocs::interval(0.04, 2.04)};
    rocs::ivec avsub{rocs::interval(0.26, 0.46),
		     rocs::interval(0.06, 1.96),
		     rocs::interval(-3.04, -1.04)};

    rocs::ivec r = a - c;
    BOOST_CHECK_EQUAL(a+b, abadd);
    BOOST_CHECK_EQUAL(a-b, absub);
    BOOST_CHECK_EQUAL(a+val, avadd);
    BOOST_CHECK_EQUAL(b-val, avsub);
    BOOST_CHECK(r.isempty());
    a += b;
    BOOST_CHECK_EQUAL(a, abadd);
    d -= b;
    BOOST_CHECK_EQUAL(d, absub);
}


#include "gtest/gtest.h"
#include <armadillo>
#include <boost/shared_ptr.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include "cluster.hpp"
using namespace gmum;

TEST(CEC_OnlineFormulas,AddPoint) {
    //arma_rng::set_seed(0);
    int n = 100;
    int dim = 2;
    int beg = dim+1;
    double acceptable_difference = 1e-10;

    std::vector<unsigned int> fits;
    unsigned id = 1;
    arma::mat data(n,dim);
    data.randu();
    for (int i = 0; i < n; ++i) {
        fits.push_back(id);
    }

    arma::mat init_matrix(beg,dim);
    for(int k = 0; k < beg; ++k) {
        for (int j = 0; j < dim; ++j) {
            init_matrix(k,j) = data(k,j);
        }
    }

    boost::shared_ptr<Cluster> m = boost::make_shared<ClusterStandard>(id,fits,init_matrix);
    // Dodajemy element o indeksie i
    for (int i = beg; i < n-1; ++i) {

        arma::mat tmp_matrix(i+1,dim);
        for (int j = 0; j <=i; ++j) {
            tmp_matrix.row(j) = data.row(j);
        }
        arma::mat covariance= cov(tmp_matrix,1);
        arma::mat real_m = mean(tmp_matrix);

        arma::rowvec point(data.row(i));
        m->add_point(point);
        ClusterStandard tmp(id,fits,tmp_matrix);
        arma::rowvec mean_online_difference = m->get_mean() - real_m;
        arma::mat mean_init_difference = real_m - tmp.get_mean();
        arma::mat cov_online_difference = m->get_cov_mat(id, fits, tmp_matrix) - covariance;
        arma::mat cov_init_difference = covariance - tmp.get_cov_mat(id, fits, tmp_matrix);

        EXPECT_EQ(m->size(),tmp.size());

        for (int j = 0; j < dim; ++j) {
            EXPECT_LT(std::abs(mean_online_difference(j)),acceptable_difference) << "at position" << j << " means differ by more than " << acceptable_difference;
            EXPECT_LT(std::abs(mean_init_difference(j)),acceptable_difference) << "at position" << j << " means differ by more than " << acceptable_difference;
        }

        for (int j = 0; j < dim; ++j) {
            for (int k = 0; k < dim; ++k) {
                EXPECT_LT(std::abs(cov_online_difference(j,k)),acceptable_difference ) << " at position (" << j << "," << k << ")" << "differs by more than " << acceptable_difference;
                EXPECT_LT(std::abs(cov_init_difference(j,k)),acceptable_difference) << " at position (" << j << "," << k << ")" << "differs by more than " << acceptable_difference;
            }
        }
    }
}

TEST(CEC_OnlineFormulas,removePoint) {
    //arma_rng::set_seed(0);
    int n = 100;
    int dim = 2;
    int end = dim+1;
    double acceptable_difference = 1e-10;

    std::vector<unsigned int> fits;
    unsigned id = 1;
    arma::mat data(n,dim);
    data.randu();
    for (int i = 0; i < n; ++i) {
        fits.push_back(id);
    }

    arma::mat init_matrix(n,dim);
    for(int k = 0; k < n; ++k) {
        for (int j = 0; j < dim; ++j) {
            init_matrix(k,j) = data(k,j);
        }
    }

    boost::shared_ptr<Cluster> m = boost::make_shared<ClusterStandard>(id,fits,init_matrix);
    // Dodajemy element o indeksie i
    for (int i = n-1; i > end; --i) {

        arma::mat tmp_matrix(i,dim);
        for (int j = 0; j <i; ++j) {
            tmp_matrix.row(j) = data.row(j);
        }
        arma::mat covariance= cov(tmp_matrix,1);
        arma::mat real_m = mean(tmp_matrix);

        arma::rowvec point(data.row(i));
        m->remove_point(point);
        ClusterStandard tmp(id,fits,tmp_matrix);
        arma::rowvec mean_online_difference = m->get_mean() - real_m;
        arma::mat mean_init_difference = real_m - tmp.get_mean();
        arma::mat cov_online_difference = m->get_cov_mat(id, fits, tmp_matrix) - covariance;
        arma::mat cov_init_difference = covariance - tmp.get_cov_mat(id, fits, tmp_matrix);

        EXPECT_EQ(m->size(),tmp.size());

        for (int j = 0; j < dim; ++j) {
            EXPECT_LT(std::abs(mean_online_difference(j)),acceptable_difference) << "at position" << j << " means differ by more than " << acceptable_difference;
            EXPECT_LT(std::abs(mean_init_difference(j)),acceptable_difference) << "at position" << j << " means differ by more than " << acceptable_difference;
        }

        for (int j = 0; j < dim; ++j) {
            for (int k = 0; k < dim; ++k) {
                EXPECT_LT(std::abs(cov_online_difference(j,k)),acceptable_difference ) << " at position (" << j << "," << k << ")" << "differs by more than " << acceptable_difference;
                EXPECT_LT(std::abs(cov_init_difference(j,k)),acceptable_difference) << " at position (" << j << "," << k << ")" << "differs by more than " << acceptable_difference;
            }
        }
    }
}