/*
 * Copyright (C) 2018 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef IGNITION_MATH_EIGEN3_CONVERSIONS_HH_
#define IGNITION_MATH_EIGEN3_CONVERSIONS_HH_

#include <Eigen/Geometry>
#include <ignition/math/AxisAlignedBox.hh>
#include <ignition/math/Matrix3.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Quaternion.hh>
#include <ignition/math/Vector3.hh>

namespace ignition
{
  namespace math
  {
    namespace eigen3
    {
      /// \brief Convert from ignition::math::Vector3d to Eigen::Vector3d.
      /// \param[in] _v ignition::math::Vector3d to convert
      /// \return The equivalent Eigen::Vector3d.
      inline Eigen::Vector3d convert(const ignition::math::Vector3d &_v)
      {
        return Eigen::Vector3d(_v[0], _v[1], _v[2]);
      }

      /// \brief Convert from ignition::math::AxisAlignedBox to
      /// Eigen::AlignedBox3d.
      /// \param[in] _b ignition::math::AxisAlignedBox to convert
      /// \return The equivalent Eigen::AlignedBox3d.
      inline Eigen::AlignedBox3d convert(
          const ignition::math::AxisAlignedBox &_b)
      {
        return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max()));
      }

      /// \brief Convert from ignition::math::Matrix3d to Eigen::Matrix3d.
      /// \param[in] _m ignition::math::Matrix3d to convert.
      /// \return The equivalent Eigen::Matrix3d.
      inline Eigen::Matrix3d convert(const ignition::math::Matrix3d &_m)
      {
        Eigen::Matrix3d matrix;
        for (std::size_t i=0; i < 3; ++i)
        {
          for (std::size_t j=0; j < 3; ++j)
          {
            matrix(i, j) = _m(i, j);
          }
        }

        return matrix;
      }

      /// \brief Convert ignition::math::Quaterniond to Eigen::Quaterniond.
      /// \param[in] _q ignition::math::Quaterniond to convert.
      /// \return The equivalent Eigen::Quaterniond.
      inline Eigen::Quaterniond convert(const ignition::math::Quaterniond &_q)
      {
        Eigen::Quaterniond quat;
        quat.w() = _q.W();
        quat.x() = _q.X();
        quat.y() = _q.Y();
        quat.z() = _q.Z();

        return quat;
      }

      /// \brief Convert ignition::math::Pose3d to Eigen::Isometry3d.
      /// \param[in] _pose ignition::math::Pose3d to convert.
      /// \return The equivalent Eigen::Isometry3d.
      inline Eigen::Isometry3d convert(const ignition::math::Pose3d &_pose)
      {
        Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
        tf.translation() = convert(_pose.Pos());
        tf.linear() = Eigen::Matrix3d(convert(_pose.Rot()));

        return tf;
      }

      /// \brief Convert Eigen::Vector3d to ignition::math::Vector3d.
      /// \param[in] _v Eigen::Vector3d to convert.
      /// \return The equivalent ignition::math::Vector3d.
      inline ignition::math::Vector3d convert(const Eigen::Vector3d &_v)
      {
        ignition::math::Vector3d vec;
        vec.X() = _v[0];
        vec.Y() = _v[1];
        vec.Z() = _v[2];

        return vec;
      }

      /// \brief Convert Eigen::AlignedBox3d to ignition::math::AxisAlignedBox.
      /// \param[in] _b Eigen::AlignedBox3d to convert.
      /// \return The equivalent ignition::math::AxisAlignedBox.
      inline ignition::math::AxisAlignedBox convert(
          const Eigen::AlignedBox3d &_b)
      {
        ignition::math::AxisAlignedBox box;
        box.Min() = convert(_b.min());
        box.Max() = convert(_b.max());

        return box;
      }

      /// \brief Convert Eigen::Matrix3d to ignition::math::Matrix3d.
      /// \param[in] _m Eigen::Matrix3d to convert.
      /// \return The equivalent ignition::math::Matrix3d.
      inline ignition::math::Matrix3d convert(const Eigen::Matrix3d &_m)
      {
        ignition::math::Matrix3d matrix;
        for (std::size_t i=0; i < 3; ++i)
        {
          for (std::size_t j=0; j < 3; ++j)
          {
            matrix(i, j) = _m(i, j);
          }
        }

        return matrix;
      }

      /// \brief Convert Eigen::Quaterniond to ignition::math::Quaterniond.
      /// \param[in] _q Eigen::Quaterniond to convert.
      /// \return The equivalent ignition::math::Quaterniond.
      inline ignition::math::Quaterniond convert(const Eigen::Quaterniond &_q)
      {
        ignition::math::Quaterniond quat;
        quat.W() = _q.w();
        quat.X() = _q.x();
        quat.Y() = _q.y();
        quat.Z() = _q.z();

        return quat;
      }

      /// \brief Convert Eigen::Isometry3d to ignition::math::Pose3d.
      /// \param[in] _tf Eigen::Isometry3d to convert.
      /// \return The equivalent ignition::math::Pose3d.
      inline ignition::math::Pose3d convert(const Eigen::Isometry3d &_tf)
      {
        ignition::math::Pose3d pose;
        pose.Pos() = convert(Eigen::Vector3d(_tf.translation()));
        pose.Rot() = convert(Eigen::Quaterniond(_tf.linear()));

        return pose;
      }
    }
  }
}

#endif


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>
#include <iostream>

#include <tdp/eigen/std_vector.h>
#include <tdp/clustering/dpvmfmeans_simple.hpp>

namespace tdp {
/// This is a simple version of the ManagedDPvMFmeansSimple algorithm in dpmeans.hpp
/// without inheritance or the use of CLData structures which can make
/// it a bit hard to read the other algorithm.
///
/// This implementation is ment as a lightweight alternative for small
/// number of datapoints or if you just want to have a look at how the
/// algorithm works.
template<class T, int D, int Options>
class ManagedDPvMFmeansSimple : public DPvMFmeansSimple<T,D,Options>
{
public:
  /// Constructor
  ///
  /// lambda = cos(lambda_in_degree * M_PI/180.) - 1.
  ManagedDPvMFmeansSimple(T lambda);
  virtual ~ManagedDPvMFmeansSimple();

  /// Adds an observation (adds obs, computes label, and potentially
  /// adds new cluster depending on label assignment).
  virtual void addObservation(const Eigen::Matrix<T,D,1,Options>& x);

protected:
};

typedef ManagedDPvMFmeansSimple<float,3,Eigen::DontAlign> ManagedDPvMFmeansSimple3fda;
typedef ManagedDPvMFmeansSimple<float,4,Eigen::DontAlign> ManagedDPvMFmeansSimple4fda;

// -------------------------------- impl ----------------------------------
template<class T, int D, int Options>
ManagedDPvMFmeansSimple<T,D,Options>::ManagedDPvMFmeansSimple(T lambda)
  : DPvMFmeansSimple<T,D,Options>(lambda)
{}

template<class T, int D, int Options>
ManagedDPvMFmeansSimple<T,D,Options>::~ManagedDPvMFmeansSimple()
{
  for (auto xi : this->xs_) delete xi;
  for (auto zi : this->zs_) delete zi;
}

template<class T, int D, int Options>
void ManagedDPvMFmeansSimple<T,D,Options>::addObservation(const Eigen::Matrix<T,D,1,Options>& x) {
  Eigen::Matrix<T,D,1,Options>* xi = new Eigen::Matrix<T,D,1,Options>(x);
  uint16_t* zi = new uint16_t(0);
  DPvMFmeansSimple<T,D,Options>::addObservation(xi, zi);
};

}


#include <string>
#include <iostream>
#include <fstream>
#include <math.h>
#include <random>
#include <ros/ros.h>
#include <trajectory_generator/coeff_msgs.h>
#include <trajectory_generator/alpha_msgs.h>
#include <ros/console.h>
#include <sensor_msgs/Joy.h>
#include <algorithm>
#include <Eigen/Eigen>
#include <vector>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>

using namespace std;
using namespace Eigen;

double _target;

double _vis_traj_width;
ros::Subscriber _alpha_sub, _coeff_sub;

MatrixXd _polyCoeff;
ros::Publisher _wp_traj_vis_pub, _st_traj_vis_pub;

vector<double> _time;
vector<double> _errs, _times, _vzs;
vector<VectorXd> _pos_vel;
vector<VectorXd> _shoot_pos, _cur_shoot_pos;
int _coeff_num, _segment;

vector<double> _alphas;
int _sample_num;
int _launch_x, _launch_z, launch_angle;
double _v0 = 3.0, _gamma=0.25, _D=0.03, _m=0.1, _g=9.81, _dt=0.05;

void coeffCallback(const trajectory_generator::coeff_msgs::ConstPtr& msg);
void alphaCallback(const trajectory_generator::alpha_msgs::ConstPtr& msg);
void getPosVel(const MatrixXd& polyCoeff, int k, double t );
Vector4d alphaCheck(double tu, double x0, double z0, double vxu, double vzu, double alpha);
void visWayPointTraj();
Vector3d getPosPoly(const MatrixXd& polyCoeff, int k, double t);
void visShootTraj();
double evaluate(const Vector4d& evaluation);

int main(int argc, char** argv){

    ros::init(argc, argv, "check_node");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private("~");
    nh_private.param("vis/vis_traj_width", _vis_traj_width, 0.05);

    _wp_traj_vis_pub = nh.advertise<visualization_msgs::Marker>("/trajectory_generator_node/vis_uav_trajectory", 10);
    _st_traj_vis_pub = nh.advertise<visualization_msgs::Marker>("/trajectory_generator_node/vis_shoot_trajectory", 10);

    _coeff_sub = nh.subscribe<trajectory_generator::coeff_msgs>("/trajectory_generator_node/coeff", 1000, coeffCallback);
    _alpha_sub = nh.subscribe<trajectory_generator::alpha_msgs>("/trajectory_generator_node/alpha", 2000, alphaCallback);

    ros::spin();
    return 0;
}

void alphaCallback(const trajectory_generator::alpha_msgs::ConstPtr& msg) {
    _sample_num = msg->sample_num;
    int size = msg->alphas.size();
    double min_distance, err;
    _alphas.clear();
    for (int i = 0; i < msg->alphas.size();i++){
         _alphas.push_back(msg->alphas[i]);
    }
    ROS_INFO_STREAM("Alpha Check Starting...");
    //for (int k = 0; k < _time.size(); k++) cout << _time[k]<<endl;
    int mode = 3;
    bool flag = 1;

    double min_err=100.0;double min_time = 100.0; double min_vz = -100.0;
    double fx,fz,fvx,fvz,ft, fa, fvh;
    int count = 0, pc=0; double tu0 = 0.0; bool cont = true;

    for (int k = 0; k < _time.size(); k++){
        for (double t = 0.0; t < _time[k]; t += _dt){
            getPosVel(_polyCoeff, k, t);
            if (_pos_vel[pc][0] > _target){
                cont = false;

            }
            for (int i = 0; i < 2; i++){
                if (!cont){
                    continue;
                }
                double alpha = _alphas[count++];
                if (int(alpha) == int(114514)){
                    continue;
                }

                double u_angle = atan(_pos_vel[pc][1] / _pos_vel[pc][0]);
                if (alpha > u_angle){
                    continue;
                }

                //err vz t
                Vector4d evaluation = alphaCheck(tu0 + t, _pos_vel[pc][0], _pos_vel[pc][1], _pos_vel[pc][2], _pos_vel[pc][3], alpha);
                // ofstream outfile("/home/hsy/points.txt", ios::app);
                // outfile << evaluation[0] << "," << evaluation[1] << "," << evaluation[2] <<"," <<_pos_vel[pc][0]<<","<< _pos_vel[pc][1]<< endl;

                // outfile.close();
                _errs.push_back(evaluation[0]); _times.push_back(evaluation[2]);_vzs.push_back(evaluation[1]);
                //MIN ERR
                if (mode == 1 && min_err > evaluation[0]){
                    min_time = evaluation[2];
                    min_err = evaluation[0];
                    min_vz = evaluation[1];
                    _shoot_pos = _cur_shoot_pos;
                    fx = _pos_vel[pc][0]; fz = _pos_vel[pc][1];
                    fvh = evaluation[1];
                    fvx = _pos_vel[pc][2]; fvz = _pos_vel[pc][3]; ft = evaluation[2]; fa = alpha;
                }
                //MIN TIME
                if (mode == 2 && min_time > evaluation[2]){
                    min_time = evaluation[2];
                    min_err = evaluation[0];
                    min_vz = evaluation[1];
                    _shoot_pos = _cur_shoot_pos;
                    fx = _pos_vel[pc][0]; fz = _pos_vel[pc][1];
                    fvh = evaluation[1];
                    fvx = _pos_vel[pc][2]; fvz = _pos_vel[pc][3]; ft = evaluation[2]; fa = alpha;
                }
                //MIN VZ
                if (mode == 3 && min_vz < evaluation[1]){
                    min_time = evaluation[2];
                    min_err = evaluation[0];
                    min_vz = evaluation[1];
                    _shoot_pos = _cur_shoot_pos;
                    fx = _pos_vel[pc][0]; fz = _pos_vel[pc][1];
                    fvh = evaluation[1];
                    fvx = _pos_vel[pc][2]; fvz = _pos_vel[pc][3]; ft = evaluation[2]; fa = alpha;
                }
                // EVAL FUNC
                // if (mode == 4 && flag && evaluation[0] < 0.001){
                //     flag = 0;
                //     min_time = evaluation[2];
                //     min_err = evaluation[0];
                //     min_vz = evaluation[1];
                //     _shoot_pos = _cur_shoot_pos;
                //     fx = _pos_vel[pc][0]; fz = _pos_vel[pc][1];
                //     fvh = evaluation[1];
                //     fvx = _pos_vel[pc][2]; fvz = _pos_vel[pc][3]; ft = evaluation[2]; fa = alpha;
                // }
                cout <<  "err: " << evaluation[0] << " vz: " << evaluation[1] << " time: " << evaluation[2] << endl;
                cout << endl;
                cout << "-";
            }
            pc += 1;
        }
        tu0 += _time[k];
    }
    cout <<endl;

    cout << count << "Alphas Checked" << endl;
    cout << "Checking x0: " <<fx<<" z0: "<<fz<<" vxu: "<<fvx<<" vzu: "<<fvz<<" a: "<<fa<<endl;
    cout <<  "err: " << min_err << " vz: " << fvh << " time: " << ft << endl;
    cout << _shoot_pos.size()<<endl;
    visWayPointTraj();
    visShootTraj();
    ROS_INFO_STREAM("DONE");
}

void getPosVel(const MatrixXd& polyCoeff, int k, double t ){
    VectorXd ret(4);
    VectorXd time = VectorXd::Zero(_coeff_num);
    VectorXd vel_time = VectorXd::Zero(_coeff_num - 1);
    for(int j = 0; j < _coeff_num; j ++){
        if(j==0)
            {time(j) = 1.0; vel_time(j) = 1.0;}
        else if (j == _coeff_num - 1){
            time(j) = pow(t, j);
        } else {
            time(j) = pow(t, j); vel_time(j) = (j+1)*pow(t, j);
        }
    }
    for (int dim = 0; dim < 2; dim++){
        VectorXd coeff = (polyCoeff.row(k)).segment(dim * _coeff_num, _coeff_num);
        ret(dim) = coeff.dot(time);
        coeff =  (polyCoeff.row(k)).segment(dim * _coeff_num + 1, _coeff_num - 1);
        ret(dim + 2) = coeff.dot(vel_time);
    }
    _pos_vel.push_back(ret);
}

void coeffCallback(const trajectory_generator::coeff_msgs::ConstPtr& msg){
    _pos_vel.clear();
    _time.clear();
    _coeff_num = msg->coeff_num; _segment = msg->segment; _target = msg->target;
    int size = msg->coeff.size();

    MatrixXd polyCoeff(1, _coeff_num*2*_segment);
    for (int i = 0; i < msg->coeff.size();i++){
        polyCoeff(0,i) = msg->coeff[i];
    }
    for (int j = 0; j < msg->time.size();j++){
        _time.push_back(msg->time[j]);
    }
    polyCoeff.resize(_segment, _coeff_num*2);
    _polyCoeff = polyCoeff;
    ROS_INFO_STREAM("Check Node coeff updated.");
}

double evaluate(const Vector4d& evaluation){
    //err vz t
    double a,b,c;
    return a*evaluation[0]+b*evaluation[1]+c*evaluation[2];
}

Vector4d alphaCheck(double tu, double x0, double z0, double vxu, double vzu, double alpha) {
    cout << "Checking x0: " <<x0<<" z0: "<<z0<<" vxu: "<<vxu<<" vzu: "<<vzu<<" a: "<<alpha <<" tu: " << tu<<endl;
    _cur_shoot_pos.clear();
    double z = z0, x = x0;
    double x1 = vxu + _v0* cos(alpha),z1 = vzu + _v0*sin(alpha);
    double v, x2, z2;
    double c = _gamma * _D * _D / _m;
    double t = 0, k;
    double _d_t = 0.001;
    VectorXd xz(2);
    while (z>0) {
        xz[0] = x; xz[1] = z;
        _cur_shoot_pos.push_back(xz);

        x += x1 * _d_t;
        z += z1 * _d_t;
        v = sqrt(x1 * x1 + z1 * z1);
        k = _gamma * _D * _D / _m * v;
        x2 = -k * x1;
        z2 = -_g - k * z1;
        x1 += x2 * _d_t;
        z1 += z2 * _d_t;
        t += _d_t;
    }
    double err = abs(x - _target);
    //double score = evaluate(vz, tu + t, 0);
    Vector4d eval(err, z1, tu+t,0);
    return eval;
}

Vector3d getPosPoly(const MatrixXd& polyCoeff, int k, double t )
{
    Vector3d ret;

    for ( int dim = 0; dim < 2; dim++ )
    {
        VectorXd coeff = (polyCoeff.row(k)).segment( dim * _coeff_num, _coeff_num );//coeff:{px0, px1, ..., py0, py1, ..., pz0, pz1, ...}
        VectorXd time  = VectorXd::Zero( _coeff_num );

        for(int j = 0; j < _coeff_num; j ++)
          if(j==0)
              time(j) = 1.0;
          else
              time(j) = pow(t, j);

        ret(dim) = coeff.dot(time);
        //ret(0) for x, 1 for y, 2 for z.
    }
    ret(3) = ret(2);
    ret(2) = 0.0;
    return ret;
}

void visWayPointTraj()
{
    ROS_INFO_STREAM("Visualization");
    visualization_msgs::Marker _traj_vis;

    //Marker params
    _traj_vis.header.stamp       = ros::Time::now();
    _traj_vis.header.frame_id    = "map";

    _traj_vis.ns = "traj_node/trajectory_waypoints";
    _traj_vis.id = 0;
    _traj_vis.type = visualization_msgs::Marker::SPHERE_LIST;
    _traj_vis.action = visualization_msgs::Marker::ADD;
    _traj_vis.scale.x = _vis_traj_width;
    _traj_vis.scale.y = _vis_traj_width;
    _traj_vis.scale.z = _vis_traj_width;
    _traj_vis.pose.orientation.x = 0.0;
    _traj_vis.pose.orientation.y = 0.0;
    _traj_vis.pose.orientation.z = 0.0;
    _traj_vis.pose.orientation.w = 1.0;

    _traj_vis.color.a = 1.0;
    _traj_vis.color.r = 1.0;
    _traj_vis.color.g = 0.0;
    _traj_vis.color.b = 0.0;

    double traj_len = 0.0;
    int count = 0;
    Vector3d cur, pre;
    cur.setZero();
    pre.setZero();

    _traj_vis.points.clear();
    Vector3d pos;
    geometry_msgs::Point pt;

    // for(int i = 0; i < time.size(); i++ )
    // {
    //     for (double t = 0.0; t < time[i]; t += 0.01, count += 1)
    //     {
    //         ROS_INFO_STREAM("HERE");
    //         pos = getPosPoly(polyCoeff, i, t);
    //         cur(0) = pt.x = pos(0);
    //         cur(1) = pt.y = pos(1);
    //         cur(2) = pt.z = pos(2);
    //         _traj_vis.points.push_back(pt);

    //         //if (count) traj_len += (pre - cur).norm();
    //         pre = cur;
    //     }
    // }
    int i;
    for (i = 0; i < _pos_vel.size(); i++){
        pt.x = _pos_vel[i][0];
        pt.y = 0.0;
        pt.z = _pos_vel[i][1];
        _traj_vis.points.push_back(pt);
    }
    cout << i << " waypoints"<<endl;
    ROS_INFO_STREAM("HERE");
    //ROS_WARN("Trajectory length is %f m", traj_len);
    _wp_traj_vis_pub.publish(_traj_vis);
}

void visShootTraj()
{
    ROS_INFO_STREAM("Visualization Shooting");
    visualization_msgs::Marker _traj_vis;

    //Marker params
    _traj_vis.header.stamp       = ros::Time::now();
    _traj_vis.header.frame_id    = "map";

    _traj_vis.ns = "traj_node/trajectory_waypoints";
    _traj_vis.id = 1;
    _traj_vis.type = visualization_msgs::Marker::SPHERE_LIST;
    _traj_vis.action = visualization_msgs::Marker::ADD;
    _traj_vis.scale.x = _vis_traj_width;
    _traj_vis.scale.y = _vis_traj_width;
    _traj_vis.scale.z = _vis_traj_width;
    _traj_vis.pose.orientation.x = 0.0;
    _traj_vis.pose.orientation.y = 0.0;
    _traj_vis.pose.orientation.z = 0.0;
    _traj_vis.pose.orientation.w = 1.0;

    _traj_vis.color.a = 1.0;
    _traj_vis.color.r = 0.0;
    _traj_vis.color.g = 1.0;
    _traj_vis.color.b = 0.0;

    double traj_len = 0.0;
    int count = 0;
    Vector3d cur, pre;
    cur.setZero();
    pre.setZero();

    _traj_vis.points.clear();
    Vector3d pos;
    geometry_msgs::Point pt;

    int i;
    for (i = 0; i < _shoot_pos.size(); i++){
        pt.x = _shoot_pos[i][0];
        pt.y = 0.0;
        pt.z = _shoot_pos[i][1];
        _traj_vis.points.push_back(pt);
    }
    cout << i << " shooting waypoints"<<endl;
    ROS_INFO_STREAM("SHOOT");
    //ROS_WARN("Trajectory length is %f m", traj_len);
    _st_traj_vis_pub.publish(_traj_vis);
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE JPetRecoImageToolsTests
#include <boost/test/unit_test.hpp>
#include <fstream>
#include <iostream>
#include <utility>
#include <vector>

#include "JPetCommonTools/JPetCommonTools.h"
#include "./JPetFilterCosine.h"
#include "./JPetFilterHamming.h"
#include "./JPetFilterNone.h"
#include "./JPetFilterRamLak.h"
#include "./JPetFilterRidgelet.h"
#include "./JPetFilterSheppLogan.h"
#include "./JPetRecoImageTools.h"

const auto inFile = "unitTestData/JPetRecoImageToolsTest/sinogramBackproject.ppm";

int getMaxValue(const JPetRecoImageTools::Matrix2DProj& result)
{
  int maxValue = 0;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      if (static_cast<int>(result[i][j]) > maxValue)
        maxValue = static_cast<int>(result[i][j]);
    }
  }
  return maxValue;
}

void saveResult(const JPetRecoImageTools::Matrix2DProj& result, const std::string& outputFileName)
{
  int maxValue = getMaxValue(result);
  std::ofstream res(outputFileName);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << maxValue << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      int resultInt = static_cast<int>(result[i][j]);
      if (resultInt < 0) {
        resultInt = 0;
      }
      res << resultInt << " ";
    }
    res << std::endl;
  }
  res.close();
}

JPetRecoImageTools::Matrix2DProj readFile(const std::string& inputFile)
{
  std::ifstream in(inputFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  JPetRecoImageTools::Matrix2DProj sinogram(height, std::vector<double>(width));
  for (unsigned int i = 0; i < height; i++) {
    for (unsigned int j = 0; j < width; j++) {
      in >> val;
      sinogram[i][j] = val;
    }
  }
  return sinogram;
}

BOOST_AUTO_TEST_SUITE(FirstSuite)

BOOST_AUTO_TEST_CASE(backProjectSinogramNone)
{
  const auto outFile = "backprojectSinogramNone.ppm";

  JPetRecoImageTools::Matrix2DProj sinogram = readFile(inFile);

  JPetFilterNone noneFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, noneFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, sinogram[0].size(), JPetRecoImageTools::nonRescale, 0, 255);

  saveResult(result, outFile);

  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectSinogramRamLak)
{
  const auto outFile = "backprojectSinogramRamLak.ppm";

  JPetRecoImageTools::Matrix2DProj sinogram = readFile(inFile);

  JPetFilterRamLak ramLakFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, ramLakFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, sinogram[0].size(), JPetRecoImageTools::nonRescale, 0, 255);

  saveResult(result, outFile);

  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectSinogramCosine)
{
  const auto outFile = "backprojectSinogramCosine.ppm";

  JPetRecoImageTools::Matrix2DProj sinogram = readFile(inFile);

  JPetFilterCosine cosineFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, cosineFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, sinogram[0].size(), JPetRecoImageTools::nonRescale, 0, 255);

  saveResult(result, outFile);

  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectSinogramHamming)
{
  const auto outFile = "backprojectSinogramHamming.ppm";

  JPetRecoImageTools::Matrix2DProj sinogram = readFile(inFile);

  JPetFilterHamming hammingFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, hammingFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, sinogram[0].size(), JPetRecoImageTools::nonRescale, 0, 255);

  saveResult(result, outFile);

  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectSinogramRidgelet)
{
  const auto outFile = "backprojectSinogramRidgelet.ppm";

  JPetRecoImageTools::Matrix2DProj sinogram = readFile(inFile);

  JPetFilterRidgelet ridgeletFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, ridgeletFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, sinogram[0].size(), JPetRecoImageTools::nonRescale, 0, 255);

  saveResult(result, outFile);

  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectSinogramSheppLogan)
{
  const auto outFile = "backprojectSinogramSheppLogan.ppm";

  JPetRecoImageTools::Matrix2DProj sinogram = readFile(inFile);

  JPetFilterSheppLogan shepploganFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, shepploganFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, sinogram[0].size(), JPetRecoImageTools::nonRescale, 0, 255);

  saveResult(result, outFile);

  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2014 Jonathan Graehl - http://graehl.org/
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef GRAEHL_SHARED_UMOD_HPP
#define GRAEHL_SHARED_UMOD_HPP

/* The binary % operator yields the remainder from the division of the first expression by the second. .... If both operands are nonnegative then the remainder is nonnegative; if not, the sign of the remainder is implementation-defined - seriously, wtf. */
// this always returns somethign in [0,m) unlike x % m which for -x can return something in (-m,m)
// NOTE: (unsigned)x % m where m is not a power of 2 will probably give wrong answers too.  it seems most often that a negative remainder IS returned.

#ifndef HAVE_NEGATIVE_REMAINDER
# define HAVE_NEGATIVE_REMAINDER 1
#endif

#include <boost/cstdint.hpp>

template <class I>
inline I umod(I x, I m) {
#if HAVE_NEGATIVE_REMAINDER
  I r = x%m;
  return r<0?r+m:r;
#else
  return x%m;
#endif
}

#define DEF_UINT_MOD(n) inline uint ## n ## _t umod(uint ## n ## _t x, uint ## n ## _t m) { return x%m; }

DEF_UINT_MOD(8)
DEF_UINT_MOD(16)
DEF_UINT_MOD(32)
DEF_UINT_MOD(64)

template <class I>
inline I unegmod(I x, I m) // return m-umod(x%m), i.e. the mod m inverse of umod(x,m)
{
#if HAVE_NEGATIVE_REMAINDER
  I r = x%m;
  return r<0?-r:m-r;
#else
  return m-(x%m);
#endif
}

#endif


//=============================================================================
//! \brief Daniel J. Greenhoe
//! \brief normed linear space R^2
//=============================================================================
//=====================================
//! \brief headers
//=====================================
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<math.h>
#include <Eigen/Dense>  // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
#include "r1.h"
#include "r6.h"
typedef Eigen::Matrix< double, 6, 1 > Vector6d;
//=====================================
//! \brief osix
//=====================================
//-----------------------------------------------------------------------------
//! \brief osix constructors
//-----------------------------------------------------------------------------
osix::osix(void)
{
  x.at(0) = 0.0;
  x.at(1) = 0.0;
  x.at(2) = 0.0;
  x.at(3) = 0.0;
  x.at(4) = 0.0;
  x.at(5) = 0.0;
}

osix::osix(const double u0, const double u1, const double u2, const double u3, const double u4, const double u5)
{
  x.at(0) = u0;
  x.at(1) = u1;
  x.at(2) = u2;
  x.at(3) = u3;
  x.at(4) = u4;
  x.at(5) = u5;
}

osix::osix(const double u)
{
  x.at(0) = u;
  x.at(1) = u;
  x.at(2) = u;
  x.at(3) = u;
  x.at(4) = u;
  x.at(5) = u;
}

//-----------------------------------------------------------------------------
//! \brief osix put member functions
//-----------------------------------------------------------------------------
void osix::put(const double u)
{
  x.at(0) = u;
  x.at(1) = u;
  x.at(2) = u;
  x.at(3) = u;
  x.at(4) = u;
  x.at(5) = u;
}

void osix::put(const osix u)
{
  x.at(0) = u.get(0);
  x.at(1) = u.get(1);
  x.at(2) = u.get(2);
  x.at(3) = u.get(3);
  x.at(4) = u.get(4);
  x.at(5) = u.get(5);
}

//-----------------------------------------------------------------------------
//! \brief return the 6-tuple value
//-----------------------------------------------------------------------------
osix osix::get(void) const
{
  osix u;
  u.put( get(0) );
  u.put( get(1) );
  u.put( get(2) );
  u.put( get(3) );
  u.put( get(4) );
  u.put( get(5) );
  return u;
}

//-----------------------------------------------------------------------------
//! \brief return the minimum element of the 6 tupple
//-----------------------------------------------------------------------------
double osix::min(void) const
{
  const Eigen::Map< const Vector6d > a( getdata() );
  const double minVal = a.minCoeff();
  return minVal;
}

//-----------------------------------------------------------------------------
//! \brief return the maximum element of the 6 tupple
//-----------------------------------------------------------------------------
double osix::max(void) const
{
  const Eigen::Map< const Vector6d > a( getdata() );
  const double maxVal = a.maxCoeff();
  return maxVal;
}

//-----------------------------------------------------------------------------
//! \brief print the tuple
//-----------------------------------------------------------------------------
void osix::list(const char *str1, const char *str2)
{
  int i;
  if(strlen(str1)!=0) printf("%s",str1);
  putchar('(');
  for( i=0; i<5; i++ ) printf("%9.6lf,", get(i));
  printf("%9.6lf)", get(5) );
  if(strlen(str2)!=0) printf("%s",str2);
}

//=====================================
//! \brief vectR6 functions
//=====================================
//-----------------------------------------------------------------------------
//! \brief return the 6-tuple value
//-----------------------------------------------------------------------------
vectR6 vectR6::get(void) const
{
  vectR6 u;
  int i;
  for( i=0; i<6; i++ ) u.put( i, get(i) );
  return u;
}

//-----------------------------------------------------------------------------
//! \brief magnitude
//-----------------------------------------------------------------------------
const double vectR6::mag(void) const
{
  const Eigen::Map< const Vector6d > a( getdata() );
  return a.norm();
}

//-----------------------------------------------------------------------------
//! \brief Multiply the vector by a scalar a
//-----------------------------------------------------------------------------
vectR6 vectR6::mpy(const double a)
{
  vectR6 w;
  const Eigen::Map< const Vector6d > vv( getdata() );
  Eigen::Map< Vector6d > ww( w.getdataa() );
  ww = a * vv;
  return w;
}

//-----------------------------------------------------------------------------
//! \brief operator: +=
//-----------------------------------------------------------------------------
void vectR6::operator+=(const vectR6 q)
{
  Eigen::Map< Vector6d > pp( getdataa() );
  const Eigen::Map< const Vector6d > qq( q.getdata() );
  pp = pp + qq;
}

//-----------------------------------------------------------------------------
//! \brief operator: -=
//-----------------------------------------------------------------------------
void vectR6::operator-=(const vectR6 q)
{
  Eigen::Map< Vector6d > pp( getdataa() );
  const Eigen::Map< const Vector6d > qq( q.getdata() );
  pp = pp - qq;
}

//-----------------------------------------------------------------------------
//! \brief operator: -=
//-----------------------------------------------------------------------------
void vectR6::operator*=(const double a)
{
  vectR6  p=get();
  p = a*p;
  put(p);
}

//-----------------------------------------------------------------------------
//! \brief operator: a*x
//-----------------------------------------------------------------------------
vectR6 operator*(const double a, const vectR6 x)
{
  vectR6 y;
  const Eigen::Map< const Vector6d > xx( x.getdata() );
  Eigen::Map< Vector6d > yy( y.getdataa() );
  yy = a * xx;
  return y;
}

//-----------------------------------------------------------------------------
//! \brief operator: dot product of p and q
//-----------------------------------------------------------------------------
double operator^(const vectR6 p, const vectR6 q)
{
  const  Eigen::Map< const Vector6d > pp( p.getdata() );
  const  Eigen::Map< const Vector6d > qq( q.getdata() );
  double innerProduct = pp.adjoint() * qq;
  return innerProduct;
}

//=====================================
//! \brief seqR6
//=====================================
//-----------------------------------------------------------------------------
//! \brief constructor initializing seqR1 to 0
//-----------------------------------------------------------------------------
seqR6::seqR6(const long M)
{
  N = M;
  x = new vectR6[N];
  clear();
}

//-----------------------------------------------------------------------------
//! \brief constructor initializing seqR1 to <u>
//-----------------------------------------------------------------------------
seqR6::seqR6(const long M, const double u)
{
  N = M;
  x = new vectR6[N];
  fill( u );
}

//-----------------------------------------------------------------------------
//! \brief fill the seqR6 with a value <u>
//-----------------------------------------------------------------------------
void seqR6::fill(const double u)
{
  for(long n=0; n<N; n++) put( n, u );
}

//-----------------------------------------------------------------------------
//! \brief fill the seqR6 with the value 0
//-----------------------------------------------------------------------------
void seqR6::clear(void)
{
  fill( 0.0 );
}

//-----------------------------------------------------------------------------
//! \brief put a single value u=(u1,u2,u3,u4,u5,u6) into the seqR6 x at location n
//-----------------------------------------------------------------------------
int seqR6::put(const long n, const double u1, const double u2, const double u3, const double u4, const double u5, const double u6)
{
  int retval=0;
  if(n<N)x[n].put(u1,u2,u3,u4,u5,u6);
  else{
    fprintf(stderr,"n=%ld larger than seqR1 size N=%ld\n",n,N);
    retval=-1;
    }
  return retval;
}

int seqR6::put(const long n, const vectR6 u)
{
  int retval=0;
  if(n<N)x[n].put(u);
  else{
    fprintf(stderr,"n=%ld larger than seqR6 size N=%ld\n",n,N);
    retval=-1;
    }
  return retval;
}

//-----------------------------------------------------------------------------
//! \brief list contents of sequence
//-----------------------------------------------------------------------------
void seqR6::list(const long start, const long end, const char* str1, const char *str2, FILE *ptr)
{
  long n,m;
  vectR6 p;
  if(ptr!=NULL){
    if(strlen(str1)>0)fprintf(ptr,"%s",str1);
    for(n=start,m=1; n<=end; n++,m++){
      p=x[n];
      fprintf(ptr,"(%5.2lf,%5.2lf,%5.2lf,%5.2lf,%5.2lf,%5.2lf) ",p.get1(),p.get2(),p.get3(),p.get4(),p.get5(),p.get6());
      if(m%2==0)fprintf(ptr,"\n");
      }
    if(strlen(str2)>0)fprintf(ptr,"%s",str2);
    }
}

//-----------------------------------------------------------------------------
//! \brief list contents of seqR1 using 1 digit per element
//-----------------------------------------------------------------------------
void seqR6::list1(const long start, const long end, const char *str1, const char *str2,FILE *ptr)
{
  long n,m;
  vectR6 p;
  if(ptr!=NULL){
    if(strlen(str1)>0)fprintf(ptr,"%s",str1);
    for(n=start,m=1; n<=end; n++,m++){
      p=x[n];
      fprintf(ptr," %1.0lf%1.0lf%1.0lf%1.0lf%1.0lf%1.0lf",p.get1(),p.get2(),p.get3(),p.get4(),p.get5(),p.get6());
      if(m%10==0)fprintf(ptr,"\n");
      }
    if(strlen(str2)>0)fprintf(ptr,"%s",str2);
    }
}

//=====================================
// External operations
//=====================================
//-----------------------------------------------------------------------------
//! \brief operator: return p+q
//-----------------------------------------------------------------------------
vectR6 operator+(const vectR6 p, const vectR6 q)
{
  vectR6 y;
  Eigen::Map< Vector6d > yy( y.getdataa() );
  const Eigen::Map< const Vector6d > pp( p.getdata() );
  const Eigen::Map< const Vector6d > qq( q.getdata() );
  yy = pp + qq;
  return y;
}

//-----------------------------------------------------------------------------
//! \brief operator: return p-q
//-----------------------------------------------------------------------------
vectR6 operator-(const vectR6 p, const vectR6 q)
{
  vectR6 y;
  Eigen::Map< Vector6d > yy( y.getdataa() );
  const Eigen::Map< const Vector6d > pp( p.getdata() );
  const Eigen::Map< const Vector6d > qq( q.getdata() );
  yy = pp - qq;
  return y;
}

//-----------------------------------------------------------------------------
//! \brief operator: return -p
//-----------------------------------------------------------------------------
vectR6 operator-(const vectR6 p)
{
  vectR6 q;
  Eigen::Map< Vector6d > qq( q.getdataa() );
  const Eigen::Map< const Vector6d > pp( p.getdata() );
  qq = - pp;
  return q;
}

//-----------------------------------------------------------------------------
//! \brief return the angle theta in radians between the two vectors induced by
//!        the points <p> and <q> in the space R^6.
//! \returns On SUCCESS return theta in the closed interval [0:PI];
//!          On ERROR   exit
//-----------------------------------------------------------------------------
double pqtheta(const vectR6 p, const vectR6 q)
{
  const double rp = p.r();
  const double rq = q.r();
  double y,theta;
  if(rp==0)
  {
    fprintf(stderr,"\nERROR using pqtheta(vectR6 p, vectR6 q): |p| = %lf\n", rp);
    exit(EXIT_FAILURE);
  }
  if(rq==0)
  {
    fprintf(stderr,"\nERROR using pqtheta(vectR6 p, vectR6 q): |q| = %lf\n", rq);
    exit(EXIT_FAILURE);
  }
  y = (p^q)/(rp*rq);
  if(y>+1)
  {
    fprintf(stderr,"\nERROR using pqtheta(vectR6 p, vectR6 q): (p^q)/(rp*rq)=%lf>+1\n",y);
    exit(EXIT_FAILURE);
  }
  if(y<-1)
  {
    fprintf(stderr,"\nERROR using pqtheta(vectR6 p, vectR6 q): (p^q)/(rp*rq)=%lf<-1\n",y);
    exit(EXIT_FAILURE);
  }
  theta = acos(y);
  return theta;
}


#ifndef INCLUDE_SWIFTVIO_CERES_NORMALVECTORPARAMETERBLOCK_HPP_
#define INCLUDE_SWIFTVIO_CERES_NORMALVECTORPARAMETERBLOCK_HPP_

#include <Eigen/Core>
#include <okvis/ceres/ParameterBlockSized.hpp>

namespace okvis {
namespace ceres {

/// \brief Wraps the parameter block for a pose estimate
class NormalVectorParameterBlock
    : public ParameterBlockSized<3, 2, Eigen::Vector3d> {
public:
  /// \brief The estimate type.
  typedef Eigen::Vector3d estimate_t;

  /// \brief The base class type.
  typedef ParameterBlockSized<3, 2, Eigen::Vector3d> base_t;

  NormalVectorParameterBlock();

  NormalVectorParameterBlock(const Eigen::Vector3d vecIn, uint64_t id);

  virtual ~NormalVectorParameterBlock();

  // setters
  /// @brief Set estimate of this parameter block.
  /// @param[in] vecIn The estimate to set this to.
  void setEstimate(const Eigen::Vector3d &vecIn) final;

  // getters
  /// @brief Get estimate.
  /// \return The estimate.
  Eigen::Vector3d estimate() const final;

  // minimal internal parameterization
  // x0_plus_Delta=Delta_Chi[+]x0
  /// \brief Generalization of the addition operation,
  ///        x_plus_delta = Plus(x, delta)
  ///        with the condition that Plus(x, 0) = x.
  /// @param[in] x0 Variable.
  /// @param[in] Delta_Chi Perturbation.
  /// @param[out] x0_plus_Delta Perturbed x.
  virtual void plus(const double *x0, const double *Delta_Chi,
                    double *x0_plus_Delta) const;

  /// \brief The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  /// @param[in] x0 Variable.
  /// @param[out] jacobian The Jacobian.
  virtual void plusJacobian(const double *x0, double *jacobian) const;

  // Delta_Chi=x0_plus_Delta[-]x0
  /// \brief Computes the minimal difference between a variable x and a
  /// perturbed variable x_plus_delta
  /// @param[in] x0 Variable.
  /// @param[in] x0_plus_Delta Perturbed variable.
  /// @param[out] Delta_Chi Minimal difference.
  /// \return True on success.
  virtual void minus(const double *x0, const double *x0_plus_Delta,
                     double *Delta_Chi) const;

  /// \brief Computes the Jacobian from minimal space to naively
  /// overparameterised space as used by ceres.
  /// @param[in] x0 Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  virtual void liftJacobian(const double *x0, double *jacobian) const;

  /// @brief Return parameter block type as string
  std::string typeInfo() const final;
};

} // namespace ceres
} // namespace okvis

#endif  // INCLUDE_SWIFTVIO_CERES_NORMALVECTORPARAMETERBLOCK_HPP_


#define BOOST_TEST_MODULE TestComplex
#include <boost/test/unit_test.hpp>

#include <complex>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/make_solver.hpp>

#include <amgcl/coarsening/aggregation.hpp>
#include <amgcl/coarsening/smoothed_aggregation.hpp>
#include <amgcl/coarsening/smoothed_aggr_emin.hpp>

#include <amgcl/relaxation/damped_jacobi.hpp>
#include <amgcl/relaxation/gauss_seidel.hpp>
#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/relaxation/ilu0.hpp>
#include <amgcl/relaxation/ilut.hpp>
#include <amgcl/relaxation/chebyshev.hpp>

#include <amgcl/solver/cg.hpp>
#include <amgcl/solver/bicgstab.hpp>
#include <amgcl/solver/bicgstabl.hpp>
#include <amgcl/solver/gmres.hpp>

#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/complex.hpp>
#include <amgcl/profiler.hpp>

#include "sample_problem.hpp"

namespace amgcl {
    profiler<> prof;
}

BOOST_AUTO_TEST_SUITE( test_complex )

BOOST_AUTO_TEST_CASE(complex_matrix_adapter)
{
    typedef std::complex<double> complex;

    std::vector<int>     ptr;
    std::vector<int>     col;
    std::vector<complex> val;
    std::vector<complex> rhs;

    size_t n = sample_problem(32, val, col, ptr, rhs);

    std::vector<complex> x(n, complex(0.0,0.0));

    typedef amgcl::backend::builtin<double> Backend;

    boost::property_tree::ptree prm;
    prm.put("precond.coarsening.aggr.block_size", 2);

    amgcl::make_solver<
        amgcl::amg<
            Backend,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0
            >,
        amgcl::solver::bicgstab<Backend>
        > solve( amgcl::adapter::complex_matrix(std::tie(n, ptr, col, val)), prm );

    std::cout << solve.precond() << std::endl;

    boost::iterator_range<const double*> f_range =
        amgcl::adapter::complex_range(rhs);

    boost::iterator_range<double*> x_range =
        amgcl::adapter::complex_range(x);

    size_t iters;
    double resid;

    std::tie(iters, resid) = solve(f_range, x_range);

    BOOST_CHECK_SMALL(resid, 1e-8);

    std::cout
        << "iters: " << iters << std::endl
        << "resid: " << resid << std::endl
        ;
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * Copyright (c) 2018, University Osnabrück
 * 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 University Osnabrück 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 University Osnabrück 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.
 */

/*
 * BaseVector.hpp
 *
 *  @date 02.06.2017
 *  @author Lukas Kalbertodt <lukas.kalbertodt@gmail.com>
 */

#ifndef LVR2_GEOMETRY_BASEVECTOR_H_
#define LVR2_GEOMETRY_BASEVECTOR_H_

// Eigen sometimes produces errors when compiled with CUDA. Disables
// all Eigen related function for CUDA code (which is currently fine).
#ifndef __NVCC__
#include <Eigen/Dense>
#endif

#include <iostream>

namespace lvr2
{

template <typename> struct Normal;

/**
 * @brief A generic, weakly-typed vector.
 *
 * This vector is weakly-typed as it allows all common operations that can be
 * executed numerically. Instead of this, `BaseVector` implementations from other libraries could be
 * used. However, they have to provide all the methods and fields that
 * `BaseVector` defines.
 */
template <typename CoordT>
struct BaseVector
{
public:
    using CoordType = CoordT;

    CoordT x;
    CoordT y;
    CoordT z;

    /// Default constructs a null-vector.
    BaseVector() : x(0), y(0), z(0) {}

    /// Builds a BaseVector with the given coordinates.
    BaseVector(const CoordT &x, const CoordT &y, const CoordT &z)
        : x(x), y(y), z(z)
    {
    }

    BaseVector(const BaseVector& o) : x(o.x), y(o.y), z(o.z)
    {
    }
    // ========================================================================
    // === Named operations
    // ========================================================================

    /**
     * @brief  Returns the length of this vector.
     */
    CoordT length() const;

    /**
     * @brief  Returns the squared length of this vector.
     *
     * The squared length is easier to calculate and sufficient for certain
     * uses cases. This method only exists for performance reasons.
     */
    CoordT length2() const;

    /**
     * @brief  Calculates the distance to another vector.
     */
    CoordT distance(const BaseVector &other) const;

    /**
     * @brief  Calculates the squared distance to another vector.
     *
     * The squared distance is easier to calculate and sufficient for certain
     * uses cases. This method only exists for performance reasons.
     */
    CoordT distance2(const BaseVector &other) const;

    /**
     * @brief    Calculates the cross product between this and
     *           the given vector. Returns a new BaseVector instance.
     */
    BaseVector<CoordT> cross(const BaseVector &other) const;

    /**
     * @brief    Calculates the rotated vector around an normal vector n with the rotation angle alpha
     *
     */
    BaseVector<CoordT> rotated(const BaseVector &n, const double &alpha) const;

    /**
     * @brief    Calculates the dot product between this and
     *           the given vector.
     */
    CoordT dot(const BaseVector &other) const;

    void normalize()
    {
        // Check for invalid vector. This check can be disabled in release mode,
        // which will probably lead to +inf and -inf values. In the documentation
        // for this function we require the vector to not be the null-vector.
        // assert(!(this->x == 0 && this->y == 0 && this->z == 0));
        if(!(this->x == 0 && this->y == 0 && this->z == 0))
        {
            auto len = this->length();
            this->x /= len;
            this->y /= len;
            this->z /= len;
        }
    }

    // ========================================================================
    // === Operator overloads
    // ========================================================================

    /// Scalar multiplication.
    BaseVector<CoordT> operator*(const CoordT &scale) const;
    /// Scalar division.
    BaseVector<CoordT> operator/(const CoordT &scale) const;

    /// Scalar multiplication.
    BaseVector<CoordT>& operator*=(const CoordT &scale);
    /// Scalar division.
    BaseVector<CoordT>& operator/=(const CoordT &scale);

    /// Element-wise addition.
    BaseVector<CoordT> operator+(const BaseVector &other) const;
    /// Element-wise subtraction.
    BaseVector<CoordT> operator-(const BaseVector &other) const;

    /// Element-wise addition.
    BaseVector<CoordT>& operator+=(const BaseVector<CoordT> &other);
    /// Element-wise subtraction.
    BaseVector<CoordT>& operator-=(const BaseVector<CoordT> &other);

    CoordType distanceFrom(const BaseVector<CoordT> &other) const;
    CoordType squaredDistanceFrom(const BaseVector<CoordType> &other) const;

      /**
     * @brief Returns the centroid of all points in the given collection.
     *
     * The collection need to work with a range-based for-loop and its elements
     * need to be `Point<BaseVecT>`. It has to contain at least one element.
     */
    template<typename CollectionT>
    static BaseVector<CoordT> centroid(const CollectionT& points);

    /**
     * @brief Returns the average of all vectors in the given collection.
     *
     * The collection need to work with a range-based for-loop and its elements
     * need to be `Vector<BaseVecT>`. It has to contain at least one element.
     */
    template<typename CollectionT>
    static BaseVector<CoordT> average(const CollectionT& vecs);

        /**
     * @brief Returns a normalized version of this vector.
     *
     * Note that `this` must not be the null vector, or else the behavior is
     * undefined.
     */
    Normal<CoordT> normalized() const;

    bool operator==(const BaseVector &other) const;
    bool operator!=(const BaseVector &other) const;

    CoordT operator*(const BaseVector<CoordType> &other) const;

    /**
     * @brief    Indexed coordinate access (reading)
     */
    CoordT operator[](const unsigned& index) const;

    /**
     * @brief   Indexed coordinate access (writing)
     */
    CoordT& operator[](const unsigned& index);

// Eigen sometimes produces errors when compiled with CUDA. Disables
// all Eigen related function for CUDA code (which is currently fine).
#ifndef __NVCC__
    // Friend declaration for Eigen multiplication
    template<typename T, typename S>
    friend BaseVector<T> operator*(const Eigen::Matrix<S, 4, 4>& mat, const BaseVector<T>& normal);

#endif // ifndef __NVCC__

};

template<typename T>
std::ostream& operator<<( std::ostream& os, const BaseVector<T>& v)
{
    os << "Vec: [" << v.x << " " << v.y << " " << v.z << "]" << std::endl;
    return os;
}

// Eigen sometimes produces errors when compiled with CUDA. Disables
// all Eigen related function for CUDA code (which is currently fine).
#ifndef __NVCC__

/**
 * @brief   Multiplication operator to support transformation with Eigen
 *          matrices. Rotates the normal, ignores translation. Implementation
 *          for RowMajor matrices.
 *
 * @tparam CoordType            Coordinate type of the normals
 * @tparam Scalar               Scalar type of the Eigen matrix
 * @param mat                   Eigen matrix
 * @param normal                Input normal
 * @return Normal<CoordType>    Transformed normal
 */
template<typename CoordType, typename Scalar = CoordType>
inline BaseVector<CoordType> operator*(const Eigen::Matrix<Scalar, 4, 4>& mat, const BaseVector<CoordType>& normal)
{
    // TODO: CHECK IF THIS IS CORRECT
    CoordType x = mat(0, 0) * normal.x + mat(1, 0) * normal.y + mat(2, 0) * normal.z;
    CoordType y = mat(0, 1) * normal.x + mat(1, 1) * normal.y + mat(2, 1) * normal.z;
    CoordType z = mat(0, 2) * normal.x + mat(1, 2) * normal.y + mat(2, 2) * normal.z;

    x += mat(0, 3);
    y += mat(1, 3);
    z += mat(2, 3);

    return BaseVector<CoordType>(x,y,z);
}

#endif // ifndef __NVCC__

} // namespace lvr

#include "lvr2/geometry/BaseVector.tcc"

#endif /* LVR2_GEOMETRY_BASEVECTOR_H_ */


/**
 * @file matrix_canvas.cc
 * An implementation of a matrix canvas class to display a sparse matrix
 * for the vismatrix program.
 */

/*
 * David Gleich
 * 21 November 2006
 * Copyright, Stanford University
 */

#include <stdlib.h>

#include <string>
#include <limits>
#include <cmath>

#include <algorithm>
#include <utility>

// the yasmic files have to be included before anything
// with OpenGL
#define YASMIC_UTIL_LOAD_GZIP
#include <yasmic/compressed_row_matrix.hpp>
#include <yasmic/util/load_crm_matrix.hpp>
#include <yasmic/transpose_matrix.hpp>
#include <yasmic/nonzero_union.hpp>
#include <yasmic/iterator_utility.hpp>

#include <boost/timer.hpp>

#include "glut_2d_canvas.h"
#include "matrix_data_panel.hpp"
#include "matrix_data_cursor.hpp"

#include "matrix_canvas.hpp"

#include "colormaps.hpp"

#include "util/file.hpp"
#include "util/array.hpp"

matrix_canvas::matrix_canvas(int w, int h)
:
  glut_2d_canvas(w,h,"vismatrix"),  // initialize the window first
      // the order of the rest is the order in which the variables are
      // declared in the class declaration
  	  p_offset(panel_offset), p_height(panel_height),
      data_panel(glut_id, width-2*p_offset, std::max(height/2,p_height), p_offset, p_offset),
            matrix_display_list(0),
            data_panel_visible(false),
            data_cursor(0,0),
            matrix_filename(""),
            matrix_loaded(false),
            point_alpha(0.5f),
            normalization_state(no_normalization),
            permutation_state(no_permutation),
            colormap_state(rainbow_colormap),
            colormap((float *)spring_color_map,
                sizeof(spring_color_map)/sizeof(spring_color_map[0])),
            colormap_invert(false),
            rperm_loaded(false),
            cperm_loaded(false)
{
    border_color[0]=1.0f; border_color[1]=1.0f; border_color[2]=1.0f;
}

void matrix_canvas::post_constructor()
{
    init_window();
    init_menu();
    init_display_list();

    set_zoom(0.95f);

    show_data_panel();
}

/**
 * This function draws the matrix.
 */
void matrix_canvas::draw()
{
    using namespace std;
    if (!matrix_loaded) { return; }

    int m = _m.nrows;
    int n = _m.ncols;

    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    // border
	glPointSize(1.0);
    glColor3f(border_color[0], border_color[1], border_color[2]);
    glBegin(GL_LINE_LOOP);
    glVertex2f(-0.5f, -0.5f);
    glVertex2f(-0.5f, m-0.5f);
    glVertex2f(n-0.5f, m-0.5f);
    glVertex2f(n-0.5f, -0.5f);
    glEnd();

    glPointSize(zoom / (virtual_width*aspect/(float)width));

    float x1,y1,x2,y2;
    world_extents(x1,y1,x2,y2);

    int r1=(int)floor(y1),r2=(int)floor(y2);
    int c1=(int)floor(x1),c2=(int)floor(x2);

    if (std::max(x2-x1,y2-y1) < 16384)
    {
        cout << "drawing partial matrix (" << r1 << ", " << c1 << ") - ("
            << r2 << ", " << c2 << ")" << endl;

        draw_partial_matrix(r1,c1,r2,c2);
    }
    else if (_m.nrows < 32768) {
        cout << "drawing full matrix" << endl;
        // only draw the full matrix if there isn't a very small portion
	    draw_full_matrix();
    }
    else {
        r1=(std::max)(r1,0);
        int r2end=(std::min)(r2,_m.nrows);

        r1=r1+16384*frame;
        r2=(std::min)(r1+16384,r2end);
        cout << "drawing matrix iteratively " << frame << " (" << r1 << ", " << c1 << ") - ("
            << r2 << ", " << c2 << ")" << " " << r2end << endl;

        draw_partial_matrix(r1,c1,r2,c2);

        if (r2 != r2end) {
            display_finished = false;
        }
    }

    if (data_panel_visible)
    {
        int r = data_cursor.get_y();
        int c = data_cursor.get_x();

        if (permutation_state == row_permutation ||
            permutation_state == row_column_permutation) {
            r = irperm[r];
        }
        if (permutation_state == column_permutation ||
            permutation_state == row_column_permutation) {
            c = icperm[c];
        }

        // workaround for stupid VC++ bug
        //data_panel.update(r, c, yasmic::value(r, c,
        //    static_cast<const Matrix&>(_m)));
        data_panel.update(r,c,(float)matrix_value(r,c),
            row_label(r), column_label(c));

        data_cursor.draw();
    }
}

template <bool partial, class NRMap, class NCMap, class PRMap, class PCMap>
void matrix_canvas::draw_matrix(int r1, int c1, int r2, int c2,
    value_type min_val, value_type inv_val_range, float alpha,
    NRMap nrv, NCMap ncv, PRMap iprm, PCMap pcm)
{
    int colormap_entry;
    value_type v;

    glBegin( GL_POINTS );
	{
        for (int pi = std::max(0,r1); pi < std::min(r2, _m.nrows); ++pi)
        {
            // i is the real row in the matrix for the pith row
            // of the display
            int i = iprm[pi];

            for (index_type ri = _m.ai[i]; ri < _m.ai[i+1]; ++ri)
            {
                // j is the real column in the matrix for the pjth
                // column of the display
                int j = _m.aj[ri];
                int pj = pcm[j];

                // skip all the columns outside
                if (partial && (pj < c1 || pj > c2)) { continue; }

                v = _m.a[ri]*nrv[i]*ncv[j];

                // scale v to the range [0,1]
                v = v - min_val;
                v = v*inv_val_range;

                if (!colormap_invert) {
                    colormap_entry = (int)(v*(colormap.size-1));
                } else {
                    colormap_entry = (int)(v*(colormap.size-1));
                    colormap_entry=colormap.size-1-colormap_entry;
                }

                glColor4f(colormap.map[colormap_entry*3],
                          colormap.map[colormap_entry*3+1],
                          colormap.map[colormap_entry*3+2],
                          alpha);
                glVertex2f((GLfloat)pj, (GLfloat)pi);
            }
        }
	}
	glEnd();
}

template <bool partial, class NRMap, class NCMap>
void matrix_canvas::draw_matrix_dispatch(int r1, int c1, int r2, int c2,
    value_type min_val, value_type inv_val_range, float alpha,
    NRMap nrv, NCMap ncv)
{
    switch (permutation_state) {
        case no_permutation:
            draw_matrix<partial>(r1,c1,r2,c2,min_val,inv_val_range,alpha,nrv,ncv,
                util::identity_array(),util::identity_array());
            break;

        case row_permutation:
            draw_matrix<partial>(r1,c1,r2,c2,min_val,inv_val_range,alpha,nrv,ncv,
                &irperm[0],util::identity_array());
            break;

        case column_permutation:
            draw_matrix<partial>(r1,c1,r2,c2,min_val,inv_val_range,alpha,nrv,ncv,
                util::identity_array(),&cperm[0]);
            break;

        case row_column_permutation:
            draw_matrix<partial>(r1,c1,r2,c2,min_val,inv_val_range,alpha,nrv,ncv,
                &irperm[0],&cperm[0]);
            break;
    }
}

template <bool partial>
void matrix_canvas::draw_matrix_dispatch(int r1, int c1, int r2, int c2)
{
    value_type max_val = matrix_stats.max_val;
    value_type min_val = matrix_stats.min_val;

    if (max_val - min_val <= 0)
    {
        // this sets min_val to something reasonable, and
        // shows the high end of the colormap if the values
        // are all equal
        min_val = max_val - 1.0;
    }
    value_type inv_val_range = 1.0/(max_val - min_val);

    float alpha = alpha_from_zoom();

    switch (normalization_state) {
        case no_normalization:
            draw_matrix_dispatch<partial>(r1,c1,r2,c2,min_val,inv_val_range,alpha,
                util::constant_array<value_type>(1),util::constant_array<value_type>(1));
            break;

        case row_normalization:
            draw_matrix_dispatch<partial>(r1,c1,r2,c2,0.0,1.0,alpha,
                &rnorm[0],util::constant_array<value_type>(1));
            break;

        case column_normalization:
            draw_matrix_dispatch<partial>(r1,c1,r2,c2,0.0,1.0,alpha,
                util::constant_array<value_type>(1),&cnorm[0]);
            break;

        case row_column_normalization:
            draw_matrix_dispatch<partial>(r1,c1,r2,c2,0.0,1.0,alpha,&rnorm[0],&cnorm[0]);
            break;
    }
}

void matrix_canvas::draw_full_matrix()
{
    draw_matrix_dispatch<false>(0,0,_m.nrows,_m.ncols);
}

void matrix_canvas::draw_partial_matrix(int r1, int c1, int r2, int c2)
{
    draw_matrix_dispatch<true>(r1,c1,r2,c2);
}

/**
 * draw a subset of the matrix parameterized on a few types of modifications
 *
 * Each matrix is drawn with value
 * v*nrv[i]*ncv[j]
 * so that nrv and ncv are the row and column normalization vectors
 *
 * Further, each row and columns are permuted based on prm and pcm, which are
 * the row and column permutation maps, respectively.
 *
 * point i,j in the matrix is drawn as point prm[i], pcm[i]
 *
 * This function always draws the full matrix, and never the
 * smaller, faster to display version.
 *
 * @param r1 the first row to draw
 * @param r2 the last row to draw
 * @param c1 the first column to draw
 * @param c2 the last column to draw
 * @param nrv a normalization vector for the rows of the matrix.
 * @param ncv a normalization vector for the columns of the matrix.
 * @param iprm a inverse permutation map for the rows of the matrix.
 * @param pcm a permutation map for the columns of the matrix.
 */
/*template <class NRMap, class NCMap, class PRMap, class PCMap>
void matrix_canvas::draw_partial_matrix(int r1, int c1, int r2, int c2,
        NRMap nrv, NCMap ncv, PRMap iprm, PCMap pcm)
{
    //
    // todo, fix max/min based on the normalization of the matrix
    //
    value_type max_val = matrix_stats.max_val;
    value_type min_val = matrix_stats.min_val;

    if (max_val - min_val <= 0)
    {
        // this sets min_val to something reasonable, and
        // shows the high end of the colormap if the values
        // are all equal
        min_val = max_val - 1.0;
    }

    value_type v;
    value_type inv_val_range = 1.0/(max_val - min_val);

    float alpha = alpha_from_zoom();
    int colormap_entry;

    glBegin( GL_POINTS );
	{
        for (int pi = std::max(0,r1); pi < std::min(r2, _m.nrows); ++pi)
        {
            // i is the real row in the matrix for the pith row
            // of the display
            int i = iprm[pi];

            for (index_type ri = _m.ai[i]; ri < _m.ai[i+1]; ++ri)
            {
                // j is the real column in the matrix for the pjth
                // column of the display
                int j = _m.aj[ri];
                int pj = pcm[j];

                // skip all the columns outside
                if (pj < c1 || pj > c2) { continue; }

                v = _m.a[ri]*nrv[i]*ncv[j];

                // scale v to the range [0 ,1]
                v = v - min_val;
                v = v*inv_val_range;

                colormap_entry = (int)(v*(colormap.size-1));

                glColor4f(colormap.map[colormap_entry*3],
                          colormap.map[colormap_entry*3+1],
                          colormap.map[colormap_entry*3+2],
                          alpha);
                glVertex2f(pj, pi);
            }
        }
	}
	glEnd();
}*/

/**
 * Compute a point_alpha from the current zoom level.
 *
 * zoom < 1: alpha = point_alpha
 * zoom s.t. point_size >= 2.0 => alpha = 1
 */
float matrix_canvas::alpha_from_zoom()
{
    float point_size = zoom / (virtual_width*aspect/(float)width);
    if (zoom < 1) { return (point_alpha); }
    else if (point_size >= 2.0f) { return (1.0f); }
    else
    {
        float zoom_1 = 1.0;
        float zoom_2 = 2.0*(virtual_width*aspect/(float)width);

        return 1.0f - (zoom_2 - zoom)/(zoom_2 - zoom_1)*(1.0f-point_alpha);
    }
}

void matrix_canvas::reshape(int w, int h)
{
    super::reshape(w,h);

    // handle the subwindow...
    glutSetWindow(data_panel.get_glut_window_id());
    if (data_panel_visible && w < 4*p_offset || h < 4*p_offset)
    {
        // if the window is too small, then hide it!
        hide_data_panel();
    }
    else if (data_panel_visible)
    {
        glutPositionWindow(p_offset, p_offset);
        glutReshapeWindow(w-2*p_offset, std::min(h/2,p_height));
    }

    glutSetWindow(get_glut_window_id());

    int x,y,w_glui,h_glui;
    GLUI_Master.get_viewport_area(&x,&y,&w_glui,&h_glui);
    if (data_panel_visible)
    {
        //y += panel_height;
        h_glui -= p_height;
    }

    super::reshape(w_glui,h_glui);
    glViewport(x,y,w_glui,h_glui);
}

void matrix_canvas::motion(int x, int y)
{
    display_finished = true;

    // rescale the mouse click
    int move_x = x-mouse_begin_x;
    int move_y = y-mouse_begin_y;

    double scaled_x, scaled_y;
    scaled_x = scale_to_world((float)move_x);
    scaled_y = scale_to_world((float)move_y);

    double norm_x=scaled_x, norm_y=scaled_y;
    norm_x /= aspect;

    if (data_cursor.scaled_motion(norm_x, norm_y))
    {
        begin_mouse_click(x,y);
    }
    else
    {
        super::motion(x, y);
    }
}

void matrix_canvas::mouse_click(int button, int state, int x, int y)
{
    if (data_cursor.mouse_click(button, state, x, y))
    {
        begin_mouse_click(x,y);
    }
    else
    {
        super::mouse_click(button, state, x, y);
    }
}

void matrix_canvas::key(unsigned char key, int x, int y)
{
    switch (key) {
        case 'o':
        case 'O':
            write_svg();
            break;
    }
}

void matrix_canvas::special_key(int key, int x, int y)
{
    int m = _m.nrows;
    int n = _m.ncols;

    switch (key) {
    case GLUT_KEY_UP:
        data_cursor.set_position(data_cursor.get_x(),data_cursor.get_y()-1);
        glutPostRedisplay();
        break;

    case GLUT_KEY_DOWN:
        data_cursor.set_position(data_cursor.get_x(),data_cursor.get_y()+1);
        glutPostRedisplay();
        break;

    case GLUT_KEY_LEFT:
        data_cursor.set_position(data_cursor.get_x()-1,data_cursor.get_y());
        glutPostRedisplay();
        break;

    case GLUT_KEY_RIGHT:
        data_cursor.set_position(data_cursor.get_x()+1,data_cursor.get_y());
        glutPostRedisplay();
        break;
    }
}

/**
 * This function initializes the window and assures it is
 * sized correctly, etc.
 */
void matrix_canvas::init_window()
{
    // make sure we are the current window
    glutSetWindow(get_glut_window_id());

    set_center(_m.ncols/2, _m.nrows/2);
    set_natural_size(_m.ncols, _m.nrows);

    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

#if !defined (__APPLE__)
    // POINT_SMOOTHING doesn't work quite right on OS X when I tried it
    glEnable(GL_POINT_SMOOTH);
#endif
}

/**
 * This function creates the display list for the matrix.
 */
void matrix_canvas::init_display_list()
{
    glutSetWindow(get_glut_window_id());

    /*if (glIsList(matrix_display_list))
    {
        glDeleteLists(matrix_display_list, 1);
    }

    matrix_display_list = glGenLists(1);

    glNewList(matrix_display_list,GL_COMPILE);
    glEndList();*/
}

/*
 * =================
 * matrix functions
 * =================
 */

matrix_canvas::value_type matrix_canvas::matrix_value(
    matrix_canvas::index_type r,
    matrix_canvas::index_type c)
{
    if (r < 0 || r >= _m.nrows || c < 0 || c >= _m.ncols)
    {
        return (value_type)0;
    }

    index_type ri,riend;
    ri = _m.ai[r];
    riend = _m.ai[r+1];

    while (ri < riend)
    {
        if (c == _m.aj[ri]) { return _m.a[ri]; }
        ++ri;
    }
    return ((value_type)0);
}

const std::string& matrix_canvas::row_label(index_type r)
{
    if (r >= 0 && r < rlabel.size()) {
        return rlabel[r];
    } else {
        return empty_label;
    }
}

const std::string& matrix_canvas::column_label(index_type c)
{
    if (c >= 0 && c < clabel.size()) {
        return clabel[c];
    } else {
        return empty_label;
    }
}

bool matrix_canvas::load_matrix(const std::string& filename,
    bool symmetrize)
{
    using namespace std;
    matrix_loaded = false;

    matrix_filename = filename;

    bool rval;
    boost::timer t0;

    t0.restart();
    cerr << "loading " << matrix_filename << "..." << endl;

    ifstream t(matrix_filename.c_str());
    t.close();
    if (t.fail())
    {
        cerr << matrix_filename << " does not exist." << endl;
        return false;
    }

	rval = load_crm_matrix(matrix_filename, _m.ai, _m.aj, _m.a,
        _m.nrows, _m.ncols, _m.nnz);
	if (!rval)
	{
        std::cerr << "error loading matrix: cannot proceed!\n" << std::endl;
		return false;
	}

	std::cerr << "read matrix in " << t0.elapsed() <<  std::endl;

	using namespace yasmic;

	// for operations that need value information, use this type
	typedef compressed_row_matrix<
		vector<index_type>::iterator, vector<index_type>::iterator,
        vector<value_type>::iterator  >
        crs_matrix;

    if (symmetrize)
	{
		t0.restart();

		_m.nnz = (int)(2*_m.aj.size());

		vector<int> rows_temp(_m.ai);
		vector<int> cols_temp(_m.aj);
		vector<value_type> vals_temp(_m.a);

		std::fill(_m.ai.begin(), _m.ai.end(), 0);
		_m.aj.resize(_m.nnz);
		_m.a.resize(_m.nnz);

		typedef transpose_matrix<crs_matrix> t_matrix;
		typedef nonzero_union<crs_matrix, t_matrix> nzu_matrix;

		crs_matrix m(rows_temp.begin(), rows_temp.end(), cols_temp.begin(), cols_temp.end(),
					vals_temp.begin(), vals_temp.end(),
                    _m.nrows, _m.ncols, _m.nnz/2);

		t_matrix mt(m);
		nzu_matrix nzu(m, mt);

		_m.nrows = nrows(nzu);
		_m.ncols = ncols(nzu);

		// load the matrix
		load_matrix_to_crm(nzu, _m.ai.begin(), _m.aj.begin(), _m.a.begin());

		std::cerr << "symmetrized matrix in " << t0.elapsed() <<  std::endl;
	}

	{
		index_type nzstart = _m.nnz;

		t0.restart();

		crs_matrix mlarge(_m.ai.begin(), _m.ai.end(), _m.aj.begin(), _m.aj.end(),
					_m.a.begin(), _m.a.end(), _m.nrows, _m.ncols, _m.nnz);

        pack_storage(mlarge, std::plus<value_type>());
		sort_storage(mlarge);

		_m.nnz = _m.ai.back();

		std::cerr << "packed matrix in " << t0.elapsed() <<  std::endl;
		std::cerr << "removed " << nzstart - _m.nnz << " nzs" << std::endl;
	}

    {
		std::cerr << "matrix: " << matrix_filename << std::endl;
		std::cerr << "nrows: " << _m.nrows << std::endl;
		std::cerr << "ncols: " << _m.ncols << std::endl;
		std::cerr << "nnz: " << _m.nnz << std::endl;
	}

    matrix_loaded = true;
    init_window();
    data_cursor.set_matrix_size(_m.nrows, _m.ncols);

    //
    // compute matrix stats
    //

    matrix_stats.max_degree = 0;
    matrix_stats.min_degree = std::numeric_limits<index_type>::max();
    matrix_stats.max_val = std::numeric_limits<value_type>::min();
    matrix_stats.min_val = std::numeric_limits<value_type>::max();

    rnorm.resize(_m.nrows);
    cnorm.resize(_m.ncols);

    for (index_type r = 0; r < _m.nrows; ++r)
    {
        index_type deg = _m.ai[r+1] - _m.ai[r];
        matrix_stats.max_degree = std::max(matrix_stats.max_degree, deg);
        matrix_stats.min_degree = std::min(matrix_stats.min_degree, deg);

        for (index_type ri = _m.ai[r]; ri < _m.ai[r+1]; ++ri)
        {
            value_type val = _m.a[ri];
            matrix_stats.max_val = std::max(matrix_stats.max_val, val);
            matrix_stats.min_val = std::min(matrix_stats.min_val, val);

            rnorm[r] += val*val;
            cnorm[_m.aj[ri]] += val*val;
        }
    }
    for (index_type r=0; r<_m.nrows; ++r) { rnorm[r]=1.0/sqrt(rnorm[r]); }
    for (index_type r=0; r<_m.ncols; ++r) { cnorm[r]=1.0/sqrt(cnorm[r]); }

    return (true);
}

bool matrix_canvas::load_permutations(const std::string& rperm_filename,
        const std::string& cperm_filename)
{
    using namespace std;

    if (!rperm_filename.empty() && !util::file_exists(rperm_filename)) {
        cerr << rperm_filename << " does not exist." << endl;
        return (false);
    }

    if (!cperm_filename.empty() && !util::file_exists(cperm_filename)) {
        cerr << cperm_filename << " does not exist." << endl;
        return (false);
    }

    // use all the boost iostreams stuff loaded from the load_crm_matrix.hpp
    // header file
    typedef boost::iostreams::filtering_stream<
                boost::iostreams::input_seekable>
        filtered_ifstream;

    if (!rperm_filename.empty())
    {
        YASMIC_VERBOSE( cerr << "reading " << rperm_filename << endl; )
        filtered_ifstream ios_fifs;
        ifstream ifs(rperm_filename.c_str());

        if (util::gzip_header(ifs))
        {
            ifs.seekg(0, ios::beg);
            ios_fifs.push(boost::iostreams::gzip_decompressor());
            YASMIC_VERBOSE(  cerr << "detected gzip" << endl; )
        }

        ios_fifs.push(ifs);

        string line;

        irperm.resize(_m.nrows);

        index_type r;
        for (r=0; r<_m.nrows && !ios_fifs.eof(); ++r) {
            index_type p;
            ifs >> p;
            irperm[r]=p;
        }

        if (r!=_m.nrows) {
            cerr << rperm_filename << " only contains " << r << " entries, "
                 << " not " << _m.nrows << std::endl;
            return (false);
        }

        rperm_loaded = true;
    }

    if (!cperm_filename.empty())
    {
        YASMIC_VERBOSE( cerr << "reading " << cperm_filename << endl; )
        filtered_ifstream ios_fifs;
        ifstream ifs(cperm_filename.c_str());

        if (util::gzip_header(ifs))
        {
            ifs.seekg(0, ios::beg);
            ios_fifs.push(boost::iostreams::gzip_decompressor());
            YASMIC_VERBOSE(  cerr << "detected gzip" << endl; )
        }

        ios_fifs.push(ifs);

        string line;

        cperm.resize(_m.ncols);
        icperm.resize(_m.ncols);

        index_type c;
        for (c=0; c<_m.ncols && !ios_fifs.eof(); ++c) {
            index_type p;
            ifs >> p;
            icperm[c]=p;
            cperm[p]=c;
        }

        if (c!=_m.ncols) {
            cerr << cperm_filename << " only contains " << c << " entries, "
                 << " not " << _m.ncols << std::endl;
            return (false);
        }

        cperm_loaded = true;
    }

    return (true);
}

bool matrix_canvas::load_labels(const std::string& rlabel_filename,
        const std::string& clabel_filename)
{
    using namespace std;

    if (!rlabel_filename.empty() && !util::file_exists(rlabel_filename)) {
        cerr << rlabel_filename << " does not exist." << endl;
        return (false);
    }

    if (!clabel_filename.empty() && !util::file_exists(clabel_filename)) {
        cerr << clabel_filename << " does not exist." << endl;
        return (false);
    }

    // use all the boost iostreams stuff loaded from the load_crm_matrix.hpp
    // header file
    typedef boost::iostreams::filtering_stream<
                boost::iostreams::input_seekable>
        filtered_ifstream;

    if (!rlabel_filename.empty())
    {
        YASMIC_VERBOSE( cerr << "reading " << rlabel_filename << endl; )
        filtered_ifstream ios_fifs;
        ifstream ifs(rlabel_filename.c_str());

        if (util::gzip_header(ifs))
        {
            ifs.seekg(0, ios::beg);
            ios_fifs.push(boost::iostreams::gzip_decompressor());
            YASMIC_VERBOSE(  cerr << "detected gzip" << endl; )
        }

        ios_fifs.push(ifs);

        string line;

        while (!ios_fifs.eof()) {
            getline(ios_fifs, line);
            rlabel.push_back(line);
        }
    }

    if (!clabel_filename.empty())
    {
        YASMIC_VERBOSE(  cerr << "reading " << clabel_filename << endl; )
        filtered_ifstream ios_fifs;
        ifstream ifs(clabel_filename.c_str());

        if (util::gzip_header(ifs))
        {
            ifs.seekg(0, ios::beg);
            ios_fifs.push(boost::iostreams::gzip_decompressor());
            YASMIC_VERBOSE(  cerr << "detected gzip" << endl; )
        }

        ios_fifs.push(ifs);

        string line;

        while (!ios_fifs.eof()) {
            getline(ios_fifs, line);
            clabel.push_back(line);
        }
    }

    return (true);
}

/*
 * =================
 * control functions
 * =================
 */

void matrix_canvas::show_data_panel()
{
    glutSetWindow(data_panel.get_glut_window_id());
    glutShowWindow();
    glutPostRedisplay();
    data_panel_visible = true;
    reshape(width, height);
}

void matrix_canvas::hide_data_panel()
{
    glutSetWindow(data_panel.get_glut_window_id());
    glutHideWindow();
    data_panel_visible = false;
    reshape(width, height);
}

void matrix_canvas::set_colormap(colormap_state_type c)
{
    // at the moment, we don't have any support for a user colormap
    if (c == user_colormap) { c = rainbow_colormap; }
    switch (c)
    {
    case rainbow_colormap:
        colormap.map = (float*)rainbow_color_map;
        colormap.size = sizeof(rainbow_color_map)/sizeof(rainbow_color_map[0]);
        break;

    case bone_colormap:
        colormap.map = (float*)bone_color_map;
        colormap.size = sizeof(bone_color_map)/sizeof(bone_color_map[0]);
        break;

    case spring_colormap:
        colormap.map = (float*)spring_color_map;
        colormap.size = sizeof(spring_color_map)/sizeof(spring_color_map[0]);
        break;
    }

    glutPostRedisplay();
}

void matrix_canvas::set_next_colormap()
{
    int next = colormap_state+1;
    if (next == last_colormap) { next = first_colormap; }
    set_colormap((colormap_state_type)next);
}

/*
 * =================
 * menu functions
 * =================
 */

void matrix_canvas::init_menu()
{
    int submenu_aspect_ratio = glutCreateMenu(glut_menu);
    glutAddMenuEntry("1:1",menu_aspect_normal);
    glutAddMenuEntry("4:1 (Tall)",menu_aspect_tall_41);
    glutAddMenuEntry("2:1 (Tall)",menu_aspect_tall_21);
    glutAddMenuEntry("1:2 (Wide)",menu_aspect_wide_12);
    glutAddMenuEntry("1:4 (Wide)",menu_aspect_wide_14);

    int submenu_colormap = glutCreateMenu(glut_menu);
    glutAddMenuEntry("Rainbow",menu_colormap_rainbow);
    glutAddMenuEntry("Bone",menu_colormap_bone);
    glutAddMenuEntry("Spring",menu_colormap_spring);
    glutAddMenuEntry("Invert Colormap",menu_colormap_invert);

    int submenu_colors = glutCreateMenu(glut_menu);
    glutAddMenuEntry("White Background",menu_colors_white_bkg);
    glutAddMenuEntry("Black Background",menu_colors_black_bkg);

    // create the main menu
    glutCreateMenu(glut_menu);

    //glutAddMenuEntry("Open File...", menu_file_id);
    glutAddMenuEntry("Toggle Cursor", menu_toggle_cursor_id);
    glutAddSubMenu("Aspect Ratio", submenu_aspect_ratio);
    glutAddSubMenu("Colormap", submenu_colormap);
    glutAddSubMenu("Colors", submenu_colors);

    glutAddMenuEntry("Exit", menu_exit_id);

    /*
    int menu,submenu;

	submenu = glutCreateMenu(processMenuEvents);
	glutAddMenuEntry("Red",RED);
	glutAddMenuEntry("Blue",BLUE);
	glutAddMenuEntry("Green",GREEN);

	menu = glutCreateMenu(processMenuEvents);
	glutAddMenuEntry("White",WHITE);
	glutAddSubMenu("RGB Menu",submenu);
	glutAttachMenu(GLUT_RIGHT_BUTTON);*/

    glutAttachMenu(GLUT_RIGHT_BUTTON);
}

void matrix_canvas::menu(int value)
{
    bool update_screen = false;
    switch (value)
    {
    case menu_file_id:
        break;

    case menu_toggle_cursor_id:
        data_panel_visible ? hide_data_panel() : show_data_panel();
        update_screen = true;
        break;

    case menu_exit_id:
        exit(0);
        break;

    case menu_aspect_normal:
        set_aspect(1.0);
        reshape(width,height);
        update_screen = true;
        break;

    case menu_aspect_wide_12:
        set_aspect(2.0);
        reshape(width,height);
        update_screen = true;
        break;

    case menu_aspect_wide_14:
        set_aspect(4.0);
        reshape(width,height);
        update_screen = true;
        break;

    case menu_aspect_tall_21:
        set_aspect(0.5);
        reshape(width,height);
        update_screen = true;
        break;

    case menu_aspect_tall_41:
        set_aspect(0.25);
        reshape(width,height);
        update_screen = true;
        break;

    case menu_colormap_rainbow:
        set_colormap(rainbow_colormap);
        break;

    case menu_colormap_bone:
        set_colormap(bone_colormap);
        break;

    case menu_colormap_spring:
        set_colormap(spring_colormap);
        break;

    case menu_colormap_invert:
        colormap_invert = !colormap_invert;
        update_screen = true;
        break;

    case menu_colors_white_bkg:
        set_background_color(1.0f,1.0f,1.0f);
        data_panel.set_background_color(0.75f,0.75f,0.75f);
        data_panel.set_text_color(0.0f,0.0f,0.0f);
        data_panel.set_border_color(0.0f,0.0f,1.0f);
        set_border_color(0.0f,0.0f,0.0f);
        data_cursor.set_color(1.0f,0.0f,0.0f);
        update_screen = true;
        break;

    case menu_colors_black_bkg:
        set_background_color(0.0f,0.0f,0.0f);
        data_panel.set_background_color(0.25f,0.25f,0.25f);
        data_panel.set_text_color(1.0f,1.0f,1.0f);
        data_panel.set_border_color(0.0f,1.0f,0.0f);
        set_border_color(1.0f,1.0f,1.0f);
        data_cursor.set_color(1.0f,1.0f,0.0f);
        update_screen = true;
        break;

    }

    if (update_screen) {
        display_finished = true;
        glutPostRedisplay();
    }
}


#include "orproblems/knapsackwithconflicts.hpp"
#include "orproblems/multidimensionalmultiplechoiceknapsack.hpp"
#include "orproblems/quadraticassignment.hpp"
#include "orproblems/cuttingstock.hpp"
#include "orproblems/multipleknapsack.hpp"
#include "orproblems/quadraticmultipleknapsack.hpp"
#include "orproblems/generalizedquadraticmultipleknapsack.hpp"
#include "orproblems/binpackingwithconflicts.hpp"

#include "orproblems/travellingsalesman.hpp"
#include "orproblems/sequentialordering.hpp"
#include "orproblems/travellingrepairman.hpp"
#include "orproblems/thieforienteering.hpp"
#include "orproblems/capacitatedvehiclerouting.hpp"
#include "orproblems/vehicleroutingwithtimewindows.hpp"
#include "orproblems/capacitatedopenvehiclerouting.hpp"

#include "orproblems/schedulingwithsdsttwt.hpp"
#include "orproblems/orderacceptanceandscheduling.hpp"
#include "orproblems/batchschedulingtotalcompletiontime.hpp"
#include "orproblems/batchschedulingtotalweightedtardiness.hpp"
#include "orproblems/parallelschedulingwithfamilysetuptimestwct.hpp"
#include "orproblems/starobservationscheduling.hpp"
#include "orproblems/permutationflowshopschedulingmakespan.hpp"
#include "orproblems/permutationflowshopschedulingtct.hpp"
#include "orproblems/permutationflowshopschedulingtt.hpp"
#include "orproblems/nowaitjobshopschedulingmakespan.hpp"
#include "orproblems/simpleassemblylinebalancing1.hpp"
#include "orproblems/ushapedassemblylinebalancing1.hpp"

#include <boost/program_options.hpp>

using namespace orproblems;

namespace po = boost::program_options;

int main(int argc, char *argv[])
{

    // Parse program options

    std::string problem = "";
    std::string instance_path = "";
    std::string format = "";
    std::string certificate_path = "";
    int verbose = 1;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message")
        ("problem,p", po::value<std::string>(&problem)->required(), "set problem (required)")
        ("input,i", po::value<std::string>(&instance_path)->required(), "set input path (required)")
        ("certificate,c", po::value<std::string>(&certificate_path), "set certificate path")
        ("format,f", po::value<std::string>(&format), "set input file format (default: orlibrary)")
        ("print-instance", "")
        ("verbose,v", po::value<int>(&verbose), "set verbose level")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
        std::cout << desc << std::endl;;
        return 1;
    }
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        return 1;
    }

    if (problem == "knapsackwithconflicts") {
        knapsackwithconflicts::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "multidimensionalmultiplechoiceknapsack") {
        multidimensionalmultiplechoiceknapsack::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "quadraticassignment") {
        quadraticassignment::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "cuttingstock") {
        cuttingstock::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "multipleknapsack") {
        multipleknapsack::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "quadraticmultipleknapsack") {
        quadraticmultipleknapsack::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "generalizedquadraticmultipleknapsack") {
        generalizedquadraticmultipleknapsack::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "binpackingwithconflicts") {
        binpackingwithconflicts::Instance instance(instance_path, format);
        //if (vm.count("print-instance"))
        //    std::cout << instance << std::endl; TODO
        //instance.check(certificate_path, verbose); TODO

    } else if (problem == "travellingsalesman") {
        travellingsalesman::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "sequentialordering") {
        sequentialordering::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "travellingrepairman") {
        travellingrepairman::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "thieforienteering") {
        thieforienteering::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "capacitatedvehiclerouting") {
        capacitatedvehiclerouting::Instance instance(instance_path, format);
        //if (vm.count("print-instance"))
        //    std::cout << instance << std::endl; TODO
        //instance.check(certificate_path, verbose); TODO

    } else if (problem == "vehicleroutingwithtimewindows") {
        vehicleroutingwithtimewindows::Instance instance(instance_path, format);
        //if (vm.count("print-instance"))
        //    std::cout << instance << std::endl; TODO
        //instance.check(certificate_path, verbose); TODO

    } else if (problem == "capacitatedopenvehiclerouting") {
        capacitatedopenvehiclerouting::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        //instance.check(certificate_path, verbose); TODO

    } else if (problem == "schedulingwithsdsttwt") {
        schedulingwithsdsttwt::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "orderacceptanceandscheduling") {
        orderacceptanceandscheduling::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "batchschedulingtotalcompletiontime") {
        batchschedulingtotalcompletiontime::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "batchschedulingtotalweightedtardiness") {
        batchschedulingtotalweightedtardiness::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "parallelschedulingwithfamilysetuptimestwct") {
        parallelschedulingwithfamilysetuptimestwct::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        //instance.check(certificate_path, verbose); TODO

    } else if (problem == "starobservationscheduling") {
        starobservationscheduling::Instance instance(instance_path, format);
        //if (vm.count("print-instance"))
        //    std::cout << instance << std::endl; TODO
        //instance.check(certificate_path, verbose); TODO

    } else if (problem == "permutationflowshopschedulingmakespan") {
        permutationflowshopschedulingmakespan::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "permutationflowshopschedulingtct") {
        permutationflowshopschedulingtct::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "permutationflowshopschedulingtt") {
        permutationflowshopschedulingtt::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "nowaitjobshopschedulingmakespan") {
        nowaitjobshopschedulingmakespan::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "simpleassemblylinebalancing1") {
        simpleassemblylinebalancing1::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else if (problem == "ushapedassemblylinebalancing1") {
        ushapedassemblylinebalancing1::Instance instance(instance_path, format);
        if (vm.count("print-instance"))
            std::cout << instance << std::endl;
        instance.check(certificate_path, verbose);

    } else {
        throw std::invalid_argument(
                "Unknown problem: '" + problem + "'.");
        return 1;
    }

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2014   LASMEA UMR 6602 CNRS/UBP
//         Copyright 2009 - 2014   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
// cover for functor expx2 in simd mode
#include <nt2/exponential/include/functions/expx2.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <cmath>
#include <iostream>
#include <nt2/sdk/unit/args.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/cover.hpp>
#include <vector>

NT2_TEST_CASE_TPL(expx2_0,  NT2_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  using nt2::unit::args;
  const std::size_t NR = args("samples", NT2_NB_RANDOM_TEST);
  const double ulpd = args("ulpd",  2.5);

  const T min = args("min", T(1));
  const T max = args("max", T(5));
  std::cout << "Argument samples #0 chosen in range: [" << min << ",  " << max << "]" << std::endl;
  NT2_CREATE_BUF(a0,T, NR, min, max);

  std::vector<T> ref1(NR), ref2(NR);
  std::vector<T> a1(NR);
  for(std::size_t i=0; i!=NR; ++i)
  {
    ref1[i] = T(nt2::expx2(a0[i]));
    ref2[i] = T(nt2::expx2(a0[i], T(-1)));
    a1[i] = T(-1.0);
  }

  NT2_COVER_ULP_EQUAL(nt2::tag::expx2_, ((vT, a0)), ref1, ulpd);

  std::cout << "Argument samples #0 chosen in range: [" << min << ",  " << max << "]" << std::endl;
  std::cout << "Argument #1 is -1" << std::endl;
  NT2_COVER_ULP_EQUAL(nt2::tag::expx2_, ((T, a0))((T, a1)), ref2, ulpd);

}


//          Copyright Maksym V. Bilinets 2015 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt )

#include "tools/trees_generator.h"

#include <dst/binary_tree/algorithm.h>
#include <dst/binary_tree/initializer_tree.h>
#include <dst/binary_tree/write_graphviz.h>

#include <boost/test/unit_test.hpp>

#include <iostream>

BOOST_AUTO_TEST_SUITE(test_binary_tree_initializer_tree)

BOOST_AUTO_TEST_CASE(test_initializer_tree_construction)
{
  // $     |     $
  // $     4     $
  // $    / \    $
  // $   2   5   $
  // $  / \   \  $
  // $ 1   3   6 $
  const dst::binary_tree::initializer_tree<int> tree(
    {{1, 2, 3}, 4, {{}, 5, 6}});

  BOOST_TEST(tree.size() == 6);
  BOOST_TEST(tree.empty() == false);

  BOOST_TEST(*tree.root() == 4);
  BOOST_TEST(*left(tree.root()) == 2);
  BOOST_TEST(*right(tree.root()) == 5);
  BOOST_TEST(*left(left(tree.root())) == 1);
  BOOST_TEST(*right(left(tree.root())) == 3);
  BOOST_TEST((left(right(tree.root())) == tree.nil()));
  BOOST_TEST(*right(right(tree.root())) == 6);

  BOOST_TEST((left(left(left(tree.root()))) == tree.nil()));
  BOOST_TEST((right(left(left(tree.root()))) == tree.nil()));
}

BOOST_AUTO_TEST_CASE(test_generate_fibonacci_tree)
{
  // $            5       $
  // $          /   \     $
  // $        4       3   $
  // $      /   \    / \  $
  // $     3     2  2   1 $
  // $    / \   /  /      $
  // $   2   1 1  1       $
  // $  /                 $
  // $ 1                  $
  const dst::binary_tree::initializer_tree<int> fibonacci_tree(
    {{{{1, 2, {}}, 3, 1}, 4, {1, 2, {}}}, 5, {{1, 2, {}}, 3, 1}});

  const auto generated_tree = dst_test::generate_fibonacci_tree(5);

  const auto is_fibonacci_tree =
    topologically_equal(generated_tree.root(), fibonacci_tree.root());

  if (!is_fibonacci_tree)
  {
    std::cout << "Not a Fibonacci tree:" << std::endl;
    dst::binary_tree::write_graphviz(std::cout, generated_tree.root());
  }

  BOOST_TEST(is_fibonacci_tree);
}

BOOST_AUTO_TEST_SUITE_END()


/*!
 * @file
 * This file contains unit tests for `boost::mpl::sequence_facade`.
 */

#include <boost/mpl/sequence_facade.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/type_traits/is_same.hpp>

using namespace boost;

template <typename ...T>
struct forward_sequence : mpl::sequence_facade {
    struct begin : mpl::begin<typename mpl::vector<T...>::type> { };
    struct end : mpl::end<typename mpl::vector<T...>::type> { };
};

static_assert(mpl::size<forward_sequence<int, float>>::value == 2, "");

static_assert(is_same<
    mpl::front<forward_sequence<int, float>>::type, int
>::value, "");

static_assert(!mpl::empty<forward_sequence<int, float>>::value, "");
static_assert(mpl::empty<forward_sequence<>>::value, "");

int main() { }