#include <pcl_conversions/pcl_conversions.h>
#include <dynamic_reconfigure/server.h>
#include <ar_track_alvar/ParamsConfig.h>

#include <deque>
#include "ar_track_alvar/CvTestbed.h"
#include "ar_track_alvar/MarkerDetector.h"
#include "ar_track_alvar/MultiMarkerBundle.h"
#include "ar_track_alvar/MultiMarkerInitializer.h"
#include "ar_track_alvar/Shared.h"
#include <cv_bridge/cv_bridge.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/registration.h>

#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/image_encodings.h>
#include <ros/ros.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/filters/extract_indices.h>
#include <boost/lexical_cast.hpp>

#include <tf/tf.h>
#include <Eigen/Core>
#include <ar_track_alvar/filter/kinect_filtering.h>
#include <ar_track_alvar/filter/medianFilter.h>
#include <fstream>
#include <iostream>

#define MAIN_MARKER 1
#define VISIBLE_MARKER 2
#define GHOST_MARKER 3

namespace gm=geometry_msgs;
namespace ata=ar_track_alvar;

typedef pcl::PointXYZRGB ARPoint;
typedef pcl::PointCloud<ARPoint> ARCloud;

using namespace alvar;
using namespace std;
using boost::make_shared;

Camera *kinectCam;
Camera *calibratedCam;
cv_bridge::CvImagePtr cv_ptr_;
image_transport::Subscriber kinectSub_;
image_transport::Subscriber calibratedSub_;
ros::Subscriber cloud_sub_;
ros::Publisher rvizMarkerPub_;
ros::Publisher rvizMarkerPub2_;
tf::TransformListener *tf_listener;
tf::TransformBroadcaster *tf_broadcaster;
MarkerDetector<MarkerData> marker_detector;
MarkerDetector<MarkerData> kinect_marker_detector;
MultiMarkerBundle **multi_marker_bundles=NULL;

Pose *bundlePoses;
Pose *kinectBundlePoses;
int *master_id;
int *bundles_seen;
bool *master_visible;
std::vector<int> *bundle_indices;
bool init = true;

ata::MedianFilter **med_filts;
int med_filt_size;

std::string launchFileName;
double marker_size;
double max_new_marker_error;
double max_track_error;
std::string kinectImageTopic;
std::string kinectInfoTopic;
std::string kinectBaseLinkFrameID;
std::string output_frame;

std::string calibratedImageTopic;
std::string calibratedInfoTopic;
std::string calibratedFrameID;
int n_bundles = 0;

std::deque<tf::Transform> kinectTransforms;
tf::Transform kinectTransform;
std::deque<std::deque<tf::Transform> > camTransforms;
std::vector<tf::Transform> camTransform;

bool isCornerNaN(const ARPoint &point)
{
  return isnan(point.x) || isnan(point.y) || isnan(point.z);
}

int *getTwoValidPoints(const ARCloud &corners_3D, int *dir1, int *dir2)
{
  if(isCornerNaN(corners_3D[dir1[0]]) || isCornerNaN(corners_3D[dir1[1]]))
  {
    if(isCornerNaN(corners_3D[dir2[0]]) || isCornerNaN(corners_3D[dir2[1]]))
    {
      return NULL;
    }
    else{
      return dir2;
    }
  }
  else{
    return dir1;
  }
}

int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, ARCloud::Ptr selected_points, const ARCloud &cloud, Pose &p){

  ata::PlaneFitResult res = ata::fitPlane(selected_points);
  gm::PoseStamped pose;
  pose.header.stamp = pcl_conversions::fromPCL(cloud.header).stamp;
  pose.header.frame_id = cloud.header.frame_id;
  pose.pose.position = ata::centroid(*res.inliers);

  //Get 2 points that point forward in marker x direction
  int *xPoints = getTwoValidPoints(corners_3D, new int[2]{0,3}, new int[2]{1,2});
  int *yPoints = getTwoValidPoints(corners_3D, new int[2]{1,0}, new int[2]{2,3});

  if (!xPoints || !yPoints)
  {
    return -1;
  }
  int i1 = xPoints[0], i2 = xPoints[1];
  int i3 = yPoints[0], i4 = yPoints[1];

  int succ =
    ata::extractOrientation(res.coeffs, corners_3D[i1], corners_3D[i2], corners_3D[i3], corners_3D[i4], pose.pose.orientation);
  if(succ < 0) return -1;

  tf::Matrix3x3 mat;
  succ = ata::extractFrame(res.coeffs, corners_3D[i1], corners_3D[i2], corners_3D[i3], corners_3D[i4], mat);
  if(succ < 0) return -1;

  p.translation[0] = pose.pose.position.x * 100.0;
  p.translation[1] = pose.pose.position.y * 100.0;
  p.translation[2] = pose.pose.position.z * 100.0;
  p.quaternion[1] = pose.pose.orientation.x;
  p.quaternion[2] = pose.pose.orientation.y;
  p.quaternion[3] = pose.pose.orientation.z;
  p.quaternion[0] = pose.pose.orientation.w;

  return 0;
}

void inferCorners()
{
  //For each master tag, infer the 3D position of its corners from other visible tags
    //Then, do a plane fit to those new corners
  ARCloud inferred_corners;
  for(int i=0; i<n_bundles; i++)
  {
    if(bundles_seen[i] > 0){
      for (size_t j=0; j<marker_detector.markers->size(); j++){
        Marker *m = &((*marker_detector.markers)[j]);
          if(m->GetId() == master_id[i])
            bundlePoses[i] = m->pose;
      }

      if(med_filt_size > 0)
      {
          Pose ret_pose;
          med_filts[i]->addPose(bundlePoses[i]);
          med_filts[i]->getMedian(ret_pose);
          kinectBundlePoses[i] = ret_pose;
      }
    }
  }
}

// Updates the bundlePoses of the multi_marker_bundles by detecting markers and using all markers in a bundle to infer the master tag's position
void GetMultiMarkerPoses(IplImage *image) {

  if (marker_detector.Detect(image, calibratedCam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)){
    for(int i=0; i<n_bundles; i++)
      multi_marker_bundles[i]->Update(marker_detector.markers, calibratedCam, bundlePoses[i]);

    if(marker_detector.DetectAdditional(image, calibratedCam, false) > 0){
      for(int i=0; i<n_bundles; i++){
  if ((multi_marker_bundles[i]->SetTrackMarkers(marker_detector, calibratedCam, bundlePoses[i], image) > 0))
    multi_marker_bundles[i]->Update(marker_detector.markers, calibratedCam, bundlePoses[i]);
      }
    }
  }
}

// Updates the bundlePoses of the multi_marker_bundles by detecting markers and
// using all markers in a bundle to infer the master tag's position
void GetMultiMarkerPoses(IplImage *image, ARCloud &cloud) {
  //Detect and track the markers
  if (kinect_marker_detector.Detect(image, kinectCam, true, false, max_new_marker_error,
           max_track_error, CVSEQ, true))
  {
    for (size_t i=0; i<kinect_marker_detector.markers->size(); i++)
      {
      vector<cv::Point, Eigen::aligned_allocator<cv::Point> > pixels;
      Marker *m = &((*kinect_marker_detector.markers)[i]);
      int id = m->GetId();

      //Get the 3D inner corner points - more stable than outer corners that can "fall off" object
      int resol = m->GetRes();
      int ori = m->ros_orientation;

      PointDouble pt1, pt2, pt3, pt4;
      pt4 = m->ros_marker_points_img[0];
      pt3 = m->ros_marker_points_img[resol-1];
      pt1 = m->ros_marker_points_img[(resol*resol)-resol];
      pt2 = m->ros_marker_points_img[(resol*resol)-1];

      m->ros_corners_3D[0] = cloud(pt1.x, pt1.y);
      m->ros_corners_3D[1] = cloud(pt2.x, pt2.y);
      m->ros_corners_3D[2] = cloud(pt3.x, pt3.y);
      m->ros_corners_3D[3] = cloud(pt4.x, pt4.y);

      if(ori >= 0 && ori < 4){
        if(ori != 0){
          std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end());
        }
      }
      else
        ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id);

      //Get the 3D marker points
      BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img)
        pixels.push_back(cv::Point(p.x, p.y));
      ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels);

      //Use the kinect data to find a plane and pose for the marker
      int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose);
      m->valid = (ret >= 0);
    }

    inferCorners();
  }
}

void broadcastTransform(bool calibrated, std::string sensorFrame, int id, tf::Transform t)
{
  std::stringstream out;
  out << "ar_marker_" << id;
  std::string markerFrame = out.str();

  std::string parentFrame = (calibrated) ? sensorFrame : markerFrame;
  std::string childFrame = (calibrated) ? markerFrame : sensorFrame;

  tf::StampedTransform camToMarker (t, ros::Time::now(), parentFrame, childFrame);
  tf_broadcaster->sendTransform(camToMarker);
  ROS_INFO_STREAM("Publishing transform from " << parentFrame << " to " << childFrame);
}

tf::Transform getTransformFromPose(Pose &p)
{
  double px,py,pz,qx,qy,qz,qw;

  px = p.translation[0]/100.0;
  py = p.translation[1]/100.0;
  pz = p.translation[2]/100.0;
  qx = p.quaternion[1];
  qy = p.quaternion[2];
  qz = p.quaternion[3];
  qw = p.quaternion[0];

  //Get the marker pose in the camera frame
  tf::Quaternion rotation (qx,qy,qz,qw);
  tf::Vector3 origin (px,py,pz);
  tf::Transform t (rotation, origin);  //transform from cam to marker
  return t;
}

void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker){

  tf::Transform t = getTransformFromPose(p);  //transform from cam to marker

  tf::Vector3 markerOrigin (0, 0, 0);
  tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
  tf::Transform markerPose = t * m;

  //Publish the cam to marker transform for main marker in each bundle
  if(type==MAIN_MARKER){
    std::stringstream out;
    out << "ar_marker_" << id;
    tf::StampedTransform camToMarker (t, ros::Time::now(), image_msg->header.frame_id, out.str().c_str());
    //tf_broadcaster->sendTransform(camToMarker);
  }

  //Create the rviz visualization message
  tf::poseTFToMsg (markerPose, rvizMarker->pose);
  rvizMarker->header.frame_id = image_msg->header.frame_id;
  rvizMarker->header.stamp = image_msg->header.stamp;
  rvizMarker->id = id;

  rvizMarker->scale.x = 1.0 * marker_size/100.0;
  rvizMarker->scale.y = 1.0 * marker_size/100.0;
  rvizMarker->scale.z = 0.2 * marker_size/100.0;

  if(type==MAIN_MARKER)
    rvizMarker->ns = "main_shapes";
  else
    rvizMarker->ns = "basic_shapes";

  rvizMarker->type = visualization_msgs::Marker::CUBE;
  rvizMarker->action = visualization_msgs::Marker::ADD;

  //Determine a color and opacity, based on marker type
  if(type==MAIN_MARKER){
    rvizMarker->color.r = 1.0f;
    rvizMarker->color.g = 0.0f;
    rvizMarker->color.b = 0.0f;
    rvizMarker->color.a = 1.0;
  }
  else if(type==VISIBLE_MARKER){
    rvizMarker->color.r = 0.0f;
    rvizMarker->color.g = 1.0f;
    rvizMarker->color.b = 0.0f;
    rvizMarker->color.a = 0.7;
  }
  else if(type==GHOST_MARKER){
    rvizMarker->color.r = 0.0f;
    rvizMarker->color.g = 0.0f;
    rvizMarker->color.b = 1.0f;
    rvizMarker->color.a = 0.5;
  }

  rvizMarker->lifetime = ros::Duration (1.0);
}

// There has got to be a better way
bool isIdentityTransform(tf::Transform &transform)
{
  tf::Quaternion quat = transform.getRotation();
  tf::Vector3 vec = transform.getOrigin();
  return (quat.x() == 0.0 && quat.y() == 0.0 && quat.z() == 0.0 && quat.w() == 1.0 &&
      vec.x() == 0.0 && vec.y() == 0.0 && vec.z() == 0.0);
}

void addTransformToAverage(tf::Transform incoming, std::deque<tf::Transform> &transforms, tf::Transform &transform)
{
  transforms.push_back(incoming);
  int n = transforms.size();
  if (n <=1)
  {
    transform.setOrigin(incoming.getOrigin());
    transform.setRotation(incoming.getRotation());
    return;
  }

  tf::Quaternion quat = transform.getRotation();
  tf::Vector3 vec = transform.getOrigin();

  if (n > 1000)
  {
    double t = 1.0 / (n - 1);
    tf::Transform outgoing = transforms.front();
    transforms.pop_front();

    tf::Quaternion oQuat = outgoing.getRotation();
    tf::Vector3 oVec = outgoing.getOrigin();

    quat.slerp(oQuat.inverse(), t);
    vec = (1+t) * vec - t * oVec;
    n--;
  }

  double t = 1.0 / (n-1);
  tf::Quaternion iQuat = incoming.getRotation();
  tf::Vector3 iVec = incoming.getOrigin();

  transform.setOrigin((1 - t) * vec + t * iVec);
  transform.setRotation(quat.slerp(iQuat, t));

  tf::Vector3 tVec = transform.getOrigin();
  tf::Quaternion tQuat = transform.getRotation();
}

//Callback to handle getting video frames and processing them
void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
{
  //If no camera info, return
  if(!calibratedCam->getCamInfo_){
    return;
  }

  //Get the transformation from the Camera to the output frame for this image capture
  tf::StampedTransform CamToOutput;
  try{
    ros::Time now = ros::Time::now();
    tf_listener->waitForTransform(output_frame, image_msg->header.frame_id, now, ros::Duration(1.0));
    tf_listener->lookupTransform(output_frame, image_msg->header.frame_id, now, CamToOutput);
  }
  catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
  }

  visualization_msgs::Marker rvizMarker;

  //Convert the image
  try{
    cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception& e){
    ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
  }

  //Get the estimated pose of the main markers by using all the markers in each bundle
  IplImage ipl_image = cv_ptr_->image;
  GetMultiMarkerPoses(&ipl_image);

  if(ipl_image.height != 800 || ipl_image.width != 1280) {
    ROS_ERROR("Wrist camera image is incorrect size! Should be 1280x800. Shutting down.");
    exit(1);
  }

  //Draw the observed markers that are visible and note which bundles have at least 1 marker seen
  for(int i=0; i<n_bundles; i++)
  {
    bundles_seen[i]++;
  }
  for (size_t i=0; i<marker_detector.markers->size(); i++)
  {
    int id = (*(marker_detector.markers))[i].GetId();

    // Draw if id is valid
    if(id >= 0){

      //Mark the bundle that marker belongs to as "seen"
      for(int j=0; j<n_bundles; j++){
        for(int k=0; k<bundle_indices[j].size(); k++){
          if(bundle_indices[j][k] == id){
            bundles_seen[j]++;
            break;
          }
        }
      }

      // Don't draw if it is a master tag...we do this later, a bit differently
      bool should_draw = true;
      for(int j=0; j<n_bundles; j++){
        if(id == master_id[j]) should_draw = false;
      }
      if(should_draw){
        Pose p = (*(marker_detector.markers))[i].pose;
        makeMarkerMsgs(VISIBLE_MARKER, id, p, image_msg, CamToOutput, &rvizMarker);
        rvizMarkerPub_.publish (rvizMarker);
      }
    }
  }

  //Draw the main markers, whether they are visible or not -- but only if at least 1 marker from their bundle is currently seen
  for(int i=0; i<n_bundles; i++)
  {
    if(bundles_seen[i] > 0){
      makeMarkerMsgs(MAIN_MARKER, master_id[i], bundlePoses[i], image_msg, CamToOutput, &rvizMarker);
      rvizMarkerPub_.publish (rvizMarker);

      //Get the pose relative to the camera
      int id = master_id[i];

      tf::Transform t = getTransformFromPose(bundlePoses[i]);
      tf::Vector3 markerOrigin (0, 0, 0);
      tf::Transform m (tf::Quaternion::getIdentity(), markerOrigin);
      tf::Transform markerPose = t * m; // marker pose in the camera frame

      //TODO What's going on here
      if (camTransforms.size() <= i)
      {
          std::deque<tf::Transform> transformDeque;
          camTransforms.push_back(transformDeque);
          tf::Quaternion p (0,0,0,1);
          tf::Vector3 o (0,0,0);
          tf::Transform t (p, o);
          camTransform.push_back(t);
      }
      if (!isIdentityTransform(markerPose))
      {
        addTransformToAverage(markerPose, camTransforms[i], camTransform[i]);
        broadcastTransform(true, calibratedFrameID.c_str(), id, camTransform[i]);
      }
    }
  }
}

//Callback to handle getting kinect point clouds and processing them
void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
{
  //If no camera info, return
  if(!kinectCam->getCamInfo_){
    ROS_WARN_STREAM("Can't get kinect info on topic " << kinectInfoTopic);
    return;
  }

  //Convert cloud to PCL
  ARCloud cloud;
  pcl::fromROSMsg(*msg, cloud);

  //Get an OpenCV image from the cloud
  sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
  pcl::toROSMsg (*msg, *image_msg);
  image_msg->header.stamp = msg->header.stamp;
  image_msg->header.frame_id = msg->header.frame_id;

  //Convert the image
  try
  {
    cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception& e){
    ROS_ERROR ("ar_track_alvar: Image error: %s", image_msg->encoding.c_str ());
  }

  IplImage ipl_image = cv_ptr_->image;
  GetMultiMarkerPoses(&ipl_image, cloud);
  ros::Time now = ros::Time::now();
  for (size_t i=0; i<kinect_marker_detector.markers->size(); i++)
  {
    int id = (*(kinect_marker_detector.markers))[i].GetId();

    bool isMaster = false;
    for(int j=0; j<n_bundles; j++){
      if(id == master_id[j]) isMaster = true;
    }
    if (!isMaster)
    {
      continue;
    }

    Pose p = (*(kinect_marker_detector.markers))[i].pose;

    tf::Transform t = getTransformFromPose(p);

    tf::Vector3 iVec = t.getOrigin();
    tf::Quaternion iQuat = t.getRotation();
    ROS_INFO_STREAM("transform origin: " << iVec.x() << ", " << iVec.y() << ", " << iVec.z() << " orientation: " << iQuat.x() << ", " << iQuat.y() << ", " << iQuat.z() << ", " << iQuat.w());

    tf::Vector3 markerOrigin (0, 0, 0);
    tf::Quaternion quat;
    quat.setRPY(0,0,-3.14159/2.0);
    tf::Transform m (quat, markerOrigin);
    tf::Transform markerPose = t * m; // marker pose in the camera frame

    tf::Transform camBaseTransform;
    tf::Vector3 camBaseTransformOrigin(0, -0.020, 0);
    tf::Quaternion camBaseTransformQuat(-0.500, 0.500, -0.500, 0.500);
    camBaseTransform.setOrigin(camBaseTransformOrigin);
    camBaseTransform.setRotation(camBaseTransformQuat);

    tf::Transform linkToMarker = camBaseTransform * markerPose;
    if (!isIdentityTransform(markerPose))
    {
      addTransformToAverage(linkToMarker.inverse(), kinectTransforms, kinectTransform);
      broadcastTransform(false, kinectBaseLinkFrameID, id, kinectTransform);
    }
  }
}

//Create a ROS frame out of the known corners of a tag in the weird marker coord frame used by Alvar markers (x right y forward z up)
//p0-->p1 should point in Alvar's pos X direction
//p1-->p2 should point in Alvar's pos Y direction
int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1,
                         const CvPoint3D64f& p2, const CvPoint3D64f& p3,
                         tf::Transform &retT)
{
  const tf::Vector3 q0(p0.x, p0.y, p0.z);
  const tf::Vector3 q1(p1.x, p1.y, p1.z);
  const tf::Vector3 q2(p2.x, p2.y, p2.z);
  const tf::Vector3 q3(p3.x, p3.y, p3.z);

  // (inverse) matrix with the given properties
  const tf::Vector3 v = (q1-q0).normalized();
  const tf::Vector3 w = (q2-q1).normalized();
  const tf::Vector3 n = v.cross(w);
  tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
  m = m.inverse();

  //Translate to quaternion
  if(m.determinant() <= 0)
      return -1;

  //Use Eigen for this part instead, because the ROS version of bullet appears to have a bug
  Eigen::Matrix3f eig_m;
  for(int i=0; i<3; i++){
      for(int j=0; j<3; j++){
          eig_m(i,j) = m[i][j];
      }
  }
  Eigen::Quaternion<float> eig_quat(eig_m);

  // Translate back to bullet
  tfScalar ex = eig_quat.x();
  tfScalar ey = eig_quat.y();
  tfScalar ez = eig_quat.z();
  tfScalar ew = eig_quat.w();
  tf::Quaternion quat(ex,ey,ez,ew);
  quat = quat.normalized();

  double qx = (q0.x() + q1.x() + q2.x() + q3.x()) / 4.0;
  double qy = (q0.y() + q1.y() + q2.y() + q3.y()) / 4.0;
  double qz = (q0.z() + q1.z() + q2.z() + q3.z()) / 4.0;
  tf::Vector3 origin (qx,qy,qz);

  tf::Transform tform (quat, origin);  //transform from master to marker
  retT = tform;

  return 0;
}

//Find the coordinates of the Master marker with respect to the coord frame of each of it's child markers
//This data is used for later estimation of the Master marker pose from the child poses
int calcAndSaveMasterCoords(MultiMarkerBundle &master)
{
  int mast_id = master.master_id;
  std::vector<tf::Vector3> rel_corner_coords;

  //Go through all the markers associated with this bundle
  for (size_t i=0; i<master.marker_indices.size(); i++){
    int mark_id = master.marker_indices[i];
    rel_corner_coords.clear();

    //Get the coords of the corners of the child marker in the master frame
    CvPoint3D64f mark_corners[4];
    for(int j=0; j<4; j++){
        mark_corners[j] = master.pointcloud[master.pointcloud_index(mark_id, j)];
    }

    //Use them to find a transform from the master frame to the child frame
    tf::Transform tform;
    makeMasterTransform(mark_corners[0], mark_corners[1], mark_corners[2], mark_corners[3], tform);

    //Finally, find the coords of the corners of the master in the child frame
    for(int j=0; j<4; j++){

      CvPoint3D64f corner_coord = master.pointcloud[master.pointcloud_index(mast_id, j)];
      double px = corner_coord.x;
      double py = corner_coord.y;
      double pz = corner_coord.z;

      tf::Vector3 corner_vec (px, py, pz);
      tf::Vector3 ans = (tform.inverse()) * corner_vec;
      rel_corner_coords.push_back(ans);
    }

    master.rel_corners.push_back(rel_corner_coords);
  }

  return 0;
}

int main(int argc, char *argv[])
{
  ros::init (argc, argv, "marker_detect");
  ros::NodeHandle n;

  if(argc < 9){
    std::cout << std::endl;
    cout << "Not enough arguments provided." << endl;
    cout << "Usage: ./bundle_calibrate <output file> <marker size in cm> <max new marker error> <max track error> <kinect image topic> <kinect info topic> <kinect base link frame ID> <calibrated image topic> <calibrated info topic> <calibrated frame ID> <output frame> <median filt size> <list of bundle XML files...>" << endl;
    std::cout << std::endl;
    return 0;
  }

  // Get params from command line

  launchFileName = argv[1];
  marker_size = atof(argv[2]);
  max_new_marker_error = atof(argv[3]);
  max_track_error = atof(argv[4]);
  kinectImageTopic = argv[5];
  kinectInfoTopic = argv[6];
  kinectBaseLinkFrameID = argv[7];
  calibratedImageTopic = argv[8];
  calibratedInfoTopic = argv[9];
  calibratedFrameID = argv[10];
  output_frame = argv[11];
  med_filt_size = atoi(argv[12]);
  int n_args_before_list = 13;
  n_bundles = argc - n_args_before_list;

  marker_detector.SetMarkerSize(marker_size);
  kinect_marker_detector.SetMarkerSize(marker_size);
  multi_marker_bundles = new MultiMarkerBundle*[n_bundles];
  bundlePoses = new Pose[n_bundles];
  kinectBundlePoses = new Pose[n_bundles];
  master_id = new int[n_bundles];
  bundle_indices = new std::vector<int>[n_bundles];
  bundles_seen = new int[n_bundles];
  master_visible = new bool[n_bundles];

  //Create median filters
  med_filts = new ata::MedianFilter*[n_bundles];
  for(int i=0; i<n_bundles; i++)
    med_filts[i] = new ata::MedianFilter(med_filt_size);

  // Load the marker bundle XML files
  for(int i=0; i<n_bundles; i++){
    kinectBundlePoses[i].Reset();
    bundlePoses[i].Reset();
    MultiMarker loadHelper;
    if(loadHelper.Load(argv[i + n_args_before_list], FILE_FORMAT_XML)){
      vector<int> id_vector = loadHelper.getIndices();
      multi_marker_bundles[i] = new MultiMarkerBundle(id_vector);
      multi_marker_bundles[i]->Load(argv[i + n_args_before_list], FILE_FORMAT_XML);
      master_id[i] = multi_marker_bundles[i]->getMasterId();
      bundle_indices[i] = multi_marker_bundles[i]->getIndices();
      calcAndSaveMasterCoords(*(multi_marker_bundles[i]));
    }
    else{
      cout<<"Cannot load file "<< argv[i + n_args_before_list] << endl;
      return 0;
    }
  }

  // Set up camera, listeners, and broadcasters
  kinectCam = new Camera(n, kinectInfoTopic);
  calibratedCam = new Camera(n, calibratedInfoTopic);
  tf_listener = new tf::TransformListener(n);
  tf_broadcaster = new tf::TransformBroadcaster();
  rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
  rvizMarkerPub2_ = n.advertise < visualization_msgs::Marker > ("visualization_marker_calibrated", 0);

  //Give tf a chance to catch up before the camera callback starts asking for transforms
  ros::Duration(1.0).sleep();
  ros::spinOnce();

  //Subscribe to topics and set up callbacks
  ROS_INFO_STREAM ("Subscribing to kinect image topic '" << kinectImageTopic << "'");
  cloud_sub_ = n.subscribe(kinectImageTopic, 1, &getPointCloudCallback);

  image_transport::ImageTransport it_(n);
//Subscribe to topics and set up callbacks
  ROS_INFO_STREAM ("Subscribing to calibrated image topic '" << calibratedImageTopic << "'");
  calibratedSub_ = it_.subscribe(calibratedImageTopic, 1, &getCapCallback);

  ros::spin();

  return 0;
}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <vector>
#include <string>

#include <random/random.hpp>

#include <sframe/testing_utils.hpp>
#include <util/testing_utils.hpp>
#include <unity/toolkits/ml_data_2/ml_data.hpp>
#include <unity/toolkits/ml_data_2/sframe_index_mapping.hpp>
#include <unity/toolkits/recsys/models.hpp>

using namespace turi;

struct recsys_recommend  {
 public:
  void test_user_item_inclusions() {

    // Build a simple dataset

    sframe obs_data = make_integer_testing_sframe( {"user", "item", "side", "target"},
                                                   { {10,20, 1, 3},
                                                     {10,21, 2, 2},
                                                     {10,22, 3, 1},
                                                     {11,20, 4, 3},
                                                     {11,21, 5, 2},
                                                     {11,25, 6, 1} } );

    auto model = new recsys::recsys_popularity();

    ////////////////////////////////////////////////////////////
    // Set the options

    std::map<std::string, flexible_type> opts;
    opts["item_id"] = "item";
    opts["user_id"] = "user";
    opts["target"] = "target";
    model->init_options(opts);

    ////////////////////////////////////////////////////////////
    // Train the model

    model->setup_and_train(obs_data);

    sframe inc_data = make_integer_testing_sframe( {"user", "item"},
                                                   { {10, 20},
                                                     {11, 20},
                                                     {10, 21},
                                                     {11, 21} } );

    sframe res_back = model->recommend(sframe(), 10, inc_data, sframe(), sframe(), sframe(), sframe(), false);

    std::vector<flex_list> res = testing_extract_sframe_data(res_back);

    ASSERT_EQ(res.size(), 4);

    auto true_0 = flex_list{10,20,3,1};
    auto true_1 = flex_list{10,21,2,2};
    auto true_2 = flex_list{11,20,3,1};
    auto true_3 = flex_list{11,21,2,2};

    // Comes as user/item/score/rank
    ASSERT_TRUE(res[0] == true_0);
    ASSERT_TRUE(res[1] == true_1);
    ASSERT_TRUE(res[2] == true_2);
    ASSERT_TRUE(res[3] == true_3);
  }

  void test_item_inclusions_with_original_data() {

    // Build a simple dataset

    sframe obs_data = make_integer_testing_sframe( {"user", "item", "side", "target"},
                                                   { {10,20, 1, 3},
                                                     {10,21, 2, 2},
                                                     {10,22, 3, 1},
                                                     {11,20, 4, 3},
                                                     {11,21, 5, 2},
                                                     {11,25, 6, 1} } );

    auto model = new recsys::recsys_popularity();

    ////////////////////////////////////////////////////////////
    // Set the options

    std::map<std::string, flexible_type> opts;
    opts["item_id"] = "item";
    opts["user_id"] = "user";
    opts["target"] = "target";
    model->init_options(opts);

    ////////////////////////////////////////////////////////////
    // Train the model

    model->setup_and_train(obs_data);

    sframe inc_data = make_integer_testing_sframe( {"item"},
                                                     { {20},
                                                       {21} } );

    sframe inc_data_2 = make_integer_testing_sframe( {"user", "item"},
                                                     { {10,20},
                                                       {10,25},
                                                       {11,21},
                                                       {11,22} });

    sframe user_sf_orig = make_integer_testing_sframe( {"user"},
                                                       { {10},
                                                         {11} });

    sframe user_sf_more = make_integer_testing_sframe( {"user"},
                                                       { {10},
                                                         {11},
                                                         {30} });

    sframe user_sf = make_integer_testing_sframe( {"user"},
                                                  { {30},
                                                    {31} });

    {
      sframe res_back = model->recommend(user_sf, 10, inc_data);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 4);

      auto true_0 = flex_list{30,20,3,1};
      auto true_1 = flex_list{30,21,2,2};
      auto true_2 = flex_list{31,20,3,1};
      auto true_3 = flex_list{31,21,2,2};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
      ASSERT_TRUE(res[2] == true_2);
      ASSERT_TRUE(res[3] == true_3);
    }

    {
      // Same one, but with exclude_training_interactions set to 0
      sframe res_back = model->recommend(user_sf, 10, inc_data, sframe(), sframe(), sframe(), sframe(), false);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 4);

      auto true_0 = flex_list{30,20,3,1};
      auto true_1 = flex_list{30,21,2,2};
      auto true_2 = flex_list{31,20,3,1};
      auto true_3 = flex_list{31,21,2,2};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
      ASSERT_TRUE(res[2] == true_2);
      ASSERT_TRUE(res[3] == true_3);
    }

    {
      // now one with per-user item inclusions
      sframe res_back = model->recommend(sframe(), 10, inc_data_2);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 2);

      auto true_0 = flex_list{10,25,1,1};
      auto true_1 = flex_list{11,22,1,1};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
    }

    {
      // now a similar one but with users specified
      sframe res_back = model->recommend(user_sf_orig, 10, inc_data_2);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 2);

      auto true_0 = flex_list{10,25,1,1};
      auto true_1 = flex_list{11,22,1,1};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
    }

    {

      // now a similar one but with users specified; plus a user not
      // in inc_data_2, which should be ignored.
      sframe res_back = model->recommend(user_sf_more, 10, inc_data_2);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 2);

      auto true_0 = flex_list{10,25,1,1};
      auto true_1 = flex_list{11,22,1,1};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
    }

    {
      // now a similar one but without the training exclusions

      sframe res_back = model->recommend(sframe(), 10, inc_data_2, sframe(),
                                         sframe(), sframe(), sframe(), false);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 4);

      auto true_0 = flex_list{10,20,3,1};
      auto true_1 = flex_list{10,25,1,2};
      auto true_2 = flex_list{11,21,2,1};
      auto true_3 = flex_list{11,22,1,2};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
      ASSERT_TRUE(res[2] == true_2);
      ASSERT_TRUE(res[3] == true_3);
    }

    {
      // now a similar one but with users specified
      sframe res_back = model->recommend(user_sf_orig, 10, inc_data_2, sframe(),
                                         sframe(), sframe(), sframe(), false);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 4);

      auto true_0 = flex_list{10,20,3,1};
      auto true_1 = flex_list{10,25,1,2};
      auto true_2 = flex_list{11,21,2,1};
      auto true_3 = flex_list{11,22,1,2};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
      ASSERT_TRUE(res[2] == true_2);
      ASSERT_TRUE(res[3] == true_3);
    }

    {
      // now a similar one but with users specified; plus a user not
      // in inc_data_2, which should be ignored.
      sframe res_back = model->recommend(user_sf_more, 10, inc_data_2, sframe(),
                                         sframe(), sframe(), sframe(), false);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 4);

      auto true_0 = flex_list{10,20,3,1};
      auto true_1 = flex_list{10,25,1,2};
      auto true_2 = flex_list{11,21,2,1};
      auto true_3 = flex_list{11,22,1,2};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
      ASSERT_TRUE(res[2] == true_2);
      ASSERT_TRUE(res[3] == true_3);
    }

    // Now make sure items marked for exclusion are indeed excluded.

    sframe exc_data = make_integer_testing_sframe( {"user", "item"},
                                                   { {30, 20} } );

    {
      sframe res_back = model->recommend(user_sf, 10, inc_data, exc_data);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 3);

      // 1,2,3 above; 0 should be excluded.
      auto true_0 = flex_list{30,21,2,1};
      auto true_1 = flex_list{31,20,3,1};
      auto true_2 = flex_list{31,21,2,2};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
      ASSERT_TRUE(res[2] == true_2);
    }

    // Now make sure items included as the new data are also excluded
    sframe new_data = make_integer_testing_sframe( {"user", "item"},
                                                   { {31, 21} } );

    {
      sframe res_back = model->recommend(user_sf, 10, inc_data, exc_data, new_data);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 2);

      // 1,2,3 above; 0 should be excluded.
      auto true_0 = flex_list{30,21,2,1};
      auto true_1 = flex_list{31,20,3,1};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
    }
  }

  void test_user_item_inclusions_with_original_data() {

    // Build a simple dataset

    sframe obs_data = make_integer_testing_sframe( {"user", "item", "side", "target"},
                                                   { {10,20, 1, 3},
                                                     {10,21, 2, 2},
                                                     {10,22, 3, 1},
                                                     {11,20, 4, 3},
                                                     {11,21, 5, 2},
                                                     {11,25, 6, 1} } );

    auto model = new recsys::recsys_popularity();

    ////////////////////////////////////////////////////////////
    // Set the options

    std::map<std::string, flexible_type> opts;
    opts["item_id"] = "item";
    opts["user_id"] = "user";
    opts["target"] = "target";
    model->init_options(opts);

    ////////////////////////////////////////////////////////////
    // Train the model

    model->setup_and_train(obs_data);

    sframe inc_data = make_integer_testing_sframe( {"user", "item"},
                                                   { {30, 20},
                                                     {30, 21},
                                                     {31, 20},
                                                     {31, 22} } );

    sframe user_sf = make_integer_testing_sframe( {"user"},
                                                  { {30},
                                                    {31} });

    {
      sframe res_back = model->recommend(user_sf, 10, inc_data);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 4);

      auto true_0 = flex_list{30,20,3,1};
      auto true_1 = flex_list{30,21,2,2};
      auto true_2 = flex_list{31,20,3,1};
      auto true_3 = flex_list{31,22,1,2};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
      ASSERT_TRUE(res[2] == true_2);
      ASSERT_TRUE(res[3] == true_3);
    }

    // Now make sure items marked for exclusion are indeed excluded.

    sframe exc_data = make_integer_testing_sframe( {"user", "item"},
                                                   { {30, 20} } );

    {
      sframe res_back = model->recommend(user_sf, 10, inc_data, exc_data);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 3);

      // 1,2,3 above; 0 should be excluded.
      auto true_0 = flex_list{30,21,2,1};
      auto true_1 = flex_list{31,20,3,1};
      auto true_2 = flex_list{31,22,1,2};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
      ASSERT_TRUE(res[2] == true_2);
    }

    // Now make sure items included as the new data are also excluded
    sframe new_data = make_integer_testing_sframe( {"user", "item"},
                                                   { {31, 22} } );

    {
      sframe res_back = model->recommend(user_sf, 10, inc_data, exc_data, new_data);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);

      ASSERT_EQ(res.size(), 2);

      // 1,2,3 above; 0 should be excluded.
      auto true_0 = flex_list{30,21,2,1};
      auto true_1 = flex_list{31,20,3,1};

      // Comes as user/item/score/rank
      ASSERT_TRUE(res[0] == true_0);
      ASSERT_TRUE(res[1] == true_1);
    }
  }

  void test_side_columns_used() {

    // The side column exactly predicts the target column;
    sframe obs_data = make_testing_sframe( {"user", "item", "side", "target"},
                                           { {10, 20, 1,  1},
                                             {10, 21, 3,  3},
                                             {10, 22, 8,  8},
                                             {11, 20, 5,  5},
                                             {11, 21, 20, 20},
                                             {11, 22, 2,  2},
                                             {12, 20, 1,  1},
                                             {12, 21, 5,  5},
                                             {12, 22, 12, 12},
                                             {13, 20, 2,  2},
                                             {13, 21, 10, 10},
                                                 // This one is 23, so each user has one unrated item
                                             {13, 23, 10, 10},

                                             {10, 20, -1,  -1},
                                             {10, 21, -3,  -3},
                                             {10, 22, -8,  -8},
                                             {11, 20, -5,  -5},
                                             {11, 21, -20, -20},
                                             {11, 22, -2,  -2},
                                             {12, 20, -1,  -1},
                                             {12, 21, -5,  -5},
                                             {12, 22, -12, -12},
                                             {13, 20, -2,  -2},
                                             {13, 21, -10, -10},
                                                 // This one is 23, so each user has one unrated item
                                             {13, 23, -10, -10} } );

    auto model = new recsys::recsys_factorization_model();

    ////////////////////////////////////////////////////////////
    // Set the options

    std::map<std::string, flexible_type> opts;
    opts["item_id"] = "item";
    opts["user_id"] = "user";
    opts["target"] = "target";
    opts["num_factors"] = 0;
    opts["max_iterations"] = 1000;
    opts["linear_regularization"] = 0;
    opts["regularization"] = 0;

    model->init_options(opts);

    ////////////////////////////////////////////////////////////
    // Train the model
    parallel_for(size_t(0), size_t(16), [&](size_t i) {
        if(i == 0)
          model->setup_and_train(obs_data);
      });

    {
      // Now there is no side data.  The predicted scores should
      // basically be the average, which is 0.
      sframe res_back = model->recommend(sframe(), 1);

      std::vector<double> scores = testing_extract_column<double>(res_back.select_column("score"));

      // Make sure each of these equals zero
      for(double v : scores) {
        TS_ASSERT_DELTA(0.0, v, 0.05);
      }
    }

    // Now with side data.
    // The side column exactly predicts the target column;
    {
      std::vector<double> rv = {1.5, -5, 5, -2};
      sframe query_data = make_testing_sframe( {"user", "side"},
                                               { {10,  rv[0]},
                                                 {11,  rv[1]},
                                                 {12,  rv[2]},
                                                 {13,  rv[3]} });

      sframe res_back = model->recommend(query_data, 1);

      std::vector<double> scores = testing_extract_column<double>(res_back.select_column("score"));

      ASSERT_EQ(scores.size(), 4);

      // Make sure each of these equals zero
      for(size_t i = 0; i < scores.size(); ++i)
        TS_ASSERT_DELTA(scores[i], rv[i], 0.05);
    }
  }

  template <typename Model>
  void _run_test_diversity() {

    // Build a simple dataset

    sframe data = make_random_sframe(1000, "CC");

    data.set_column_name(0, "user");
    data.set_column_name(1, "item");

    std::unique_ptr<recsys::recsys_model_base> model(new Model);

    ////////////////////////////////////////////////////////////
    // Set the options

    std::map<std::string, flexible_type> opts;
    opts["item_id"] = "item";
    opts["user_id"] = "user";
    opts["target"] = "";
    model->init_options(opts);

    ////////////////////////////////////////////////////////////
    // Train the model

    model->setup_and_train(data);

    for(size_t _k = 1; _k < 10; ++_k) {

      size_t k = (_k < 5) ? _k : 5 + 3*(_k - 5);

      sframe res_back = model->recommend(sframe(), k, sframe(), sframe(), sframe(), sframe(), sframe(),
                                         false, /*diversity=*/ 1, /*random_seed=*/0);

      sframe res_back_2 = model->recommend(sframe(), k, sframe(), sframe(), sframe(), sframe(), sframe(),
                                           false, /*diversity=*/ 1, /*random_seed=*/1);

      sframe res_back_3 = model->recommend(sframe(), k, sframe(), sframe(), sframe(), sframe(), sframe(),
                                           false, /*diversity=*/ 2, /*random_seed=*/0);

      std::vector<flex_list> res = testing_extract_sframe_data(res_back);
      std::vector<flex_list> res_2 = testing_extract_sframe_data(res_back_2);
      std::vector<flex_list> res_3 = testing_extract_sframe_data(res_back_3);

      ASSERT_EQ(res.size(), res_2.size());
      ASSERT_EQ(res.size(), res_3.size());

      bool all_equal_1 = true;
      bool all_equal_2 = true;

      for(size_t i = 0; i < res.size(); ++i) {
        if(res[i] != res_2[i]) {
          all_equal_1 = false;
        }

        if(res[i] != res_3[i]) {
          all_equal_2 = false;
        }
      }

      ASSERT_FALSE(all_equal_1);
      ASSERT_FALSE(all_equal_2);
    }
  }

  void test_diversity_popularity() {
    _run_test_diversity<recsys::recsys_popularity>();
  }

  void test_diversity_mf() {
    _run_test_diversity<recsys::recsys_ranking_factorization_model>();
  }

  void test_diversity_itemcf() {
    _run_test_diversity<recsys::recsys_itemcf>();
  }

};

BOOST_FIXTURE_TEST_SUITE(_recsys_recommend, recsys_recommend)
BOOST_AUTO_TEST_CASE(test_user_item_inclusions) {
  recsys_recommend::test_user_item_inclusions();
}
BOOST_AUTO_TEST_CASE(test_item_inclusions_with_original_data) {
  recsys_recommend::test_item_inclusions_with_original_data();
}
BOOST_AUTO_TEST_CASE(test_user_item_inclusions_with_original_data) {
  recsys_recommend::test_user_item_inclusions_with_original_data();
}
BOOST_AUTO_TEST_CASE(test_side_columns_used) {
  recsys_recommend::test_side_columns_used();
}
BOOST_AUTO_TEST_CASE(test_diversity_popularity) {
  recsys_recommend::test_diversity_popularity();
}
BOOST_AUTO_TEST_CASE(test_diversity_mf) {
  recsys_recommend::test_diversity_mf();
}
BOOST_AUTO_TEST_CASE(test_diversity_itemcf) {
  recsys_recommend::test_diversity_itemcf();
}
BOOST_AUTO_TEST_SUITE_END()


/* Copyright (c) 2016 - 2020, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef THERMAL_PHYSICS_TEMPLATES_HH
#define THERMAL_PHYSICS_TEMPLATES_HH

#include <ThermalOperator.hh>
#ifdef ADAMANTINE_HAVE_CUDA
#include <ThermalOperatorDevice.hh>
#endif
#include <ThermalPhysics.hh>

#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_gmres.h>

#include <algorithm>

namespace adamantine
{
namespace
{
template <int dim, int fe_degree, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value,
              int> = 0>
dealii::LA::distributed::Vector<double, MemorySpaceType> vmult_and_scale(
    std::shared_ptr<ThermalOperatorBase<dim, MemorySpaceType>> thermal_operator,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &y,
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> &value,
    std::vector<Timer> &timers)
{
  // Apply the Thermal Operator.
  thermal_operator->vmult_add(value, y);

  // Multiply by the inverse of the mass matrix.
  value.scale(*thermal_operator->get_inverse_mass_matrix());

  timers[evol_time_eval_th_ph].stop();

  return value;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value,
              int> = 0>
void init_dof_vector(
    dealii::DoFHandler<dim> const &dof_handler, dealii::FE_Q<dim> const &fe,
    double const value,
    dealii::LinearAlgebra::distributed::Vector<double, MemorySpaceType> &vector)
{
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dummy;
  dealii::QGauss<dim> quadrature(1);
  dealii::FEValues<dim> fe_values(fe, quadrature, dealii::update_values);
  unsigned int const dofs_per_cell = fe.dofs_per_cell;
  std::vector<dealii::types::global_dof_index> local_dof_indices(dofs_per_cell);
  dealii::IndexSet local_elements = vector.locally_owned_elements();
  for (auto cell :
       dealii::filter_iterators(dof_handler.active_cell_iterators(),
                                dealii::IteratorFilters::LocallyOwnedCell()))
  {
    fe_values.reinit(cell);
    cell->get_dof_indices(local_dof_indices);
    for (auto const dof_index : local_dof_indices)
    {
      if (local_elements.is_element(dof_index) == true)
        vector[dof_index] = value;
    }
  }
}

#ifdef ADAMANTINE_HAVE_CUDA
template <int dim, int fe_degree, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::CUDA>::value,
              int> = 0>
dealii::LA::distributed::Vector<double, MemorySpaceType> vmult_and_scale(
    std::shared_ptr<ThermalOperatorBase<dim, MemorySpaceType>> thermal_operator,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &y,
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> &value,
    std::vector<Timer> &timers)
{
  dealii::LA::distributed::Vector<double, MemorySpaceType> value_dev(
      value.get_partitioner());
  value_dev.import(value, dealii::VectorOperation::insert);

  // Apply the Thermal Operator.
  thermal_operator->vmult_add(value_dev, y);

  // Multiply by the inverse of the mass matrix.
  value_dev.scale(*thermal_operator->get_inverse_mass_matrix());

  timers[evol_time_eval_th_ph].stop();

  return value_dev;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          std::enable_if_t<
              std::is_same<MemorySpaceType, dealii::MemorySpace::CUDA>::value,
              int> = 0>
void init_dof_vector(
    dealii::DoFHandler<dim> const &dof_handler, dealii::FE_Q<dim> const &fe,
    double const value,
    dealii::LinearAlgebra::distributed::Vector<double, MemorySpaceType> &vector)
{
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>
      vector_host(vector.get_partitioner());
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dummy;

  dealii::QGauss<dim> quadrature(1);
  dealii::FEValues<dim> fe_values(fe, quadrature, dealii::update_values);
  unsigned int const dofs_per_cell = fe.dofs_per_cell;
  std::vector<dealii::types::global_dof_index> local_dof_indices(dofs_per_cell);
  dealii::IndexSet local_elements = vector.locally_owned_elements();
  for (auto cell :
       dealii::filter_iterators(dof_handler.active_cell_iterators(),
                                dealii::IteratorFilters::LocallyOwnedCell()))
  {
    fe_values.reinit(cell);
    cell->get_dof_indices(local_dof_indices);
    for (auto const dof_index : local_dof_indices)
    {
      if (local_elements.is_element(dof_index) == true)
        vector_host[dof_index] = value;
    }
  }
  vector.import(vector_host, dealii::VectorOperation::insert);
}
#endif
} // namespace

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::ThermalPhysics(
    MPI_Comm const &communicator, boost::property_tree::ptree const &database,
    Geometry<dim> &geometry)
    : _embedded_method(false), _implicit_method(false), _geometry(geometry),
      _fe(fe_degree), _dof_handler(_geometry.get_triangulation()),
      _quadrature(fe_degree + 1)
{
  // Create the material properties
  boost::property_tree::ptree const &material_database =
      database.get_child("materials");
  _material_properties.reset(new MaterialProperty<dim>(
      communicator, _geometry.get_triangulation(), material_database));

  // Create the electron beams
  boost::property_tree::ptree const &source_database =
      database.get_child("sources");
  unsigned int const n_beams = source_database.get<unsigned int>("n_beams");
  _electron_beams.resize(n_beams);
  for (unsigned int i = 0; i < n_beams; ++i)
  {
    boost::property_tree::ptree const &beam_database =
        source_database.get_child("beam_" + std::to_string(i));
    _electron_beams[i] = std::make_unique<ElectronBeam<dim>>(beam_database);
    _electron_beams[i]->set_max_height(_geometry.get_max_height());
  }

  // Create the thermal operator
  if (std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value)
    _thermal_operator =
        std::make_shared<ThermalOperator<dim, fe_degree, MemorySpaceType>>(
            communicator, _material_properties);
#ifdef ADAMANTINE_HAVE_CUDA
  else
    _thermal_operator = std::make_shared<
        ThermalOperatorDevice<dim, fe_degree, MemorySpaceType>>(
        communicator, _material_properties);
#endif

  // Create the time stepping scheme
  boost::property_tree::ptree const &time_stepping_database =
      database.get_child("time_stepping");
  std::string method = time_stepping_database.get<std::string>("method");
  std::transform(method.begin(), method.end(), method.begin(),
                 [](unsigned char c) { return std::tolower(c); });
  if (method.compare("forward_euler") == 0)
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ExplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::FORWARD_EULER);
  else if (method.compare("rk_third_order") == 0)
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ExplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::RK_THIRD_ORDER);
  else if (method.compare("rk_fourth_order") == 0)
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ExplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::RK_CLASSIC_FOURTH_ORDER);
  else if (method.compare("heun_euler") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::HEUN_EULER);
    _embedded_method = true;
  }
  else if (method.compare("bogacki_shampine") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::BOGACKI_SHAMPINE);
    _embedded_method = true;
  }
  else if (method.compare("dopri") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::DOPRI);
    _embedded_method = true;
  }
  else if (method.compare("fehlberg") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::FEHLBERG);
    _embedded_method = true;
  }
  else if (method.compare("cash_karp") == 0)
  {
    _time_stepping = std::make_unique<
        dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector>>(
        dealii::TimeStepping::CASH_KARP);
    _embedded_method = true;
  }
  else if (method.compare("backward_euler") == 0)
  {
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::BACKWARD_EULER);
    _implicit_method = true;
  }
  else if (method.compare("implicit_midpoint") == 0)
  {
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::IMPLICIT_MIDPOINT);
    _implicit_method = true;
  }
  else if (method.compare("crank_nicolson") == 0)
  {
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::CRANK_NICOLSON);
    _implicit_method = true;
  }
  else if (method.compare("sdirk2") == 0)
  {
    _time_stepping =
        std::make_unique<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector>>(
            dealii::TimeStepping::SDIRK_TWO_STAGES);
    _implicit_method = true;
  }

  if (_embedded_method == true)
  {
    double coarsen_param =
        time_stepping_database.get("coarsening_parameter", 1.2);
    double refine_param = time_stepping_database.get("refining_parameter", 0.8);
    double min_delta = time_stepping_database.get("min_time_step", 1e-14);
    double max_delta = time_stepping_database.get("max_time_step", 1e100);
    double refine_tol = time_stepping_database.get("refining_tolerance", 1e-8);
    double coarsen_tol =
        time_stepping_database.get("coarsening_tolerance", 1e-12);
    dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector> *embedded_rk =
        static_cast<
            dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector> *>(
            _time_stepping.get());
    embedded_rk->set_time_adaptation_parameters(coarsen_param, refine_param,
                                                min_delta, max_delta,
                                                refine_tol, coarsen_tol);
  }

  // If the time stepping scheme is implicit, set the parameters for the solver
  // and create the implicit operator.
  if (_implicit_method == true)
  {
    _max_iter = time_stepping_database.get("max_iteration", 1000);
    _tolerance = time_stepping_database.get("tolerance", 1e-12);
    _right_preconditioning =
        time_stepping_database.get("right_preconditioning", false);
    _max_n_tmp_vectors = time_stepping_database.get("n_tmp_vectors", 30);
    unsigned int newton_max_iter =
        time_stepping_database.get("newton_max_iteration", 100);
    double newton_tolerance =
        time_stepping_database.get("newton_tolerance", 1e-6);
    dealii::TimeStepping::ImplicitRungeKutta<LA_Vector> *implicit_rk =
        static_cast<dealii::TimeStepping::ImplicitRungeKutta<LA_Vector> *>(
            _time_stepping.get());
    implicit_rk->set_newton_solver_parameters(newton_max_iter,
                                              newton_tolerance);

    bool jfnk = time_stepping_database.get("jfnk", false);
    _implicit_operator = std::make_unique<ImplicitOperator<MemorySpaceType>>(
        _thermal_operator, jfnk);
  }
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType,
                    QuadratureType>::setup_dofs()
{
  _dof_handler.distribute_dofs(_fe);
  dealii::IndexSet locally_relevant_dofs;
  dealii::DoFTools::extract_locally_relevant_dofs(_dof_handler,
                                                  locally_relevant_dofs);
  _affine_constraints.clear();
  _affine_constraints.reinit(locally_relevant_dofs);
  dealii::DoFTools::make_hanging_node_constraints(_dof_handler,
                                                  _affine_constraints);
  _affine_constraints.close();

  _thermal_operator->reinit(_dof_handler, _affine_constraints, _quadrature);
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType,
                    QuadratureType>::compute_inverse_mass_matrix()
{
  _thermal_operator->compute_inverse_mass_matrix(_dof_handler,
                                                 _affine_constraints);
  if (_implicit_method == true)
    _implicit_operator->set_inverse_mass_matrix(
        _thermal_operator->get_inverse_mass_matrix());
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
double ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    evolve_one_time_step(
        double t, double delta_t,
        dealii::LA::distributed::Vector<double, MemorySpaceType> &solution,
        std::vector<Timer> &timers)
{
  auto eval = [&](double const t, LA_Vector const &y) {
    return evaluate_thermal_physics(t, y, timers);
  };
  auto id_m_Jinv = [&](double const t, double const tau, LA_Vector const &y) {
    return id_minus_tau_J_inverse(t, tau, y, timers);
  };

  double time = _time_stepping->evolve_one_time_step(eval, id_m_Jinv, t,
                                                     delta_t, solution);

  // If the method is embedded, get the next time step. Otherwise, just use the
  // current time step.
  if (_embedded_method == false)
    _delta_t_guess = delta_t;
  else
  {
    dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector> *embedded_rk =
        static_cast<
            dealii::TimeStepping::EmbeddedExplicitRungeKutta<LA_Vector> *>(
            _time_stepping.get());
    _delta_t_guess = embedded_rk->get_status().delta_t_guess;
  }

  // Return the time at the end of the time step. This may be different than
  // t+delta_t for embedded methods.
  return time;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    initialize_dof_vector(
        dealii::LA::distributed::Vector<double, MemorySpaceType> &vector) const
{
  _thermal_operator->initialize_dof_vector(vector);
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
void ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    initialize_dof_vector(
        double const value,
        dealii::LA::distributed::Vector<double, MemorySpaceType> &vector) const
{
  // Resize the vector
  _thermal_operator->initialize_dof_vector(vector);

  init_dof_vector<dim, fe_degree, MemorySpaceType>(_dof_handler, _fe, value,
                                                   vector);
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
dealii::LA::distributed::Vector<double, MemorySpaceType>
ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    evaluate_thermal_physics(
        double const t,
        dealii::LA::distributed::Vector<double, MemorySpaceType> const &y,
        std::vector<Timer> &timers) const
{
  timers[evol_time_eval_mat_prop].start();
  if (std::is_same<MemorySpaceType, dealii::MemorySpace::Host>::value)
    _thermal_operator->evaluate_material_properties(y);
  else
  {
    // TODO do this on the GPU
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> y_host(
        y.get_partitioner());
    y_host.import(y, dealii::VectorOperation::insert);
    _thermal_operator->evaluate_material_properties(y_host);
  }
  timers[evol_time_eval_mat_prop].stop();

  timers[evol_time_eval_th_ph].start();
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> source(
      y.get_partitioner());
  source = 0.;

  // Compute the source term.
  for (auto &beam : _electron_beams)
    beam->set_time(t);
  dealii::QGauss<dim> source_quadrature(fe_degree + 1);
  dealii::FEValues<dim> fe_values(_fe, source_quadrature,
                                  dealii::update_quadrature_points |
                                      dealii::update_values |
                                      dealii::update_JxW_values);
  unsigned int const dofs_per_cell = _fe.dofs_per_cell;
  unsigned int const n_q_points = source_quadrature.size();
  std::vector<dealii::types::global_dof_index> local_dof_indices(dofs_per_cell);
  dealii::Vector<double> cell_source(dofs_per_cell);

  for (auto cell :
       dealii::filter_iterators(_dof_handler.active_cell_iterators(),
                                dealii::IteratorFilters::LocallyOwnedCell()))
  {
    cell_source = 0.;
    fe_values.reinit(cell);
    double const inv_rho_cp = _thermal_operator->get_inv_rho_cp(cell);

    for (unsigned int i = 0; i < dofs_per_cell; ++i)
    {
      for (unsigned int q = 0; q < n_q_points; ++q)
      {
        double quad_pt_source = 0.;
        dealii::Point<dim> const &q_point = fe_values.quadrature_point(q);
        for (auto &beam : _electron_beams)
          quad_pt_source += beam->value(q_point);

        cell_source[i] += inv_rho_cp * quad_pt_source *
                          fe_values.shape_value(i, q) * fe_values.JxW(q);
      }
    }
    cell->get_dof_indices(local_dof_indices);
    _affine_constraints.distribute_local_to_global(cell_source,
                                                   local_dof_indices, source);
  }

  return vmult_and_scale<dim, fe_degree, MemorySpaceType>(_thermal_operator, y,
                                                          source, timers);
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
dealii::LA::distributed::Vector<double, MemorySpaceType>
ThermalPhysics<dim, fe_degree, MemorySpaceType, QuadratureType>::
    id_minus_tau_J_inverse(
        double const /*t*/, double const tau,
        dealii::LA::distributed::Vector<double, MemorySpaceType> const &y,
        std::vector<Timer> &timers) const
{
  timers[evol_time_J_inv].start();
  _implicit_operator->set_tau(tau);
  dealii::LA::distributed::Vector<double, MemorySpaceType> solution(
      y.get_partitioner());

  // TODO Add a geometric multigrid preconditioner.
  dealii::PreconditionIdentity preconditioner;

  dealii::SolverControl solver_control(_max_iter, _tolerance * y.l2_norm());
  // We need to inverse (I - tau M^{-1} J). While M^{-1} and J are SPD,
  // (I - tau M^{-1} J) is symmetric indefinite in the general case.
  typename dealii::SolverGMRES<
      dealii::LA::distributed::Vector<double, MemorySpaceType>>::AdditionalData
      additional_data(_max_n_tmp_vectors, _right_preconditioning);
  dealii::SolverGMRES<dealii::LA::distributed::Vector<double, MemorySpaceType>>
      solver(solver_control, additional_data);
  solver.solve(*_implicit_operator, solution, y, preconditioner);

  timers[evol_time_J_inv].stop();

  return solution;
}
} // namespace adamantine

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_MODF_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_MODF_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/modf.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <boost/simd/include/functions/simd/trunc.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/fusion/include/vector.hpp>

namespace boost { namespace simd { namespace ext
{
    BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::modf_, tag::cpu_, (A0)(X),
                                ((simd_< arithmetic_<A0>, X>))
                                ((simd_< arithmetic_<A0>, X>))
                                ((simd_< arithmetic_<A0>, X>))
                            )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0,A0 & r1,A0 & r0) const
      {
        r1 = boost::simd::trunc(a0);
        r0 = a0-r1;
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::modf_, tag::cpu_, (A0)(X),
                              ((simd_< arithmetic_<A0>, X>))
                              ((simd_< arithmetic_<A0>, X>))
                            )
  {
    typedef A0 result_type;
    inline A0 operator()(A0 const& a0,A0 & r1) const
      {
        r1 = boost::simd::trunc(a0);
        return a0-r1;
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::modf_, tag::cpu_,
                       (A0)(X),
                       ((simd_<arithmetic_<A0>,X>))
                      )
  {
    typedef boost::fusion::vector<A0, A0> result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      result_type res;
      boost::fusion::at_c<0>(res) = boost::simd::modf(a0,boost::fusion::at_c<1>(res));
      return res;
    }
  };
} } }
#endif


//
// ExpansionHunter Denovo
// Copyright 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Egor Dolzhenko <edolzhenko@illumina.com>,
//         Michael Eberle <meberle@illumina.com>
//
// 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.
//
//

#include "reads/IrrFinder.hh"

#include <algorithm>
#include <boost/bind.hpp>
#include <iostream>
#include <map>
#include <string>
#include <unordered_map>
#include <vector>

#include "common/SequenceUtils.hh"
#include "reads/Purity.hh"

using std::logic_error;
using std::map;
using std::string;
using std::to_string;
using std::unordered_map;
using std::vector;

int32_t MaxMatchesAtOffset(int32_t offset, const std::string& bases)
{
    if (bases.length() < offset)
        return 0;
    return bases.length() - offset;
}

double MatchFrequencyAtOffset(int32_t offset, const string& bases)
{
    if (offset <= 0)
    {
        throw logic_error(to_string(offset) + " is not a valid offset for " + bases);
    }

    if (bases.length() / 2 + 1 <= offset)
    {
        return 0;
    }

    int32_t num_matches = 0;
    for (size_t position = 0; position != bases.length() - offset; ++position)
        if (bases[position] == bases[position + offset])
            ++num_matches;

    const int32_t max_matches = MaxMatchesAtOffset(offset, bases);
    const double match_frequency = (double)num_matches / max_matches;
    return match_frequency;
}

int SmallestFrequentPeriod(double minFrequency, const string& bases, const Interval& periodSizeRange)
{
    const int smallestPeriod = std::max(periodSizeRange.start(), 1);
    const int largestPeriod = std::min(periodSizeRange.end(), static_cast<int>(bases.length() / 2 + 1));

    double maxMatchFrequency = minFrequency;
    int bestOffset = -1;
    for (int offset = largestPeriod; offset + 1 != smallestPeriod; --offset)
    {
        const double match_frequency = MatchFrequencyAtOffset(offset, bases);
        if (match_frequency >= maxMatchFrequency)
        {
            maxMatchFrequency = match_frequency;
            bestOffset = offset;
        }
    }

    return bestOffset;
}

char ExtractConsensusBase(int32_t offset, int32_t period, const std::string& bases)
{
    unordered_map<char, int32_t> char_frequency;
    for (int32_t index = offset; index < bases.length(); index += period)
    {
        ++char_frequency[bases[index]];
    }

    char consensus_char = '?';
    int32_t max_frequency = 0;
    for (const auto& kv : char_frequency)
    {
        char current_char = kv.first;
        int32_t current_frequency = kv.second;
        if (current_frequency > max_frequency)
        {
            max_frequency = current_frequency;
            consensus_char = current_char;
        }
    }

    return consensus_char;
}

string ExtractConsensusRepeatUnit(double period, const string& bases)
{
    string repeat_unit;
    for (int32_t offset = 0; offset != period; ++offset)
        repeat_unit += ExtractConsensusBase(offset, period, bases);

    return repeat_unit;
}

string MinimialUnitUnderShift(const string& unit)
{
    string minimal_unit = unit;
    const string double_unit = unit + unit;
    for (int32_t index = 0; index != unit.length(); ++index)
    {
        string current_unit = double_unit.substr(index, unit.length());
        if (current_unit < minimal_unit)
            minimal_unit = current_unit;
    }
    return minimal_unit;
}

string ComputeCanonicalRepeatUnit(const string& unit)
{
    const string minimal_unit = MinimialUnitUnderShift(unit);

    const string unit_rc = reverseComplement(unit);
    const string minimal_unit_rc = MinimialUnitUnderShift(unit_rc);

    if (minimal_unit_rc < minimal_unit)
        return minimal_unit_rc;
    return minimal_unit;
}

string ComputeCanonicalRepeatUnit(double minFrequency, const string& bases, const Interval& motifSizeRange)
{
    const int period = SmallestFrequentPeriod(minFrequency, bases, motifSizeRange);
    if (period == -1)
    {
        return "";
    }
    string motif = ExtractConsensusRepeatUnit(period, bases);
    const double kPerfectMatchFrequency = 1.0;
    const int32_t reducedPeriod = SmallestFrequentPeriod(kPerfectMatchFrequency, motif);
    if (reducedPeriod != -1 && reducedPeriod != period)
    {
        motif = ExtractConsensusRepeatUnit(reducedPeriod, motif);
    }
    return ComputeCanonicalRepeatUnit(motif);
}

bool IsInrepeatRead(const string& bases, const string& quals, string& unit, const Interval& motifSizeRange)
{
    const double min_frequency = 0.8;
    unit = ComputeCanonicalRepeatUnit(min_frequency, bases, motifSizeRange);
    if (unit.empty() || unit == "N")
    {
        return false;
    }

    const vector<string> units = { unit };
    const vector<vector<string>> units_shifts = ShiftUnits(units);

    double score = MatchRepeatRc(units_shifts, bases, quals);
    score /= bases.length();

    const double min_score = 0.90;
    return score >= min_score;
}


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

#pragma once

/// @file
/// Classes and functions used to manipulate grids in reciprocal space.

#include <boost/mpi.hpp>
#include <structures.hpp>
#include <dynamical_matrix.hpp>

namespace alma {
/// Convenient shorthand for an array of four indices.
using Tetrahedron = std::array<std::size_t, 4>;
/// Convenient shorthand for an array of three indices.
using Triangle = std::array<std::size_t, 3>;

/// Objects of this class represent a regular grid with the Gamma
/// point in one corner.
class Gamma_grid {
public:
    /// Size of the grid along the first reciprocal axis.
    const int na;
    /// Size of the grid along the second reciprocal axis.
    const int nb;
    /// Size of the grid along the third reciprocal axis.
    const int nc;
    /// Total number of q points in the grid.
    const std::size_t nqpoints;
    /// Reciprocal lattice basis vectors.
    const Eigen::MatrixXd rlattvec;
    /// Side vectors of each element in reciprocal space.
    const Eigen::MatrixXd dq;
    /// Constructor: initialize all internal variables and compute
    /// the spectrum at each q point. Do not correct the dynamical
    /// matrix for the effect of long-range interactions.
    Gamma_grid(const Crystal_structure& poscar,
               const Symmetry_operations& symms,
               const Harmonic_ifcs& force_constants,
               int _na,
               int _nb,
               int _nc);

    /// Constructor: initialize all internal variables and compute
    /// the spectrum at each q point. Correct the dynamical
    /// matrix for the effect of long-range interactions.
    Gamma_grid(const Crystal_structure& poscar,
               const Symmetry_operations& symms,
               const Harmonic_ifcs& force_constants,
               const Dielectric_parameters& born,
               int _na,
               int _nb,
               int _nc);

    /// Set the three lowest frequencies at Gamma to zero.
    void enforce_asr() {
        this->spectrum[0].omega.head(3).fill(0.);
    }

    /// Return the index of a q point identified by its position
    /// along the three axes.
    ///
    /// Indices are interpreted modulo-{na, nb, nc}, so even
    /// negative values are accepted.
    /// @param[in] indices - positions of the q point
    /// along the three axes
    /// @return the index of the q point in this grid
    std::size_t three_to_one(const std::array<int, 3>& indices) const {
        auto ia = python_mod(indices[0], this->na);
        auto ib = python_mod(indices[1], this->nb);
        auto ic = python_mod(indices[2], this->nc);

        return ic + nc * (ib + nb * ia);
    }

    /// Return the coordinates of a q point identified by its
    /// index.
    ///
    /// @param[in] index - index of the q point
    /// @return - an array with the positions of the q point
    /// along the three axes.
    std::array<int, 3> one_to_three(std::size_t iq) const {
        std::array<int, 3> nruter;
        nruter[2] = iq % nc;
        iq /= nc;
        nruter[1] = iq % nb;
        nruter[0] = iq / nb;
        return nruter;
    }

    /// @return the number of q-point equivalence classes in the grid.
    std::size_t get_nequivalences() const {
        return this->equivalences.size();
    }

    /// Access the harmonic properties at a point in the grid.
    ///
    /// The index is interpreted using modular arithmetic,
    /// so even negative values are accepted.
    /// @param[in] iq - index of the q point
    /// @return the harmonic properties at the q point.
    const Spectrum_at_point& get_spectrum_at_q(int iq) const {
        std::size_t index = python_mod(iq, this->nqpoints);

        return this->spectrum[index];
    }

    /// Return the cardinal of an equivalence class.
    ///
    /// @param[in] ic - index of the equivalence class.
    /// @return the number of points in an equivalence class.
    std::size_t get_cardinal(std::size_t ic) const {
        if (ic > this->equivalences.size())
            throw value_error("wrong equivalence class index");
        return this->equivalences[ic].size();
    }

    /// Return a representative of an equivalence class.
    ///
    /// The method is guaranteed to always return the same point.
    /// @param[in] ic - index of the equivalence class.
    /// @return a representative of the equivalence class.
    std::size_t get_representative(std::size_t ic) const {
        if (ic > this->equivalences.size())
            throw value_error("wrong equivalence class index");
        return this->equivalences[ic][0];
    }

    /// Return the elements in an equivalence class.
    ///
    /// @param[in] ic - index of the equivalence class.
    /// @return a vector with the elements in the equivalence
    /// class.
    std::vector<std::size_t> get_equivalence(std::size_t ic) const {
        if (ic > this->equivalences.size())
            throw value_error("wrong equivalence class index");
        return this->equivalences[ic];
    }

    /// Return the Cartesian coordinates of a q point.
    ///
    /// @param[in] iq - a q point index
    /// @return three Cartesian coordinates
    Eigen::VectorXd get_q(std::size_t iq) const {
        std::size_t index = python_mod(iq, this->nqpoints);

        return this->cpos.col(index);
    }

    /// Return the base broadening (without any prefactor) for a
    /// mode.
    ///
    /// @param[in] v - group velocity, or difference
    /// of group velocities in the case of three-phonon processes
    /// @return - the standard deviation of a Gaussian
    double base_sigma(const Eigen::VectorXd& v) const {
        return (v.transpose() * this->dq).norm() / std::sqrt(12.);
    }

    /// Very basic constructor that builds a stub object. Useful
    /// for deserialization or for obtaining an equivalences vector
    /// without computing the spectrum.
    Gamma_grid(const Crystal_structure& poscar,
               const Symmetry_operations& symms,
               int _na,
               int _nb,
               int _nc);

    /// Find the index of the polar opposite q point.
    ///
    /// @param[in] q - a q point
    /// @return the index of the polar opposite of q
    std::size_t polar_opposite(std::size_t q) {
        auto indices = this->one_to_three(q);

        return this->three_to_one({{-indices[0], -indices[1], -indices[2]}});
    }

    /// Return the images of a q point through all the symmetry
    /// operations, including inversions.
    ///
    /// @param[in] q - a q point
    /// @return a vector of q point indices. Elements with even indices
    /// correspond to operations in the space group,
    /// while elements with odd indices compound them with time
    /// reversal.
    std::vector<size_t> equivalent_qpoints(std::size_t original) const {
        std::size_t index = python_mod(original, this->nqpoints);
        return this->symmetry_map[index];
    }

    /// Find all q-point pairs equivalent to the input.
    ///
    /// Given a pair of indices, obtain all equivalent pairs
    /// after looking the up in the symmetry_map.
    /// @param[in] pair a pair of q point indices
    /// @return a vector of pairs, including the input
    std::vector<std::array<std::size_t, 2>> equivalent_qpairs(
        const std::array<std::size_t, 2>& original) const;

    /// Find all q-point triplets equivalent to the input.
    ///
    /// Given a triplet of indices, obtain all equivalent triplets
    /// after looking the up in the symmetry_map.
    /// @param[in] a triplet of q point indices
    /// @return a vector of triplets, including the input
    std::vector<std::array<std::size_t, 3>> equivalent_qtriplets(
        const std::array<std::size_t, 3>& original) const;

    /// Decompose the q-th microcell in five tetrahedra.
    ///
    /// @param[in] q - the index of the q point
    /// @return a vector of five Tetrahedron objects
    std::vector<Tetrahedron> get_tetrahedra(std::size_t q) const {
        // Find out the indices of the eight corners of the
        // microcell.
        std::size_t i000 = q;
        auto indices = this->one_to_three(i000);
        std::size_t i001 =
            this->three_to_one({{indices[0], indices[1], indices[2] + 1}});
        std::size_t i010 =
            this->three_to_one({{indices[0], indices[1] + 1, indices[2]}});
        std::size_t i011 =
            this->three_to_one({{indices[0], indices[1] + 1, indices[2] + 1}});
        std::size_t i100 =
            this->three_to_one({{indices[0] + 1, indices[1], indices[2]}});
        std::size_t i101 =
            this->three_to_one({{indices[0] + 1, indices[1], indices[2] + 1}});
        std::size_t i110 =
            this->three_to_one({{indices[0] + 1, indices[1] + 1, indices[2]}});
        std::size_t i111 = this->three_to_one(
            {{indices[0] + 1, indices[1] + 1, indices[2] + 1}});

        // Build the four equivalent tetrahedra.
        std::vector<Tetrahedron> nruter;
        nruter.reserve(5);
        nruter.emplace_back(Tetrahedron({{i000, i001, i010, i100}}));
        nruter.emplace_back(Tetrahedron({{i110, i111, i100, i010}}));
        nruter.emplace_back(Tetrahedron({{i101, i100, i111, i001}}));
        nruter.emplace_back(Tetrahedron({{i011, i010, i001, i111}}));
        // And the central, inequivalent one.
        nruter.emplace_back(Tetrahedron({{i010, i100, i111, i001}}));
        return nruter;
    }

    /// @return a vector of triangle objects with each containing three
    /// indices
    std::vector<Triangle> get_triangles(std::size_t ia) const;

    /// @return the index of the representative of the equivalence
    /// class
    /// of the provided q-point
    std::size_t getParentIdx(std::size_t iq) const;

    /// @return the index of the symmetry operation that maps the
    /// provided q-point
    /// to the representative of the equivalence class to which it
    /// belongs
    std::size_t getSymIdxToParent(std::size_t iq) const;

    /// @return the grid's symmetry map
    std::vector<std::vector<std::size_t>> getSymmetryMap() {
        return this->symmetry_map;
    }

    /// Remove the component of a Cartesian vector which does transform as a
    /// q-point in the grid.
    ///
    /// @param[in] iq - index of the q point
    /// @param[in] symms - set of symmetry operations of the crystal
    /// @param[in] x - vector in Cartesian coordinates. Several vectors can
    /// be provided by making v a matrix and each vector a column
    /// @return the symmetrized version of x
    template <typename T>
    auto copy_symmetry(std::size_t iq,
                       const Symmetry_operations& symms,
                       const Eigen::MatrixBase<T>& x) const
        -> Eigen::Matrix<typename T::Scalar, Eigen::Dynamic, Eigen::Dynamic> {
        std::size_t index = python_mod(iq, this->nqpoints);
        std::size_t nops = symms.get_nsym();

        typename T::PlainObject nruter(x);
        nruter.fill(0.);
        std::size_t nfound = 0;

        for (std::size_t iop = 0; iop < nops; ++iop) {
            if (this->symmetry_map[index][2 * iop] == index) {
                nruter += symms.rotate_v(x, iop, true);
                ++nfound;
            }
        }
        nruter /= nfound;
        return nruter;
    }

private:
    friend void save_bulk_hdf5(const char* filename,
                               const std::string& description,
                               const Crystal_structure& cell,
                               const Symmetry_operations& symmetries,
                               const Gamma_grid& grid,
                               const std::vector<Threeph_process>& processes,
                               const boost::mpi::communicator& comm);

    friend std::tuple<std::string,
                      std::unique_ptr<Crystal_structure>,
                      std::unique_ptr<Symmetry_operations>,
                      std::unique_ptr<Gamma_grid>,
                      std::unique_ptr<std::vector<Threeph_process>>>
    load_bulk_hdf5(const char* filename, const boost::mpi::communicator& comm);

    /// Cartesian coordinates of each q point.
    Eigen::MatrixXd cpos;
    /// Harmonic properties at each q point.
    std::vector<Spectrum_at_point> spectrum;
    /// Vector of equivalence classes. Two q points in the same
    /// equivalence class are related by a symmetry operation.
    /// Note that both space group symmetries and time reversal
    /// symmetry are taken into account.
    std::vector<std::vector<std::size_t>> equivalences;
    /// Detailed map between q points through the symmetry
    /// operations.
    std::vector<std::vector<std::size_t>> symmetry_map;
    /// Initialize the Cartesian coordinates of the q points.
    /// @param[in] poscar - a description of the crystal structure
    void initialize_cpos();

    /// Compute the spectrum at a subset of the q points.
    ///
    /// That subset is selected on the basis of this process'
    /// position in an MPI communicator.
    /// @param[in] factory - object in charge of building the
    /// dynamical matrix at each point.
    /// @param[in] symms - set of symmetry operations of the crystal
    /// @param[in] communicator - MPI communicator to use
    std::vector<Spectrum_at_point> compute_my_spectrum(
        const Dynamical_matrix_builder& factory,
        const Symmetry_operations& symms,
        const boost::mpi::communicator& communicator);

    /// Fill the equivalences vector.
    ///
    /// @param[in] symms - the set of symmetry operations to try.
    void fill_equivalences(const Symmetry_operations& symms);

    /// Parentlookup table: stores for each q-point the index of
    /// the representative of the equivalence class to which it
    /// belongs.
    std::vector<std::size_t> parentlookup;

    /// Fill the parentlookup table
    void fill_parentlookup();

    /// Fill the symmetry_map vector.
    ///
    /// @param[in] symms - the set of symmetry operations to try.
    void fill_map(const Symmetry_operations& symms);
};
} // namespace alma


#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/console/parse.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
//#include <pcl/visualization/pcl_visualizer.h>  // deprecated, treba nahradit
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>

int main(int argc, char *argv[])
{
  pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);

  std::string file_name = "/media/Data2/Data/GF2/cloud.pcd";
  pcl::PointCloud<pcl::PointXYZRGBNormal> my_cloud;
  pcl::PCDReader input;
  if (input.read(file_name, my_cloud, 0) == -1)
  {
    std::cout << "Failed." << std::endl;
    return -1;
  }

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;

  return 0;
}