/*
Copyright 2016-2017 Robotics and Biology Lab, TU Berlin. 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.

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

The views and conclusions contained in the software and documentation are those of the authors and should not be interpreted as representing official policies, either expressed or implied, of the FreeBSD Project.
*/

#include <ecto_pcl/ecto_pcl.hpp>
#include <ecto_pcl/pcl_cell.hpp>
#include <ecto_pcl/pcl_cell_with_normals.hpp>

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/common/io.h>
#include <pcl/common/angles.h>
#include <pcl/common/centroid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/eigen.h>

#include <pcl_conversions/pcl_conversions.h>

#include <ros/ros.h>
#include <ros/console.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <pregrasp_msgs/GraspStrategyArray.h>

#include <Eigen/Geometry>

using namespace ecto;

namespace ecto_rbo_grasping
{

typedef Eigen::Matrix<float, 4, 1, Eigen::DontAlign> UnalignedVector4f;
typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> UnalignedVector3f;
typedef Eigen::Matrix<float, 2, 1, Eigen::DontAlign> UnalignedVector2f;
typedef Eigen::Transform<float,3,Eigen::Affine,Eigen::DontAlign> UnalignedAffine3f;

struct SlidingSeeds
{
    spore< UnalignedAffine3f > transformation_;

    spore<std::vector< UnalignedVector3f > > seed_lines_1_;
    spore<std::vector< UnalignedVector3f > > seed_lines_2_;
    spore<std::vector< UnalignedVector3f > > seed_directions_;

    static void declare_params(ecto::tendrils& params)
    {
    }

    static void declare_io(const tendrils& params, tendrils& inputs, tendrils& outputs)
    {
        inputs.declare< UnalignedAffine3f >("transformation", "Bounding Box frame.");

        outputs.declare<std::vector< UnalignedVector3f > >("seed_lines_1", "Starting points for pushing.");
        outputs.declare<std::vector< UnalignedVector3f > >("seed_lines_2", "Starting points for pushing.");
        outputs.declare<std::vector< UnalignedVector3f > >("seed_directions", "Directions for pushing.");
    }

    void configure(const tendrils& params, const tendrils& inputs, const tendrils& outputs)
    {
        transformation_ = inputs["transformation"];

        seed_lines_1_ = outputs["seed_lines_1"];
        seed_lines_2_ = outputs["seed_lines_2"];
        seed_directions_ = outputs["seed_directions"];

    }

    int process(const tendrils& inputs, const tendrils& outputs)
    {
        seed_lines_1_->clear();
        seed_lines_2_->clear();
        seed_directions_->clear();

        // assume bounding box and generate slides towards four faces
        UnalignedVector3f corner_1 = transformation_->translation() + 0.05 * transformation_->rotation().col(2) + 0.05 * transformation_->rotation().col(1);
        UnalignedVector3f corner_2 = transformation_->translation() + 0.05 * transformation_->rotation().col(2) - 0.05 * transformation_->rotation().col(1);
        UnalignedVector3f corner_3 = transformation_->translation() - 0.05 * transformation_->rotation().col(2) + 0.05 * transformation_->rotation().col(1);
        UnalignedVector3f corner_4 = transformation_->translation() - 0.05 * transformation_->rotation().col(2) - 0.05 * transformation_->rotation().col(1);

        seed_lines_1_->push_back(corner_1);
        seed_lines_2_->push_back(corner_2);
        seed_lines_1_->push_back(corner_3);
        seed_lines_2_->push_back(corner_4);
        seed_lines_1_->push_back(corner_1);
        seed_lines_2_->push_back(corner_3);
        seed_lines_1_->push_back(corner_2);
        seed_lines_2_->push_back(corner_4);

        seed_directions_->push_back(transformation_->rotation().col(2));
        seed_directions_->push_back(-(transformation_->rotation().col(2)));
        seed_directions_->push_back(transformation_->rotation().col(1));
        seed_directions_->push_back(-(transformation_->rotation().col(1)));

        return OK;
    }
};

}

ECTO_CELL(ecto_rbo_grasping, ecto_rbo_grasping::SlidingSeeds, "SlidingSeeds", "Finding pushing seeds.");


#include <algorithm>
#include <functional>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include "bus_network.hpp"
#include "time.hpp"

#include <iostream>

inline DifTime adjust(const RouteId& rida, const RouteId& ridb) {
    if (rida == ridb || rida == walkingRouteId) {
        return DifTime{0};
    }
    return std::chrono::minutes{5};
}

BusNetwork::BusNetwork(Lines&& lines): lines_{std::move(lines)}, graph_{}, stopMap_{} {
    auto    stops = lines_.getStopSet();
    for(const auto& stop: stops) {
        stopMap_[stop] = boost::add_vertex(stop, graph_);
//        std::clog << "vertex " << stopMap_[stop] << ": " << stop << std::endl;
    }
    auto    stepsLines = lines_.getBackwardStepsLines();
    for (const auto& stepsByLine: stepsLines) {
        const auto& linen = stepsByLine.first;
        for (const auto& stepsByRoute: stepsByLine.second) {
            const auto& routen = stepsByRoute.first;
            for (const auto& step: stepsByRoute.second) {
                const auto& from = step.first;
                const auto& to = step.second;
                /*auto    e =*/ boost::add_edge(
                    stopMap_.at(from),
                    stopMap_.at(to),
                    Section{RouteId{linen, routen}, from, to}, graph_);
//                std::clog << "edge " << e.first << ": " << from << " -> " << to << std::endl;
            }
        }
    }
    for (const auto& walkingTime: lines_.walkingTimes()) {
        const auto& from = walkingTime.first.first;
        const auto& to = walkingTime.first.second;
        boost::add_edge(
            stopMap_.at(from), stopMap_.at(to), Section{walkingRouteId, from, to}, graph_);
        boost::add_edge(
            stopMap_.at(to), stopMap_.at(from), Section{walkingRouteId, to, from}, graph_);
    }
}

BusNetwork::NodeList BusNetwork::planFromArrive(
    Day day, const Stop& from, const Stop& to, Time arrive, Details details) {

    BusNetwork::NodeList    rv;

    using DistanceMap = std::map<VertexDesc, StopTime>;
    using PredecessorMap = std::map<VertexDesc, VertexDesc>;
    using WeightMap = std::map<EdgeDesc, SectionTime>;

    PredecessorMap  p;
    DistanceMap     d;
    WeightMap       w;

    auto    compare = [](const StopTime& stopta, const StopTime& stoptb) {
//        std::clog << "\t" << stopta.routeid.linen << "." << stopta.routeid.routen << "\t";
//        std::clog << toString(stopta.time) << "\t";
//        std::clog << "\t" << stoptb.routeid.linen << "." << stoptb.routeid.routen << "\t";
//        std::clog << toString(stoptb.time) << "\t";
//        std::clog << (stopta.time > stoptb.time + adjust(stopta.routeid, stoptb.routeid));
//        std::clog << std::endl;

        return stopta.time > stoptb.time + adjust(stopta.routeid, stoptb.routeid);
    };
    auto    combine = [](const StopTime& stopt, const SectionTime& sectiont) {
        Time    toTime = stopt.time - adjust(stopt.routeid, sectiont.routeid);
        Time    fromTime = minusInf;
        if (sectiont.routeid == walkingRouteId) {
            fromTime = toTime - sectiont.diftime;
        } else {
            auto    fromIt = std::lower_bound(
                sectiont.stopTimes.crbegin(),
                sectiont.stopTimes.crend(),
                toTime,
                std::greater<Time>{});
            if (fromIt != sectiont.stopTimes.crend()) {
                fromTime = *fromIt;
            }
        }
//        std::clog << stopt.routeid.linen << "." << stopt.routeid.routen << "\t";
//        std::clog << toString(stopt.time) << "\t";
//        std::clog << sectiont.routeid.linen << "." << sectiont.routeid.routen << "\t";
//        std::clog << toString(fromTime) << std::endl;

        return StopTime{sectiont.routeid, fromTime};
    };

    auto    edger = boost::edges(graph_);
    std::for_each(edger.first, edger.second, [this, &w, day](const EdgeDesc& ed) {
        const auto& section = graph_[ed];
        if (section.routeid == walkingRouteId) {
            w[ed] = SectionTime{
                section.routeid,
                lines_.walkingTimes().at(WalkingStep{section.from, section.to})};
        } else {
            w[ed] = SectionTime{
                section.routeid,
                lines_.getStopTimes(day, section.routeid, section.to)};
        }
//        std::clog << "weight " << ed << ": ";
//        std::clog << toString(*w[ed].stopTimes.cbegin());
//        std::clog << " ~ " << toString(*(w[ed].stopTimes.cend() - 1));
//        std::clog << std::endl;
    });

    auto    u = stopMap_[to];
    boost::dijkstra_shortest_paths(
        graph_,
        u,
        boost::predecessor_map(boost::associative_property_map<PredecessorMap>(p)).
            distance_map(boost::associative_property_map<DistanceMap>(d)).
            weight_map(boost::associative_property_map<WeightMap>(w)).
            distance_compare(compare).
            distance_combine(combine).
            distance_zero(StopTime{{}, arrive}).
            distance_inf(StopTime{{}, minusInf}));

    auto    edge_list = [this](VertexDesc u, VertexDesc v) {
        std::vector<EdgeDesc>   rv;
        auto                    er = boost::out_edges(u, graph_);
        auto&                   g = graph_;
        std::for_each(er.first, er.second, [&rv, v, &g](EdgeDesc e) {
            if (boost::target(e, g) == v) {
                rv.push_back(e);
            }
        });
        return rv;
    };

    auto    v = stopMap_[from];
    auto    dv = d.at(v);
    Stop    stop = from;
    Time    time = dv.time;
    auto    pred = p[v];
    while (pred != v && v != u) {
        auto    dpred = d.at(pred);
//        auto    e = boost::edge(pred, v, graph_);
        auto    el = edge_list(pred, v);
        auto    eit =
            std::min_element(el.cbegin(), el.cend(), [pred, &dpred, &w, &combine, &compare](
                EdgeDesc e1, EdgeDesc e2) {

            return compare(combine(dpred, w[e1]), combine(dpred, w[e2]));
        });
        auto        section = graph_[*eit];
        const auto& to = section.from;
        const auto& routeid = section.routeid;
        rv.push_back(
            Node{
                {stop, time, lines_.getPlatform(routeid, stop)},
                {to, lines_.getArriveTime(day, routeid, stop, time, to), lines_.getPlatform(routeid, to)},
                routeid});
        stop = graph_[pred];
        time = dpred.time;
        v = pred;
        pred = p[v];
    }
//    rv.push_back(Node{RouteId{}, stop, time, {}});

    if (details == Details::transfers) {
        return fromStepToTransferList(rv);
    }
    if (details == Details::ends) {
        return fromStepToEndList(rv);
    }
    return rv;
}

BusNetwork::Table BusNetwork::table(Day day, const Stop& from, const Stop& to, Details details) {
    Table   rv;
    auto    timeline = lines_.getStopTimes(day, to);
    for (const auto& time: timeline) {
        auto    nlist = planFromArrive(day, from, to, time, details);
        if (!nlist.empty()) {
            rv.push_back(nlist);
        }
    }

    std::stable_sort(rv.begin(), rv.end(), [](const NodeList& nl1, const NodeList& nl2) {
        assert(!nl1.empty());
        assert(!nl2.empty());
        assert(nl1.front().from.stop == nl2.front().from.stop);
        assert(nl1.back().to.stop == nl2.back().to.stop);

        return
            nl1.back().to.time < nl2.back().to.time ||
                (nl1.back().to.time == nl2.back().to.time && nl1.front().from.time > nl2.front().from.time);
    });

    rv.erase(std::unique(rv.begin(), rv.end(), [](const NodeList& nl1, const NodeList& nl2) {
        assert(nl1.back().to.stop == nl2.back().to.stop);

        return nl1.back().to.time == nl2.back().to.time || nl1.front().from.time == nl2.front().from.time;
    }), rv.end());
    return rv;
}

std::string BusNetwork::routeName(const RouteId& routeid) const {
    return lines_.getRouteDescription(routeid);
}

BusNetwork::NodeList BusNetwork::fromStepToTransferList(const BusNetwork::NodeList& stepList) {
    if (stepList.empty()) {
        return NodeList{};
    }

    NodeList    rv;
    RouteId     routeid = stepList.front().routeid;
    RoutePoint  from = stepList.front().from;
    RoutePoint  to;
    for (const auto& node: stepList) {
        if (node.routeid != routeid) {
            rv.push_back(Node{from, to, routeid});
            from = node.from;
        }
        routeid = node.routeid;
        to = node.to;
    }
    rv.push_back(Node{from, to, routeid});
    return rv;
}

BusNetwork::NodeList BusNetwork::fromTransferToEndList(const BusNetwork::NodeList& transferList) {
    if (transferList.empty()) {
        return NodeList{};
    }

    return NodeList{Node{transferList.front().from, transferList.back().to, RouteId{}}};
}

BusNetwork::NodeList BusNetwork::fromStepToEndList(const BusNetwork::NodeList& stepList) {
    return fromTransferToEndList(stepList);
}


#include "localization-evaluator/localization-evaluator.h"

#include <Eigen/Core>
#include <aslam/common/statistics/statistics.h>
#include <loop-closure-handler/loop-closure-handler.h>
#include <maplab-common/parallel-process.h>

DEFINE_double(
    benchmark_position_error_threshold, 0.05,
    "Position error threshold in meters to count vertex "
    "as correctly localized.");

namespace localization_evaluator {

bool LocalizationEvaluator::evaluateSingleKeyframe(
    const pose_graph::VertexId& query_vertex_id, Eigen::Vector3d* pnp_p_G_I,
    unsigned int* lc_matches_count, unsigned int* inliers_count,
    double* error_meters, bool* ransac_ok) {
  CHECK_NOTNULL(pnp_p_G_I);
  CHECK_NOTNULL(lc_matches_count);
  CHECK_NOTNULL(inliers_count);
  CHECK_NOTNULL(error_meters);
  CHECK_NOTNULL(ransac_ok);

  statistics::StatsCollector stats_collector_error_successes(
      "LocalizationEvaluator -- successes");
  statistics::StatsCollector stats_collector_error_norm(
      "LocalizationEvaluator -- error norm");
  statistics::StatsCollector stats_collector_matches(
      "LocalizationEvaluator -- matches");
  statistics::StatsCollector stats_collector_ransac_inliers(
      "LocalizationEvaluator -- RANSAC inliers");

  CHECK(map_->hasVertex(query_vertex_id))
      << "Couldn't find map vertex with ID: " << query_vertex_id.hexString();
  const vi_map::Vertex& query_vertex = map_->getVertex(query_vertex_id);

  const bool kMergeLandmarks = false;
  const bool kAddLoopclosureEdges = false;
  vi_map::LoopClosureConstraint inlier_constraints;
  pose::Transformation pnp_T_G_I;
  *ransac_ok = loop_detector_node_.findVertexInDatabase(
      query_vertex, kMergeLandmarks, kAddLoopclosureEdges, map_, &pnp_T_G_I,
      lc_matches_count, &inlier_constraints);
  *inliers_count = inlier_constraints.structure_matches.size();
  *pnp_p_G_I = pnp_T_G_I.getPosition();

  Eigen::Vector3d p_G_I = map_->getVertex_G_p_I(query_vertex_id);
  double position_error = (p_G_I - pnp_T_G_I.getPosition()).norm();

  stats_collector_matches.AddSample(*lc_matches_count);
  stats_collector_ransac_inliers.AddSample(*inliers_count);

  if (*ransac_ok) {
    VLOG(1) << "\t" << pnp_T_G_I.getPosition().transpose() << " vs "
            << p_G_I.transpose() << " --> norm: " << position_error << " [m]";
    stats_collector_error_norm.AddSample(position_error);
    *error_meters = position_error;
  } else {
    LOG(WARNING) << "\tRansac failed:" << *lc_matches_count << " matches, "
                 << *inliers_count << " inliers.";
    *error_meters = std::numeric_limits<double>::infinity();
  }
  if (*ransac_ok && position_error < FLAGS_benchmark_position_error_threshold) {
    stats_collector_error_successes.AddSample(1.0);
    return true;
  } else {
    LOG(WARNING) << "\tCouldn't localize " << query_vertex_id;
    stats_collector_error_successes.AddSample(0.0);
    return false;
  }
}

void LocalizationEvaluator::evaluateMission(
    const vi_map::MissionId& mission_id, MissionEvaluationStats* statistics) {
  CHECK_NOTNULL(statistics);

  pose_graph::VertexIdList vertices;
  map_->getAllVertexIdsInMission(mission_id, &vertices);

  std::vector<char> is_correct;
  std::vector<char> ransac_ok;
  std::vector<unsigned int> inlier_counts;
  std::vector<unsigned int> lc_matches_counts;
  Aligned<std::vector, Eigen::Vector3d> localization_p_G_I;
  is_correct.resize(vertices.size(), false);
  ransac_ok.resize(vertices.size(), false);
  inlier_counts.resize(vertices.size(), 0u);
  lc_matches_counts.resize(vertices.size(), 0u);
  localization_p_G_I.resize(vertices.size(), Eigen::Vector3d::Zero());
  std::vector<double> errors_meters(
      vertices.size(), std::numeric_limits<double>::infinity());

  std::function<void(const std::vector<size_t>&)> pose_query =
      [this, &vertices, &is_correct, &inlier_counts, &lc_matches_counts,
       &localization_p_G_I, &errors_meters,
       &ransac_ok](const std::vector<size_t>& batch) {
        for (size_t item : batch) {
          const pose_graph::VertexId& vertex_id = vertices[item];
          Eigen::Vector3d& pnp_p_G_I = localization_p_G_I[item];
          unsigned int& lc_matches_count = lc_matches_counts[item];
          unsigned int& inliers_count = inlier_counts[item];
          double& error_meters = errors_meters[item];
          bool ransac_ok_item;
          if (evaluateSingleKeyframe(
                  vertex_id, &pnp_p_G_I, &lc_matches_count, &inliers_count,
                  &error_meters, &ransac_ok_item)) {
            CHECK_GE(lc_matches_count, inliers_count);
            is_correct[item] = true;
          }
          ransac_ok[item] = ransac_ok_item;
        }
      };

  constexpr bool kAlwaysParallelize = true;
  const size_t num_threads = common::getNumHardwareThreads();
  common::ParallelProcess(
      is_correct.size(), pose_query, kAlwaysParallelize, num_threads);

  // Copy back all valid results.
  statistics->num_vertices = 0u;
  statistics->successful_localizations = 0u;
  statistics->inliers_counts.reserve(vertices.size());
  statistics->lc_matches_counts.reserve(vertices.size());
  statistics->localization_p_G_I.reserve(vertices.size());
  statistics->num_vertices = 0u;
  statistics->successful_localizations = 0u;
  for (size_t i = 0; i < is_correct.size(); ++i) {
    if (is_correct[i]) {
      statistics->localization_p_G_I.emplace_back(localization_p_G_I[i]);
      ++statistics->successful_localizations;
    } else if (ransac_ok[i]) {
      statistics->bad_localization_p_G_I.emplace_back(localization_p_G_I[i]);
    }
    statistics->inliers_counts.emplace_back(inlier_counts[i]);
    statistics->lc_matches_counts.emplace_back(lc_matches_counts[i]);
    statistics->localization_errors_meters.emplace_back(errors_meters[i]);
    ++statistics->num_vertices;
  }

  if (statistics->num_vertices > 0) {
    VLOG(3) << "Ratio "
            << static_cast<double>(statistics->successful_localizations) /
                   statistics->num_vertices
            << " (" << statistics->successful_localizations << "/"
            << statistics->num_vertices << ")";
  } else {
    LOG(WARNING) << "No vertices in mission: " << mission_id;
  }
}

}  // namespace localization_evaluator


#include <numpy_eigen/boost_python_headers.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/EuclideanExpressionNode.hpp>
#include <aslam/backend/HomogeneousExpression.hpp>
#include <aslam/backend/HomogeneousExpressionNode.hpp>
#include <aslam/backend/TransformationExpression.hpp>
#include <aslam/backend/TransformationExpressionNode.hpp>
#include <aslam/backend/TransformationBasic.hpp>
#include <aslam/backend/RotationQuaternion.hpp>
#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/HomogeneousPoint.hpp>
#include <aslam/backend/DesignVariableMappedVector.hpp>
#include <aslam/backend/DesignVariableVector.hpp>

#include <aslam/backend/MatrixBasic.hpp>
#include <aslam/backend/MatrixExpression.hpp>
#include <aslam/backend/MatrixExpressionNode.hpp>

#include <aslam/backend/VectorExpression.hpp>
#include <aslam/backend/VectorExpressionNode.hpp>

#include <sstream>
#include <boost/shared_ptr.hpp>
#include <aslam/python/ExportBackendExpressions.hpp>
#include <aslam/backend/ScalarExpression.hpp>
#include <aslam/backend/Scalar.hpp>
#include <aslam/backend/EuclideanDirection.hpp>
#include <aslam/backend/VectorExpression.hpp>
//#include <aslam/python/ExportAPrioriInformationError.hpp>

using namespace boost::python;
using namespace aslam::backend;
using namespace aslam::python;

template<int D>
void exportDesignVariableMappedVector()
{
  std::stringstream str;
  str << "DesignVariableMappedVector" << D;
  class_< DesignVariableMappedVector<D>, boost::shared_ptr< DesignVariableMappedVector<D> >, bases< DesignVariable> >(str.str().c_str(), no_init)
    .def("value", &DesignVariableMappedVector<D>::value)
    .def("toExpression", &DesignVariableMappedVector<D>::toExpression)
    ;
}

template<int D>
void exportDesignVariableVector()
{
  std::stringstream str;
  str << "DesignVariableVector" << D;
  class_< DesignVariableVector<D>, boost::shared_ptr< DesignVariableVector<D> >, bases< DesignVariable> >(str.str().c_str(), no_init)
    .def("value", &DesignVariableVector<D>::value, return_value_policy<copy_const_reference>())
    .def("toExpression", &DesignVariableVector<D>::toExpression)
    ;
}

template<int D>
void exportVectorExpression()
{
    std::stringstream str;
    str << "VectorExpression" << D;
   // class_< VectorExpression<D>, boost::shared_ptr<VectorExpression<D> >("VectorExpression", init<boost::shared_ptr<VectorExpressionNode<D> > >() )
   // .def("value", &DesignVariableVector<D>::value, return_value_policy<copy_const_reference>())
   // .def("toExpression", &DesignVariableVector<D>::toExpression)
   // ;

    class_<VectorExpression<D>, boost::shared_ptr<VectorExpression<D> > >("VectorExpression", init<boost::shared_ptr<VectorExpressionNode<D> > >() )
    .def("toValue", &VectorExpression<D>::toValue)
    .def("evaluateJacobians", &evaluateJacobians1<VectorExpression<D> >)
    .def("evaluateJacobians", &evaluateJacobians2<VectorExpression<D> >)
    .def("getDesignVariables", &getDesignVariables<VectorExpression<D> >)
    // Decide later if it is useful to export the expression nodes.
    //.def("root", &EuclideanExpression::root)
    ;

}

void exportBackendExpressions()
{

  class_<EuclideanExpression, boost::shared_ptr<EuclideanExpression> >("EuclideanExpression", init<boost::shared_ptr<EuclideanExpressionNode> >() )
    .def(init<Eigen::Vector3d>())
    .def("toEuclidean", &EuclideanExpression::toEuclidean)
    .def("toHomogeneousExpression", &EuclideanExpression::toHomogeneousExpression)
    .def("evaluateJacobians", &evaluateJacobians1<EuclideanExpression>)
    .def("evaluateJacobians", &evaluateJacobians2<EuclideanExpression>)
    .def("getDesignVariables", &getDesignVariables<EuclideanExpression>)
    .def(self - self)
    .def(self + self)
    .def("cross", &EuclideanExpression::cross)
    .def("elementwiseMultiply", &EuclideanExpression::elementwiseMultiply)
    // Decide later if it is useful to export the expression nodes.
    //.def("root", &EuclideanExpression::root)
    ;

  class_<HomogeneousExpression, boost::shared_ptr<HomogeneousExpression> >("HomogeneousExpression", init<boost::shared_ptr<HomogeneousExpressionNode> >() )
      .def(init<Eigen::Vector4d>())
    .def("toHomogeneous", &HomogeneousExpression::toHomogeneous)
    //.def("toEuclideanExpression", &HomogeneousExpression::toEuclideanExpression)
    .def("evaluateJacobians", &evaluateJacobians1<HomogeneousExpression>)
    .def("evaluateJacobians", &evaluateJacobians2<HomogeneousExpression>)
    .def("getDesignVariables", &getDesignVariables<HomogeneousExpression>)
    // Decide later if it is useful to export the expression nodes.
    //.def("root", &HomogeneousExpression::root)
    ;

  // It is tricky to create expressions. Here we need the types to
  // define the operators so I have to create these here.
  EuclideanPoint euclideanPoint(Eigen::Vector3d::Random());
  EuclideanExpression euclideanExpression = euclideanPoint.toExpression();
  RotationQuaternion rotationQuaternion(Eigen::Vector4d(0,0,0,1));
  RotationExpression rotationExpression = rotationQuaternion.toExpression();
  HomogeneousPoint homogeneousPoint(Eigen::Vector4d::Random());
  HomogeneousExpression homogeneousExpression = homogeneousPoint.toExpression();
  TransformationBasic transformationBasic(rotationExpression, euclideanExpression);
  MatrixBasic matrixBasic(Eigen::Matrix3d::Random());
  MatrixExpression matrixExpression = matrixBasic.toExpression();

  class_<TransformationExpression,boost::shared_ptr<TransformationExpression> >("TransformationExpression", init<boost::shared_ptr<TransformationExpressionNode> >() )
    .def("toTransformationMatrix", &TransformationExpression::toTransformationMatrix)
      .def(init< Eigen::Matrix4d>())
    // These guys are not implemented yet.
    //.def("toRotationExpression", &TransformationExpression::toRotationExpression)
    //.def("toHomogeneousExpression", &TransformationExpression::toHomogeneousExpression)
    //.def("toEuclideanExpression", &TransformationExpression::toEuclideanExpression)
    //.def(self * euclideanExpression)
    .def(self * homogeneousExpression)
    .def(self * self)
    .def("inverse", &TransformationExpression::inverse)

    .def("getDesignVariables", &getDesignVariables<TransformationExpression>)
    ;

  class_<RotationExpression,boost::shared_ptr<RotationExpression> >("RotationExpression", init<boost::shared_ptr<RotationExpressionNode> >() )
    .def("toRotationMatrix", &RotationExpression::toRotationMatrix)
    .def(self * euclideanExpression)
    .def(self * self)
    .def("inverse", &RotationExpression::inverse)
    .def("getDesignVariables", &getDesignVariables<RotationExpression>)
    ;

  // class_<EuclideanPoint, boost::shared_ptr<EuclideanPoint>, bases<DesignVariable> >("EuclideanPointDv", init<const Eigen::Vector3d>())
  //   .def("toExpression", &EuclideanPoint::toExpression)
  //   .def("toEuclidean", &EuclideanPoint::toEuclidean);
  // ;

  class_<RotationQuaternion, boost::shared_ptr<RotationQuaternion>, bases<DesignVariable> >("RotationQuaternionDv", init<const Eigen::Vector4d>())
    .def("toExpression", &RotationQuaternion::toExpression)
    .def("toRotationMatrix", &RotationQuaternion::toRotationMatrix)
    .def("getDesignVariables", &getDesignVariables<RotationQuaternion>)
    ;

  class_<EuclideanPoint, boost::shared_ptr<EuclideanPoint>, bases<DesignVariable> >("EuclideanPointDv", init<const Eigen::Vector3d>())
      .def("toExpression", &EuclideanPoint::toExpression)
      .def("toHomogeneousExpression", &EuclideanPoint::toHomogeneousExpression)
      .def("toEuclidean", &EuclideanPoint::toEuclidean)
      .def("getDesignVariables", &getDesignVariables<EuclideanPoint>)
     ;

  class_<HomogeneousPoint, boost::shared_ptr<HomogeneousPoint>, bases<DesignVariable> >("HomogeneousPointDv", init<const Eigen::Vector4d>())
    .def("toExpression", &HomogeneousPoint::toExpression)
    .def("toHomogeneous", &HomogeneousPoint::toHomogeneous)
  .def("getDesignVariables", &getDesignVariables<HomogeneousPoint>)
     ;

  class_<TransformationBasic, boost::shared_ptr<TransformationBasic>, bases<DesignVariable> >("TransformationBasicDv", init<RotationExpression, EuclideanExpression>())
    .def("toExpression", &TransformationBasic::toExpression)
    .def("toTransformationMatrix", &TransformationBasic::toTransformationMatrix)
  .def("getDesignVariables", &getDesignVariables<TransformationBasic>)
     ;

  class_<MatrixExpression,boost::shared_ptr<MatrixExpression> >("MatrixExpression", init<boost::shared_ptr<MatrixExpressionNode> >() )
    .def("toMatrix3x3", &MatrixExpression::toMatrix3x3)
    .def(self * euclideanExpression)
    .def("getDesignVariables", &getDesignVariables<MatrixExpression>)
    ;
  class_<MatrixBasic, boost::shared_ptr<MatrixBasic>, bases<DesignVariable> >("MatrixBasicDv", init<const Eigen::Matrix3d, const Eigen::Matrix3i>())
      .def("toExpression", &MatrixBasic::toExpression)
      .def("toMatrix3x3", &MatrixBasic::toMatrix3x3)
      .def("getDesignVariables", &getDesignVariables<MatrixBasic>)
      ;

  exportDesignVariableMappedVector<1>();
  exportDesignVariableMappedVector<2>();
  exportDesignVariableMappedVector<3>();
  exportDesignVariableMappedVector<4>();
  exportDesignVariableMappedVector<5>();
  exportDesignVariableMappedVector<6>();
  exportDesignVariableMappedVector<7>();
  exportDesignVariableMappedVector<8>();
  exportDesignVariableMappedVector<9>();
  exportDesignVariableMappedVector<10>();

  exportDesignVariableVector<1>();
  exportDesignVariableVector<2>();
  exportDesignVariableVector<3>();
  exportDesignVariableVector<4>();
  exportDesignVariableVector<5>();
  exportDesignVariableVector<6>();
  exportDesignVariableVector<7>();
  exportDesignVariableVector<8>();
  exportDesignVariableVector<9>();
  exportDesignVariableVector<10>();

    exportVectorExpression<1>();

  class_<ScalarExpression, boost::shared_ptr<ScalarExpression> >("ScalarExpression", init<boost::shared_ptr<ScalarExpressionNode> > () )
      .def(init<double>())
      .def("toScalar", &ScalarExpression::toScalar)
      .def("toValue", &ScalarExpression::toScalar)
      .def("evaluateJacobians", &evaluateJacobians1<ScalarExpression>)
      .def("evaluateJacobians", &evaluateJacobians2<ScalarExpression>)
      .def("getDesignVariables", &getDesignVariables<ScalarExpression>)
      .def(self + self)
      .def(self * self)
      .def(self - self)
      .def(self + double())
      .def(self - double())
      .def(self * double())
      //.def(self + float())
      //.def(self - float())
      //.def(self * float())
      ;

  class_<Scalar, boost::shared_ptr<Scalar>, bases<DesignVariable> >("Scalar", init<double>("Scalar(double value)") )
      .def("toExpression", &Scalar::toExpression)
      .def("toScalar", &Scalar::toScalar)
      .def("toValue", &Scalar::toScalar)
      ;
  // \todo reenable this
  // exportAPrioriInformationError<Scalar>("ScalarAPrioriInformationError");

  class_<EuclideanDirection, boost::shared_ptr<EuclideanDirection>, bases<DesignVariable> >("EuclideanDirection", init< const Eigen::Vector3d & >("EuclideanDirection(Vector3d p)"))
      .def("toExpression", &EuclideanDirection::toExpression)
      .def("toEuclidean", &EuclideanDirection::toEuclidean)
      ;

}


//  win/chrono.cpp  --------------------------------------------------------------//

//  Copyright Beman Dawes 2008
//  Copyright 2009-2010 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

//----------------------------------------------------------------------------//
//                                Windows                                     //
//----------------------------------------------------------------------------//
#ifndef BOOST_CHRONO_DETAIL_INLINED_WIN_CHRONO_HPP
#define BOOST_CHRONO_DETAIL_INLINED_WIN_CHRONO_HPP

#include <boost/detail/win/time.hpp>
#include <boost/detail/win/timers.hpp>
#include <boost/detail/win/GetLastError.hpp>

namespace boost
{
namespace chrono
{
namespace chrono_detail
{

  BOOST_CHRONO_INLINE double get_nanosecs_per_tic() BOOST_NOEXCEPT
  {
      boost::detail::win32::LARGE_INTEGER_ freq;
      if ( !boost::detail::win32::QueryPerformanceFrequency( &freq ) )
          return 0.0L;
      return double(1000000000.0L / freq.QuadPart);
  }

}

  steady_clock::time_point steady_clock::now() BOOST_NOEXCEPT
  {
    static double nanosecs_per_tic = chrono_detail::get_nanosecs_per_tic();

    boost::detail::win32::LARGE_INTEGER_ pcount;
    if ( (nanosecs_per_tic <= 0.0L) ||
            (!boost::detail::win32::QueryPerformanceCounter( &pcount )) )
    {
      BOOST_ASSERT(0 && "Boost::Chrono - Internal Error");
      return steady_clock::time_point();
    }

    return steady_clock::time_point(steady_clock::duration(
      static_cast<steady_clock::rep>((nanosecs_per_tic) * pcount.QuadPart)));
  }

#if !defined BOOST_CHRONO_DONT_PROVIDE_HYBRID_ERROR_HANDLING
  steady_clock::time_point steady_clock::now( system::error_code & ec )
  {
    static double nanosecs_per_tic = chrono_detail::get_nanosecs_per_tic();

    boost::detail::win32::LARGE_INTEGER_ pcount;
    if ( (nanosecs_per_tic <= 0.0L)
            || (!boost::detail::win32::QueryPerformanceCounter( &pcount )) )
    {
        boost::detail::win32::DWORD_ cause =
            ((nanosecs_per_tic <= 0.0L)
                    ? ERROR_NOT_SUPPORTED
                    : boost::detail::win32::GetLastError());
        if (BOOST_CHRONO_IS_THROWS(ec)) {
            boost::throw_exception(
                    system::system_error(
                            cause,
                            BOOST_CHRONO_SYSTEM_CATEGORY,
                            "chrono::steady_clock" ));
        }
        else
        {
            ec.assign( cause, BOOST_CHRONO_SYSTEM_CATEGORY );
            return steady_clock::time_point(duration(0));
        }
    }

    if (!BOOST_CHRONO_IS_THROWS(ec))
    {
        ec.clear();
    }
    return time_point(duration(
      static_cast<steady_clock::rep>(nanosecs_per_tic * pcount.QuadPart)));
  }
#endif

  BOOST_CHRONO_INLINE
  system_clock::time_point system_clock::now() BOOST_NOEXCEPT
  {
    boost::detail::win32::FILETIME_ ft;
  #if defined(UNDER_CE)
    // Windows CE does not define GetSystemTimeAsFileTime so we do it in two steps.
    boost::detail::win32::SYSTEMTIME_ st;
    boost::detail::win32::GetSystemTime( &st );
    boost::detail::win32::SystemTimeToFileTime( &st, &ft );
  #else
    boost::detail::win32::GetSystemTimeAsFileTime( &ft );  // never fails
  #endif
    return system_clock::time_point(
      system_clock::duration(
        ((static_cast<__int64>( ft.dwHighDateTime ) << 32) | ft.dwLowDateTime)
       -116444736000000000LL
      )
    );
  }

#if !defined BOOST_CHRONO_DONT_PROVIDE_HYBRID_ERROR_HANDLING
  BOOST_CHRONO_INLINE
  system_clock::time_point system_clock::now( system::error_code & ec )
  {
    boost::detail::win32::FILETIME_ ft;
  #if defined(UNDER_CE)
    // Windows CE does not define GetSystemTimeAsFileTime so we do it in two steps.
    boost::detail::win32::SYSTEMTIME_ st;
    boost::detail::win32::GetSystemTime( &st );
    boost::detail::win32::SystemTimeToFileTime( &st, &ft );
  #else
    boost::detail::win32::GetSystemTimeAsFileTime( &ft );  // never fails
  #endif
    if (!BOOST_CHRONO_IS_THROWS(ec))
    {
        ec.clear();
    }
    return time_point(duration(
      (static_cast<__int64>( ft.dwHighDateTime ) << 32) | ft.dwLowDateTime));
  }
#endif

  BOOST_CHRONO_INLINE
  std::time_t system_clock::to_time_t(const system_clock::time_point& t) BOOST_NOEXCEPT
  {
      __int64 temp = t.time_since_epoch().count();
      temp /= 10000000;
      return static_cast<std::time_t>( temp );
  }

  BOOST_CHRONO_INLINE
  system_clock::time_point system_clock::from_time_t(std::time_t t) BOOST_NOEXCEPT
  {
      __int64 temp = t;
      temp *= 10000000;
      return time_point(duration(temp));
  }

}  // namespace chrono
}  // namespace boost

#endif


#include "CMRTGadget.h"
#include "cuNFFT.h"
#include "vector_td_utilities.h"
#include "permutationOperator.h"
#include "hoNDArray_utils.h"
#include "hoNDArray_fileio.h"
#include "cuNDArray_utils.h"
#include "cuNDArray_elemwise.h"
#include "cuNDArray_operators.h"
#include "radial_utilities.h"
#include "vector_td_operators.h"
#include "../../toolboxes/nfft/NFFTOperator.h"
#include "multiplicationOperatorContainer.h"
#include "cuCgSolver.h"
#include "cuTvOperator.h"
#include "lbfgsSolver.h"
#include "cuSbcCgSolver.h"
#include "cuPartialDerivativeOperator.h"
#include "cuPartialDerivativeOperator2.h"
#include <numeric>
#include <functional>
#include "cuNlcgSolver.h"
#include <boost/make_shared.hpp>

#include <ismrmrd/xml.h>
#include <cmath>

namespace Gadgetron{

int CMRTGadget::process_config(ACE_Message_Block* mb)
{
	ISMRMRD::IsmrmrdHeader h;
	ISMRMRD::deserialize(mb->rd_ptr(),h);

	if (h.encoding.size() != 1) {
		GDEBUG("This Gadget only supports one encoding space\n");
		return GADGET_FAIL;
	}

	ISMRMRD::EncodingSpace e_space = h.encoding[0].encodedSpace;
	ISMRMRD::EncodingSpace r_space = h.encoding[0].reconSpace;
	ISMRMRD::EncodingLimits e_limits = h.encoding[0].encodingLimits;

	// Matrix size x is the oversampled readout size : size FH*2 -- not for the hyperplarization?
	// Matrix size y is the AP/RL size

	image_space_dimensions_3D_.push_back(e_space.matrixSize.y);
	image_space_dimensions_3D_.push_back(e_space.matrixSize.y);
	image_space_dimensions_3D_.push_back(e_space.matrixSize.x/*/2*/);

	GDEBUG("Matrix size: %d, %d, %d\n",
			image_space_dimensions_3D_[0],
			image_space_dimensions_3D_[1],
			image_space_dimensions_3D_[2] );

	GDEBUG("Matrix size: %d, %d\n", e_space.matrixSize.x, e_space.matrixSize.y, e_space.matrixSize.z);
	dimensions_.push_back(r_space.matrixSize.x);
	dimensions_.push_back(r_space.matrixSize.y);

	field_of_view_.push_back(e_space.fieldOfView_mm.x);
	field_of_view_.push_back(e_space.fieldOfView_mm.y);
	GDEBUG("FOV: %f, %f\n", r_space.fieldOfView_mm.x, r_space.fieldOfView_mm.y);

	repetitions_ = e_limits.repetition.is_present() ? e_limits.repetition.get().maximum + 1 : 1;
	GDEBUG("#Repetitions: %d\n", repetitions_);

	golden_ratio_ =golden_ratio.value();
	use_TV_ = use_TV.value();
	projections_per_recon_ = projections_per_recon.value();
	iterations_ = iterations.value();

	return GADGET_OK;
}

int CMRTGadget::process(GadgetContainerMessage< ISMRMRD::AcquisitionHeader > *m1,        // header
		GadgetContainerMessage< hoNDArray< std::complex<float> > > *m2,  // data
		GadgetContainerMessage< hoNDArray<float> > *m3 )                 // traj/dcw
{
	// Throw away any noise samples if they have been allowed to pass this far down the chain...
	//

	bool is_noise = m1->getObjectPtr()->isFlagSet(ISMRMRD::ISMRMRD_ACQ_IS_NOISE_MEASUREMENT);
	if (is_noise) {
		m1->release();
		return GADGET_OK;
	}

	// First pass initialization
	//

	if (frame_readout_queue_.empty()) {
		samples_per_readout_ = m1->getObjectPtr()->number_of_samples;
		num_coils_ = m1->getObjectPtr()->active_channels;
		dimensions_.push_back(m1->getObjectPtr()->active_channels);
		dimensions_.push_back(repetitions_);
		num_trajectory_dims_ = m3->getObjectPtr()->get_size(0); // 2 for trajectories only, 3 for both trajectories + dcw
	}

	// Enqueue incoming readouts and trajectories
	//

	frame_readout_queue_.push(std::unique_ptr<ReadoutMessage>(duplicate_array(m2)));

	//Only extract trajectories for first frame. Assume next frames are equal
	if (frames.empty()) frame_traj_queue_.push(std::unique_ptr<TrajectoryMessage>(duplicate_array(m3)));

	// If the last readout for a slice has arrived then perform a reconstruction
	//

	bool is_last_scan_in_repetition =
			m1->getObjectPtr()->isFlagSet(ISMRMRD::ISMRMRD_ACQ_LAST_IN_REPETITION);

	if (is_last_scan_in_repetition) {
		num_frames++;
		GDEBUG("FRAME # %d \n",num_frames);
		// Get samples for frame
		//
		GDEBUG("Extracting samples \n");
		frames.push_back(extract_samples_from_queue(frame_readout_queue_));
		// Get trajectories/dcw for frame - Only for first frame
		//
		if (frames.size() == 1 ){
			extract_trajectory_and_dcw_from_queue(frame_traj_queue_, this->traj, this->dcw);
			GDEBUG("Extracting trajectory \n");
		}

		bool is_last_scan_in_slice= m1->getObjectPtr()->isFlagSet(ISMRMRD::ISMRMRD_ACQ_LAST_IN_SLICE);
		GDEBUG("Last scan in slice %i \n",is_last_scan_in_slice);
		//If we have enough projections, get the show on the road
		if (is_last_scan_in_slice ){
			GDEBUG("Framing %i frames \n",num_frames);

			if (num_frames%projections_per_recon_ != 0) {
				GDEBUG("Number of frames must be divisible by number of projecitons");
				return GADGET_FAIL;
			}
			boost::shared_ptr<cuNDArray<float_complext> > data = get_combined_frames();
			auto data_dims = data->get_dimensions();
			size_t ntimeframes = data_dims->back()/projections_per_recon_;
			data_dims->back() = projections_per_recon_;
			data_dims->push_back(ntimeframes);
			data->reshape(data_dims);
			// Initialize plan
			//
			GDEBUG("Data size: %i %i %i",data->get_size(0),data->get_size(1),data->get_size(2));
			boost::shared_ptr<cuNDArray<floatd2> >cu_traj(new cuNDArray<floatd2>(*traj));

			std::vector<size_t> projection_dims;
			projection_dims.push_back(dimensions_[0]*2);
			projection_dims.push_back(dimensions_[1]);

			projection_dims.push_back(projections_per_recon_);
			projection_dims.push_back(ntimeframes);

			//cuNDArray<float_complext> result(&image_space_dimensions_3D_);
			boost::shared_ptr<CMRTOperator<float> > E(new CMRTOperator<float>);
			E->setup(cu_traj,image_space_dimensions_3D_,projection_dims,0,golden_ratio_);

			auto image_space_dimensions_4D = image_space_dimensions_3D_;
			image_space_dimensions_4D.push_back(ntimeframes);
			E->set_domain_dimensions(&image_space_dimensions_4D);
			E->set_codomain_dimensions(data->get_dimensions().get());

			boost::shared_ptr<cuNDArray<float_complext> > result;
			//cuCgSolver<float_complext> solver;
			//cuNlcgSolver<float_complext> solver;

			if (use_TV_){
				cuSbcCgSolver<float_complext> solver;
				solver.set_encoding_operator(E);
				//solver.set_max_iterations(20);
				solver.set_max_outer_iterations(iterations_);
				solver.get_inner_solver()->set_max_iterations(10);
				solver.set_tc_tolerance(1e-8f);
				auto Rx1_ = boost::make_shared< cuPartialDerivativeOperator<float_complext,4> >(0);

				auto Ry1_ = boost::make_shared< cuPartialDerivativeOperator<float_complext,4> >(1);
				auto Rz1_ = boost::make_shared< cuPartialDerivativeOperator<float_complext,4> >(2);

				auto Rt1_ = boost::make_shared< cuPartialDerivativeOperator2<float_complext,4> >();

				Rx1_->set_domain_dimensions(&image_space_dimensions_4D);
				Rx1_->set_codomain_dimensions(&image_space_dimensions_4D);

				Ry1_->set_domain_dimensions(&image_space_dimensions_4D);
				Ry1_->set_codomain_dimensions(&image_space_dimensions_4D);

				Rz1_->set_domain_dimensions(&image_space_dimensions_4D);
				Rz1_->set_codomain_dimensions(&image_space_dimensions_4D);

				Rt1_->set_domain_dimensions(&image_space_dimensions_4D);
				Rt1_->set_codomain_dimensions(&image_space_dimensions_4D);
				float lambda = 2000;
				float mu = 1000;
				Rx1_ ->set_weight(lambda);
				Ry1_ ->set_weight(lambda);
				Rz1_ ->set_weight(lambda);
				Rt1_->set_weight(lambda);
				E->set_weight(mu);
				solver.add_regularization_group_operator(Rx1_);
				solver.add_regularization_group_operator(Ry1_);
				solver.add_regularization_group_operator(Rz1_);
				solver.add_regularization_group_operator(Rt1_);
				solver.add_group();

				solver.set_output_mode(cuCgSolver<float_complext>::OUTPUT_VERBOSE);

				result = solver.solve(data.get());
			} else {
				cuCgSolver<float_complext> solver;
				solver.set_encoding_operator(E);
				solver.set_max_iterations(iterations_);
				solver.set_tc_tolerance(1e-8f);
				solver.set_output_mode(cuCgSolver<float_complext>::OUTPUT_VERBOSE);

				result = solver.solve(data.get());
			}

			size_t nelements3d = std::accumulate(image_space_dimensions_3D_.begin(),image_space_dimensions_3D_.end(),1,std::multiplies<size_t>());

			for (size_t i = 0; i < ntimeframes; i++){

				// Define the image header
				//

				GadgetContainerMessage<ISMRMRD::ImageHeader> *cm1 =
						new GadgetContainerMessage<ISMRMRD::ImageHeader>();

				GadgetContainerMessage< hoNDArray< std::complex<float> > > *cm2 =
						new GadgetContainerMessage<hoNDArray< std::complex<float> > >();

				cm1->getObjectPtr()->flags = 0;
				cm1->cont(cm2);

				cm1->getObjectPtr()->field_of_view[0]   = field_of_view_[0];
				cm1->getObjectPtr()->field_of_view[1]   = field_of_view_[1];
				cm1->getObjectPtr()->channels           = num_coils_;
				cm1->getObjectPtr()->repetition         = m1->getObjectPtr()->idx.repetition;

				memcpy(cm1->getObjectPtr()->patient_table_position,
						m1->getObjectPtr()->patient_table_position, sizeof(float)*3);

				cm1->getObjectPtr()->data_type = ISMRMRD::ISMRMRD_CXFLOAT;
				cm1->getObjectPtr()->image_index = 0;
				cm1->getObjectPtr()->image_series_index = 0;

				// std::complex<float> and Gadgetron::complext<float> are binary compatible
				cuNDArray<complext<float> > cuView(image_space_dimensions_3D_,result->get_data_ptr()+i*nelements3d);
				boost::shared_ptr< hoNDArray< complext<float> > > host_result = cuView.to_host();
				*cm2->getObjectPtr() = *((hoNDArray< std::complex<float> >*) host_result.get());

				cm1->getObjectPtr()->matrix_size[0] = image_space_dimensions_3D_[0];
				cm1->getObjectPtr()->matrix_size[1] = image_space_dimensions_3D_[1];
				cm1->getObjectPtr()->matrix_size[2] = image_space_dimensions_3D_[2];
				cm1->getObjectPtr()->channels       = 1;
				cm1->getObjectPtr()->image_index    = i+1;

				if (this->next()->putq(cm1) < 0) {
					GDEBUG("Failed to put result image on to queue\n");
					cm1->release();
					return GADGET_FAIL;
				}
			}

			num_frames = 0;
		}

	}

	m1->release();
	return GADGET_OK;
}

template<class T> GadgetContainerMessage< hoNDArray<T> >*
CMRTGadget::duplicate_array( GadgetContainerMessage< hoNDArray<T> > *array )
{
	GadgetContainerMessage< hoNDArray<T> > *copy = new GadgetContainerMessage< hoNDArray<T> >();
	*(copy->getObjectPtr()) = *(array->getObjectPtr());
	return copy;
}

boost::shared_ptr< hoNDArray<float_complext> >
CMRTGadget::extract_samples_from_queue ( std::queue<std::unique_ptr<ReadoutMessage>> &queue )
{
	unsigned int readouts_buffered = queue.size();

	std::vector<size_t> dims;
	dims.push_back(samples_per_readout_);
	dims.push_back(readouts_buffered);
	dims.push_back(num_coils_);

	boost::shared_ptr< hoNDArray<float_complext> > host_samples(new hoNDArray<float_complext>(dims));

	for (unsigned int p=0; p<readouts_buffered; p++) {

            ReadoutMessage *mbq = queue.front().release();
            queue.pop();

            GadgetContainerMessage< hoNDArray< std::complex<float> > > *daq = AsContainerMessage<hoNDArray< std::complex<float> > >(mbq);

            if (!daq) {
                    GDEBUG("Unable to interpret data on message queue\n");
                    throw std::runtime_error("CMRTGadget::extract_samples_from_queue: failed to interpret data");
            }

            for (unsigned int c = 0; c < num_coils_; c++) {

                    float_complext *data_ptr = host_samples->get_data_ptr();
                    data_ptr += c*samples_per_readout_*readouts_buffered+p*samples_per_readout_;

                    std::complex<float> *r_ptr = daq->getObjectPtr()->get_data_ptr();
                    r_ptr += c*daq->getObjectPtr()->get_size(0);

                    memcpy(data_ptr, r_ptr, samples_per_readout_*sizeof(float_complext));
            }

            mbq->release();
	}

	return host_samples;
}

boost::shared_ptr< hoNDArray<float> >
CMRTGadget::extract_trajectory_from_queue ( std::queue<std::unique_ptr<TrajectoryMessage>> &queue )
{
	unsigned int readouts_buffered = queue.size();

	std::vector<size_t> dims;
	dims.push_back(num_trajectory_dims_); // 2 for trajectories only, 3 for both trajectories + dcw
	dims.push_back(samples_per_readout_);
	dims.push_back(readouts_buffered);

	boost::shared_ptr< hoNDArray<float> > host_traj(new hoNDArray<float>(dims));

	for (unsigned int p=0; p<readouts_buffered; p++) {

            TrajectoryMessage *mbq = queue.front().release();
            queue.pop();

            GadgetContainerMessage< hoNDArray<float> > *daq = AsContainerMessage<hoNDArray<float> >(mbq);

            if (!daq) {
                    GDEBUG("Unable to interpret data on message queue\n");
                    throw std::runtime_error("CMRTGadget::extract_trajectory_from_queue: failed to interpret data");
            }

            float *data_ptr = host_traj->get_data_ptr();
            data_ptr += num_trajectory_dims_*samples_per_readout_*p;

            float *r_ptr = daq->getObjectPtr()->get_data_ptr();

            memcpy(data_ptr, r_ptr, num_trajectory_dims_*samples_per_readout_*sizeof(float));

            mbq->release();
	}

	return host_traj;
}

void CMRTGadget::extract_trajectory_and_dcw_from_queue
( std::queue<std::unique_ptr<TrajectoryMessage>> &queue, boost::shared_ptr< hoNDArray<floatd2> > & traj, boost::shared_ptr< hoNDArray<float> > & dcw )
{
	// Extract trajectory and (if present) density compensation weights.
	// They are stored as a float array of dimensions: {2,3} x #samples_per_readout x #readouts.
	// We need
	// - a floatd2 trajectory array
	// - a float dcw array
	//

	if( num_trajectory_dims_ == 2 ){
		//This is an evil, evil hack to get the trajectories out. Ohh the horror.
		boost::shared_ptr<hoNDArray<float> > tmp_traj = extract_trajectory_from_queue( queue );
		std::vector<size_t> dims_1d; dims_1d.push_back(tmp_traj->get_size(1)*tmp_traj->get_size(2));
		traj = boost::shared_ptr<hoNDArray<floatd2> >(new hoNDArray<floatd2>(dims_1d));
		memcpy(traj->get_data_ptr(),tmp_traj->get_data_ptr(),tmp_traj->get_number_of_elements()*sizeof(float));

	}
	else{

		boost::shared_ptr< hoNDArray<float> > host_traj_dcw = extract_trajectory_from_queue( queue );

		std::vector<size_t> order;
		order.push_back(1); order.push_back(2); order.push_back(0);

		auto host_traj_dcw_shifted = permute( *host_traj_dcw, order );

		std::vector<size_t> dims_1d;
		dims_1d.push_back(host_traj_dcw_shifted.get_size(0)*host_traj_dcw_shifted.get_size(1));

		dcw = boost::shared_ptr<hoNDArray<float> > (new hoNDArray<float>(dims_1d, host_traj_dcw_shifted.get_data_ptr()+2*dims_1d[0]));

		std::vector<size_t> dims_2d = dims_1d; dims_2d.push_back(2);
		order.clear(); order.push_back(1); order.push_back(0);

		hoNDArray<float> tmp(dims_2d, host_traj_dcw_shifted.get_data_ptr());

		auto _traj = permute( tmp, order );

		traj = boost::shared_ptr<hoNDArray<floatd2> > (new hoNDArray<floatd2>(dims_1d, (floatd2*)_traj.get_data_ptr()));
	}

	std::vector<size_t >dims_2d;
	dims_2d.push_back(traj->get_number_of_elements());
	dims_2d.push_back(1); // Number of frames

	traj->reshape(&dims_2d);
	if( num_trajectory_dims_ == 3 ) dcw->reshape(&dims_2d);
}

boost::shared_ptr<cuNDArray<float_complext> > CMRTGadget::get_combined_frames(){
    if (frames.size() == 0)
            throw std::runtime_error("No frames received. This should not be possible. Your RAM might be replaced with live salmon, or you may have set the expected number of frames to 0");

    for (unsigned int i = 1; i < frames.size(); i++){
            if (!frames[0]->dimensions_equal(frames[i].get()))
                    throw std::runtime_error("CMRTGadget: Frames received do not have equal size");
    }
    //Get data dimensions. Assume all frames have the same dimensions
    std::vector<size_t> dims = *frames[0]->get_dimensions();
    dims.push_back(frames.size());

    boost::shared_ptr<cuNDArray<float_complext> > combined(new cuNDArray<float_complext>(dims));

    //Copy data into 1 array on device
    size_t offset = 0;
    for (unsigned int i = 0; i < frames.size(); i++){
            cudaMemcpy(combined->get_data_ptr()+offset,frames[i]->get_data_ptr(),frames[i]->get_number_of_elements()*sizeof(float_complext),cudaMemcpyHostToDevice);
            offset += frames[i]->get_number_of_elements();
    }

    frames.clear();
    return combined;
}

GADGET_FACTORY_DECLARE(CMRTGadget)
}


#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <cassert>
#include <algorithm>
#include <numeric>
#include <boost/align/aligned_allocator.hpp>
#ifdef USE_TBB
#include <tbb/tbb.h>
#endif
#include "massociate/migrate.hpp"
#include "massociate/migrationParameters.hpp"
#include "massociate/pick.hpp"
#include "massociate/waveformIdentifier.hpp"
#include "private/analyticSignals.hpp"
#include "private/dbscan.hpp"

#define ALIGNMENT 64
using namespace MAssociate;

namespace
{
/// @brief Computes the matrix padding length so that we get bit aligned memory.
/// @param[in] n              The array length.  This must be positive.
/// @param[in] precisionSize  The size of the precision, ex: sizeof(float) = 4.
/// @param[in] alignment      The byte alignment.  This should be a power of 2.
/// @result The padded array length.  This will be greater than or equal to n.
int padLength(const int n,
              const size_t precisionSize = sizeof(double),
              const int alignment = ALIGNMENT)
{
    auto size = static_cast<int> (precisionSize);
    int padLength = 0;
    auto xmod = (n*size)%alignment;
    if (xmod != 0){padLength = (alignment - xmod)/size;}
    auto nPointsPadded = n + padLength;
    return nPointsPadded;
}

/*!
 * @brief Defines the table name, e.g., NN.SS.PP
 * @param network   The network name.
 * @param station   The station name.
 * @param phase     The phase name.
 * @return The travel time table name.
 */
std::string makeTableName(const std::string &network,
                          const std::string &station,
                          const std::string &phase)
{
    auto result = network + "." + station + "." + phase;
    return result;
}

struct PickTablePair
{
    /// The travel IDs of the first and second table.
    std::pair<int, int> tableIDs;
    /// The index of the first and second pick in mPicks.
    std::pair<int, int> pickIndices;
};
/// @brief Makes the travel time pairs.
/// @param ids   Travel time table IDs.
/// @return A list of picks to migrate.
template<class T>
std::vector<struct PickTablePair> makePairs(const std::vector<T> &ids)
{
    auto n = static_cast<int> (ids.size());
    auto nPairs = (n*(n - 1))/2;
    std::vector<PickTablePair> pairs;
    PickTablePair pair;
    pairs.reserve(nPairs);
    for (int i = 0; i < n; ++i)
    {
        auto id = ids[i];
        for (int j = i+1; j < n; ++j)
        {
            auto jd = ids[j];
            // For example, a station may get two subsequent P wave triggers.
            // We don't want to migrate that pair since those picks cannot
            // be associated.
            if (id != jd)
            {
                pair.tableIDs = std::make_pair(id, jd);
                pair.pickIndices = std::make_pair(i, j);
                pairs.push_back(pair);
            }
        }
    }
    return pairs;
}

std::vector<struct PickTablePair>
    makePairs(const std::vector<MAssociate::Pick> &picks,
              const std::vector<int> &ids)
{
    auto n = static_cast<int> (ids.size());
    auto nPairs = (n*(n - 1))/2;
    std::vector<PickTablePair> pairs;
    PickTablePair pair;
    pairs.reserve(nPairs);
    for (int i = 0; i < n; ++i)
    {
        auto id = ids[i];
        auto iOnsetTime = picks[i].getTime();
        auto iPhase = picks[i].getPhaseName();
        auto iSNCL = picks[i].getWaveformIdentifier();
        for (int j = i+1; j < n; ++j)
        {
            auto jd = ids[j];
            auto jOnsetTime = picks[j].getTime();
            auto jPhase = picks[j].getPhaseName();
            auto jSNCL = picks[j].getWaveformIdentifier();
            // Causality checks:
            //  (1) id == jd means that the station/phase match.  In this case,
            //            the picks cannot be associated.
            if (id == jd){continue;}
            // An S on a station wave cannot show up prior to a P wave
            if (iSNCL.getNetwork() == jSNCL.getNetwork() &&
                iSNCL.getStation() == jSNCL.getStation())
            {
                if (iPhase == jPhase){continue;} // Should be handled by id==jd
                // P arrives after S
                if (iPhase == "P" && jPhase == "S")
                {
                    if (iOnsetTime >= jOnsetTime){continue;}
                }
                // S arrives before P
                if (iPhase == "S" && jPhase == "P")
                {
                    if (iOnsetTime <= jOnsetTime){continue;}
                }
            }
            pair.tableIDs = std::make_pair(id, jd);
            pair.pickIndices = std::make_pair(i, j);
            pairs.push_back(pair);
        }
    }
    return pairs;
}

/*!
 * @brief Argument sort in descending order.
 * @param[in] v  The values to argument sort.
 * @result A permutation array such that v[perm[:]] is in descending order.
 */
/*
template <typename T>
std::vector<int> argSortDescending(const std::vector<T> &v)
{
    // initialize original index locations
    std::vector<int> perm(v.size());
    std::iota(perm.begin(), perm.end(), 0);
    // Sort indexes based on comparing values in v using std::stable_sort
    // instead of std::sort to avoid unnecessary index re-orderings
    // when v contains elements of equal values
    std::stable_sort(perm.begin(), perm.end(),
                     [&v](const int i1, const int i2)
                     {
                          return v[i1] > v[i2];
                     });
    return perm;
}
*/

}

template<class T>
class Migrate<T>::MigrateImpl
{
public:
    [[nodiscard]] size_t getTableIndex(const size_t tile,
                                       const size_t tableID) const
    {
        auto result
           = tile*static_cast<size_t> (mNumberOfTables)
                 *static_cast<size_t> (mPaddedTileSize)
           + tableID*static_cast<size_t> (mPaddedTileSize);
        return result;
    }
    [[nodiscard]] int getTableIdentifier(const std::string &network,
                                         const std::string &station,
                                         const std::string &phase) const
    {
        auto name = makeTableName(network, station, phase);
        auto it = std::find(mTableNames.begin(),mTableNames.end(), name);
        if (it == mTableNames.end()){return -1;}
        auto id = static_cast<int> (std::distance(mTableNames.begin(), it));
        return id;
    }
    /// Holds the travel time tables.  This has dimension:
    /// [mNumberOfTiles x mNumberOfTables x mPaddedTileSize]
    [[maybe_unused]]
    std::vector<T, boost::alignment::aligned_allocator<T, 64>> mTables;
    [[maybe_unused]]
    std::vector<T, boost::alignment::aligned_allocator<T, 64>> mImage;
    [[maybe_unused]] std::vector<std::string> mTableNames;
    /// Contains the pick information and table identifier
    [[maybe_unused]] std::vector<MAssociate::Pick> mPicks;
    // For the ia'th pick this returns the corresponding table identifier.
    [[maybe_unused]] std::vector<int> mPickTableIdentifier;
    // The ia'th's picks contribution to the maximum objective function.
    [[maybe_unused]] std::vector<double> mContribution;
    // The travel time from the location of the objective function's
    // maximum to the ia'th pick.
    [[maybe_unused]] std::vector<double> mTravelTimesToMaximum;
    // Defines the connectivity between picks
    std::vector<double> mSimilarityMatrix;
    std::vector<std::pair<int, int>> mAdjacency;
    std::vector<double> mEdgeWeights;
    MigrationParameters mParameters;
    T mMaxDifferentialTravelTime =-1;
    [[maybe_unused]] int mNumberOfTiles = 0;
    [[maybe_unused]] int mNumberOfTables = 0;
    [[maybe_unused]] int mTileSize = 0;
    [[maybe_unused]] int mPaddedTileSize = 0;
    [[maybe_unused]] int mMaxIndex =-1;
    [[maybe_unused]] bool mHaveImage = false;
    [[maybe_unused]] bool mInitialized = false;
};

/// Constructor
template<class T>
Migrate<T>::Migrate() :
    pImpl(std::make_unique<MigrateImpl> ())
{
}

/// Copy c'tor
template<class T>
[[maybe_unused]]
Migrate<T>::Migrate(const Migrate &migrate)
{
    *this = migrate;
}

/// Move c'tor
template<class T>
[[maybe_unused]]
Migrate<T>::Migrate(Migrate &&migrate) noexcept
{
    *this = std::move(migrate);
}

/// Copy assignment
template<class T>
Migrate<T>& Migrate<T>::operator=(const Migrate<T> &migrate)
{
    if (&migrate == this){return *this;}
    pImpl = std::make_unique<MigrateImpl> (*migrate.pImpl);
    return *this;
}

/// Move assignment
template<class T>
Migrate<T>& Migrate<T>::operator=(Migrate<T> &&migrate) noexcept
{
    if (&migrate == this){return *this;}
    pImpl = std::move(migrate.pImpl);
    return *this;
}

template<class T>
void Migrate<T>::clear() noexcept
{
    pImpl->mParameters.clear();
    pImpl->mTables.clear();
    pImpl->mImage.clear();
    pImpl->mTableNames.clear();
    pImpl->mPicks.clear();
    pImpl->mPickTableIdentifier.clear();
    pImpl->mSimilarityMatrix.clear();
    pImpl->mAdjacency.clear();
    pImpl->mEdgeWeights.clear();
    pImpl->mTravelTimesToMaximum.clear();
    pImpl->mContribution.clear();
    pImpl->mMaxDifferentialTravelTime =-1;
    pImpl->mNumberOfTiles = 0;
    pImpl->mNumberOfTables = 0;
    pImpl->mTileSize = 0;
    pImpl->mPaddedTileSize = 0;
    pImpl->mMaxIndex =-1;
    pImpl->mHaveImage = false;
    pImpl->mInitialized = false;
}

/// Destructor
template<class T>
Migrate<T>::~Migrate() = default;

template<class T>
void Migrate<T>::initialize(const MigrationParameters &parameters)
{
    // Reset the class
    clear();
    // Check the inputs
    if (!parameters.haveNumberOfTravelTimeTables())
    {
        throw std::invalid_argument("Number of tables not set");
    }
    if (!parameters.haveNumberOfPointsInTravelTimeTable())
    {
        throw std::invalid_argument("Number of points in table not set");
    }
    // Determine how much space to allocate
    auto nPoints = parameters.getNumberOfPointsInTravelTimeTable(); // Throws
    auto nTables = parameters.getNumberOfTravelTimeTables(); // Throws
    auto tileSize = parameters.getTileSize();
    pImpl->mNumberOfTables = nTables;
    pImpl->mPaddedTileSize = padLength(tileSize, sizeof(T), ALIGNMENT);
    pImpl->mTileSize = tileSize;
    pImpl->mNumberOfTiles
        = static_cast<int> (std::ceil(static_cast<double> (nPoints)
                                     /static_cast<double> (tileSize)));
    if (pImpl->mTileSize*pImpl->mNumberOfTiles < nPoints)
    {
        throw std::runtime_error("Algorithm failure");
    }
    auto workSpace = static_cast<size_t> (pImpl->mNumberOfTiles)
                    *static_cast<size_t> (pImpl->mNumberOfTables)
                    *static_cast<size_t> (pImpl->mPaddedTileSize);
    pImpl->mTables.resize(workSpace, 0);
    pImpl->mImage.resize(tileSize*pImpl->mNumberOfTiles, 0);
    pImpl->mTableNames.reserve(pImpl->mNumberOfTables);
    // Checks out
    pImpl->mParameters = parameters;
    pImpl->mInitialized = true;
}

/// Sets a travel time table
template<class T>
template<typename U>
[[maybe_unused]]
void Migrate<T>::setTravelTimeTable(const std::string &network,
                                    const std::string &station,
                                    const std::string &phase,
                                    const int nPoints, const U *times)
{
    auto nPts = getNumberOfPointsInTravelTimeTable(); //Throws if uninitialized
    pImpl->mMaxDifferentialTravelTime =-1;
    pImpl->mHaveImage = false;
    if (nPts != nPoints)
    {
        throw std::invalid_argument("nPoints = " + std::to_string(nPoints)
                                 + " must equal " + std::to_string(nPts));
    }
    if (times == nullptr){throw std::invalid_argument("Times is NULL");}
    if (network.empty()){throw std::invalid_argument("Network is empty");}
    if (station.empty()){throw std::invalid_argument("Station is empty");}
    if (phase.empty()){throw std::invalid_argument("Phase is empty");}
    auto minTime = std::min_element(times, times + nPoints);
    if (*minTime < 0)
    {
        throw std::invalid_argument("Negative travel time detected");
    }
    // Figure out the tableID
    int tableID = pImpl->getTableIdentifier(network, station, phase);
    auto tableName = makeTableName(network, station, phase);
    //
    if (tableID >= 0)
    {
        std::cerr << "Overwriting table: " << tableName << std::endl;
    }
    else
    {
        if (haveAllTravelTimeTables())
        {
            auto mTables
                = static_cast<size_t>(
                     pImpl->mParameters.getNumberOfTravelTimeTables());
            throw std::invalid_argument("All tables set - max is: "
                                       + std::to_string(mTables) );
        }
        pImpl->mTableNames.push_back(tableName);
        tableID = pImpl->getTableIdentifier(network, station, phase);
    }
    // Copy the table tile by tile
    auto numberOfTiles = pImpl->mNumberOfTiles;
    auto tileSize = pImpl->mTileSize;
    for (int tile=0; tile<numberOfTiles; ++tile)
    {
        // Source
        auto it1 = tile*tileSize;
        auto it2 = std::min(nPts, it1 + tileSize);
        // Destination
        auto jt1 = pImpl->getTableIndex(tile, tableID);
        T *tPtr __attribute__((aligned(64))) = pImpl->mTables.data() + jt1;
        std::copy(times + it1, times + it2, tPtr);
    }
}

/// Gets a travel time
template<class T>
T Migrate<T>::getTravelTime(const std::string &network,
                            const std::string &station,
                            const std::string &phase, const int index) const
{
    auto nPts = getNumberOfPointsInTravelTimeTable(); //Throws if uninitialized
    if (!haveTravelTimeTable(network, station, phase))
    {
        throw std::invalid_argument("Table does not exist for: "
                                  + network + "," + station + "," + phase);
    }
    if (index < 0 || index >= nPts)
    {
        throw std::invalid_argument("index = " + std::to_string(index)
                                  + " must be in the range [0,"
                                  + std::to_string(nPts - 1) + "]");
    }
    auto tableID = pImpl->getTableIdentifier(network, station, phase);
    int tile = index/pImpl->mTileSize;
    int gridIndexInTile = index - tile*pImpl->mTileSize;
    assert(tile*pImpl->mTileSize + gridIndexInTile == index);
    auto it = pImpl->getTableIndex(tile, tableID);
    T tEst = pImpl->mTables.at(it + gridIndexInTile);
    return tEst;
}

/// Gets a specific travel time table
template<class T>
std::vector<T> Migrate<T>::getTravelTimeTable(const std::string &network,
                                              const std::string &station,
                                              const std::string &phase) const
{
    auto nPts = getNumberOfPointsInTravelTimeTable(); //Throws if uninitialized
    if (!haveTravelTimeTable(network, station, phase))
    {
        throw std::invalid_argument("Table does not exist for: "
                                  + network + "," + station + "," + phase);
    }
    // Copy table tile by tile
    auto tableID = pImpl->getTableIdentifier(network, station, phase);
    auto numberOfTiles = pImpl->mNumberOfTiles;
    auto tileSize = pImpl->mTileSize;
    std::vector<T> times(nPts);
    for (int tile=0; tile<numberOfTiles; ++tile)
    {
        // Source
        auto it1 = pImpl->getTableIndex(tile, tableID);
        // Destination
        auto jt1 = tile*tileSize;
        auto jt2 = std::min(nPts, jt1 + tileSize);
        auto nCopy = jt2 - jt1;
        T *tPtr __attribute__((aligned(64))) = pImpl->mTables.data() + it1;
        std::copy(tPtr, tPtr + nCopy,  times.data() + jt1);
    }
    return times;
}

/// Have the travel time table?
template<class T>
bool Migrate<T>::haveTravelTimeTable(const std::string &network,
                                     const std::string &station,
                                     const std::string &phase) const noexcept
{
    if (!isInitialized()){return false;}
    auto id = pImpl->getTableIdentifier(network, station, phase);
    return (id >= 0);
}

/// Get the current max differential travel time
template<class T>
T Migrate<T>::getMaximumDifferentialTravelTime() const noexcept
{
    T dtMax = 0;
    if (!isInitialized()){return dtMax;}
    auto nPts = getNumberOfPointsInTravelTimeTable();
    if (pImpl->mMaxDifferentialTravelTime >= 0)
    {
        return pImpl->mMaxDifferentialTravelTime;
    }
    auto nTables = static_cast<int> (pImpl->mTableNames.size());
    if (nTables < 2){return dtMax;} // Can't compute anything
    auto numberOfTiles = pImpl->mNumberOfTiles;
    auto tileSize = pImpl->mTileSize;
    auto mTables = pImpl->mTables.data();
    for (int tile=0; tile<numberOfTiles; ++tile)
    {
        auto jt1 = tile*tileSize;
        auto jt2 = std::min(nPts, jt1 + tileSize);
        auto nLocal = jt2 - jt1;
        for (int it=0; it<nTables; ++it)
        {
            for (int jt=it+1; jt<nTables; ++jt)
            {
                // Source
                auto it1 = pImpl->getTableIndex(tile, it);
                auto it2 = pImpl->getTableIndex(tile, jt);
                // Destination
                const T *tPtr1 __attribute__((aligned(64))) = mTables + it1;
                const T *tPtr2 __attribute__((aligned(64))) = mTables + it2;
                #pragma omp simd reduction(max:dtMax) aligned(tPtr1, tPtr2: 64)
                for (int i=0; i<nLocal; ++i)
                {
                    dtMax = std::max(dtMax, std::abs(tPtr1[i] - tPtr2[i]));
                }
            }
        }
    }
    pImpl->mMaxDifferentialTravelTime = dtMax;
    return pImpl->mMaxDifferentialTravelTime;
}

/// Have all the tables?
template<class T>
bool Migrate<T>::haveAllTravelTimeTables() const noexcept
{
    if (!isInitialized()){return false;}
    return (static_cast<int> (pImpl->mTableNames.size()) ==
            pImpl->mNumberOfTables);
}

/// Picks
template<class T>
void Migrate<T>::addPick(const Pick &newPick)
{
    if (!isInitialized()){throw std::runtime_error("Class not initialized");}
    pImpl->mHaveImage = false;
    // Check pick has a corresponding travel time table and pick time
    auto newSNCL = newPick.getWaveformIdentifier();
    auto networkNew = newSNCL.getNetwork();
    auto stationNew = newSNCL.getStation();
    auto phaseNew = newPick.getPhaseName();
    auto id = pImpl->getTableIdentifier(networkNew, stationNew, phaseNew);
    if (id < 0)
    {
        auto errmsg = "Travel time table for: "
                    + makeTableName(networkNew, stationNew, phaseNew)
                    + " does not exist";
        throw std::invalid_argument(errmsg);
    }
    if (!newPick.haveTime())
    {
        throw std::invalid_argument("Pick does not have time");
    }
    pImpl->mPicks.push_back(newPick);
    pImpl->mPickTableIdentifier.push_back(id);
}

template<class T>
void Migrate<T>::clearPicks() noexcept
{
    pImpl->mPicks.clear();
    pImpl->mPickTableIdentifier.clear();
}

template<class T>
int Migrate<T>::getNumberOfPicks() const noexcept
{
    return static_cast<int> (pImpl->mPicks.size());
}

/// Do the hard work and migrate
template<class T>
void Migrate<T>::migrate()
{
    const T sqrt12 = 3.464101615137754587054892683011744733;
    if (!isInitialized())
    { throw std::runtime_error("Class not initialized"); }
    // Null out result
    pImpl->mHaveImage = false;
    pImpl->mMaxIndex =-1;
    std::fill(pImpl->mImage.begin(), pImpl->mImage.end(), 0);
    auto nPicks = getNumberOfPicks();
    if (nPicks < 2)
    {
        throw std::runtime_error("At least two picks required");
    }
    std::vector<int> tableIDs(nPicks);
    for (int ia=0; ia<nPicks; ++ia)
    {
        tableIDs[ia] = pImpl->mPickTableIdentifier[ia];
    }
    // Figure out the travel time table index pairs
    auto pickTablePairs = makePairs(pImpl->mPicks,
                                       pImpl->mPickTableIdentifier); //tableIDs);
    auto nPairs = static_cast<int> (pickTablePairs.size());
    if (nPairs < 1)
    {
        std::cerr << "No pick pairs to migrate" << std::endl;
        return;
    }
    // Get some other constants needed for the loop
    auto method = pImpl->mParameters.getAnalyticCorrelationFunction();
    auto nPts = getNumberOfPointsInTravelTimeTable();
    auto tileSize = pImpl->mParameters.getTileSize();
    auto numberOfTiles = pImpl->mNumberOfTiles;
    const T *mTables = pImpl->mTables.data();
    // Loop on the tiles.  This prevents us from having each thread allocate
    // space for an image which would have to be reduced.
    #pragma omp parallel for \
     shared(pickTablePairs, method, mTables, numberOfTiles) \
     shared(nPicks, nPairs, nPts, tileSize) \
     default(none)
    for (int tile = 0; tile < numberOfTiles; ++tile)
    {
        // Destination
        auto jt1 = tile*tileSize;
        auto jt2 = std::min(nPts, jt1 + tileSize);
        auto nLocal = jt2 - jt1;
        auto imageLocal = pImpl->mImage.data() + jt1;
        // Loop on travel times pairs to migrate
        for (int iPair=0; iPair<nPairs; ++iPair)
        {
            auto t1 = pickTablePairs[iPair].tableIDs.first;
            auto t2 = pickTablePairs[iPair].tableIDs.second;
            auto ip1 = pickTablePairs[iPair].pickIndices.first;
            auto ip2 = pickTablePairs[iPair].pickIndices.second;
            T p1 = pImpl->mPicks[ip1].getTime();
            T std1 = pImpl->mPicks[ip1].getStandardDeviation();
            T tCorr1 = pImpl->mPicks[ip1].getStaticCorrection();
            T p2 = pImpl->mPicks[ip2].getTime();
            T std2 = pImpl->mPicks[ip2].getStandardDeviation();
            T tCorr2 = pImpl->mPicks[ip2].getStaticCorrection();
            // Source
            auto it1 = pImpl->getTableIndex(tile, t1);
            auto it2 = pImpl->getTableIndex(tile, t2);
            const T *tPtr1 __attribute__((aligned(64))) = mTables + it1;
            const T *tPtr2 __attribute__((aligned(64))) = mTables + it2;
            // Perform Gaussian migration
            if (method == AnalyticCorrelationFunction::GAUSSIAN)
            {
                T normalizeAmp
                    = analyticGaussianCorrelationAmplitudeNormalization(std1, std2);
                T normalizeExp
                    = analyticGaussianCorrelationExponentNormalization(std1, std2);
                #pragma omp simd aligned(tPtr1, tPtr2 : 64)
                for (int i = 0; i < nLocal; ++i)
                {
                    auto deltaT = (tPtr2[i] + tCorr2)
                                - (tPtr1[i] + tCorr1);
                    imageLocal[i] = imageLocal[i]
                        + analyticGaussianCorrelation(deltaT, p1, p2,
                                                      normalizeAmp,
                                                      normalizeExp);
                     //if (jt1 + i == 13873){std::cout << deltaT << "," << p2 - p1 << std::endl;}
                } // Loop on points in tile
            }
            else
            {
                T w1 = sqrt12*std1;
                T w2 = sqrt12*std2;
                //std::cout << w1 << "," << w2 << std::endl;
                #pragma omp simd aligned(tPtr1, tPtr2 : 64)
                for (int i = 0; i < nLocal; ++i)
                {
                    auto deltaT = (tPtr2[i] + tCorr2)
                                - (tPtr1[i] + tCorr1);
                    imageLocal[i] = imageLocal[i]
                        + analyticBoxcarCorrelation(deltaT, p1, p2, w1, w2);
                     //if (jt1 + i == 13873){std::cout << deltaT << "," << p2 - p1 << std::endl;}
//if (jt1 +i == 13873){std::cout << analyticBoxcarCorrelation(deltaT, p1, p2, w1, w2) << "," << w1 << ", " << w2 << "," << deltaT << "," << p2 - p1 << std::endl;}
                } // Loop on points in tile
            }
        } // Loop on pairs
    } // Loop on tiles
    // Get the maximum point
    auto xMax = std::max_element(pImpl->mImage.begin(), pImpl->mImage.end());
    int maxIndex = std::distance(pImpl->mImage.begin(), xMax);
    //std::cout << maxIndex << "," << *xMax << std::endl;
    int tile = maxIndex/tileSize;
    int optimumGridInTile = maxIndex - tile*tileSize;
    assert(tile*tileSize + optimumGridInTile == maxIndex);
    // Figure out how much each pick contributed to the max
    std::vector<T> contribution(nPicks, 0);
    std::vector<double> travelTime(nPicks, 0);
    std::vector<bool> haveTravelTime(nPicks, false);
    pImpl->mSimilarityMatrix.resize(nPicks*nPicks, 0);
    pImpl->mAdjacency.resize(nPairs);
    pImpl->mEdgeWeights.resize(nPairs, 0);
    T optSum = 0;
    T half = 0.5;
    for (int iPair=0; iPair<nPairs; ++iPair)
    {
        auto t1 = pickTablePairs[iPair].tableIDs.first;
        auto t2 = pickTablePairs[iPair].tableIDs.second;
        auto ip1 = pickTablePairs[iPair].pickIndices.first;
        auto ip2 = pickTablePairs[iPair].pickIndices.second;
        T p1 = pImpl->mPicks[ip1].getTime();
        T std1 = pImpl->mPicks[ip1].getStandardDeviation();
        T tCorr1 = pImpl->mPicks[ip1].getStaticCorrection();
        T p2 = pImpl->mPicks[ip2].getTime();
        T std2 = pImpl->mPicks[ip2].getStandardDeviation();
        T tCorr2 = pImpl->mPicks[ip2].getStaticCorrection();
        // Source
        auto it1 = pImpl->getTableIndex(tile, t1);
        auto it2 = pImpl->getTableIndex(tile, t2);
        T tEst1 = mTables[it1 + optimumGridInTile] + tCorr1;
        T tEst2 = mTables[it2 + optimumGridInTile] + tCorr2;
        auto deltaT = tEst2 - tEst1;
        travelTime[ip1] = tEst1;
        travelTime[ip2] = tEst2;
        haveTravelTime[ip1] = true;
        haveTravelTime[ip2] = true;
        T pdf = 0;
        if (method == AnalyticCorrelationFunction::GAUSSIAN)
        {
            T normalizeAmp
               = analyticGaussianCorrelationAmplitudeNormalization(std1, std2);
            T normalizeExp
               = analyticGaussianCorrelationExponentNormalization(std1, std2);
            pdf = analyticGaussianCorrelation(deltaT, p1, p2,
                                              normalizeAmp,
                                              normalizeExp);
            contribution[ip1] = contribution[ip1] + pdf;
            contribution[ip2] = contribution[ip2] + pdf;
        }
        else
        {
            T w1 = sqrt12*std1;
            T w2 = sqrt12*std2;
            pdf = analyticBoxcarCorrelation(deltaT, p1, p2, w1, w2);
        }
        pImpl->mAdjacency[iPair] = std::make_pair(ip1, ip2);
        pImpl->mEdgeWeights[iPair] = pdf;
        // Both pick 1 and pick 2 contribute equally.
        // This ensures that sum(contributions) = value in mImage[maxIndex]
        auto indx = ip1*nPicks + ip2;
        auto jndx = ip2*nPicks + ip1;
        pImpl->mSimilarityMatrix[indx] = pdf;
        pImpl->mSimilarityMatrix[jndx] = pdf;
        contribution[ip1] = contribution[ip1] + half*pdf;
        contribution[ip2] = contribution[ip2] + half*pdf;
        optSum = optSum + pdf;
    }
    pImpl->mContribution.resize(contribution.size());
    std::copy(contribution.begin(), contribution.end(),
              pImpl->mContribution.begin());
    pImpl->mTravelTimesToMaximum = travelTime;
    // Create the main diagonal
    for (int ip=0; ip<nPicks; ++ip)
    {
        T std1 = pImpl->mPicks[ip].getStandardDeviation();
        T pdf = 0;
        T zero = 0;
        if (method == AnalyticCorrelationFunction::GAUSSIAN)
        {
            T normalizeAmp
               = analyticGaussianCorrelationAmplitudeNormalization(std1, std1);
            T normalizeExp
               = analyticGaussianCorrelationExponentNormalization(std1, std1);
            pdf = analyticGaussianCorrelation(zero, zero, zero,
                                              normalizeAmp, normalizeExp);
        }
        else
        {
            T w1 = sqrt12*std1;
            pdf = analyticBoxcarCorrelation(zero, zero, zero, w1, w1);
        }
        auto indx = ip*nPicks + ip;
        //std::cout << pdf << ","  << indx << std::endl;
        pImpl->mSimilarityMatrix[indx] = pdf;
    }

    // Normalize and sort into descending order
    if (optSum == 0){optSum = 1;}
    for (auto &c : contribution){c = c/optSum;}
/*
    auto perm = argSortDescending(contribution);
    // Perform some basic clustering
    std::vector<double> originTimes;
    std::vector<double> weights;
    originTimes.reserve(nPicks);
    weights.reserve(nPicks);
    for (int ia=0; ia<nPicks; ++ia)
    {
        if (haveTravelTime[ia])
        {
            originTimes.push_back(pImpl->mPicks[ia].getTime()
                                - travelTime[ia]);
            weights.push_back(std::max(std::numeric_limits<T>::epsilon(),
                                       contribution[ia]));
        }
    }
    // Cluster
    DBSCAN dbscan;
    dbscan.initialize(pImpl->mParameters.getDBSCANEpsilon(),
                      pImpl->mParameters.getDBSCANMinimumClusterSize());
    dbscan.setWeightedData(originTimes.size(), 1,
                           originTimes.data(), weights.data());
    dbscan.cluster();
    auto nClusters = dbscan.getNumberOfClusters();
    if (nClusters > 0)
    {
        auto labels = dbscan.getLabels();
        if (nClusters == 1)
        {
            for (int ia=0; ia<nPicks; ++ia)
            {
                if (labels[ia] >-1)
                {
                }
            }
        }
        else
        {
            auto uniqueLabels = labels;
            std::sort(uniqueLabels.begin(), uniqueLabels.end());
            auto it = std::unique(uniqueLabels.begin(), uniqueLabels.end());
            uniqueLabels.resize(std::distance(uniqueLabels.begin(), it));
       }
    }
for (auto &ot : originTimes){std::cout << ot << std::endl;}
std::cout << contribution[perm[0]] << std::endl;
std::cout << nClusters << "," << optSum << std::endl;
*/
    pImpl->mMaxIndex = maxIndex;
    pImpl->mHaveImage = true;
}

/// Gets the max index in the image
template<class T>
std::pair<int, T> Migrate<T>::getImageMaximum() const
{
    if (!haveImage())
    {
        throw std::invalid_argument("Migration image not yet computed");
    }
    return std::pair(pImpl->mMaxIndex, pImpl->mImage[pImpl->mMaxIndex]);
}

/// Gets the travel times to all picks from the maximum index
template<class T>
std::vector<double> Migrate<T>::getTravelTimesToMaximum() const
{
    if (!haveImage()){throw std::runtime_error("Image not yet computed");}
    return pImpl->mTravelTimesToMaximum;
}

/// Gets the picks contribution to maximum index
template<class T>
std::vector<double>
    Migrate<T>::getContributionToMaximum(const bool normalize) const
{
    if (!haveImage()){throw std::runtime_error("Image not yet computed");}
    if (normalize)
    {
        const double zero = 0;
        std::vector<double> result(pImpl->mContribution.size(), zero);
        auto xnorm = std::accumulate(pImpl->mContribution.begin(),
                                     pImpl->mContribution.end(), zero);
        if (xnorm > 0)
        {
            xnorm = 1./xnorm;
            std::transform(pImpl->mContribution.begin(),
                           pImpl->mContribution.end(),
                           result.begin(),
                           [xnorm](const double x) -> double
                           {
                               return x*xnorm;
                           });
        }
        return result;
    }
    return pImpl->mContribution;
}

/// Migration image computed?
template<class T>
bool Migrate<T>::haveImage() const noexcept
{
    return pImpl->mHaveImage;
}

/// Initialized?
template<class T>
bool Migrate<T>::isInitialized() const noexcept
{
    return pImpl->mInitialized;
}

template<class T>
int Migrate<T>::getNumberOfPointsInTravelTimeTable() const
{
    if (!isInitialized()){throw std::runtime_error("Class not initialized");}
    return pImpl->mParameters.getNumberOfPointsInTravelTimeTable();
}

///--------------------------------------------------------------------------///
///                         Instantiate the Templates                        ///
///--------------------------------------------------------------------------///
template class MAssociate::Migrate<double>;
template class MAssociate::Migrate<float>;

template void MAssociate::Migrate<double>::setTravelTimeTable(
    const std::string &network, const std::string &station,
    const std::string &phase, int nPoints, const double times[]);
template void MAssociate::Migrate<double>::setTravelTimeTable(
    const std::string &network, const std::string &station,
    const std::string &phase, int nPoints, const float times[]);

template void MAssociate::Migrate<float>::setTravelTimeTable(
    const std::string &network, const std::string &station,
    const std::string &phase, int nPoints, const double times[]);
template void MAssociate::Migrate<float>::setTravelTimeTable(
    const std::string &network, const std::string &station,
    const std::string &phase, int nPoints, const float times[]);