#include "find_doors.h"

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/version.hpp>

#include <cstring>
#include <cstdlib>
#include <cmath>
#include <cstdint>

#include <algorithm>
#include <string>
#include <iostream>
#include <limits>
#include <set>
#include <map>
#include <list>
#include <utility>
#include <array>
#include <iterator>

using namespace cslibs_vectormaps;

namespace {

// some utility boost::geometry is lacking
point_t add(const point_t& p1, const point_t& p2)
{
    point_t p = p1;
    boost::geometry::add_point(p, p2);
    return p;
}

point_t subtract(const point_t& p1, const point_t& p2)
{
    point_t p = p1;
    boost::geometry::subtract_point(p, p2);
    return p;
}

point_t multiply(const point_t& p, double v)
{
    point_t p2 = p;
    boost::geometry::multiply_value(p2, v);
    return p2;
}

point_t divide(const point_t& p, double v)
{
    point_t p2 = p;
    boost::geometry::divide_value(p2, v);
    return p2;
}

bool equal(const point_t& a, const point_t& b)
{
    return a.x() == b.x() && a.y() == b.y();
}

}

FindDoors::FindDoors(const FindDoorsParameter& parameter) : parameter_(parameter)
{

}

std::vector<segment_t> FindDoors::round_segments(const std::vector<segment_t>& segments, double precision)
{
    double p = precision;
    if (p != 0.) {
        std::vector<segment_t> rounded_segments = segments;
        for (segment_t& toround : rounded_segments) {
            toround.first.x(std::round(toround.first.x() * p) / p);
            toround.first.y(std::round(toround.first.y() * p) / p);
            toround.second.x(std::round(toround.second.x() * p) / p);
            toround.second.y(std::round(toround.second.y() * p) / p);
        }
        return rounded_segments;
    } else {
        return segments;
    }
}

std::vector<segment_t> FindDoors::clean_segments(const std::vector<segment_t>& rounded_segments)
{
    // make a new vector that only contains successive segments, no overlapping
    // segments. we can only replace exact vertical or exact horizontal
    // overlappings at the moment.
    std::vector<segment_t> non_overlapping_segments;

    // sort original segments into std::vectors (vertical, horizontal, other)
    std::vector<segment_t> vertical, horizontal;
    for (const segment_t& segment : rounded_segments) {
        if (segment.first.x() == segment.second.x()) {
            segment_t s = {
                {segment.first.x(), std::min(segment.first.y(), segment.second.y())},
                {segment.second.x(), std::max(segment.first.y(), segment.second.y())}
            };
            vertical.push_back(s);
        } else if (segment.first.y() == segment.second.y()) {
            segment_t s = {
                {std::min(segment.first.x(), segment.second.x()), segment.first.y()},
                {std::max(segment.first.x(), segment.second.x()), segment.second.y()}
            };
            horizontal.push_back(s);
        } else {
            non_overlapping_segments.push_back(segment);
        }
    }
    // sort the std::vectors
    std::sort(vertical.begin(), vertical.end(), [](const segment_t& s1, const segment_t& s2) {
        return s1.first.x() < s2.first.x()
               || s1.first.x() == s2.first.x()
                  && (s1.first.y() < s2.first.y()/*
                      || s1.first.y() == s2.first.y()
                         && s1.second.y() < s2.second.y()*/);
    });
    std::sort(horizontal.begin(), horizontal.end(), [](const segment_t& s1, const segment_t& s2) {
        return s1.first.y() < s2.first.y()
               || s1.first.y() == s2.first.y()
                  && (s1.first.x() < s2.first.x()/*
                      || s1.first.x() == s2.first.x()
                         && s1.second.x() < s2.second.x()*/);
    });
    // finally, detect overlapping segments and replace those by single segments
    std::size_t overlaps = 0;
    for (std::size_t i = 0, n = vertical.size(), j; i < n; i = j) {
        double miny = vertical[i].first.y();
        double maxy = vertical[i].second.y();
        for (j = i + 1; j < n && vertical[i].first.x() == vertical[j].first.x(); j++) {
            if (vertical[j].first.y() < maxy) {
                if (vertical[j].second.y() > maxy)
                    maxy = vertical[j].second.y();
                overlaps++;
            } else {
                break;
            }
        }
        segment_t s = {{vertical[i].first.x(), miny}, {vertical[i].first.x(), maxy}};
        non_overlapping_segments.push_back(s);
    }
    for (std::size_t i = 0, n = horizontal.size(), j; i < n; i = j) {
        double minx = horizontal[i].first.x();
        double maxx = horizontal[i].second.x();
        for (j = i + 1; j < n && horizontal[i].first.y() == horizontal[j].first.y(); j++) {
            if (horizontal[j].first.x() < maxx) {
                if (horizontal[j].second.x() > maxx)
                    maxx = horizontal[j].second.x();
                overlaps++;
            } else {
                break;
            }
        }
        segment_t s = {{minx, horizontal[i].first.y()}, {maxx, horizontal[i].first.y()}};
        non_overlapping_segments.push_back(s);
    }
    std::cout << "Merged " << overlaps << " overlapping segments\n";

    // TODO: workaround for older Boost using bounding boxes
#if BOOST_VERSION < 105600
    std::cerr << "Boost version >= 1.56.0 is required for creating the segment graph! The results are probably unusable.\n";
    return non_overlapping_segments;
#else
    // replace intersecting line segments by multiple new segments that end in a common intersection point
    // use a spatial index to ease finding those intersecting line segments
    namespace bgi = boost::geometry::index;
    bgi::rtree<segment_t, bgi::rstar<16>> rtree(non_overlapping_segments);
    std::vector<segment_t> clean_segments;
    for (const segment_t& query_segment : non_overlapping_segments) {
        std::vector<point_t> intersections;
        for (auto qit = rtree.qbegin(bgi::intersects(query_segment)), end = rtree.qend(); qit != end; ++qit) {
            const segment_t& result_segment = *qit;
            boost::geometry::intersection(result_segment, query_segment, intersections);
        }
        std::set<point_t, point_compare> newpoints(intersections.begin(), intersections.end());
        for (auto pit1 = newpoints.begin(), pit2 = std::next(pit1), end = newpoints.end(); pit2 != end; ++pit1, ++pit2) {
            clean_segments.push_back({*pit1, *pit2});
        }
    }
    std::cout << "Created " << (clean_segments.size() - non_overlapping_segments.size()) << " new segments because of intersections\n";

    return clean_segments;
#endif
}

std::vector<FindDoors::door_t> FindDoors::find_doors(const std::vector<segment_t>& cleaned_segments)
{
    const FindDoorsParameter& params = parameter_;

    // find doors
    double door_depth_min = params.door_depth_min;
    double door_depth_max = params.door_depth_max;
    double door_width_min = params.door_width_min;
    double door_width_max = params.door_width_max;

    auto check_length = [](const point_t& v, double min, double max) {
        // checks the length of a vector
        double d = v.x() * v.x() + v.y() * v.y();
        return d >= min * min && d <= max * max;
    };
    auto check_angle = [&params](const point_t& v1, const point_t& v2) {
        double angle = std::atan2(v2.y(), v2.x()) - std::atan2(v1.y(), v1.x());
        if (angle < 0)
            angle += boost::math::double_constants::two_pi;
        return angle >= boost::math::double_constants::half_pi - params.door_angle_diff_max
            && angle <= boost::math::double_constants::half_pi + params.door_angle_diff_max;
    };
    /*auto check_door_side = [&door_depth_min, &door_depth_max, &check_length, &check_angle](const point_t& v1, const point_t& v2, const point_t& v3) {
        // Checks if v1, v2, v3 could form the side of a door like this:
        //     v1
        //  +<----+
        //        |v2
        //        v
        //  +<----+
        //     v3
        // This could be the left side of a door. The length of v2 is
        // checked. angle(v1, v2) and angle(v3, v2) are checked to be
        // approximately 90 deg. 270 deg does not count! This is so that
        // v2's direction is consistent and so that no duplicates occur
        // when considering edges in both directions.
        // The angles are not checked anymore right now.
        return check_length(v2, door_depth_min, door_depth_max);
            //&& check_angle(v1, v2) && check_angle(v3, v2);
    };
    auto check_node = [&check_door_side](const node_t& firstnode, const edge_t& e1, const edge_t& e2) {
        // checks if edge e2 from firstnode to secondnode could form
        // the side of a door as explained in check_door_side() above
        const node_t* secondnode = e2.target->node;
        if (secondnode->edges.size() == 2) {
            const edge_t& e3 = secondnode->edges[
                secondnode->edges[0].target->node == &firstnode ? 1 : 0
            ];
            auto edge_to_vector = [](const edge_t& e) {
                return subtract(e.target->point, e.start->point);
            };
            if (check_door_side(edge_to_vector(e1), edge_to_vector(e2), edge_to_vector(e3)))
                return true;
        }
        return false;
    };*/
    std::vector<segment_t> side_candidates;
    /*for (const node_t& firstnode : graph.nodes) {
        // find all nodes that connect exactly two edges. those can be corner points of doors.
        if (firstnode.edges.size() != 2)
            continue;
        // if one of the connected nodes also has exactly two edges, the
        // three edges might form one of the two walls on either side of
        // the door
        if (check_node(firstnode, firstnode.edges[1], firstnode.edges[0]))
            side_candidates.emplace_back(firstnode.edges[0].start->point, firstnode.edges[0].target->point);
        if (check_node(firstnode, firstnode.edges[0], firstnode.edges[1]))
            side_candidates.emplace_back(firstnode.edges[1].start->point, firstnode.edges[1].target->point);
    }*/
    // here, segments become door side candidates if their length fits.
    // just forget the madness above.
    for (const segment_t& segment : cleaned_segments) {
        if (check_length(subtract(segment.second, segment.first), door_depth_min, door_depth_max)) {
            side_candidates.push_back(segment);
            side_candidates.emplace_back(segment.second, segment.first);
        }
    }
    std::cout << "Found " << side_candidates.size() << " door side candidates\n";

    std::size_t ncandidates = side_candidates.size();
    std::vector<std::size_t> side_candidate_partners(ncandidates, static_cast<std::size_t>(-1));
    std::vector<double> side_candidate_distances(ncandidates, std::numeric_limits<double>::max());
    for (std::size_t i1 = 0; i1 < ncandidates; i1++) {
        // we search for the other side of the door frame by drawing
        // a ray r from the middle of the edge like this:
        //     v1
        //  +<----+          +---->+
        //        |  v       ^
        //      v2|----->----|--- r
        //        v          |w2
        //  +<----+          +---->+
        //     v3
        // If r cuts another possible door frame side nearly
        // perpendicularly and that intersection point is not too far
        // and not too close from v2, we found a door.
        const point_t& v21 = side_candidates[i1].first;
        const point_t& v22 = side_candidates[i1].second;
        // r = middle + t * v
        point_t middle = multiply(add(v21, v22), .5);
        point_t v2 = subtract(v22, v21);
        point_t v = {-v2.y(), v2.x()}; // 90 deg turn
        double v_length = std::sqrt(v.x() * v.x() + v.y() * v.y());

        // check all other candidates
        for (std::size_t i2 = 0; i2 < ncandidates; i2++) {
            if (i2 == i1)
                continue;
            // line intersection adapted from https://stackoverflow.com/a/565282/1007605
            auto cross = [](const point_t& v, const point_t& w) {
                return v.x() * w.y() - v.y() * w.x();
            };
            const point_t& w21 = side_candidates[i2].first;
            const point_t& w22 = side_candidates[i2].second;
            point_t w2 = subtract(w22, w21);
            double det = cross(v, w2);
            // check if vectors are neither parallel nor collinear
            if (det == 0.)
                continue;
            // check that second side is cut by ray
            double u = cross(subtract(w21, middle), v) / det;
            if (u < 0. || u > 1.)
                continue;
            // check distance to second side
            double d = cross(subtract(w21, middle), w2) * v_length / det;
            if (d < door_width_min || d > door_width_max)
                continue;
            // check that sides are approximately parallel
            if (!check_angle(v, w2))
                continue;
            // we found a match! if there are multiple matches we only keep the closest
            if (d < side_candidate_distances[i1]) {
                side_candidate_distances[i1] = d;
                side_candidate_partners[i1] = i2;
            }
            if (d < side_candidate_distances[i2]) {
                side_candidate_distances[i2] = d;
                side_candidate_partners[i2] = i1;
            }
        }
    }

    // match side candidate pairs
    std::vector<door_t> door_candidates;
    std::vector<double> door_candidate_distances;
    for (std::size_t i1 = 0; i1 < ncandidates; i1++) {
        std::size_t i2 = side_candidate_partners[i1];
        if (i2 != static_cast<std::size_t>(-1) && i1 < i2
        && side_candidate_partners[i2] == i1) {
            const point_t& v21 = side_candidates[i1].first;
            const point_t& v22 = side_candidates[i1].second;
            const point_t& w21 = side_candidates[i2].first;
            const point_t& w22 = side_candidates[i2].second;
            door_t door_candidate = {segment_t{v21, v22}, segment_t{w21, w22}};
            door_candidates.push_back(door_candidate);
            door_candidate_distances.push_back(side_candidate_distances[i1]);
        }
    }
    std::cout << "Found " << door_candidates.size() << " door candidates\n";

#if BOOST_VERSION < 105600
    std::cerr << "Boost version >= 1.56.0 is required for checking space between door frames, so you might get more false positives\n";
    return door_candidates;
#else
    // check if area between door frame sides is unobstructed
    std::vector<door_t> doors;
    namespace bgi = boost::geometry::index;
    bgi::rtree<segment_t, bgi::rstar<16>> rtree(cleaned_segments);
    for (std::size_t i = 0; i < door_candidates.size(); i++) {
        const door_t& door_candidate = door_candidates[i];
        double d1 = boost::geometry::distance(door_candidate[0].first, door_candidate[0].second);
        double d2 = boost::geometry::distance(door_candidate[1].first, door_candidate[1].second);

        // aligned at the middle of the longer door frame side, create a
        // rectangle between the door sides. it looks like this:
        //      door_width
        //      +---------+
        //
        //        +-----+             +
        //  ----+ |     | +----       |
        // frame| |rect | |frame      | door_width * unobstructed_area_depth
        //  ----+ |     | +----       |
        //        +-----+             +
        //
        //        +-----+
        // door_width * unobstructed_area_width
        const segment_t& longer_side = door_candidate[d1 > d2 ? 0 : 1];
        double longer_side_length = d1 > d2 ? d1 : d2;
        const point_t& v21 = longer_side.first;
        const point_t& v22 = longer_side.second;
        // r = middle + t * v
        point_t middle = multiply(add(v21, v22), .5);
        point_t v2 = subtract(v22, v21);
        point_t v = {-v2.y(), v2.x()}; // 90 deg turn
        double factor = door_candidate_distances[i] / longer_side_length;
        // t2 and t are as long as the door is wide, t2 points in direction of
        // the first door side, t points perpendicularly to the other door side
        point_t t2 = multiply(v2, factor);
        point_t t = multiply(v, factor);
        double rectangle_length = params.unobstructed_area_depth;
        double width_padding = .5 - params.unobstructed_area_width * .5;
        point_t p1 = add(add(middle, multiply(t,      width_padding)), multiply(t2, -.5 * rectangle_length));
        point_t p2 = add(add(middle, multiply(t, 1. - width_padding)), multiply(t2, -.5 * rectangle_length));
        point_t p3 = add(add(middle, multiply(t, 1. - width_padding)), multiply(t2,  .5 * rectangle_length));
        point_t p4 = add(add(middle, multiply(t,      width_padding)), multiply(t2,  .5 * rectangle_length));

        // if this rectangle is not obstructed, we found an actual door
        ring_t unobstructed_area;
        unobstructed_area.insert(unobstructed_area.end(), {p1, p2, p3, p4, p1});
        std::vector<segment_t> result;
        bgi::query(rtree, bgi::intersects(unobstructed_area), std::back_inserter(result));
        if (result.size() == 0) {
            door_t d = {
                segment_t{door_candidate[0].second, door_candidate[1].first},
                segment_t{door_candidate[1].second, door_candidate[0].first}
            };
            doors.push_back(d);
        }
    }

    // near the door frames, the door might still intersect the map. prune door sides appropriately
    for (door_t& door : doors) {
        for (segment_t& side : door) {
            std::vector<segment_t> intersections;
            bgi::query(rtree, bgi::intersects(side), std::back_inserter(intersections));
            for (const segment_t& intersection : intersections) {
                // work around buggy boost::geometry::intersection rarely giving incorrect result
                if (equal(intersection.first, side.first) || equal(intersection.first, side.second)
                || equal(intersection.second, side.first) || equal(intersection.second, side.second))
                    continue;
                std::vector<point_t> intersection_points;
                boost::geometry::intersection(side, intersection, intersection_points);
                for (const point_t& intersection_point : intersection_points) {
                    segment_t newside1 = {side.first, intersection_point};
                    segment_t newside2 = {intersection_point, side.second};
                    side = boost::geometry::length(newside1) > boost::geometry::length(newside2)
                         ? newside1 : newside2;
                }
            }
        }
    }
    std::cout << "Found " << doors.size() << " doors!\n";
    return doors;
#endif
}


#include "engaging.h"

#include <fstream>
#include <cstdlib>  // For srand() and rand()

#include <Eigen/SVD>
#include <RobotUtilities/utilities.h>
#include <RobotUtilities/TimerLinux.h>

#include <solvehfvc.h>

#define PI 3.14159265

using namespace RUT;
using std::cout;
using std::cerr;
using std::endl;
using std::string;
using Eigen::Vector2d;
using Eigen::Matrix2d;

bool EngagingTaskServer::initEngagingTaskServer() {
  ROS_INFO_STREAM("Engaging server is starting");
  if (_ros_handle_p == nullptr) {
    ROS_ERROR_STREAM("[EngagingTaskServer] You must call .init() before .initEngagingTaskServer().");
    exit(1);
  }

  std::vector<double> scale_force_vector;
  _ros_handle_p->getParam("/constraint_estimation/scale_force_vector", scale_force_vector);
  if (!_ros_handle_p->hasParam("/constraint_estimation/scale_force_vector"))
    ROS_WARN_STREAM("Parameter [/constraint_estimation/scale_force_vector] not found!");

  _force_scale_matrix_inv = Matrix6d::Zero();
  for (int i = 0; i < 6; ++i) _force_scale_matrix_inv(i,i) = 1.0/scale_force_vector[i];

  double pool_duration = 0;
  _ros_handle_p->param(string("/constraint_estimation/pool_duration"), pool_duration, 0.5);
  if (!_ros_handle_p->hasParam("/constraint_estimation/pool_duration"))
      ROS_WARN_STREAM("Parameter [/constraint_estimation/pool_duration] not found");
  _pool_size = (int)round(pool_duration*_main_loop_rate);

  _ros_handle_p->param(string("/task/v_singular_value_threshold"),
      _v_singular_value_threshold, 0.1);
  _ros_handle_p->param(string("/task/f_singular_value_threshold"),
      _f_singular_value_threshold, 0.1);
  if (!_ros_handle_p->hasParam("/task/v_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter [/task/v_singular_value_threshold] not found");
  if (!_ros_handle_p->hasParam("/task/f_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter [/task/f_singular_value_threshold] not found");

  _ros_handle_p->param(std::string("/task/time_step"), _kTimeStepSec, 0.1);
  if (!_ros_handle_p->hasParam("/task/time_step"))
    ROS_WARN_STREAM("Parameter [/task/time_step] not found");
  _ros_handle_p->param(std::string("/task/goal_veloicty_meter"), _kGoalVelocityM, 0.01);
  if (!_ros_handle_p->hasParam("/task/goal_veloicty_meter"))
    ROS_WARN_STREAM("Parameter [/task/goal_veloicty_meter] not found");
  _ros_handle_p->param(std::string("/task/max_normal_force"), _kNormalForceMax, 4.0);
  if (!_ros_handle_p->hasParam("/task/max_normal_force"))
    ROS_WARN_STREAM("Parameter [/task/max_normal_force] not found");
  _ros_handle_p->param(std::string("/task/engaging_force"), _kEngagingForce, 1.0);
  if (!_ros_handle_p->hasParam("/task/engaging_force"))
    ROS_WARN_STREAM("Parameter [/task/engaging_force] not found");

  _ros_handle_p->param(std::string("/task/num_timesteps_file_path"), _timesteps_file_path, std::string("/tmp"));
  if (!_ros_handle_p->hasParam("/task/num_timesteps_file_path"))
    ROS_WARN_STREAM("Parameter [/task/num_timesteps_file_path] not found");

  std::vector<double> goal_W_vector;
  _ros_handle_p->getParam("/task/goal_direction_W1", goal_W_vector);
  if (!_ros_handle_p->hasParam("/task/goal_direction_W1"))
    ROS_WARN_STREAM("Parameter [/task/goal_direction_W1] not found!");
  _goal_W1(0) = goal_W_vector[0];
  _goal_W1(1) = goal_W_vector[1];
  _goal_W1(2) = goal_W_vector[2];
  _goal_W1.normalize();

  _ros_handle_p->getParam("/task/goal_direction_W2", goal_W_vector);
  if (!_ros_handle_p->hasParam("/task/goal_direction_W2"))
    ROS_WARN_STREAM("Parameter [/task/goal_direction_W2] not found!");
  _goal_W2(0) = goal_W_vector[0];
  _goal_W2(1) = goal_W_vector[1];
  _goal_W2(2) = goal_W_vector[2];
  _goal_W2.normalize();

  return true;
}

bool EngagingTaskServer::hostServices() {
  // --------------------------------------------------------
  // Establish Services
  // --------------------------------------------------------
  ros::ServiceServer reset_service            = _ros_handle_p->advertiseService("reset", &EngagingTaskServer::SrvReset, (RobotBridge*)this);
  ros::ServiceServer move_tool_service        = _ros_handle_p->advertiseService("move_tool", &EngagingTaskServer::SrvMoveTool, (RobotBridge*)this);
  ros::ServiceServer move_until_touch_service = _ros_handle_p->advertiseService("move_until_touch", &EngagingTaskServer::SrvMoveUntilTouch, (RobotBridge*)this);
  ros::ServiceServer get_pose_service         = _ros_handle_p->advertiseService("get_pose", &EngagingTaskServer::SrvGetPose, (RobotBridge*)this);
  ros::ServiceServer execute_task_service     = _ros_handle_p->advertiseService("execute_task", &EngagingTaskServer::SrvExecuteTask, this);
  ros::ServiceServer compliant_engage_service = _ros_handle_p->advertiseService("compliant_engage", &EngagingTaskServer::SrvCompliantEngage, this);

  cout << endl << "[EngagingTaskServer] Service servers are listening.." << endl;
  ros::spin();

  ROS_INFO_STREAM(endl << "[EngagingTaskServer] Service servers stopped." << endl);
  return true;
}

bool EngagingTaskServer::SrvCompliantEngage(std_srvs::Empty::Request  &req,
    std_srvs::Empty::Response &res) {
  _controller.reset();

  // read number of time steps
  std::ifstream fp;
  fp.open(_timesteps_file_path);
  if (!fp) {
    cerr << "Unable to open file for fp'.";
        exit(1); // terminate with error
  }
  int num_timesteps;
  fp >> num_timesteps;
  cout << "Number of timesteps: " << num_timesteps << endl;
  fp.close();

  for (int fr = 0; fr < num_timesteps; ++fr) {
    /**
     * Execute the hybrid action (6D)
     */
    // Tool frame velocity
    Matrix6d R_a = Matrix6d::Identity();

    // World frame velocity, set pose
    double pose_set[7];
    _robot.getPose(pose_set);

    Matrix4d SE3_WT_fb = posemm2SE3(pose_set);
    Matrix6d Adj_WT = SE32Adj(SE3_WT_fb);

    Vector6d force_W;
    if (fr < 0.5*num_timesteps)
      force_W << _goal_W1(0), _goal_W1(1), _goal_W1(2), 0, 0, 0;
    else
      force_W << _goal_W2(0), _goal_W2(1), _goal_W2(2), 0, 0, 0;
    force_W.normalize();
    force_W *= _kEngagingForce;
    Vector6d force_T = Adj_WT.transpose() * force_W;

    printf("F in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_W[0],
        force_W[1], force_W[2],force_W[3],force_W[4],force_W[5]);
    // cout << "Press Enter to begin motion: \n";
    // getchar();
    cout << "motion begins:" << endl;
    _controller.ExecuteHFVC(6, 0, R_a, pose_set, force_T.data(),
        HS_CONTINUOUS, _main_loop_rate, _kTimeStepSec);
  }
  return true;
}

bool EngagingTaskServer::SrvExecuteTask(std_srvs::Empty::Request  &req,
    std_srvs::Empty::Response &res) {
  _controller.reset();

  if (_controller._f_queue.size() < 0.5 * _pool_size) {
    cout << "Run update() for " << _pool_size << " frames:" << endl;
    // first, run update for 1s to populate the data deques
    ros::Rate pub_rate(_main_loop_rate);
    for (int i = 0; i < _main_loop_rate; ++i) {
      bool b_is_safe = _controller.update();
      pub_rate.sleep();
    }
    cout << "Done." << endl;
  }

  // read number of time steps
  std::ifstream fp;
  fp.open(_timesteps_file_path);
  if (!fp) {
    cerr << "Unable to open file for fp'.";
        exit(1); // terminate with error
  }
  int num_timesteps;
  fp >> num_timesteps;
  cout << "Number of timesteps: " << num_timesteps << endl;
  fp.close();

  Timer timer;
  std::srand(std::time(0));
  MatrixXd f_data, v_data;
  for (int fr = 0; fr < num_timesteps; ++fr) {
    cout << "time step " << fr << endl;
    timer.tic();
    /**
     * Estimate Natural Constraints
     */
    // get the weighted data
    f_data = MatrixXd::Zero(6, _controller._f_queue.size());
    v_data = MatrixXd::Zero(6, _controller._v_queue.size());
    for (int i = 0; i < _controller._f_queue.size(); ++i)
      f_data.col(i) = _controller._f_queue[i] * _controller._f_weights[i];
    for (int i = 0; i < _controller._v_queue.size(); ++i)
      v_data.col(i) = _controller._v_queue[i] * _controller._v_weights[i];

    // SVD on velocity data
    Eigen::JacobiSVD<MatrixXd> svd_v(v_data.transpose(), Eigen::ComputeThinV);
    VectorXd sigma_v = svd_v.singularValues();
    int DimV = 0;
    for (int i = 0; i < 6; ++i)
      if (sigma_v(i) > _v_singular_value_threshold) DimV ++;

    cout << "DimV: " << DimV << endl;
    // get a basis for row space of velocity data
    MatrixXd rowspace_v = svd_v.matrixV().leftCols(DimV);

    // filter out force data that:
    //    1. has a small weight
    std::vector<int> f_id;
    for (int i = 0; i < f_data.cols(); ++i) {
      double length = f_data.col(i).norm();
      if (length > 1.5) { // weighted length in newton
        f_id.push_back(i);
      }
    }
    int f_data_length = f_id.size();
    cout << "f_data_length: " << f_data_length << endl;
    MatrixXd f_data_filtered = MatrixXd::Zero(6, f_data_length);
    for (int i = 0; i < f_data_length; ++i)
      f_data_filtered.col(i) = f_data.col(f_id[i]);

    int DimF;
    MatrixXd Nf, Nf_null;
    MatrixXd f_data_selected;

    if (f_data_length < 6) {
      DimF = 0;
      Nf = MatrixXd(0, 6);
      Nf_null = MatrixXd::Identity(6, 6);
      f_data_selected = MatrixXd(6, 0);
    } else {
      // SVD on filtered force data, get the prime direction
      Eigen::JacobiSVD<MatrixXd> svd_f(f_data_filtered.transpose(), Eigen::ComputeThinV);
      VectorXd sigma_f = svd_f.singularValues();
      // check dimensions, estimate natural constraints
      double threshold = std::max(_f_singular_value_threshold, 0.1*sigma_f(0));
      DimF = 0;
      for (int i = 0; i < 6; ++i)
        if (sigma_f(i) > threshold) DimF ++;
      cout << "DimF: " << DimF << endl;

      // Sample DimF force directions
      MatrixXd f_data_normalized = f_data_filtered;
      for (int i = 0; i < f_data_length; ++i) f_data_normalized.col(i).normalize();
      int kNFSamples = (int)pow(double(f_data_length), 0.7);
      f_data_selected = MatrixXd(6, DimF);
      if (DimF == 1) {
        f_data_selected = f_data_filtered.rowwise().mean();
        f_data_selected.normalize();
      } else {
        MatrixXd f_data_selected_new(6, DimF);
        double f_distance = 0;
        assert(f_data_length < 32767);
        for (int i = 0; i < kNFSamples; ++i) {
          // 1. sample
          for (int s = 0; s < DimF; s++) {
            int r = (rand() % f_data_length);
            f_data_selected_new.col(s) = f_data_normalized.col(r);
          }
          // 2. compute distance
          double f_distance_new = 0;
          for (int ii = 0; ii < DimF-1; ++ii)
            for (int jj = ii+1; jj < DimF; ++jj)
              f_distance_new += (f_data_selected_new.col(ii) -
                  f_data_selected_new.col(jj)).norm();
          // 3. Update data
          if (f_distance_new > f_distance) {
            f_data_selected = f_data_selected_new;
            f_distance = f_distance_new;
          }
        } // end sampling
      }
      // Get Nf
      Nf = f_data_selected.transpose() * _force_scale_matrix_inv;
      Eigen::JacobiSVD<MatrixXd> svd_Nf(Nf, Eigen::ComputeFullV);
      Nf_null = svd_Nf.matrixV().leftCols(6 - DimF);
    }

    // compute goal
    double pose_fb[7], pose_set[7];
    _robot.getPose(pose_fb);
    Matrix3d R_WT = quat2SO3(pose_fb[3], pose_fb[4], pose_fb[5], pose_fb[6]);

    Vector3d goal_T;
    if (fr < 0.5*num_timesteps)
      goal_T = R_WT.transpose() * _goal_W1;
    else
      goal_T = R_WT.transpose() * _goal_W2;
    MatrixXd G0 = MatrixXd::Zero(1, 6);
    G0.block<1, 3>(0, 0) = goal_T.transpose();
    /**
     * Project Goal to the Null space of Nf
     */
    MatrixXd G = G0*Nf_null;
    double G_norm = G.norm();
    if (G_norm < 1e-5) {
      cerr << "[Engaging] Nf is conflicting with G." << endl;
      getchar();
    }
    G = G/G_norm;
    VectorXd b_G = VectorXd::Zero(1);
    b_G(0) = _kGoalVelocityM;

    /**
     * Do Hybrid Servoing (3D)
     */
    HFVC action;
    const int kDimActualized      = 6;
    const int kDimUnActualized    = 0;
    const int kDimSlidingFriction = 0;
    const int kNumSeeds           = 5;
    const int kDimLambda          = DimF;
    const int kPrintLevel         = 1;

    VectorXd F = VectorXd::Zero(6);
    MatrixXd Aeq(0, DimF + 6); // dummy
    VectorXd beq(0); // dummy
    MatrixXd A = MatrixXd::Zero(DimF, DimF + 6);
    VectorXd b_A = VectorXd::Zero(DimF);
    if (DimF > 0) {
      A.leftCols(DimF) = MatrixXd::Identity(DimF,DimF);
      for (int i = 0; i < DimF; ++i) b_A(i) = _kNormalForceMax;
      // trick:
      // one big force, others are small force
      if (DimF > 1) {
        A(0, 0) = -1;
        b_A(0) = -_kEngagingForce;
        // Vector6d goal_dir = G.row(0).transpose();
        // Vector6d rand_dir = Vector6d::Random().normalized();
        // while (goal_dir.dot(rand_dir) < 0) {
        //   rand_dir = Vector6d::Random().normalized();
        // }
        // rand_dir.tail(3) *= 25; // scale between N and Nm
        // MatrixXd Atemp = A;
        // VectorXd b_ATemp = b_A;
        // A = MatrixXd::Zero(DimF + 1, DimF + 6);
        // A.topRows(DimF) = Atemp;
        // A.block<1, 6>(DimF, kDimLambda) = rand_dir.transpose();
        // b_A = VectorXd::Zero(DimF + 1);
        // b_A.head(DimF) = b_ATemp;
        // b_A(DimF) = -_kEngagingForce;
      }
    }

    cout << "Nf: " << Nf.format(MatlabFmt) << endl;
    // cout << "G: " << G.format(MatlabFmt) << endl;
    // cout << "b_G: " << b_G.format(MatlabFmt) << endl;
    // cout << "F: " << F.format(MatlabFmt) << endl;
    // cout << "Aeq: " << Aeq.format(MatlabFmt) << endl;
    // cout << "beq: " << beq.format(MatlabFmt) << endl;
    // cout << "A: " << A.format(MatlabFmt) << endl;
    // cout << "b_A: " << b_A.format(MatlabFmt) << endl;

    solvehfvc(Nf, G, b_G, F, Aeq, beq, A, b_A,
      kDimActualized, kDimUnActualized,
      kDimSlidingFriction, kDimLambda,
      kNumSeeds, kPrintLevel,
      &action);

    double computation_time_ms = timer.toc();

    /**
     * Execute the hybrid action (6D)
     */
    Vector6d v_Tr = Vector6d::Zero();
    for (int i = 0; i < action.n_av; ++i)  v_Tr(i+action.n_af) = action.w_av(i);

    // Tool frame velocity
    Matrix6d R_a_inv = action.R_a.inverse();
    Vector6d v_T = R_a_inv*v_Tr;

    // limit the magnitude of v_T
    const double scale_rot_to_tran = 0.5; // 1m = 2rad
    Vector6d v_T_scaled = v_T;
    v_T_scaled.tail(3) *= scale_rot_to_tran;
    if (v_T_scaled.norm() > _kGoalVelocityM) {
      double scale_safe = _kGoalVelocityM/v_T_scaled.norm();
      v_T *= scale_safe;
    }

    // World frame velocity, set pose
    _robot.getPose(pose_fb);
    Matrix4d SE3_WT_fb = posemm2SE3(pose_fb);
    Matrix6d Adj_WT = SE32Adj(SE3_WT_fb);
    Vector6d v_W = Adj_WT*v_T;
    Matrix4d SE3_WT_command;
    SE3_WT_command = SE3_WT_fb + wedge6(v_W)*SE3_WT_fb*_kTimeStepSec;
    SE32Posemm(SE3_WT_command, pose_set);

    // Tool frame force
    Vector6d force_Tr_set = Vector6d::Zero();
    for (int i = 0; i < action.n_af; ++i)  force_Tr_set[i] = action.eta_af(i);
    Vector6d force_T = R_a_inv*force_Tr_set;
    Matrix6d Adj_TW = SE32Adj(SE3Inv(SE3_WT_fb));
    Vector6d force_W = Adj_TW.transpose() * force_T;

    double force_fb[6];
    int safe_flag = _robot.getWrench(force_fb);

    printf("F in tool: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_T[0],
        force_T[1], force_T[2],force_T[3],force_T[4],force_T[5]);
    printf("F fb: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_fb[0],
        force_fb[1], force_fb[2],force_fb[3],force_fb[4],force_fb[5]);
    printf("V in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", v_W[0], v_W[1],
        v_W[2],v_W[3],v_W[4],v_W[5]);
    printf("F in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_W[0],
        force_W[1], force_W[2],force_W[3],force_W[4],force_W[5]);
    cout << "Computation Time: " << computation_time_ms << endl;
    if (safe_flag == 2) {
      ROS_ERROR_STREAM("Force feedback stall!!!");
      return true;
    }

    if ((safe_flag != 0)||(fr == num_timesteps-1)) {
      // print to file
      std::ofstream fp;
      // f_queue
      fp.open(_task_data_file_path + "f_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_queue.size(); ++i) {
        stream_array_in(fp, _controller._f_queue[i].data(), 3);
        fp << endl;
      }
      fp.close();
      // v_queue
      fp.open(_task_data_file_path + "v_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._v_queue.size(); ++i) {
        stream_array_in(fp, _controller._v_queue[i].data(), 3);
        fp << endl;
      }
      fp.close();
      // f_weights
      fp.open(_task_data_file_path + "f_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_weights.size(); ++i)
        fp << _controller._f_weights[i] << endl;
      fp.close();

      // f_prob
      fp.open(_task_data_file_path + "f_prob.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_probability.size(); ++i)
        fp << _controller._f_probability[i] << endl;
      fp.close();

      // v_weights
      fp.open(_task_data_file_path + "v_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._v_weights.size(); ++i)
        fp << _controller._v_weights[i] << endl;
      fp.close();

      // f_data_filtered
      fp.open(_task_data_file_path + "f_data_filtered.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_filtered.transpose() << endl;
      fp.close();
      // f_data_selected
      fp.open(_task_data_file_path + "f_data_selected.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_selected.transpose() << endl;
      fp.close();
      // others
      fp.open(_task_data_file_path + "process.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << DimV << endl << DimF << endl;
      stream_array_in(fp, v_T.data(), 3);
      fp << endl;
      stream_array_in(fp, force_T.data(), 3);
      fp.close();
    }

    if (std::isnan(force_Tr_set[0])) {
      cout << "================== NaN =====================" << endl;
      cout << "Press Enter to continue.." << endl;
      getchar();
    }

    if (safe_flag == 3) {
      ROS_ERROR_STREAM("Force feedback violated safety limit!!!");
      if (fr < 0.5*num_timesteps) {
        pose_fb[0] -= _goal_W1[0]*2.0;
        pose_fb[1] -= _goal_W1[1]*2.0;
        pose_fb[2] -= _goal_W1[2]*2.0;
      } else {
        pose_fb[0] -= _goal_W2[0]*2.0;
        pose_fb[1] -= _goal_W2[1]*2.0;
        pose_fb[2] -= _goal_W2[2]*2.0;
      }
      _robot.setPose(pose_fb);
    } else {
      // cout << "Current pose: " << pose_fb[0] << ", " << pose_fb[1] << ", " << pose_fb[2] << endl;
      // cout << "Set pose: " << pose_set[0] << ", " << pose_set[1] << ", " << pose_set[2] << endl;
      // cout << "Press Enter to begin motion: \n";
      // getchar();
      cout << "motion begins:" << endl;
      _controller.ExecuteHFVC(action.n_af, action.n_av,
          action.R_a, pose_set, force_Tr_set.data(),
          HS_CONTINUOUS, _main_loop_rate, _kTimeStepSec);

      cout << endl;
    }

  } // end for
  return true;
}


//
// Created by lukas on 07.12.17.
//

#include "dtilp.hpp"

#include "../db/storage.hpp"
#include "../instance/instance.hpp" // IWYU pragma: keep
#include "../instance/job.hpp"
#include "../instance/laggraph.hpp"
#include "../instance/resource.hpp"
#include "../manager/errors.hpp"   // for Inconsisten...
#include "../util/fault_codes.hpp" // for FAULT_INVAL...
#include "../util/solverconfig.hpp"
#include "generated_config.hpp"

#include <limits>

#if defined(GUROBI_FOUND)
#include "../contrib/ilpabstraction/src/ilpa_gurobi.hpp"
#endif

#if defined(CPLEX_FOUND)
#include "../contrib/ilpabstraction/src/ilpa_cplex.hpp"
#endif

#include <boost/progress.hpp>

template <class SolverT>
DTILP<SolverT>::DTILP(const Instance & instance_in,
                      AdditionalResultStorage & additional,
                      const SolverConfig & sconf_in)
    : ILPBase<SolverT>(instance_in, additional, sconf_in), l("DTILP")
{
	this->use_sos1_for_starts = this->sconf.has_config("use_sos1_for_starts") &&
	                            this->sconf.as_bool("use_sos1_for_starts");
}

template <class SolverT>
void
DTILP<SolverT>::prepare_warmstart()
{
	this->prepare();
	this->job_is_fixed = std::vector<bool>(this->instance.job_count(), false);
}

template <class SolverT>
void
DTILP<SolverT>::prepare_start_point_constraints()
{
	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		Expression expr = this->env.create_expression();
		for (unsigned int t = 0; t < this->time_step_bounds[jid].second; ++t) {
			expr += (this->time_step_bounds[jid].first + t) * this->variables[jid][t];
		}

		this->model.add_constraint(expr, this->start_points[jid], expr);
	}
}

template <class SolverT>
void
DTILP<SolverT>::prepare_overshoot_costs()
{
	Expression overshoot_sum = this->env.create_expression();

	for (unsigned int i = 0; i < this->instance.resource_count(); ++i) {
		const Resource & res = this->instance.get_resource(i);
		// Overshoot costs
		for (unsigned int t = 0; t <= this->latest_deadline; ++t) {
			polynomial overshoot_costs = res.get_overshoot_costs(t);
			for (poly_term term : overshoot_costs) {
				double coefficient = std::get<0>(term);
				double exponent = std::get<1>(term);

				if ((exponent > 1.0) || (exponent < 1.0)) {
					throw InconsistentResultError(this->instance, this->seed,
					                              FAULT_INVALID_COST_EXPONENTS,
					                              "Only 1.0 is supported as exponent");
				}

				overshoot_sum += coefficient * this->overshoot_variables[i][t];
			}
		}
	}

	this->model.add_constraint(overshoot_sum, this->overshoot_cost_variable,
	                           MIPSolver::INFTY);
}

template <class SolverT>
void
DTILP<SolverT>::prepare_variables()
{
	// We use the start-variables from the base ILP
	this->generate_vars_start_points();

	// Switch-On-Variables
	this->variables.resize(this->instance.job_count());
	this->time_step_bounds.resize(this->instance.job_count());

	this->overduration_and_swon_variables.resize(this->instance.job_count());

	std::vector<double> sos1_weights;

	for (unsigned int i = 0; i < this->instance.job_count(); i++) {
		const Job & job = this->instance.get_job(i);
		int earliest_start_with_extension =
		    std::max(((int)job.get_release() -
		              (int)this->instance.get_window_extension_limit()),
		             0);
		int latest_finish_with_extension =
		    std::min((int)job.get_deadline() +
		                 (int)this->instance.get_window_extension_limit(),
		             (int)this->instance.get_window_extension_hard_deadline()
		                 .value_or_default(std::numeric_limits<int>::max()));

		this->time_step_bounds[i] = {earliest_start_with_extension,
		                             latest_finish_with_extension -
		                                 earliest_start_with_extension -
		                                 (int)job.get_duration() + 1};

		this->variables[i].resize(this->time_step_bounds[i].second);
		for (unsigned int j = 0; j < this->time_step_bounds[i].second; j++) {
			this->variables[i][j] = this->model.add_var(
			    ilpabstraction::VariableType::BINARY, 0, 1,
			    std::to_string(i) + std::string("_at_") +
			        std::to_string(this->time_step_bounds[i].first + j));
			// ABSTRACTED
			// this->variables[i][j] = this->model->addVar(0, 1, 0, GRB_BINARY,
			// std::string("start_") + 		    std::to_string(i) + std::string("_at_")
			// +
			// std::to_string(j));
		}

		// Add SOS1
		if (sos1_weights.size() < this->time_step_bounds[i].second) {
			auto old_size = sos1_weights.size();
			sos1_weights.resize(this->time_step_bounds[i].second);
			for (auto j = old_size; j < this->time_step_bounds[i].second; ++j) {
				sos1_weights[j] = (double)j;
			}
		} else if (sos1_weights.size() > this->time_step_bounds[i].second) {
			sos1_weights.resize(this->time_step_bounds[i].second);
		}
		if (this->use_sos1_for_starts) {
			this->model.add_sos1_constraint(this->variables[i], sos1_weights,
			                                std::string("sos1_") + std::to_string(i));
		}
	}

	// Duration variables
	/*
	this->duration_variables.resize(this->instance.job_count());
	for (unsigned int i = 0 ; i < this->instance.job_count() ; i++) {
	  const Job & job = this->instance.get_job(i);

	  this->duration_variables[i] =
	this->model.add_var(ilpabstraction::VariableType::INTEGER, job.get_duration(),
	                                                    MIPSolver::INFTY,
	                                                    std::string("duration_") +
	std::to_string(i));
	  // ABSTRACTED
	  //this->duration_variables[i] = this->model->addVar(0,
	std::numeric_limits<int>::max(), 0,
	  //                                                   GRB_INTEGER,
	std::string("duration_") +
	  //			  std::to_string(i));
	}
	*/

	// Duration & Overduration variables
	this->overduration_variables.resize(this->instance.job_count());
	for (unsigned int i = 0; i < this->instance.job_count(); i++) {
		unsigned int max_overduration = 0;

		for (const auto & edge :
		     this->instance.get_laggraph().reverse_neighbors(i)) {
			max_overduration += edge.max_recharge;
		}

		if (max_overduration > 0) {
			// Create Overduration indicators
			for (unsigned int t = 1; t <= max_overduration; ++t) {
				this->overduration_variables[i].push_back(this->model.add_var(
				    ilpabstraction::VariableType::BINARY, 0, 1,
				    std::string("overduration_") + std::to_string(t) +
				        std::string("_ofjob_") + std::to_string(i)));
			}

			// every t is combined with at most <max_overduration>
			// overduration-indicators
			this->overduration_and_swon_variables[i].resize(
			    this->time_step_bounds[i].second);
			for (unsigned int t = 0; t < this->time_step_bounds[i].second; ++t) {
				for (unsigned int o = 0; o < max_overduration; ++o) {
					this->overduration_and_swon_variables[i][t].push_back(
					    this->model.add_var(
					        ilpabstraction::VariableType::BINARY, 0, 1,
					        std::string("overduration_") + std::to_string(o) +
					            std::string("_AND_start_at_") +
					            std::to_string(this->time_step_bounds[i].first + t) +
					            std::string("_job_") + std::to_string(i)));
				}
			}
		}
	}

	this->overshoot_variables.resize(this->instance.resource_count());
	for (unsigned int i = 0; i < this->instance.resource_count(); i++) {
		this->overshoot_variables[i].resize((unsigned int)this->latest_deadline +
		                                    1);
		for (unsigned int j = 0; j <= this->latest_deadline; j++) {
			this->overshoot_variables[i][j] = this->model.add_var(
			    ilpabstraction::VariableType::CONTINUOUS, 0, MIPSolver::INFTY,
			    std::string("res_") + std::to_string(i) + std::string("_timestep_") +
			        std::to_string(j));
		}
	}

	/*
	 * Extension variables
	 */
	this->left_extension_var.resize(this->instance.job_count());
	this->right_extension_var.resize(this->instance.job_count());
	this->window_not_modified_var.resize(this->instance.job_count());
	for (unsigned int i = 0; i < this->instance.job_count(); i++) {
		this->left_extension_var[i] = this->model.add_var(
		    ilpabstraction::VariableType::INTEGER, 0, MIPSolver::INFTY,
		    std::string("left_extension_") + std::to_string(i));
		this->right_extension_var[i] = this->model.add_var(
		    ilpabstraction::VariableType::INTEGER, 0, MIPSolver::INFTY,
		    std::string("right_extension_") + std::to_string(i));
		this->window_not_modified_var[i] = this->model.add_var(
		    ilpabstraction::VariableType::BINARY, 0, 1,
		    std::string("window_not_modified_") + std::to_string(i));
	}
	this->window_extension_time_var = this->model.add_var(
	    ilpabstraction::VariableType::CONTINUOUS, 0, MIPSolver::INFTY,
	    std::string("total_extension_time"));
	this->window_extension_job_var =
	    this->model.add_var(ilpabstraction::VariableType::CONTINUOUS, 0,
	                        MIPSolver::INFTY, std::string("total_extension_job"));

	this->model.commit_variables();
}

template <class SolverT>
void
DTILP<SolverT>::prepare_extension_constraints()
{
	/*
	 * Limit the sum of the extensions / number of extended jobs
	 */
	Expression extension_sum = this->env.create_expression();
	Expression modified_sum = this->env.create_expression();
	for (unsigned int i = 0; i < this->instance.job_count(); ++i) {
		extension_sum += this->left_extension_var[i];
		extension_sum += this->right_extension_var[i];
		modified_sum += (1 - this->window_not_modified_var[i]);
	}

	this->model.add_constraint(extension_sum, this->window_extension_time_var,
	                           MIPSolver::INFTY);
	this->model.add_constraint(modified_sum, this->window_extension_job_var,
	                           MIPSolver::INFTY);

	this->window_extension_time_constraint = this->model.add_constraint(
	    MIPSolver::NEGATIVE_INFTY, this->window_extension_time_var,
	    this->instance.get_window_extension_limit());
	this->window_extension_job_constraint = this->model.add_constraint(
	    MIPSolver::NEGATIVE_INFTY, this->window_extension_job_var,
	    this->instance.get_window_extension_job_limit());

	/*
	 * Force unmodified-var to zero
	 */
	for (unsigned int i = 0; i < this->instance.job_count(); ++i) {
		this->model.add_sos1_constraint({this->left_extension_var[i],
		                                 this->right_extension_var[i],
		                                 this->window_not_modified_var[i]},
		                                {});
	}
}

template <class SolverT>
void
DTILP<SolverT>::prepare_resource_constraints()
{
	std::vector<std::vector<Expression>> usages;
	usages.resize(this->instance.resource_count());
	for (unsigned int rid = 0; rid < this->instance.resource_count(); ++rid) {
		usages[rid].resize(this->latest_deadline);
		for (unsigned int i = 0; i < this->latest_deadline; ++i) {
			usages[rid][i] = this->env.create_expression();
		}
	}

	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		const Job & job = this->instance.get_job(jid);

		for (unsigned int rid = 0; rid < this->instance.resource_count(); ++rid) {

			if (job.get_resource_usage(rid) <= 0) {
				continue;
			}

			for (unsigned int t = 0; t < this->time_step_bounds[jid].second; ++t) {

				// Handle normal duration of the job if started at t
				for (unsigned int t_offset = 0; t_offset < job.get_duration();
				     ++t_offset) {
					usages[rid][this->time_step_bounds[jid].first + t + t_offset] +=
					    this->variables[jid][t] * job.get_resource_usage(rid);
				}

				// Handle overduration
				for (unsigned int od_length = 0;
				     od_length < this->overduration_variables[jid].size();
				     ++od_length) {
					usages[rid][this->time_step_bounds[jid].first + t +
					            job.get_duration() + od_length] +=
					    this->overduration_and_swon_variables[jid][t][od_length] *
					    job.get_resource_usage(rid);
				}
			}
		}
	}

	for (unsigned int rid = 0; rid < this->instance.resource_count(); ++rid) {
		Variable capacity = this->max_usage_variables[rid];
		const Resource & res = this->instance.get_resource(rid);
		auto availability_it = res.get_availability().begin();

		for (unsigned int t = 0; t < this->latest_deadline; ++t) {
			if ((availability_it + 1) != (res.get_availability().end()) &&
			    (availability_it + 1)->first <= t) {
				availability_it++;
			}
			double available = availability_it->second;

			this->model.add_constraint(
			    usages[rid][t] - available, this->overshoot_variables[rid][t],
			    MIPSolver::INFTY,
			    std::string("res_") + std::to_string(rid) + "_at_" +
			        std::to_string(t) + std::string("_overshoot"));
			this->model.add_constraint(
			    usages[rid][t] - available, capacity, MIPSolver::INFTY,
			    std::string("res_") + std::to_string(rid) + "_at_" +
			        std::to_string(t) + std::string("_investment"));
		}
	}
}

template <class SolverT>
void
DTILP<SolverT>::prepare_job_constraints()
{
	for (unsigned int job_no = 0; job_no < this->instance.job_count(); ++job_no) {
		Expression expr = this->env.create_expression();
		const Job & job = this->instance.get_job(job_no);
		unsigned int deadline = job.get_deadline();
		unsigned int release = job.get_release();

		for (unsigned int t = 0; t < this->time_step_bounds[job_no].second; ++t) {
			expr += this->variables[job_no][t];
		}

		this->model.add_constraint(1, expr, 1,
		                           std::string("pull_up_constraint_") +
		                               std::to_string(job_no));

		// ABSTRACTED
		// this->model->addConstr(expr, GRB_EQUAL, 1,
		// std::string("pull_up_constraint_") + 			  std::to_string(job_no));

		Expression release_expr = this->env.create_expression();
		release_expr += release;
		release_expr -= this->left_extension_var[job_no];
		this->model.add_constraint(
		    release_expr, this->start_points[job_no], INFINITY,
		    std::string("release_constraint_") + std::to_string(job_no));

		Expression deadline_expr = this->env.create_expression();
		deadline_expr += deadline;
		deadline_expr += this->right_extension_var[job_no];
		this->model.add_constraint(
		    0, this->start_points[job_no] + this->duration_variables[job_no],
		    deadline_expr,
		    std::string("deadline_constraint_") + std::to_string(job_no));
	}
}

template <class SolverT>
void
DTILP<SolverT>::prepare()
{
	this->prepare_pre();

	BOOST_LOG(l.d()) << "Preparing Variables";
	this->prepare_variables();
	BOOST_LOG(l.d()) << "Preparing Start Point Constraints";
	this->prepare_start_point_constraints();
	BOOST_LOG(l.d()) << "Preparing Job Constraints";
	this->prepare_job_constraints();
	BOOST_LOG(l.d()) << "Preparing Duration Constraints";
	this->prepare_duration_constraint();
	BOOST_LOG(l.d()) << "Preparing Resource Constraints";
	// BOOST_LOG(l.d()) << std::flush;
	this->prepare_resource_constraints();
	BOOST_LOG(l.d()) << "Preparing Extension Constraints";
	this->prepare_extension_constraints();
	BOOST_LOG(l.d()) << "Preparing overshoot costs";
	this->prepare_overshoot_costs();

	this->prepare_post();
}

// TODO re-write this. It's a mess.
template <class SolverT>
void
DTILP<SolverT>::prepare_duration_constraint()
{
	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		Expression dur_expr = this->env.create_expression();
		dur_expr += this->instance.get_job(jid).get_duration();

		//
		// Duration variable
		//
		for (const auto & edge :
		     this->instance.get_laggraph().reverse_neighbors(jid)) {
#ifdef ENABLE_ASSERTIONS
			assert(edge.s == jid);
#endif

			if (edge.max_recharge == 0) {
				continue;
			}

			// Necessary to cap at 0
			Variable drain_addition_var = this->model.add_var(
			    VarType::CONTINUOUS, 0, MIPSolver::INFTY,
			    std::string("drain_addition_") + std::to_string(edge.s) +
			        std::string("->") + std::to_string(edge.s));

			// TODO FIXME this is slow
			this->model.commit_variables();

			// TODO FIXME how is the max recharge honored here?
			Expression gap =
			    this->start_points[edge.t] - this->start_points[edge.s] - edge.lag;
			Expression extension = gap * edge.drain_factor;
			this->model.add_constraint(extension, drain_addition_var,
			                           MIPSolver::INFTY);

			dur_expr += drain_addition_var;
		}

		// TODO is an upper bounding enough here?
		// FIXME NO!
		Expression var_expr = this->duration_variables[jid] * 1;
		this->model.add_constraint(dur_expr, var_expr, MIPSolver::INFTY);

		//
		// Overduration variables
		//
		for (unsigned int t = 0; t < this->overduration_variables[jid].size();
		     ++t) {
			Expression rhs = this->env.create_expression();
			Expression lhs = this->env.create_expression();
			rhs += this->duration_variables[jid] -
			       this->instance.get_job(jid).get_duration() - t;

			// TODO FIXME I'm not sure the M is large enough?
			// TODO FIXME SOS
			lhs += this->overduration_variables[jid][t] * 2 * this->latest_deadline;

			this->model.add_constraint(rhs, lhs, MIPSolver::INFTY);
		}

		//
		// ANDing of overduration and switch-on
		//
		for (unsigned int od_length = 0;
		     od_length < this->overduration_variables[jid].size(); ++od_length) {
			for (unsigned int t = 0; t < this->time_step_bounds[jid].second; ++t) {
				this->model.add_constraint(
				    this->overduration_variables[jid][od_length] +
				        this->variables[jid][t],
				    this->overduration_and_swon_variables[jid][t][od_length] * 2,
				    MIPSolver::INFTY);
			}
		}
	}
}

template <class SolverT>
void
DTILP<SolverT>::print_profile() const
{
	std::vector<double> sums(this->instance.resource_count(), 0.0);

	for (unsigned int t = 0; t < this->latest_deadline; t++) {
		BOOST_LOG(l.d(2)) << "====> Step " << t;
		for (unsigned int r = 0; r < this->instance.resource_count(); ++r) {
			double overshoot =
			    this->model.get_variable_assignment(this->overshoot_variables[r][t]);
			// double overshoot =
			// this->overshoot_variables[r][t].get(GRB_DoubleAttr_X);
			sums[r] += overshoot;
			BOOST_LOG(l.d(2)) << "    Res " << r << " overshoot: " << overshoot;
		}
	}

	for (unsigned int r = 0; r < this->instance.resource_count(); ++r) {
		BOOST_LOG(l.d(2)) << "======> Overshoot sum for Res " << r << ": "
		                  << sums[r];
	}
}

template <class SolverT>
void
DTILP<SolverT>::warmstart_with_fixed(
    std::vector<bool> fixed_jobs, std::vector<unsigned int> start_pos,
    unsigned int time_limit,
    Maybe<double> extension_time_usage_cost_coefficient,
    Maybe<double> extension_job_usage_cost_coefficient,
    Maybe<unsigned int> extension_time_limit,
    Maybe<unsigned int> extension_job_limit)
{
	BOOST_LOG(l.i()) << "Warm-starting.";

	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		this->model.set_start(this->start_points[jid], start_pos[jid]);
		// TODO also set start-indicators?

		if (fixed_jobs[jid] && !this->job_is_fixed[jid]) {
			this->fix_job(jid, start_pos[jid]);
		} else if (!fixed_jobs[jid] && this->job_is_fixed[jid]) {
			this->unfix_job(jid);
		}
	}

	if (extension_time_usage_cost_coefficient.valid()) {
		this->model.change_objective_coefficient(
		    this->window_extension_time_var,
		    extension_time_usage_cost_coefficient.value());
	} else {
		this->model.change_objective_coefficient(this->window_extension_time_var,
		                                         0);
	}

	if (extension_job_usage_cost_coefficient.valid()) {
		this->model.change_objective_coefficient(
		    this->window_extension_job_var,
		    extension_job_usage_cost_coefficient.value());
	} else {
		this->model.change_objective_coefficient(this->window_extension_job_var, 0);
	}

	if (extension_time_limit.valid()) {
		this->model.change_constraint_ub(this->window_extension_time_constraint,
		                                 extension_time_limit.value());
	} else {
		// TODO FIXME
		// this->model.change_constraint_ub(this->window_extension_time_constraint,
		//                                 this->instance.get_window_extension_limit());
	}

	if (extension_job_limit.valid()) {
		this->model.change_constraint_ub(this->window_extension_job_constraint,
		                                 extension_job_limit.value());
	} else {
		// TODO FIXME
		// this->model.change_constraint_ub(this->window_extension_job_constraint,
		//                                this->instance.get_window_extension_job_limit());
	}

	this->solve(time_limit);
}

template <class SolverT>
void
DTILP<SolverT>::fix_job(unsigned int jid, unsigned int time)
{
	this->model.change_var_bounds(this->start_points[jid], time, time);
	// TODO also change derived variables?
}

template <class SolverT>
void
DTILP<SolverT>::unfix_job(unsigned int jid)
{
	this->model.change_var_bounds(
	    this->start_points[jid], this->time_step_bounds[jid].first,
	    this->time_step_bounds[jid].first + this->time_step_bounds[jid].second);
	// TODO also change derived variables?
}

template <class SolverT>
void
DTILP<SolverT>::run()
{
	this->prepare();
	this->base_run();
}

template <class SolverT>
std::string
DTILP<SolverT>::get_id()
{
	return "DTILP v2.3 (" + std::string(SolverT::NAME) + ")";
}

// explicit instantiation

#if defined(GUROBI_FOUND)
template class DTILP<ilpabstraction::GurobiInterface>;
#endif

#if defined(CPLEX_FOUND)
template class DTILP<ilpabstraction::CPLEXInterface>;
#endif


#include <algorithm>
#include <fstream>
#include <memory>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>
#include <utility>

#include <cstring>
#include <cstdlib>
#include <cmath>

#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <boost/fusion/include/adapt_struct.hpp>

#include "GLRenderWindow.h"
#include "GLContext.h"
#include "GLDrawIndexedCommand.h"
#include "GLRenderTargetView.h"

#include "Camera.h"

#include "Utilities.h"
#include "BufferUtils/AlignedWriter.h"

#include "Model/Mesh.h"

#include "Messaging/Listener.h"
#include "Messaging/KeyMessage.h"
#include "Messaging/MouseMessage.h"
#include "Messaging/MessageTypes.h"

struct GlobalBlock
{
	glm::mat4 cameraClipMat;
	float time;
};
BOOST_FUSION_ADAPT_STRUCT
(GlobalBlock,
 (glm::mat4, cameraClipMat)
 (float, time)
);

struct ModelBlock
{
	glm::mat4 modelCameraMat;
	glm::vec3 color;
};
BOOST_FUSION_ADAPT_STRUCT
(ModelBlock,
 (glm::mat4, modelCameraMat)
 (glm::vec3, color)
);

std::unique_ptr<GLRenderWindow> createWindow(int argc, char **argv);

class TestApp : private Listener
{
private:
	GLRenderWindow *window;
	GLContext *context;

	void initState()
	{
		context->srgbWriteEnabled(true);
		context->faceCullingEnabled(false);
		context->depthTestEnabled(false);
		context->setClearColor(glm::vec4(0.0, 0.0, 0.5, 1.0));
		context->useRenderTarget(window);
	}

	std::unique_ptr<GLDrawIndexedCommand> squareDrawCommand, sphereDrawCommand;
	Mesh squareMesh, sphereMesh;

	void loadMesh()
	{
		// Vertices for a square specified clockwise from top left
		const float testSquareTriAttribs[] =
		{
		 -0.5, 0.5, 0.0, 1.0,	0.0f, 0.0f,		// 0
		 0.5, 0.5, 0.0, 1.0,	0.0f, 1.0f,		// 1
		 0.5, -0.5, 0.0, 1.0,	1.0f, 1.0f,		// 2
		 -0.5, -0.5, 0.0, 1.0,	1.0f, 0.0f		// 3
		};
		const GLushort testSquareInd[] = {0, 1, 3, 3, 2, 1};

		MeshVertexAttribDescriptor squareAttribDesc[] =
		{
		 {GLVertexArrayObject::Position, 4, GL_FLOAT, GL_FALSE, sizeof(float) * 6, 0},
		 {GLVertexArrayObject::TexCoord, 2, GL_FLOAT, GL_FALSE, sizeof(float) * 6, sizeof(float) * 4}
		};
		squareMesh = loadMeshFromMem(context, testSquareTriAttribs, squareAttribDesc, 2, 4,
		                                  testSquareInd, GL_UNSIGNED_SHORT, 6, GL_TRIANGLES);

		sphereMesh = loadMeshFromFile(context, "resources/meshes/sphere/mesh.dae");
	}

	std::unique_ptr<GLMutableBuffer> squareUniformBuffer, globalUniformBuffer;

	void loadBuffers()
	{
		squareUniformBuffer = context->getMutableBuffer(GL_UNIFORM_BUFFER, GL_STREAM_DRAW, 256, nullptr);
		globalUniformBuffer = context->getMutableBuffer(GL_UNIFORM_BUFFER, GL_DYNAMIC_DRAW, 256, nullptr);
		squareUniformBuffer->bindBase(GLUniformBlockBinding::ModelBlock);
		globalUniformBuffer->bindBase(GLUniformBlockBinding::GlobalBlock);
	}

	std::unique_ptr<GLShaderProgram> vertProg, fragProg;
	std::unique_ptr<GLProgramPipeline> pipeline;

	std::unique_ptr<GLTexture2D> testTexture;
	std::unique_ptr<GLSampler> testSampler;

	void loadTextures()
	{
		std::vector<unsigned char> texData;
		const unsigned int colorBlocks = 5;
		for(unsigned int i = 0; i < colorBlocks; ++i)
		{
			for(unsigned int j = 0; j < 3; ++j)
				texData.push_back(255*i/(colorBlocks-1));
			texData.push_back(255);
		}
		testTexture = context->getTexture2D(0, true, GL_RGB8, colorBlocks, 1, GL_RGBA, GL_UNSIGNED_BYTE, &texData[0]);

		testSampler = context->getSampler();
		testSampler->setMagFilter(GL_NEAREST);
		testSampler->setMinFilter(GL_NEAREST_MIPMAP_NEAREST);
		testSampler->setWrapMode(GL_CLAMP_TO_EDGE);
	}

	void loadShaders()
	{
		using namespace std;

		ofstream outLog("log.txt");
		try
		{
			string vertProgSource = readFile("resources/shaders/testing/vert.glsl");
			string fragProgSource = readFile("resources/shaders/testing/frag.glsl");

			string log;
			vertProg = context->getStandaloneShaderProgram(GL_VERTEX_SHADER, vertProgSource, &log);
			outLog << log << endl;
			fragProg = context->getStandaloneShaderProgram(GL_FRAGMENT_SHADER, fragProgSource, &log);
			outLog << log << endl;
		}
		catch(GLLinkerError& e)
		{
			outLog << e.getErrorLog();
			throw;
		}

		pipeline = context->getProgramPipeline();
		pipeline->useProgramStages(GL_VERTEX_SHADER_BIT, *vertProg);
		pipeline->useProgramStages(GL_FRAGMENT_SHADER_BIT, *fragProg);

		vertProg->setUniformBlockBinding(GLProgramUniformBlockIndex::GlobalBlock, GLUniformBlockBinding::GlobalBlock);
		vertProg->setUniformBlockBinding(GLProgramUniformBlockIndex::ModelBlock, GLUniformBlockBinding::ModelBlock);
		GLint diffuseLocation = fragProg->getUniformLocation("diffuse");
		fragProg->setUniform(diffuseLocation, 0);
	}

	Camera cam;

	struct Movement
	{
		float pitchMouseChange;
		float yawMouseChange;
		bool forwardPressed : 1;
		bool backwardPressed : 1;
		bool rightPressed : 1;
		bool leftPressed : 1;
		bool upPressed : 1;
		bool downPressed : 1;
		bool rotRightPressed : 1;
		bool rotLeftPressed : 1;
	} mutable movement;

	virtual void acceptMessage(Message& m) override final
	{
		if(m.getType() == MessageType::Window_Resized)
		{
			float aspectRatio = (float) window->getWidth() / (float) window->getHeight();
			cam.setAspectRatio(aspectRatio);
		}
		else if(m.getType() == MessageType::MouseInput)
		{
			MouseMessage& mm = static_cast<MouseMessage&>(m);
			glm::vec2 mouseChange = mm.getChange();
			movement.pitchMouseChange += mouseChange.y;
			movement.yawMouseChange += mouseChange.x;
			std::cout << movement.pitchMouseChange << ' ' << movement.yawMouseChange << std::endl;
		}
		else if(m.getType() == MessageType::KeyboardInput)
		{
			KeyMessage& km = static_cast<KeyMessage&>(m);
			switch(km.getKey())
			{
			case 'W':
				movement.forwardPressed =
						km.getAction() == KeyMessage::Release ? false : true;
				break;
			case 'A':
				movement.leftPressed =
						km.getAction() == KeyMessage::Release ? false : true;
				break;
			case 'S':
				movement.backwardPressed =
						km.getAction() == KeyMessage::Release ? false : true;
				break;
			case 'D':
				movement.rightPressed =
						km.getAction() == KeyMessage::Release ? false : true;
				break;
			case 'R':
				movement.upPressed =
						km.getAction() == KeyMessage::Release ? false : true;
				break;
			case 'F':
				movement.downPressed =
						km.getAction() == KeyMessage::Release ? false : true;
				break;
			case 'Q':
				movement.rotLeftPressed =
						km.getAction() == KeyMessage::Release ? false : true;
				break;
			case 'E':
				movement.rotRightPressed =
						km.getAction() == KeyMessage::Release ? false : true;
				break;
			case '=':
				if(km.getAction() != KeyMessage::Release)
					if(cam.getFov() < 1.0f)
						cam.setFov(cam.getFov() + 0.01f);
				break;
			case '-':
				if(km.getAction() != KeyMessage::Release)
					if(cam.getFov() > 0.1f)
						cam.setFov(cam.getFov() - 0.01f);
				break;
			case '[':
				context->srgbWriteEnabled(true);
				break;
			case ']':
				context->srgbWriteEnabled(false);
				break;
			default:
				break;
			}
		}
	}

	void calcMovement(glm::vec3& linearV, glm::vec3& angularV) const
	{
		int forwardDir = movement.forwardPressed ?
				1 : movement.backwardPressed ? -1 : 0;
		int rightDir = movement.rightPressed ?
				1 : movement.leftPressed ? -1 : 0;
		int upDir = movement.upPressed ?
				1 : movement.downPressed ? -1 : 0;
		glm::vec3 camForwardDir = cam.calcForwardDirection();
		glm::vec3 camUpDir = cam.getUpDirection();
		glm::vec3 camRightDir = -glm::cross(camUpDir, camForwardDir);

		int rotRightDir = movement.rotRightPressed ?
				1 : movement.rotLeftPressed ? -1 : 0;
		angularV = glm::vec3(rotRightDir) * camForwardDir;
		angularV += glm::vec3(-movement.pitchMouseChange) * camRightDir;
		angularV += glm::vec3(-movement.yawMouseChange) * camUpDir;

		// Zero out mouse changes after processing them
		movement.pitchMouseChange = 0.0f;
		movement.yawMouseChange = 0.0f;

		linearV = glm::vec3(0.0f);
		linearV += glm::vec3((float)forwardDir) * camForwardDir;
		linearV += glm::vec3((float)rightDir) * camRightDir;
		linearV += glm::vec3((float)upDir) * camUpDir;
	}

public:
	TestApp(GLRenderWindow *window_in): window(window_in)
	{
		using namespace std;

		context = window->getContext();
		auto version = context->getGLVersion();
		cout << version.first << ',' << version.second << endl;
		context->setCurrent();

		initState();
		loadMesh();
		loadBuffers();
		loadTextures();
		loadShaders();

		squareDrawCommand.reset(new GLDrawIndexedCommand(squareMesh.getVao(), squareMesh.getIndexCount(), squareMesh.getPrimType(),
                                                         (const void*) squareMesh.getIndexOffset(), squareMesh.getIndexType()));
		sphereDrawCommand.reset(new GLDrawIndexedCommand(sphereMesh.getVao(), sphereMesh.getIndexCount(), sphereMesh.getPrimType(),
                                                         (const void*) sphereMesh.getIndexOffset(), sphereMesh.getIndexType()));

		window->registerListener(this);

		cam.setAspectRatio((float) window->getWidth() / (float) window->getHeight());
	}

	void run()
	{
		glm::mat4 modelWorldMat(1.0);

		GlobalBlock globalBlock;
		ModelBlock modelBlock;

		double prevFrameTime = 0.0;
		double curFrameTime = 0.0;
		double elapsedTime = 0.0;

		while(!window->shouldClose())
		{
			prevFrameTime = curFrameTime;
			curFrameTime = window->getTime();
			elapsedTime = curFrameTime - prevFrameTime;
			globalBlock.time = curFrameTime;
//			// Draw to framebuffer object
//			// On my system this is the only way I can get SRGB to work...
			context->useRenderTarget(window);
			context->clear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

			glm::vec3 linearV;
			glm::vec3 angularV;
			calcMovement(linearV, angularV);
			cam.offsetCamera(glm::vec3((float)elapsedTime * 3.0f) * linearV);
			if(glm::length(angularV) > 0.01f)
			{
				cam.orientCamera(glm::rotate(glm::quat(), (float)elapsedTime * length(angularV) * 2.5f, angularV));
			}

			modelBlock.modelCameraMat = cam.calcViewMatrix() * modelWorldMat;
			globalBlock.cameraClipMat = cam.calcPerspectiveMatrix();

			modelBlock.color = glm::mix(glm::vec3(1.0, 0.0, 0.0), glm::vec3(0.0, 1.0, 0.0), sin(window->getTime())*0.5+0.5);

			// Update uniform buffer with perspective projection matrix
			{
				GLBufferMapping map = globalUniformBuffer->mapRange(0, globalUniformBuffer->getSize(), GL_MAP_WRITE_BIT);
				void *raw = map.getMapPtr();
				AlignedWriter globalWriter(raw);
				globalWriter.write<AlignedWriterLayoutSTD140>(globalBlock);
			}

			// Update modelview matrix
			{
				GLBufferMapping map = squareUniformBuffer->mapRange(0, squareUniformBuffer->getSize(), GL_MAP_WRITE_BIT);
				void *raw = map.getMapPtr();
				AlignedWriter modelWriter(raw);
				modelWriter.write<AlignedWriterLayoutSTD140>(modelBlock);
			}

			context->useExecutable(pipeline.get());
			context->bindTexture(0, testTexture.get());
			context->bindSampler(0, testSampler.get());
			sphereDrawCommand->draw(context);

//			// Blit to backbuffer
//			context->useRenderTarget(window);
//			fb->blitToCurrent(0, 0, win->getWidth(), win->getHeight(),
//							  0, 0, win->getWidth(), win->getHeight(),
//							  GL_COLOR_BUFFER_BIT, GL_NEAREST);

			window->present();
			window->handleEvents();
		}
	}
};

int main(int argc, char **argv)
{
try
{
	std::unique_ptr<GLRenderWindow> window = createWindow(argc, argv);
	TestApp testApp(window.get());
	testApp.run();
} // try
catch(std::exception& e)
{
	std::cout << e.what() << std::endl;
	return 1;
}
catch(...)
{
	std::cout << "Unknown exception" << std::endl;
	return 1;
}
}

std::unique_ptr<GLRenderWindow> createWindow(int argc, char **argv)
{
	unsigned int x = 800, y = 600;
	bool fullscreen = false;

	if(argc >= 3)
	{
		x = std::strtoul(argv[1], nullptr, 10);
		y = std::strtoul(argv[2], nullptr, 10);
	}
	if(argc >= 4)
	{
		if(argv[3][0] == 'f' || argv[3][0] == 'F')
			fullscreen = true;
		else if(argv[3][0] == 'w' || argv[3][0] == 'W')
			fullscreen = false;
	}

	GLRenderWindowCreator creator;
	creator.hintGLVersion(4, 0);
	creator.hintDebug(true);
	creator.hintFullscreen(fullscreen);
	creator.hintSize(x, y);
	creator.hintTitle("Test Window");

	return creator.create();
}


#include <iostream>
#include <vector>

#include <algorithm>
#include <unordered_map>
#include <set>
#include <utility>
#include <stdexcept>
#include <cmath>
#include <chrono>

#include "dynet/lstm.h"
#include "dynet/training.h"
#include "dynet/dynet.h"
#include "dynet/dict.h"

#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>
#include <boost/serialization/unordered_set.hpp>
#include <boost/program_options.hpp>

#include "decoder.h"
#include "serialization.h"
#include "nn/nn.h"
#include "graph_generator.h"
#include "utils.h"
#include "graph.h"
#include "status.h"

#include "dependency.h"
#include "reader.h"

int main(int argc, char **argv)
{
    std::string train_path;
    std::string dev_path;
    std::string model_path;
    unsigned n_iteration;
    bool use_cpos;
    bool limit_pos_distance;

    bool eval_on_dev;
    unsigned start_eval_at;

    StepsizeOptions stepsize_options;
    unsigned max_iteration;
    bool use_reduction;

    std::string ignore_dynet_mem;
    std::string ignore_dynet_wd;

    unsigned dim_embeddings;
    unsigned lstm_shared_dim;
    unsigned lstm_shared_stack;
    unsigned lstm_shared_layer;
    unsigned lstm_pos_dim;
    unsigned lstm_pos_stack;
    unsigned lstm_pos_layer;
    unsigned lstm_dep_dim;
    unsigned lstm_dep_stack;
    unsigned lstm_dep_layer;
    unsigned nn_pos_hidden_units;
    unsigned nn_dep_hidden_units;

    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
        ("train", po::value<std::string>(&train_path)->required(), "")
        ("model", po::value<std::string>(&model_path)->required(), "")
        ("iteration", po::value<unsigned>(&n_iteration)->default_value(30u), "")
        ("cpos", po::value<bool>(&use_cpos)->default_value(false), "")
        ("limit-pos-distance", po::value<bool>(&limit_pos_distance)->default_value(true), "")
        ("eval-on-dev", po::value<bool>(&eval_on_dev)->default_value(false), "")
        ("start-eval-at", po::value<unsigned>(&start_eval_at)->default_value(0u), "")
        ("dev-path", po::value<std::string>(&dev_path)->default_value(""), "")
        // SGD options for dev
        ("reduction", po::value<bool>(&use_reduction)->default_value(false), "")
        ("max-iteration", po::value<unsigned>(&max_iteration)->default_value(500), "")
        ("stepsize-scale", po::value<double>(&stepsize_options.stepsize_scale)->default_value(1.0), "SGD: stepsize scale")
        ("polyak", po::value<bool>(&stepsize_options.polyak)->default_value(false), "SGD: use polyak steapsize")
        ("polyak-wub", po::value<double>(&stepsize_options.polyak_wub)->default_value(1.0), "SGD: weight of the UB (>= 1.0)")
        ("decreasing", po::value<bool>(&stepsize_options.decreasing)->default_value(true), "SGD: automatiquely decrease stepsize")
        ("constant-decreasing", po::value<bool>(&stepsize_options.constant_decreasing)->default_value(false), "SGD: decrease stepsize at each iteration")
        ("camerini", po::value<bool>(&stepsize_options.camerini)->default_value(false), "SGD: use Camerini et al. momentum subgradient")
        ("gamma", po::value<double>(&stepsize_options.gamma)->default_value(1.5), "SGD: gamma paremeter for Camerini et al. momentum subgradient")
        ("dynet-mem", po::value<std::string>(&ignore_dynet_mem), "")
        ("dynet-weight-decay", po::value<std::string>(&ignore_dynet_wd), "")
        // NN options
        ("dim-embeddings", po::value<unsigned>(&dim_embeddings)->default_value(100))
        ("lstm-shared-dim", po::value<unsigned>(&lstm_shared_dim)->default_value(125))
        ("lstm-shared-stack", po::value<unsigned>(&lstm_shared_stack)->default_value(0))
        ("lstm-shared-layer", po::value<unsigned>(&lstm_shared_layer)->default_value(1))
        ("lstm-pos-dim", po::value<unsigned>(&lstm_pos_dim)->default_value(125))
        ("lstm-pos-stack", po::value<unsigned>(&lstm_pos_stack)->default_value(1))
        ("lstm-pos-layer", po::value<unsigned>(&lstm_pos_layer)->default_value(1))
        ("lstm-dep-dim", po::value<unsigned>(&lstm_dep_dim)->default_value(125))
        ("lstm-dep-stack", po::value<unsigned>(&lstm_dep_stack)->default_value(1))
        ("lstm-dep-layer", po::value<unsigned>(&lstm_dep_layer)->default_value(1))
        ("nn-pos-hidden-units", po::value<unsigned>(&nn_pos_hidden_units)->default_value(100))
        ("nn-dep-hidden-units", po::value<unsigned>(&nn_dep_hidden_units)->default_value(100))
    ;

    po::positional_options_description pod;
    pod.add("train", 1);
    pod.add("model", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(pod).run(), vm);
    po::notify(vm);

    dynet::initialize(argc, argv);

    dynet::Dict word_dict;
    dynet::Dict pos_dict;

    std::vector<IntSentence> train_data;
    std::vector<IntSentence> dev_data;

    std::cerr << "Reading train data..." << std::endl << std::flush;
    read_conll_file(
            train_path,
            std::back_inserter(train_data),
            [&] (const StringToken& token) {
                return convert(word_dict, pos_dict, token);
            },
            use_cpos
    );

    word_dict.freeze();
    pos_dict.freeze();

    word_dict.set_unk("*UNKNOWN*");

    if (eval_on_dev)
    {
        std::cerr << "Reading dev data..." << std::endl << std::flush;
        read_conll_file(
                dev_path,
                std::back_inserter(dev_data),
                [&] (const StringToken& token) {
                    return convert(word_dict, pos_dict, token);
                },
                use_cpos
        );
    }

    dynet::Model model;
    dynet::AdamTrainer trainer(model);
    trainer.sparse_updates_enabled = false;

    RNNSettings shared_rnn_settings;
    shared_rnn_settings.dim = lstm_shared_dim;
    shared_rnn_settings.n_stack = lstm_shared_stack;
    shared_rnn_settings.n_layer = lstm_shared_layer;

    RNNSettings arc_rnn_settings;
    arc_rnn_settings.dim = lstm_dep_dim;
    arc_rnn_settings.n_stack = lstm_dep_stack;
    arc_rnn_settings.n_layer = lstm_dep_layer;

    RNNSettings node_rnn_settings;
    node_rnn_settings.dim = lstm_pos_dim;
    node_rnn_settings.n_stack = lstm_pos_stack;
    node_rnn_settings.n_layer = lstm_pos_layer;

    NNSettings nn_settings;
    nn_settings.n_word = word_dict.size();
    nn_settings.n_pos = pos_dict.size();
    nn_settings.word_unknown = word_dict.convert("*UNKNOWN*");
    nn_settings.word_dim = dim_embeddings;

    NNArcSettings nn_arc_settings;
    nn_arc_settings.hidden_units = nn_pos_hidden_units;

    NNNode2Settings nn_node_settings;
    nn_node_settings.hidden_units = nn_dep_hidden_units;

    NN<dynet::LSTMBuilder> rnn(model, nn_settings, shared_rnn_settings, arc_rnn_settings, node_rnn_settings, nn_arc_settings, nn_node_settings);

    // Allowed POS by word + allowed dependencies between POS
    GraphGenerator<IntSentence> graph_generator(pos_dict.size());
    for (auto const& sentence : train_data)
    {
        graph_generator.update(sentence);
        // Count word frequence (used for dropout)
        for (auto const& token : sentence)
            rnn.m_word_count[token.word] += 1;
    }

    if (eval_on_dev)
        graph_generator.populate_with_everything(word_dict.convert("*UNKNOWN*"));

    double loss = 0.0;

    save_object(model_path + ".word_dict", word_dict);
    save_object(model_path + ".pos_dict", pos_dict);
    save_object(model_path + ".nn_settings", nn_settings);
    save_object(model_path + ".shared_rnn_settings", shared_rnn_settings);
    save_object(model_path + ".arc_rnn_settings", arc_rnn_settings);
    save_object(model_path + ".node_rnn_settings", node_rnn_settings);
    save_object(model_path + ".arc_nn_settings", nn_arc_settings);
    save_object(model_path + ".node_nn_settings", nn_node_settings);
    save_object(model_path + ".graph_generator", graph_generator);

    for (unsigned iteration = 0 ; iteration <= n_iteration ; ++iteration)
    {
        std::cerr << "Iteration: " << iteration << std::endl;
        std::cerr << std::flush;

        unsigned n_correct_head = 0;
        unsigned n_correct_pos = 0;
        unsigned n_total = 0;

        std::random_shuffle(std::begin(train_data), std::end(train_data));

        for (auto const& sentence : train_data)
        {
            dynet::ComputationGraph cg;

            Status status;
            status.n_cluster = sentence.size() + 1;

            // TODO: this may create some inaccessible node & arcs
            // => do a reduction step before computing weights ?
            graph_generator.build_arcs(
                    sentence,
                    [&] (const Arc& arc)
                    {
                        status.arcs.push_back(arc);
                    },
                    [&] (const Node& node)
                    {
                        status.nodes.push_back(node);
                    },
                    limit_pos_distance
            );

            std::vector<dynet::expr::Expression> arc_exprs;
            std::vector<dynet::expr::Expression> node_exprs;
            rnn.compute_exprs(
                cg,
                sentence,
                [&] (const Arc& arc, double weight, dynet::expr::Expression& expr) -> void
                {
                    arc_exprs.push_back(expr);

                    status.original_weights.push_back(weight);

                    // loss-augmented inference
                    auto const& token = sentence[arc.destination];
                    if (!arc.is(
                        token.head,
                        token.head == 0 ? nn_settings.n_pos : sentence[token.head].pos,
                        token.index,
                        token.pos
                    ))
                        weight += 1.0;

                    double w = weight / 3.0;
                    status.cmsa_weights.push_back(w);
                    status.incoming_weights.push_back(w);
                    status.outgoing_weights.push_back(w);
                },
                [&] (const Node& node, double weight, dynet::expr::Expression& expr) -> void
                {
                    unused_parameter(expr);

                    node_exprs.push_back(expr);

                    if (node.cluster != 0)
                    {
                        auto const& token = sentence[node.cluster];
                        if (token.pos != node.node)
                            weight += 1.0;
                    }
                    status.node_weights.push_back(weight);
                },
                status.arcs,
                status.nodes,
                true // dropout
            );

            Subgradient subgradient(stepsize_options, status);
            DecoderTimer timer;
            // TODO: use decode_dual instead
            // but then we need to have parameter for loss augmented + word dropout
            bool converged = decode(
                status,
                subgradient,
                max_iteration,
                use_reduction,
                timer
            );

            std::vector<double> arc_outputs(status.arcs.size(), 0.0);
            std::vector<double> node_outputs(status.nodes.size(), 0.0);

            // if converged primal = dual
            if (converged)
            {
                assert(std::isfinite(status.primal_weight));
                std::vector<int> nodes(status.n_cluster);

                for (unsigned i = 0u ; i < status.arcs.size() ; ++i)
                {
                    if (!status.primal_arcs.at(i))
                        continue;

                    arc_outputs[i] = 1.0;
                    auto const arc = status.arcs.at(i);
                    nodes.at(arc.destination) = arc.destination_node;
                }

                for (unsigned i = 0u ; i < status.nodes.size() ; ++i)
                {
                    auto const node = status.nodes.at(i);
                    if (node.cluster == 0)
                        continue;

                    if (node.node == nodes.at(node.cluster))
                        node_outputs[i] = 1.0;
                }
            }
            else
            {
                // TODO: use subgradient data instead
                DualDecoder dual_decoder(status.n_cluster, status.arcs, status.nodes);
                dual_decoder.maximize(
                        status.cmsa_weights,
                        status.incoming_weights,
                        status.outgoing_weights,
                        status.node_weights,
                        [&] (const int i)
                        {
                            arc_outputs.at(i) += 1.0 / 3.0;
                        },
                        [&] (const int i)
                        {
                            arc_outputs.at(i) += 1.0 / 3.0;
                        },
                        [&] (const int i)
                        {
                            arc_outputs.at(i) += 1.0 / 3.0;
                        },
                        [&] (const int i)
                        {
                            node_outputs[i] = 1.0;
                        }
                );
            }

            n_total += sentence.size();

            // build loss output
            std::vector<Expression> errs;

            // for arcs
            for (unsigned i = 0 ; i < status.arcs.size() ; ++i)
            {
                auto const& arc = status.arcs[i];

                // if head is root, do not check the source_node, it will always be correct
                int head_pos = (arc.source > 0 ? sentence[arc.source].pos : arc.source_node);
                auto const& modifier = sentence[arc.destination];
                auto pred = arc_outputs[i];

                double gold = (head_pos == arc.source_node && modifier.pos == arc.destination_node && modifier.head == arc.source) ? 1.0 : 0.0;

                if (!NEARLY_EQ_TOL(pred, gold))
                {
                    auto expr = arc_exprs.at(i);

                    if (!NEARLY_ZERO_TOL(pred))
                    {
                        if (NEARLY_EQ_TOL(pred, 1.0))
                            errs.push_back(expr);
                        else
                            errs.push_back(pred * expr);
                    }
                    if (!NEARLY_ZERO_TOL(gold))
                    {
                        if (NEARLY_EQ_TOL(gold, 1.0))
                            errs.push_back(- expr);
                        else
                            errs.push_back(- gold * expr);
                    }
                }
                // equal to gold so we know it's binary
                else if (gold > 0.5)
                {
                    n_correct_head += 1.0;
                }
            }

            // for nodes
            for (unsigned i = 0 ; i < status.nodes.size() ; ++i)
            {
                auto const& node = status.nodes[i];

                // Do not check node if root
                if (node.cluster == 0)
                    continue;

                auto const& token_pos = sentence[node.cluster].pos;

                auto pred = node_outputs[i];

                double gold = (token_pos == node.node) ? 1.0 : 0.0;
                if (!NEARLY_EQ_TOL(pred, gold))
                {
                    auto expr = node_exprs.at(i);

                    if (!NEARLY_ZERO_TOL(pred))
                    {
                        if (NEARLY_EQ_TOL(pred, 1.0))
                            errs.push_back(expr);
                        else
                            errs.push_back(pred * expr);
                    }
                    if (!NEARLY_ZERO_TOL(gold))
                    {
                        if (NEARLY_EQ_TOL(gold, 1.0))
                            errs.push_back(- expr);
                        else
                            errs.push_back(- gold * expr);
                    }
                }
                // equal to gold so we know it's binary
                else if (gold > 0.5)
                {
                    n_correct_pos += 1;
                }
            }

            // backprop
            if (errs.size() > 0)
            {
                Expression sum_errs = dynet::expr::sum(errs);
                loss += as_scalar(cg.get_value(sum_errs.i));
                cg.backward(sum_errs.i);
                trainer.update(1.0);
            }
        }

        trainer.update_epoch();
        trainer.status();

        std::cerr
            << "E = "
            << (loss / (double) (n_total * 2))
            << " ppl="
            << exp(loss / (double) (n_total * 2))
            << std::endl
        ;
        std::cerr << "\tNode acc: " << n_correct_pos / (double) n_total << std::endl;
        std::cerr << "\tArc acc: " << n_correct_head / (double) n_total << std::endl;
        std::cerr << std::flush;

        save_object(model_path + ".param." + std::to_string(iteration), model);

        // Evaluate on dev set
        if (eval_on_dev && iteration >= start_eval_at)
        {
            unsigned n_exact = 0u;

            unsigned n_good_head = 0u;
            unsigned n_total_head = 0u;
            unsigned n_good_pos = 0u;
            unsigned n_total_pos = 0u;

            std::cerr << std::endl << "Evaluating on dev..." << std::endl << std::flush;

            for (auto const& sentence : dev_data)
            {
                std::vector<int> poss(sentence.size());
                std::vector<int> heads(sentence.size());
                DecoderTimer timer;

                bool exact = decode_primal(
                    sentence,
                    graph_generator,
                    stepsize_options,
                    max_iteration,
                    use_reduction,
                    rnn,
                    [&] (unsigned index, unsigned head)
                    {
                        heads[index] = head;
                    },
                    [&] (unsigned index, unsigned pos)
                    {
                        poss[index] = pos;
                    },
                    timer
                );

                if (exact)
                    ++ n_exact;

                for (unsigned i = 0u ; i < sentence.size() ; ++ i)
                {
                    if (sentence[i + 1].head == heads[i])
                        ++ n_good_head;
                    if (sentence[i + 1].pos == poss[i])
                        ++ n_good_pos;
                }
                n_total_head += sentence.size();
                n_total_pos += sentence.size();
            }

            std::cerr << " -> Converged: " << n_exact << " / " << dev_data.size() << std::endl;
            std::cerr << " -> UAS: " << n_good_head / (double) n_total_head << std::endl;
            std::cerr << " -> POS precision: " << n_good_pos / (double) n_total_pos << std::endl;
            std::cerr << " -> Overall precision: " << (n_good_pos + n_good_head) / (double) (n_total_pos + n_total_head) << std::endl << std::flush;
        }
        std::cerr << "------" << std::endl;
    }

    save_object(model_path + ".param", model);
}