#include "laser_slam_ros/laser_slam_worker.hpp"

#include "laser_slam/benchmarker.hpp"

//TODO clean
#include <Eigen/Eigenvalues>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <laser_slam/benchmarker.hpp>
#include <laser_slam/common.hpp>
#include <laser_slam_ros/common.hpp>
#include <laser_slam_ros/laser_slam_worker.hpp>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pointmatcher_ros/point_cloud.h>
#include <pointmatcher_ros/transform.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/String.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

namespace laser_slam_ros {

using namespace laser_slam;

LaserSlamWorker::LaserSlamWorker() { }

LaserSlamWorker::~LaserSlamWorker() { }

void LaserSlamWorker::init(
    ros::NodeHandle& nh, const LaserSlamWorkerParams& params,
    std::shared_ptr<laser_slam::IncrementalEstimator> incremental_estimator,
    unsigned int worker_id) {
  params_ = params;
  incremental_estimator_ = incremental_estimator;
  worker_id_ = worker_id;

  // Get the LaserTrack object from the IncrementalEstimator.
  laser_track_ = incremental_estimator_->getLaserTrack(worker_id);

  // Setup subscriber.
  scan_sub_ = nh.subscribe(params_.assembled_cloud_sub_topic, kScanSubscriberMessageQueueSize,
                           &LaserSlamWorker::scanCallback, this);

  // Setup publishers.
  trajectory_pub_ = nh.advertise<nav_msgs::Path>(params_.trajectory_pub_topic,
                                                 kPublisherQueueSize, true);

  if (params_.publish_local_map) {
    local_map_pub_ = nh.advertise<sensor_msgs::PointCloud2>(params_.local_map_pub_topic,
                                                            kPublisherQueueSize);
  }

  // Setup services.
  get_laser_track_srv_ = nh.advertiseService(
      "get_laser_track",
      &LaserSlamWorker::getLaserTracksServiceCall, this);
  export_trajectory_srv_ = nh.advertiseService(
      "export_trajectory",
      &LaserSlamWorker::exportTrajectoryServiceCall, this);

  voxel_filter_.setLeafSize(params_.voxel_size_m, params_.voxel_size_m,
                            params_.voxel_size_m);
  voxel_filter_.setMinimumPointsNumberPerVoxel(params_.minimum_point_number_per_voxel);

  // Set the first world to odom transform.
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> matrix;
  matrix.resize(4, 4);
  matrix = Eigen::Matrix<float, 4,4>::Identity();
  world_to_odom_ = PointMatcher_ros::eigenMatrixToStampedTransform<float>(
      matrix, params_.world_frame, params_.odom_frame, ros::Time::now());

  // TODO reactivate or rm.
  //  odometry_trajectory_pub_ = nh_.advertise<nav_msgs::Path>(params_.odometry_trajectory_pub_topic,
  //
  //  if (params_.publish_distant_map) {
  //    distant_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(params_.distant_map_pub_topic,
  //                                                               kPublisherQueueSize);
  //  }
  //  if (params_.publish_full_map) {
  //    point_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(params_.full_map_pub_topic,
  //                                                               kPublisherQueueSize);
  //  }
  //  new_fixed_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("new_fixed_cloud",
  //                                                               kPublisherQueueSize);
}

void LaserSlamWorker::scanCallback(const sensor_msgs::PointCloud2& cloud_msg_in) {
  std::lock_guard<std::recursive_mutex> lock_scan_callback(scan_callback_mutex_);
  if (!lock_scan_callback_) {
    if (tf_listener_.waitForTransform(params_.odom_frame, params_.sensor_frame,
                                      cloud_msg_in.header.stamp, ros::Duration(kTimeout_s))) {
      // Get the tf transform.
      tf::StampedTransform tf_transform;
      tf_listener_.lookupTransform(params_.odom_frame, params_.sensor_frame,
                                   cloud_msg_in.header.stamp, tf_transform);

      bool process_scan = false;
      SE3 current_pose;

      if (!last_pose_set_) {
        process_scan = true;
        last_pose_set_ = true;
        last_pose_ = tfTransformToPose(tf_transform).T_w;
      } else {
        current_pose = tfTransformToPose(tf_transform).T_w;
        float dist_m = distanceBetweenTwoSE3(current_pose, last_pose_);
        if (dist_m > params_.minimum_distance_to_add_pose) {
          process_scan = true;
          last_pose_ = current_pose;
        }
      }

      if (process_scan) {
        // Convert input cloud to laser scan.
        LaserScan new_scan;
        new_scan.scan = PointMatcher_ros::rosMsgToPointMatcherCloud<float>(cloud_msg_in);
        new_scan.time_ns = rosTimeToCurveTime(cloud_msg_in.header.stamp.toNSec());

        // Process the new scan and get new values and factors.
        gtsam::NonlinearFactorGraph new_factors;
        gtsam::Values new_values;
        bool is_prior;
        if (params_.use_odometry_information) {
          laser_track_->processPoseAndLaserScan(tfTransformToPose(tf_transform), new_scan,
                                                &new_factors, &new_values, &is_prior);
        } else {
          Pose new_pose;

          Time new_pose_time_ns = tfTransformToPose(tf_transform).time_ns;

          if (laser_track_->getNumScans() > 2u) {
            Pose current_pose = laser_track_->getCurrentPose();

            if (current_pose.time_ns > new_pose_time_ns - current_pose.time_ns) {
              Time previous_pose_time = current_pose.time_ns -
                  (new_pose_time_ns - current_pose.time_ns);
              if (previous_pose_time >= laser_track_->getMinTime() &&
                  previous_pose_time <= laser_track_->getMaxTime()) {
                SE3 previous_pose = laser_track_->evaluate(previous_pose_time);
                new_pose.T_w = last_pose_sent_to_laser_track_.T_w *
                    previous_pose.inverse()  * current_pose.T_w ;
                new_pose.T_w = SE3(SO3::fromApproximateRotationMatrix(
                    new_pose.T_w.getRotation().getRotationMatrix()), new_pose.T_w.getPosition());
              }
            }
          }

          new_pose.time_ns = new_pose_time_ns;
          laser_track_->processPoseAndLaserScan(new_pose, new_scan,
                                                &new_factors, &new_values, &is_prior);

          last_pose_sent_to_laser_track_ = new_pose;
        }

        // Process the new values and factors.
        gtsam::Values result;
        if (is_prior) {
          result = incremental_estimator_->registerPrior(new_factors, new_values, worker_id_);
        } else {
          result = incremental_estimator_->estimate(new_factors, new_values, new_scan.time_ns);
        }

        // Update the trajectory.
        laser_track_->updateFromGTSAMValues(result);

        // Adjust the correction between the world and odom frames.
        Pose current_pose = laser_track_->getCurrentPose();
        SE3 T_odom_sensor = tfTransformToPose(tf_transform).T_w;
        SE3 T_w_sensor = current_pose.T_w;
        SE3 T_w_odom = T_w_sensor * T_odom_sensor.inverse();

        Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> matrix;

        // TODO resize needed?
        matrix.resize(4, 4);
        matrix = T_w_odom.getTransformationMatrix().cast<float>();

        {
          std::lock_guard<std::recursive_mutex> lock_world_to_odom(world_to_odom_mutex_);
          world_to_odom_ = PointMatcher_ros::eigenMatrixToStampedTransform<float>(
              matrix, params_.world_frame, params_.odom_frame, cloud_msg_in.header.stamp);
        }

        publishTrajectories();

        // Get the last cloud in world frame.
        DataPoints new_fixed_cloud;
        laser_track_->getLocalCloudInWorldFrame(laser_track_->getMaxTime(), &new_fixed_cloud);

        // Transform the cloud in sensor frame
        //TODO(Renaud) move to a transformPointCloud() fct.
        //      laser_slam::PointMatcher::TransformationParameters transformation_matrix =
        //          T_w_sensor.inverse().getTransformationMatrix().cast<float>();
        //
        //      laser_slam::correctTransformationMatrix(&transformation_matrix);
        //
        //      laser_slam::PointMatcher::Transformation* rigid_transformation =
        //          laser_slam::PointMatcher::get().REG(Transformation).create("RigidTransformation");
        //      CHECK_NOTNULL(rigid_transformation);
        //
        //      laser_slam::PointMatcher::DataPoints fixed_cloud_in_sensor_frame =
        //          rigid_transformation->compute(new_fixed_cloud,transformation_matrix);
        //
        //
        //      new_fixed_cloud_pub_.publish(
        //          PointMatcher_ros::pointMatcherCloudToRosMsg<float>(fixed_cloud_in_sensor_frame,
        //                                                             params_.sensor_frame,
        //                                                             cloud_msg_in.header.stamp));

        PointCloud new_fixed_cloud_pcl = lpmToPcl(new_fixed_cloud);

        if (params_.remove_ground_from_local_map) {
          const double robot_height_m = current_pose.T_w.getPosition()(2);
          PointCloud new_fixed_cloud_no_ground;
          for (size_t i = 0u; i < new_fixed_cloud_pcl.size(); ++i) {
            if (new_fixed_cloud_pcl.points[i].z > robot_height_m -
                params_.ground_distance_to_robot_center_m) {
              new_fixed_cloud_no_ground.push_back(new_fixed_cloud_pcl.points[i]);
            }
          }
          new_fixed_cloud_no_ground.width = 1;
          new_fixed_cloud_no_ground.height = new_fixed_cloud_no_ground.points.size();
          new_fixed_cloud_pcl = new_fixed_cloud_no_ground;
        }

        // Add the local scans to the full point cloud.
        if (params_.create_filtered_map) {
          if (new_fixed_cloud_pcl.size() > 0u) {
            std::lock_guard<std::recursive_mutex> lock(local_map_mutex_);
            if (local_map_.size() > 0u) {
              local_map_ += new_fixed_cloud_pcl;
            } else {
              local_map_ = new_fixed_cloud_pcl;
            }
            local_map_queue_.push_back(new_fixed_cloud_pcl);
          }
        }
      }
    } else {
      ROS_WARN_STREAM("[SegMapper] Timeout while waiting between " + params_.odom_frame  +
                      " and " + params_.sensor_frame  + ".");
    }
  }
}

void LaserSlamWorker::setLockScanCallback(bool new_state) {
  std::lock_guard<std::recursive_mutex> lock(scan_callback_mutex_);
  lock_scan_callback_ = new_state;
}

bool LaserSlamWorker::getLaserTracksServiceCall(
    laser_slam_ros::GetLaserTrackSrv::Request& request,
    laser_slam_ros::GetLaserTrackSrv::Response& response) {
  std::vector<std::shared_ptr<LaserTrack> > laser_tracks =
      incremental_estimator_->getAllLaserTracks();
  Trajectory trajectory;
  ros::Time scan_stamp;
  tf::StampedTransform tf_transform;
  geometry_msgs::TransformStamped ros_transform;
  std::vector<std::tuple<laser_slam::Time, sensor_msgs::PointCloud2, geometry_msgs::TransformStamped> > data;
  for (const auto& track: laser_tracks) {
    track->getTrajectory(&trajectory);
    for (const auto& scan: track->getLaserScans()) {
      // Get data.
      scan_stamp.fromNSec(curveTimeToRosTime(scan.time_ns));

      sensor_msgs::PointCloud2 pc = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(
	scan.scan, params_.sensor_frame, scan_stamp);

      tf_transform = PointMatcher_ros::eigenMatrixToStampedTransform<float>(
          trajectory.at(scan.time_ns).getTransformationMatrix().cast<float>(),
          params_.world_frame,
          params_.sensor_frame,
          scan_stamp);
      tf::transformStampedTFToMsg(tf_transform, ros_transform);

      data.push_back(std::make_tuple(scan.time_ns, pc, ros_transform));
    }
  }

  std::sort(data.begin(),data.end(),
       [](const std::tuple<laser_slam::Time, sensor_msgs::PointCloud2, geometry_msgs::TransformStamped>& a,
       const std::tuple<laser_slam::Time, sensor_msgs::PointCloud2, geometry_msgs::TransformStamped>& b) -> bool
       {
         return std::get<0>(a) <= std::get<0>(b);
       });

  bool zero_added = false;
  // Fill response.
  for (const auto& elem : data) {
    laser_slam::Time time;
    sensor_msgs::PointCloud2 pc;
    geometry_msgs::TransformStamped tf;
    std::tie(time, pc, tf) = elem;
    LOG(INFO) << "Time " << time;
    if (time == 0u) {
      if (!zero_added) {
	zero_added = true;
      } else {
	continue;
      }
    }
    response.laser_scans.push_back(pc);
    response.transforms.push_back(tf);
  }

  return true;
}

void LaserSlamWorker::publishTrajectory(const Trajectory& trajectory,
                                        const ros::Publisher& publisher) const {
  nav_msgs::Path traj_msg;
  traj_msg.header.frame_id = params_.world_frame;
  Time traj_time = curveTimeToRosTime(trajectory.rbegin()->first);
  traj_msg.header.stamp.fromNSec(traj_time);

  for (const auto& timePose : trajectory) {
    geometry_msgs::PoseStamped pose_msg;
    pose_msg.header = traj_msg.header;
    pose_msg.header.stamp.fromNSec(curveTimeToRosTime(timePose.first));

    //TODO functionize
    pose_msg.pose.position.x = timePose.second.getPosition().x();
    pose_msg.pose.position.y = timePose.second.getPosition().y();
    pose_msg.pose.position.z = timePose.second.getPosition().z();
    pose_msg.pose.orientation.w = timePose.second.getRotation().w();
    pose_msg.pose.orientation.x = timePose.second.getRotation().x();
    pose_msg.pose.orientation.y = timePose.second.getRotation().y();
    pose_msg.pose.orientation.z = timePose.second.getRotation().z();
    traj_msg.poses.push_back(pose_msg);
  }
  publisher.publish(traj_msg);
}

void LaserSlamWorker::publishMap() {
  // TODO make thread safe.
  if (local_map_.size() > 0) {
    PointCloud filtered_map;
    getFilteredMap(&filtered_map);

    //maximumNumberPointsFilter(&filtered_map);
    //    if (params_.publish_full_map) {
    //      sensor_msgs::PointCloud2 msg;
    //      convert_to_point_cloud_2_msg(filtered_map, params_.world_frame, &msg);
    //      point_cloud_pub_.publish(msg);
    //    }
    if (params_.publish_local_map) {
      sensor_msgs::PointCloud2 msg;
      {
        std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
        convert_to_point_cloud_2_msg(local_map_filtered_, params_.world_frame, &msg);
      }
      local_map_pub_.publish(msg);
    }
    //    if (params_.publish_distant_map) {
    //      sensor_msgs::PointCloud2 msg;
    //      convert_to_point_cloud_2_msg(distant_map_, params_.world_frame, &msg);
    //      distant_map_pub_.publish(msg);
    //    }
  }
}

void LaserSlamWorker::publishTrajectories() {
  Trajectory trajectory;
  laser_track_->getTrajectory(&trajectory);
  if (trajectory.size() > 0u) publishTrajectory(trajectory, trajectory_pub_);
}

// TODO can we move?
Pose LaserSlamWorker::tfTransformToPose(const tf::StampedTransform& tf_transform) {
  // Register new pose.
  Pose pose;
  SE3::Position pos(tf_transform.getOrigin().getX(), tf_transform.getOrigin().getY(),
                    tf_transform.getOrigin().getZ());
  SE3::Rotation::Implementation rot(tf_transform.getRotation().getW(),
                                    tf_transform.getRotation().getX(),
                                    tf_transform.getRotation().getY(),
                                    tf_transform.getRotation().getZ());
  pose.T_w = SE3(pos, rot);
  pose.time_ns = rosTimeToCurveTime(tf_transform.stamp_.toNSec());

  return pose;
}

Time LaserSlamWorker::rosTimeToCurveTime(const Time& timestamp_ns) {
  if (!base_time_set_) {
    base_time_ns_ = timestamp_ns;
    base_time_set_ = true;
  }
  return timestamp_ns - base_time_ns_;
}

Time LaserSlamWorker::curveTimeToRosTime(const Time& timestamp_ns) const {
  CHECK(base_time_set_);
  return timestamp_ns + base_time_ns_;
}

std::vector<laser_slam_ros::PointCloud> LaserSlamWorker::getQueuedPoints() {
  std::lock_guard<std::recursive_mutex> lock(local_map_mutex_);
  std::vector<laser_slam_ros::PointCloud> new_points;
  new_points.swap(local_map_queue_);
  return new_points;
}

// TODO one shot of cleaning.
void LaserSlamWorker::getFilteredMap(PointCloud* filtered_map) {
  laser_slam::Pose current_pose = laser_track_->getCurrentPose();

  PclPoint current_position;
  current_position.x = current_pose.T_w.getPosition()[0];
  current_position.y = current_pose.T_w.getPosition()[1];
  current_position.z = current_pose.T_w.getPosition()[2];

  // Apply the cylindrical filter on the local map and get a copy.
  PointCloud local_map;
  {
    std::lock_guard<std::recursive_mutex> lock(local_map_mutex_);
    local_map = local_map_;
    applyCylindricalFilter(current_position, params_.distance_to_consider_fixed,
                           40, false, &local_map_);
  }
  // Apply a voxel filter.
  laser_slam::Clock clock;

  PointCloudPtr local_map_ptr(new PointCloud());
  pcl::copyPointCloud<PclPoint, PclPoint>(local_map, *local_map_ptr);

  PointCloud local_map_filtered;

  voxel_filter_.setInputCloud(local_map_ptr);
  voxel_filter_.filter(local_map_filtered);

  clock.takeTime();

  if (params_.separate_distant_map) {
    // If separating the map is enabled, the distance between each point in the local_map_ will
    // be compared to the current robot position. Points which are far from the robot will
    // be transfered to the distant_map_. This is helpful for publishing (points in distant_map_
    // need to be filtered only once) and for any other processing which needs to be done only
    // when a map is distant from robot and can be assumed as static (until loop closure).

    // TODO(renaud) Is there a way to separate the cloud without having to transform in sensor
    // frame by setting the position to compute distance from?
    // Transform local_map_ in sensor frame.
    clock.start();

    // Save before removing points.
    PointCloud new_distant_map = local_map_filtered;

    applyCylindricalFilter(current_position, params_.distance_to_consider_fixed,
                           40, false, &local_map_filtered);

    applyCylindricalFilter(current_position, params_.distance_to_consider_fixed,
                           40, true, &new_distant_map);
    {
      std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
      local_map_filtered_ = local_map_filtered;
    }

    // Add the new_distant_map to the distant_map_.
    // TODO add lock if used
    if (distant_map_.size() > 0u) {
      distant_map_ += new_distant_map;
    } else {
      distant_map_ = new_distant_map;
    }

    *filtered_map = local_map_filtered;
    *filtered_map += distant_map_;

    clock.takeTime();
    // LOG(INFO) << "new_local_map.size() " << local_map.size();
    // LOG(INFO) << "new_distant_map.size() " << new_distant_map.size();
    // LOG(INFO) << "distant_map_.size() " << distant_map_.size();
    // LOG(INFO) << "Separating done! Took " << clock.getRealTime() << " ms.";
  } else {
    *filtered_map = local_map;
  }
}

void LaserSlamWorker::getLocalMapFiltered(PointCloud* local_map_filtered) {
  CHECK_NOTNULL(local_map_filtered);
  std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
  *local_map_filtered = local_map_filtered_;
}

void LaserSlamWorker::clearLocalMap() {
  {
    std::lock_guard<std::recursive_mutex> lock(local_map_mutex_);
    local_map_.clear();
  }

  {
    std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
    local_map_filtered_.clear();
  }
}

tf::StampedTransform LaserSlamWorker::getWorldToOdom() {
  std::lock_guard<std::recursive_mutex> lock_world_to_odom(world_to_odom_mutex_);
  tf::StampedTransform world_to_odom = world_to_odom_;
  return world_to_odom;
}

void LaserSlamWorker::getTrajectory(Trajectory* out_trajectory) const {
  laser_track_->getTrajectory(out_trajectory);
}

void LaserSlamWorker::getOdometryTrajectory(Trajectory* out_trajectory) const {
  laser_track_->getOdometryTrajectory(out_trajectory);
}

void LaserSlamWorker::updateLocalMap(const SE3& last_pose_before_update,
                                     const laser_slam::Time last_pose_before_update_timestamp_ns) {

  Trajectory new_trajectory;
  laser_track_->getTrajectory(&new_trajectory);

  SE3 new_last_pose = new_trajectory.at(last_pose_before_update_timestamp_ns);

  const Eigen::Matrix4f transform_matrix = (new_last_pose * last_pose_before_update.inverse()).
      getTransformationMatrix().cast<float>();
  {
    std::lock_guard<std::recursive_mutex> lock(local_map_mutex_);
    pcl::transformPointCloud(local_map_, local_map_, transform_matrix);
  }
  {
    std::lock_guard<std::recursive_mutex> lock(local_map_filtered_mutex_);
    pcl::transformPointCloud(local_map_filtered_, local_map_filtered_, transform_matrix);
  }
}

laser_slam::SE3 LaserSlamWorker::getTransformBetweenPoses(
    const laser_slam::SE3& start_pose, const laser_slam::Time end_pose_timestamp_ns) const {
  Trajectory new_trajectory;
  laser_track_->getTrajectory(&new_trajectory);

  SE3 last_pose = new_trajectory.at(end_pose_timestamp_ns);
  return last_pose * start_pose.inverse();
}

void LaserSlamWorker::exportTrajectories() const {
  Trajectory traj;
  laser_track_->getTrajectory(&traj);
  Eigen::MatrixXd matrix;
  matrix.resize(traj.size(), 4);
  unsigned int i = 0u;
  for (const auto& pose : traj) {
    matrix(i,0) = pose.first;
    matrix(i,1) = pose.second.getPosition()(0);
    matrix(i,2) = pose.second.getPosition()(1);
    matrix(i,3) = pose.second.getPosition()(2);
    ++i;
  }
  writeEigenMatrixXdCSV(matrix, "/tmp/trajectory_" + std::to_string(worker_id_) + ".csv");
}

void LaserSlamWorker::exportTrajectoryHead(laser_slam::Time head_duration_ns,
                                           const std::string& filename) const {
  Eigen::MatrixXd matrix;
  Trajectory traj;
  laser_track_->getTrajectory(&traj);
  CHECK_GE(traj.size(), 1u);
  matrix.resize(traj.size(), 4);

  const Time traj_end_ns = traj.rbegin()->first;
  Time head_start_ns;
  if (traj_end_ns > head_duration_ns) {
    head_start_ns = traj_end_ns - head_duration_ns;
  } else {
    head_start_ns = 0u;
  }

  unsigned int i = 0u;
  for (const auto& pose : traj) {
    if (pose.first > head_start_ns) {
      matrix(i,0) = pose.first;
      matrix(i,1) = pose.second.getPosition()(0);
      matrix(i,2) = pose.second.getPosition()(1);
      matrix(i,3) = pose.second.getPosition()(2);
      ++i;
    }
  }
  matrix.conservativeResize(i, 4);
  writeEigenMatrixXdCSV(matrix, filename);
  LOG(INFO) << "Exported " << i << " trajectory poses to " << filename << ".";
}

bool LaserSlamWorker::exportTrajectoryServiceCall(std_srvs::Empty::Request& req,
                                                  std_srvs::Empty::Response& res) {
  exportTrajectoryHead(laser_track_->getMaxTime(),
                       "/tmp/online_matcher/trajectory.csv");
  return true;
}

} // namespace laser_slam_ros


/*=========================================================================

  Program:   NeuroLib (DTI command line tools)
  Language:  C++
  Date:      $Date: 2008-08-06 12:51:30 -0400 (Wed, 06 Aug 2008) $
  Version:   $Revision: 597 $
  Author:    Marc Niethammer (mn@cs.unc.edu)
             Casey Goodlett (gcasey@sci.utah.edu)

  Copyright (c)  Casey Goodlett. All rights reserved.
  See NeuroLibCopyright.txt or http://www.ia.unc.edu/dev/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

// This program creates a DWI atlas from a set of DWIs and associated
// deformation maps to a common atlas space

// Authors: Sami Benzaid, Casey Goodlett, Marc Niethammer, 2010

// STL includes
#include <string>
#include <iostream>
#include <iomanip>
#include <cmath>
#include <math.h>

// boost includes
#include <boost/program_options/option.hpp>
#include <boost/program_options/positional_options.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/cmdline.hpp>

#include "boost/date_time/gregorian/gregorian.hpp"
#include "boost/date_time/posix_time/posix_time.hpp"

// ITK includes
// datastructures
#include <itkImage.h>
#include <itkVector.h>
#include <itkMetaDataObject.h>

// IO
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>

// Filters
#include <itkTensorFractionalAnisotropyImageFilter.h>
#include <itkShiftScaleImageFilter.h>
#include <itkVectorIndexSelectionCastImageFilter.h>
#include <itkBinaryThresholdImageFilter.h>

#include <itkNthElementImageAdaptor.h>

#include "itkVectorMaskNegatedImageFilter.h"
#include "itkVectorMaskImageFilter.h"
#include "itkDiffusionTensor3DReconstructionLinearImageFilter.h"

#include "itkHFieldToDeformationFieldImageFilter.h"
#include "itkDeformationFieldJacobianFilter.h"

#include <vnl/algo/vnl_svd.h>
#include <vnl/vnl_inverse.h>

#include "dtitypes.h"
#include "deformationfieldio.h"

#include "dwiAtlasCLP.h"

#include "auxFunctions.h"
#include "sphericalHarmonicsFunctions.h"

#include <itkVariableLengthVector.h>
#include <ctime>

#include "itkDWIAtlasBuilder.h"

const char* NRRD_MEASUREMENT_KEY = "NRRD_measurement frame";

namespace po = boost::program_options;

// Define necessary types for images
// typedef double MyRealType;
typedef float MyRealType;

// const unsigned int DIM = 3;

typedef unsigned short                      DWIPixelType;
typedef itk::VectorImage<DWIPixelType, DIM> VectorImageType;
typedef itk::Image<DWIPixelType, DIM>       ScalarImageType;
typedef itk::Image<unsigned char, DIM>      MaskImageType;
typedef itk::Image<DWIPixelType, DIM>       IntImageType;
typedef itk::DiffusionTensor3DReconstructionLinearImageFilter<DWIPixelType, MyRealType>
  DiffusionEstimationFilterType;
typedef DiffusionEstimationFilterType::GradientDirectionContainerType GradientDirectionContainerType;
typedef vnl_vector_fixed<MyRealType, 3>                               MyGradientType;

typedef itk::DWIAtlasBuilder<MyRealType, DWIPixelType> DWIAtlasBuilderType;

int outputAllSettings(int argc, char* argv[] )
{
  PARSE_ARGS;

  std::cout << "Parameter settings:" << std::endl;
  std::cout << "-----------------------------------------" << std::endl;
  std::cout << "output volume        = " << dwiOutputVolume << std::endl;
  std::cout << "gradient vector file = " << gradientVectorFile << std::endl;
  std::cout << "case file            = " << sCaseFile << std::endl;
  std::cout << "outlierImageName     = " << outlierImageName << std::endl;
  std::cout << "maskImageFileName    = " << maskImageFileName << std::endl;
  std::cout << "h-field              = " << bHField << std::endl;
  std::cout << "bJustDoResampling    = " << bJustDoResampling << std::endl;
  std::cout << "verbose              = " << bVERBOSE << std::endl;
  std::cout << "SHOrder              = " << SHOrder << std::endl;
  std::cout << "lambda               = " << dLambda << std::endl;
  std::cout << "nrOfThreads          = " << nrOfThreads << std::endl;
  std::cout << "interpolationType    = " << interpolationType << std::endl;
  std::cout << "averagingType        = " << averagingType << std::endl;
  std::cout << "bNoLogFit            = " << bNoLogFit << std::endl;
  std::cout << "dScalingFactor       = " << dScalingFactor << std::endl;
  std::cout << "dHuberC              = " << dHuberC << std::endl;
  std::cout << "bDoWeightedLSEst     = " << bDoWeightedLSEst << std::endl;
  std::cout << "dRiceSigma           = " << dRiceSigma << std::endl;
  std::cout << "iLogMinArgumentValue = " << iLogMinArgumentValue << std::endl;
  std::cout << "iWLSIter             = " << iWLSIter << std::endl;
  std::cout << "-----------------------------------------" << std::endl;
  std::cout << std::endl << std::endl << std::endl;

  return 0;

}

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

//  unsigned int scale;

  PARSE_ARGS;

  if( bVERBOSE )
    {
    outputAllSettings( argc, argv );
    }

  DWIAtlasBuilderType::Pointer pAtlasBuilder = DWIAtlasBuilderType::New();

  // Set all the command-line arguments we specified

  // the case file is the input; the filter takes care of loading and
  // processing everything
  pAtlasBuilder->SetInput( &sCaseFile );

  pAtlasBuilder->SetVerbose( bVERBOSE );
  pAtlasBuilder->SetIsHField( bHField );
  pAtlasBuilder->SetJustDoResampling( bJustDoResampling );
  pAtlasBuilder->SetGradientVectorFile( gradientVectorFile );
  pAtlasBuilder->SetSHOrder( SHOrder );
  pAtlasBuilder->SetLambda( dLambda );
  pAtlasBuilder->SetDoWeightedLS( bDoWeightedLSEst );
  pAtlasBuilder->SetRiceSigma( (MyRealType)dRiceSigma );
  pAtlasBuilder->SetHuberC( (MyRealType)dHuberC );
  pAtlasBuilder->SetInterpolationType( interpolationType );
  pAtlasBuilder->SetAveragingType( averagingType );
  pAtlasBuilder->SetNrOfThreads( nrOfThreads );
  pAtlasBuilder->SetNoLogFit( bNoLogFit );
  pAtlasBuilder->SetScalingFactor( (MyRealType)dScalingFactor );
  pAtlasBuilder->SetLogMinArgumentValue( (DWIPixelType)iLogMinArgumentValue );
  pAtlasBuilder->SetNrOfWLSIterations( (unsigned int)iWLSIter );
  pAtlasBuilder->SetMaskImageFileName( maskImageFileName );

  if( bNoLogFit && bDoWeightedLSEst )
    {
    std::cout
    << "ERROR: WLS in the original signal domain is not supported. Remove --noLogFit specifier and try again. ABORT."
    << std::endl;
    return EXIT_FAILURE;
    }

  if( iLogMinArgumentValue != 1 )
    {
    std::cout << "WARNING: iLogMinArgumentValue should always be one, but it is " << iLogMinArgumentValue << std::endl;
    std::cout << "Unless you really know what you are doing, do NOT proceed." << std::endl;
    }

  if( bVERBOSE )
    {
    using namespace boost::posix_time;
    ptime now = second_clock::local_time();
    // write out time when the computation started
    std::cout << "Processing started at: " << now << std::endl << std::endl;
    }

  pAtlasBuilder->Update();

  if( bVERBOSE )
    {
    std::cerr << "Writing the final result to: " << dwiOutputVolume << std::endl;
    }

  // Write reconstructed DWI file
  try
    {
    typedef itk::ImageFileWriter<VectorImageType> VectorFileWriterType;

    VectorFileWriterType::Pointer dwiWriter = VectorFileWriterType::New();
    dwiWriter->SetFileName( dwiOutputVolume.c_str() );

    dwiWriter->SetInput( pAtlasBuilder->GetOutput() );
    dwiWriter->SetUseCompression(true);
    dwiWriter->Update();

    }
  catch( itk::ExceptionObject e )
    {
    std::cerr << e << std::endl;
    return EXIT_FAILURE;
    }

  if( outlierImageName.compare("None") != 0 )
    {
    if( bVERBOSE )
      {
      std::cout << "Writing outlier image to " << outlierImageName << " ... ";
      }

    typedef itk::ImageFileWriter<ScalarImageType> ScalarFileWriterType;

    ScalarFileWriterType::Pointer scalarWriter = ScalarFileWriterType::New();
    scalarWriter->SetFileName( outlierImageName.c_str() );

    scalarWriter->SetInput( pAtlasBuilder->GetOutlierImage() );
    scalarWriter->SetUseCompression(true);
    scalarWriter->Update();

    if( bVERBOSE )
      {
      std::cout << "done." << std::endl;
      }
    }

  return EXIT_SUCCESS;
}


#include <vsim/env/mesh.hpp>
#include <vsim/env/material.hpp>
#include <vsim/env/light.hpp>
#include <vsim/env/node.hpp>
#include <vsim/env/geometry.hpp>
#include <vsim/env/drawable.hpp>

#include <vsim/util/filesystem.hpp>

#include "scene_loader.hpp"

#include <assimp/scene.h>
#include <assimp/postprocess.h>
#include <assimp/cimport.h>

#include <Eigen/Dense>

using namespace std ;
using namespace Eigen ;

using namespace vsim ;

static Vector4f color4_to_float4(const aiColor4D &c) {
    return Vector4f(c.r, c.g, c.b, c.a) ;
}

static void importMaterial(Material &data, const struct aiMaterial *mtl, const string &model_path) {
    data.type_ = Material::PHONG ;

    aiColor4D diffuse, specular, ambient, emission;

    float shininess, strength;
    unsigned int max;

    if ( AI_SUCCESS == aiGetMaterialColor(mtl, AI_MATKEY_COLOR_DIFFUSE, &diffuse))
        data.diffuse_.set<Vector4f>(color4_to_float4(diffuse));

    if( AI_SUCCESS == aiGetMaterialColor(mtl, AI_MATKEY_COLOR_SPECULAR, &specular) )
        data.specular_.set<Vector4f>(color4_to_float4(specular));

    if( AI_SUCCESS == aiGetMaterialColor(mtl, AI_MATKEY_COLOR_AMBIENT, &ambient) )
        data.ambient_.set<Vector4f>(color4_to_float4(ambient)) ;

    if( AI_SUCCESS == aiGetMaterialColor(mtl, AI_MATKEY_COLOR_EMISSIVE, &emission) )
        data.emission_.set<Vector4f>(color4_to_float4(emission));

    int shading_model ;
    mtl->Get((const char *)AI_MATKEY_SHADING_MODEL, shading_model);

    switch ( shading_model ) {
    case aiShadingMode_Flat:
        data.type_ = Material::CONSTANT ;
        break ;
    case aiShadingMode_Gouraud:
        data.type_ = Material::LAMBERTIAN ;
        break ;
    case aiShadingMode_Phong:
        data.type_ = Material::PHONG ;
        break ;
    case aiShadingMode_Blinn:
        data.type_ = Material::BLINN ;
        break ;
    default:
        break ;
    }

    max = 1;
    aiReturn ret1 = aiGetMaterialFloatArray(mtl, AI_MATKEY_SHININESS, &shininess, &max);

    if ( ret1 == AI_SUCCESS ) {
        max = 1;
        aiReturn ret2 = aiGetMaterialFloatArray(mtl, AI_MATKEY_SHININESS_STRENGTH, &strength, &max);
        if(ret2 == AI_SUCCESS)
            data.shininess_.set<float>(shininess * strength) ;
        else
            data.shininess_.set<float>(shininess) ;
    }
    else {
        data.shininess_.set<float>(0) ;
        data.specular_.set<Vector4f>(0, 0, 0, 0) ;
    }

    string model_dir, model_base, model_ext ;
    util::split_path(model_path, model_dir, model_base, model_ext) ;

    aiString tex_path ;
    aiTextureMapping tmap ;
    aiTextureMapMode mode ;

    if ( AI_SUCCESS == mtl->GetTexture(aiTextureType_DIFFUSE, 0, &tex_path, &tmap, 0, 0, 0, &mode) ) {
        string file_name(tex_path.data, tex_path.length) ;

        Sampler2D sampler ;
        sampler.image_url_ = model_dir + '/' + file_name ;

        data.texture_.set<Sampler2D>(sampler) ;
    }
}

static bool importMaterials(ModelPtr &scene, const string &mpath, const aiScene *sc, map<const aiMaterial *, MaterialPtr> &material_map) {
    for( uint m=0 ; m<sc->mNumMaterials ; m++ ) {
        const aiMaterial *material = sc->mMaterials[m] ;
        MaterialPtr smat(new Material) ;
        importMaterial(*smat, material, mpath) ;
        scene->materials_.push_back(smat) ;
        material_map[material] = smat ;
    }

    return true ;
}

static bool importMeshes(ModelPtr &scene, const aiScene *sc, map<const aiMesh *, MeshPtr> &mesh_map) {

    for( uint m=0 ; m<sc->mNumMeshes ; m++ ) {
        const aiMesh *mesh = sc->mMeshes[m] ;

     //   if ( mesh->mPrimitiveTypes != aiPrimitiveType_TRIANGLE ) continue ;

        MeshPtr smesh(new Mesh) ;

        if ( mesh->mPrimitiveTypes == aiPrimitiveType_TRIANGLE ) smesh->ptype_ = Mesh::Triangles ;
        else if ( mesh->mPrimitiveTypes == aiPrimitiveType_LINE ) smesh->ptype_ = Mesh::Lines ;
        else if ( mesh->mPrimitiveTypes == aiPrimitiveType_POINT ) smesh->ptype_ = Mesh::Points ;
        else continue ;

        if ( mesh->HasPositions() ) {
            uint n = mesh->mNumVertices ;
            smesh->vertices_.resize(n) ;

            for(int i = 0; i < n; ++i)
                smesh->vertices_[i] = Vector3f(mesh->mVertices[i].x, mesh->mVertices[i].y, mesh->mVertices[i].z) ;
        }

        if ( mesh->HasNormals() ) {
            uint n = mesh->mNumVertices ;
            smesh->normals_.resize(n) ;

            for(int i = 0; i < n; ++i)
                smesh->normals_[i] = Vector3f(mesh->mNormals[i].x, mesh->mNormals[i].y, mesh->mNormals[i].z) ;
        }

        if ( mesh->HasVertexColors(0) ) {
            uint n = mesh->mNumVertices ;
            smesh->colors_.resize(n) ;

            for(int i = 0; i < n; ++i)
                smesh->colors_[i] = Vector3f(mesh->mColors[0][i].r, mesh->mColors[0][i].g, mesh->mColors[0][i].b) ;
        }

        for( uint t=0 ; t<MAX_MESH_TEXTURES ; t++ ) {
            uint n = mesh->mNumVertices ;
            if ( mesh->HasTextureCoords(t) ) {
                smesh->tex_coords_[t].resize(n) ;

                for(int i = 0; i < n; ++i)
                    smesh->tex_coords_[t][i] = Vector2f(mesh->mTextureCoords[t][i].x, mesh->mTextureCoords[t][i].y) ;
            }
        }

        if ( mesh->HasFaces() ) {
            uint n = mesh->mNumFaces ;
            smesh->vertex_indices_.resize(n * 3) ;

            for(int i = 0, k=0; i < n ; i++) {
                smesh->vertex_indices_[k++] = mesh->mFaces[i].mIndices[0];
                smesh->vertex_indices_[k++] = mesh->mFaces[i].mIndices[1];
                smesh->vertex_indices_[k++] = mesh->mFaces[i].mIndices[2];
            }
        }

        scene->meshes_.push_back(smesh) ;
        mesh_map[mesh] = smesh ;
    }

    return true ;
}

static bool importLights(ModelPtr &scene, const aiScene *sc) {
    for( uint m=0 ; m<sc->mNumLights ; m++ ) {
        const aiLight *light = sc->mLights[m] ;
        aiNode *lnode = sc->mRootNode->FindNode(light->mName) ;

        LightPtr slight ;

        // find global transformation of the light by accumulating transforms in the scene graph

        Matrix4f tf  = Matrix4f::Identity();

        while ( lnode ) {
            aiMatrix4x4 m = lnode->mTransformation;
            Matrix4f lf ;
            lf << m.a1, m.a2, m.a3, m.a4,
                    m.b1, m.b2, m.b3, m.b4,
                    m.c1, m.c2, m.c3, m.c4,
                    m.d1, m.d2, m.d3, m.d4 ;
            tf *= lf ;
            lnode = lnode->mParent ;
        }

        switch ( light->mType ) {
        case aiLightSource_DIRECTIONAL:
        {
            DirectionalLight *dl = new DirectionalLight ;
            dl->type_ = Light::DIRECTIONAL ;
            Vector4f tdir = tf * Vector4f(light->mDirection.x, light->mDirection.y, light->mDirection.z, 1) ;
            dl->direction_ = tdir.block<3, 1>(0, 0) ;
            dl->color_ << light->mColorDiffuse.r, light->mColorDiffuse.g, light->mColorDiffuse.b ;
            slight.reset(dl) ;
            break ;
        }
        case aiLightSource_POINT:
        {
            PointLight *pl = new PointLight ;
            pl->type_ = Light::POINT ;
            Vector4f tpos = tf * Vector4f(light->mPosition.x, light->mPosition.y, light->mPosition.z, 1) ;
            pl->position_ = tpos.block<3, 1>(0, 0) ;
            pl->color_ << light->mColorDiffuse.r, light->mColorDiffuse.g, light->mColorDiffuse.b ;
            pl->constant_attenuation_ = light->mAttenuationConstant ;
            pl->linear_attenuation_ = light->mAttenuationLinear ;
            pl->quadratic_attenuation_ = light->mAttenuationQuadratic ;

            slight.reset(pl) ;
            break ;

        }
        case aiLightSource_SPOT:
        {
            SpotLight *sl = new SpotLight ;
            sl->type_ = Light::SPOT ;

            Vector4f tpos = tf * Vector4f(light->mPosition.x, light->mPosition.y, light->mPosition.z, 1) ;
            sl->position_ = tpos.block<3, 1>(0, 0) ;

            Vector4f tdir = tf * Vector4f(light->mDirection.x, light->mDirection.y, light->mDirection.z, 1) ;
            sl->direction_ = tdir.block<3, 1>(0, 0) ;

            sl->color_ << light->mColorDiffuse.r, light->mColorDiffuse.g, light->mColorDiffuse.b ;
            sl->constant_attenuation_ = light->mAttenuationConstant ;
            sl->linear_attenuation_ = light->mAttenuationLinear ;
            sl->quadratic_attenuation_ = light->mAttenuationQuadratic ;
            sl->falloff_angle_ = light->mAngleOuterCone ;
            sl->falloff_exponent_ = 0 ;

            slight.reset(sl) ;
            break ;

        }

        }

        if ( slight )
            scene->lights_.push_back(slight) ;
    }

    return true ;
}

static bool importNodes(ModelPtr &scene, NodePtr &pnode, const struct aiScene *sc, const struct aiNode* nd,
                        const map<const aiMesh *, MeshPtr> &meshes,
                        const map<const aiMaterial *, MaterialPtr> &materials)
{
    unsigned int i;
    unsigned int n = 0, t;
    aiMatrix4x4 m = nd->mTransformation;
    /* update transform */

    NodePtr snode(new Node) ;

    Matrix4f tf ;
    tf << m.a1, m.a2, m.a3, m.a4,
            m.b1, m.b2, m.b3, m.b4,
            m.c1, m.c2, m.c3, m.c4,
            m.d1, m.d2, m.d3, m.d4 ;

    snode->pose_.mat_ = tf.eval() ;
  //  snode->name_ = nd->mName.C_Str();

    /* draw all meshes assigned to this node */
    for (; n < nd->mNumMeshes; ++n) {

        DrawablePtr dr(new Drawable) ;

        const aiMesh* mesh = sc->mMeshes[nd->mMeshes[n]];

        map<const aiMesh *, MeshPtr>::const_iterator mit = meshes.find(mesh) ;

        if ( mit == meshes.end() ) continue ;

        dr->geometry_ =  std::static_pointer_cast<Geometry>(mit->second) ;

        const aiMaterial* material = sc->mMaterials[mesh->mMaterialIndex];

        map<const aiMaterial *, MaterialPtr>::const_iterator cit = materials.find(material) ;

        if ( cit != materials.end() )
            dr->material_ = cit->second ;

        snode->drawables_.push_back(dr) ;
    }

    if ( pnode ) {
        pnode->children_.push_back(snode) ;
        snode->parent_ = pnode ;
    }
    else
        scene->nodes_.push_back(snode) ;

    /* draw all children */
    for (n = 0; n < nd->mNumChildren; ++n) {
        if ( !importNodes(scene, snode, sc, nd->mChildren[n], meshes, materials) )
            return false ;
    }

    return true ;
}

static ModelPtr importAssimp(const aiScene *sc, const std::string &fname) {

    map<const aiMesh *, MeshPtr> meshes ;
    map<const aiMaterial *, MaterialPtr> materials ;

    ModelPtr res(new Model) ;
    if ( !importMeshes(res, sc, meshes) ) return ModelPtr() ;
    if ( !importMaterials(res, fname, sc, materials) ) return ModelPtr() ;
    if ( !importLights(res, sc) ) return ModelPtr() ;

    NodePtr root ;
    if ( !importNodes(res, root, sc, sc->mRootNode, meshes, materials ) ) return ModelPtr() ;

    return res ;
}

namespace vsim {

ModelPtr Model::loadAssimp(const std::string &fname) {
    const aiScene *sc = aiImportFile(fname.c_str(), aiProcessPreset_TargetRealtime_MaxQuality | aiProcess_FlipUVs | aiProcess_TransformUVCoords);
    if ( !sc ) {
        throw ModelLoaderException("assimp", aiGetErrorString(), fname) ;
    }

    ModelPtr res = importAssimp(sc, fname) ;

    aiReleaseImport(sc) ;
    return res ;
}

ModelPtr Model::loadAssimp(const aiScene *sc, const std::string &fname) {
    return importAssimp(sc, fname) ;
}

}


#include <utility/generation.h>
#include <utility/helpers/pathfinder.h>
#include <utility/helpers/log.h>
#include <render/quadRender/quadRender.h>
#include <render/quadRender/loader.h>
#include <experimental/filesystem>
#include <boost/container_hash/hash.hpp>
#include <fstream>
#include <string>
#include <memory>
#include <utility>
void objectLoader::appendMesh(mesh toAdd) {
	meshes.push_back(toAdd);
	dirty = true;
}

void objectLoader::appendObject(std::string fileName) {
  LOG_INFO << "Started loading object " << fileName << std::endl;
  auto [objVertices, objTriangles, edges, minp, maxp] = generation::fileToObj(resolveFile(fileName, { get<parameters::config_folder>() }));
  auto numVtx = objVertices.size();
  auto numTri = objTriangles.size();
  LOG_INFO << "Vertices:  " << numVtx << std::endl;
  LOG_INFO << "Triangles: " << numTri << std::endl;

  std::vector<Vertex> vertices;
  std::vector<Triangle> triangles;

  for (int32_t i = 0; i < numVtx; ++i) {
    vertices.push_back(Vertex{objVertices[i], float3{0.f, 0.f, 0.f}});
  }
  for (int32_t i = 0; i < numTri; ++i) {
    auto [idx1, idx2, idx3] = objTriangles[i];
    triangles.push_back(Triangle(idx1, idx2, idx3, vertices));
  }
  for (auto &v : vertices) {
    v.normal = math::normalize(v.normal);
  }
  for (auto &t : triangles) {
	  t.recalculate(vertices);
  }
  LOG_INFO << "Finished loading object " << fileName << std::endl;
  meshes.push_back(mesh{vertices, triangles});
  dirty = true;
}
mesh objectLoader::mergeMeshes() {
  std::vector<Vertex> mergedVertices;
  std::vector<Triangle> mergedTriangles;
  int32_t offset = 0;
  for (int32_t i = 0; i < meshes.size(); ++i) {
    auto &vtx = meshes[i].vertices;
    auto &tri = meshes[i].triangles;
    for (auto &v : vtx)
      mergedVertices.push_back(v);
    for (auto t : tri) {
      t.i0 += offset;
      t.i1 += offset;
      t.i2 += offset;
      mergedTriangles.push_back(t);
    }
    offset = (int32_t) mergedVertices.size();
  }
  merged = mesh{mergedVertices, mergedTriangles};
  hash();
  dirty = false;
  return merged;
}
template <typename T, typename... Ts> auto combine(std::size_t &seed, T arg, Ts &&... args) {
  boost::hash_combine(seed, boost::hash_value(arg));
  combine(seed, std::forward<Ts>(args)...);
}
auto combine(std::size_t &seed) {}
namespace boost {
auto hash_value(float3 v) {
  std::size_t seed = 0x129037;
  ::combine(seed, v.x, v.y, v.z);
  return seed;
}
auto hash_value(float4 v) {
	std::size_t seed = 0x129037;
	::combine(seed, v.x, v.y, v.z, v.w);
	return seed;
}

auto hash_value(Vertex v) {
  std::size_t seed = 0x0983274;
  ::combine(seed, v.normal, v.position);
  return seed;
}
auto hash_value(Triangle t) {
  std::size_t seed = 0x01892364;
  ::combine(seed, t.center, t.bottom, t.color, t.normal);
  ::combine(seed, t.e1, t.e2, t.e3, t.d, t.d2, t.d3);
  ::combine(seed, t.i0, t.i1, t.i2, t.twoSided);
  return seed;
}
} // namespace boost
std::size_t objectLoader::hash() {
  if (!dirty)
    return stored_hash;
  if (!hashable)
    return SIZE_MAX;
  stored_hash = boost::hash_range(merged.triangles.begin(), merged.triangles.end());
  boost::hash_range(stored_hash, merged.vertices.begin(), merged.vertices.end());
  return stored_hash;
}
BVHNode *objectLoader::buildBVH(BBoxEntries &work, int depth) {
  if (work.size() < 4) {
    BVHLeaf *leaf = new BVHLeaf;
    BVHnodes.push_back(leaf);
    for (auto &t : work)
      leaf->triangles.push_back(t.triangles);
    return leaf;
  }
  float3 bottom{FLT_MAX, FLT_MAX, FLT_MAX};
  float3 top{-FLT_MAX, -FLT_MAX, -FLT_MAX};
  for(auto& v : work){
    bottom = math::min(bottom, v.bottom);
    top = math::max(top, v.top);
  }
  float side1 = top.x - bottom.x;
  float side2 = top.y - bottom.y;
  float side3 = top.z - bottom.z;

  float minCost = work.size() * (side1 * side2 + side2 * side3 + side3 * side1);

  float bestSplit = FLT_MAX;
  int bestAxis = -1;

  for (int axis = 0; axis < 3; axis++) {
    auto start = math::weak_at(bottom, axis + 1);
    auto stop = math::weak_at(top, axis + 1);

    if (fabsf(stop - start) < 1e-4)
      continue;

    auto step = (stop - start) / (1024.f / (depth + 1.f));
    for (float testSplit = start + step; testSplit < stop - step; testSplit += step) {
      float3 lbottom{FLT_MAX, FLT_MAX, FLT_MAX};
      float3 ltop{-FLT_MAX, -FLT_MAX, -FLT_MAX};

      float3 rbottom{FLT_MAX, FLT_MAX, FLT_MAX};
      float3 rtop{-FLT_MAX, -FLT_MAX, -FLT_MAX};

      int countLeft = 0, countRight = 0;
      for (auto &v : work) {
        float value = math::weak_at(v.center, axis + 1);
        if (value < testSplit) {
          lbottom = math::min(lbottom, v.bottom);
          ltop = math::max(ltop, v.top);
          countLeft++;
        } else {
          rbottom = math::min(rbottom, v.bottom);
          rtop = math::max(rtop, v.top);
          countRight++;
        }
      }
      if (countLeft <= 1 || countRight <= 1)
        continue;

      float lside1 = ltop.x - lbottom.x;
      float lside2 = ltop.y - lbottom.y;
      float lside3 = ltop.z - lbottom.z;

      float rside1 = rtop.x - rbottom.x;
      float rside2 = rtop.y - rbottom.y;
      float rside3 = rtop.z - rbottom.z;

      float surfaceLeft = lside1 * lside2 + lside2 * lside3 + lside3 * lside1;
      float surfaceRight = rside1 * rside2 + rside2 * rside3 + rside3 * rside1;

      float totalCost = surfaceLeft * countLeft + surfaceRight * countRight;

      if (totalCost < minCost) {
        minCost = totalCost;
        bestSplit = testSplit;
        bestAxis = axis;
      }
    }
  }
  if (bestAxis == -1) {
    BVHLeaf *leaf = new BVHLeaf;
    BVHnodes.push_back(leaf);
    for (auto &t : work)
      leaf->triangles.push_back(t.triangles);
    return leaf;
  }

  BBoxEntries left;
  BBoxEntries right;
  float3 lbottom{FLT_MAX, FLT_MAX, FLT_MAX};
  float3 ltop{-FLT_MAX, -FLT_MAX, -FLT_MAX};
  float3 rbottom{FLT_MAX, FLT_MAX, FLT_MAX};
  float3 rtop{-FLT_MAX, -FLT_MAX, -FLT_MAX};

  for (auto &v : work) {
    float value = math::weak_at(v.center, bestAxis + 1);
    if (value < bestSplit) {
      left.push_back(v);
      lbottom = math::min(lbottom, v.bottom);
      ltop = math::max(ltop, v.top);
    } else {
      right.push_back(v);
      rbottom = math::min(rbottom, v.bottom);
      rtop = math::max(rtop, v.top);
    }
  }
  BVHInner *inner = new BVHInner;
  BVHnodes.push_back(inner);

  inner->left = buildBVH(left, depth + 1);
  inner->left->bottom = lbottom;
  inner->left->top = ltop;

  inner->right = buildBVH(right, depth + 1);
  inner->right->bottom = rbottom;
  inner->right->top = rtop;

  return inner;
}
void objectLoader::CreateBVH() {
  std::vector<BBoxTmp> work;
  float3 bottom{FLT_MAX, FLT_MAX, FLT_MAX};
  float3 top{-FLT_MAX, -FLT_MAX, -FLT_MAX};
  for(const auto& triangle : merged.triangles) {
    BBoxTmp b;
    b.triangles = ▵
	auto eval = [&](auto idx) {
		b.bottom = math::min(b.bottom, math::castTo<float3>(merged.vertices[idx].position));
		b.top = math::max(b.top, math::castTo<float3>(merged.vertices[idx].position));
	};
	eval(triangle.i0);
	eval(triangle.i1);
	eval(triangle.i2);

	b.center = (b.top + b.bottom) * 0.5f;

    bottom = math::min(bottom, b.bottom);
    top = math::max(top, b.top);
    work.push_back(b);
  }
  BVHNode *root = buildBVH(work);
  root->bottom = bottom;
  root->top = top;

  BVHroot = root;
}
int32_t objectLoader::boxCount(BVHNode *root) {
  if (!root->leafNode()) {
    BVHInner *p = dynamic_cast<BVHInner *>(root);
    return 1 + boxCount(p->left) + boxCount(p->right);
  } else
    return 1;
}
uint32_t objectLoader::triangleCount(BVHNode *root) {
  if (!root->leafNode()) {
    BVHInner *p = dynamic_cast<BVHInner *>(root);
    return triangleCount(p->left) + triangleCount(p->right);
  } else {
    BVHLeaf *p = dynamic_cast<BVHLeaf *>(root);
    return (unsigned)p->triangles.size();
  }
}
void objectLoader::depthCount(BVHNode *root, int32_t depth, int32_t &maxDepth) {
  if (maxDepth < depth)
    maxDepth = depth;
  if (!root->leafNode()) {
    BVHInner *p = dynamic_cast<BVHInner *>(root);
	depthCount(p->left, depth + 1, maxDepth);
	depthCount(p->right, depth + 1, maxDepth);
  }
}
void objectLoader::PopulateCacheFriendlyBVH(const Triangle *pFirstTriangle, BVHNode *root, uint32_t &idxBoxes,
                                            uint32_t &idxTriList) {
  uint32_t currIdxBoxes = idxBoxes;
  CFBVH[currIdxBoxes].bottom = root->bottom;
  CFBVH[currIdxBoxes].top = root->top;
  if (!root->leafNode()) { // inner node
    BVHInner *p = dynamic_cast<BVHInner *>(root);
    int32_t idxLeft = ++idxBoxes;
    PopulateCacheFriendlyBVH(pFirstTriangle, p->left, idxBoxes, idxTriList);
    int32_t idxRight = ++idxBoxes;
    PopulateCacheFriendlyBVH(pFirstTriangle, p->right, idxBoxes, idxTriList);
    CFBVH[currIdxBoxes].u.inner.idxLeft = idxLeft;
    CFBVH[currIdxBoxes].u.inner.idxRight = idxRight;
  }
  else { // leaf
    BVHLeaf *p = dynamic_cast<BVHLeaf *>(root);
    uint32_t count = (uint32_t)p->triangles.size();
    CFBVH[currIdxBoxes].u.leaf.count =
        0x80000000 | count; // highest bit set indicates a leaf node (inner node if highest bit is 0)
    CFBVH[currIdxBoxes].u.leaf.startIdx = idxTriList;

    for (auto it = p->triangles.begin(); it != p->triangles.end(); it++) {
      triIndices[idxTriList++] = *it - pFirstTriangle;
    }
  }
}
void objectLoader::CreateCFBVH() {
  if (!BVHroot) {
    puts("Internal bug in CreateCFBVH, please report it...");
    fflush(stdout);
    exit(1);
  }

  unsigned idxTriList = 0;
  unsigned idxBoxes = 0;

  triIndicesLength = triangleCount(BVHroot);
  triIndices = std::make_unique<int[]>(triIndicesLength);

  CFBVHLength = boxCount(BVHroot);
  CFBVH = std::make_unique<CacheFriendlyBVHNode[]>(CFBVHLength); // array

  PopulateCacheFriendlyBVH(&merged.triangles[0], BVHroot, idxBoxes, idxTriList);

  if ((idxBoxes != CFBVHLength - 1) || (idxTriList != triIndicesLength)) {
    puts("Internal bug in CreateCFBVH, please report it...");
    fflush(stdout);
    exit(1);
  }

  int maxDepth = 0;
  depthCount(BVHroot, 0, maxDepth);
  if (maxDepth >= BVH_STACK_SIZE) {
    printf("Max depth of BVH was %d\n", maxDepth);
    puts("Recompile with BVH_STACK_SIZE set to more than that...");
    fflush(stdout);
    exit(1);
  }
}
namespace fs = std::experimental::filesystem;
void objectLoader::buildBVH() {
  std::string hash_string = std::to_string(hash());
  LOG_INFO << "Creating BVH for mesh with hash " << hash_string << std::endl;
  fs::path p(hash_string);
  p.replace_extension(".bvh");
  if (!BVHroot) {
    if (/*!hashable || !fs::exists(p)*/ true) {
      // No cached BVH data - we need to calculate them
      LOG_INFO << "Creating BVH for mesh with hash " << hash_string << std::endl;
      CreateBVH();
      // Now that the BVH has been created, copy its data into a more cache-friendly format
      // (CacheFriendlyBVHNode occupies exactly 32 bytes, i.e. a cache-line)
      LOG_INFO << "Creating CFBVH for mesh with hash " << hash_string << std::endl;
      CreateCFBVH();
      if (!hashable) {
        LOG_INFO << "Writing CFBVH to file for hash " << hash_string << std::endl;
        std::ofstream ofs(p, std::ios::out | std::ios::binary);
        auto write = [&](auto &arg, auto size) { ofs.write(reinterpret_cast<char *>(&arg), size); };
        write(CFBVHLength, sizeof(int32_t));
        write(triIndicesLength, sizeof(int32_t));
        write(CFBVH, CFBVHLength * sizeof(CacheFriendlyBVHNode));
        write(triIndices, triIndicesLength * sizeof(int32_t));
        ofs.close();
      }
    }
  } else { // BVH has been built already and stored in a file, read the file
    LOG_INFO << "Reading BVH from file for mehs with hash " << hash_string << std::endl;
    std::ifstream ifs(p, std::ios::in | std::ios::binary);
    auto read = [&](auto &arg, auto size) { ifs.read(reinterpret_cast<char *>(&arg), size); };
    read(CFBVHLength, sizeof(int32_t));
    read(triIndicesLength, sizeof(int32_t));
    CFBVH = std::make_unique<CacheFriendlyBVHNode[]>(CFBVHLength);
	triIndices = std::make_unique<int32_t[]>(triIndicesLength);
    read(CFBVH[0], CFBVHLength * sizeof(CacheFriendlyBVHNode));
    read(triIndices[0], triIndicesLength * sizeof(int32_t));
    ifs.close();
  }
  createGPUArrays();
}
void objectLoader::createGPUArrays() {
	if (!active)
		return;
  auto allocAndCopy = [&](auto &ptr, auto &data) {
    using T = std::decay_t<decltype(data)>::value_type;
    cudaMalloc((void **)&ptr, data.size() * sizeof(T));
    cudaMemcpy(ptr, data.data(), data.size() * sizeof(T), cudaMemcpyHostToDevice);
  };

  std::vector<TriangleIntersectionData> intersectData;
  std::vector<BVHLimits> limits;
  std::vector<uint4> indexlist;
  for (const auto &t : merged.triangles) {
    TriangleIntersectionData current;
    current.center = float4{t.center.x, t.center.y, t.center.z, t.twoSided ? 1.f : 0.f};
    current.normal = float4{t.normal.x, t.normal.y, t.normal.z, t.d};
    current.e1d1 = float4{t.e1.x, t.e1.y, t.e1.z, t.d1};
    current.e2d2 = float4{t.e2.x, t.e2.y, t.e2.z, t.d2};
    current.e3d3 = float4{t.e3.x, t.e3.y, t.e3.z, t.d3};
    intersectData.push_back(current);
  }
  for (int32_t i = 0; i < CFBVHLength; i++) {
	  limits.push_back(BVHLimits{ CFBVH[i].top, CFBVH[i].bottom });
	  auto u = CFBVH[i].u;
	  indexlist.push_back(uint4{ (uint32_t)u.leaf.count, (uint32_t)u.inner.idxRight, (uint32_t)u.inner.idxLeft,
								(uint32_t)u.leaf.startIdx });
  }

  cudaMalloc((void **)&cuTriIndices, triIndicesLength * sizeof(int));
  cudaMemcpy(cuTriIndices, triIndices.get(), triIndicesLength * sizeof(int), cudaMemcpyHostToDevice);

  allocAndCopy(cuTriIntersectionData, intersectData);
  allocAndCopy(cuVertices, merged.vertices);
  allocAndCopy(cuTriangles, merged.triangles);
  allocAndCopy(cuBVHLimits, limits);
  allocAndCopy(cuBVHIndices, indexlist);
}
objectLoader::~objectLoader() {
  auto freeCUDA = [&](auto ptr) {
    if (ptr != nullptr)
      cudaFree(ptr);
  };
  freeCUDA(cuVertices);
  freeCUDA(cuTriangles);
  freeCUDA(cuTriIntersectionData);
  freeCUDA(cuTriIndices);
  freeCUDA(cuBVHLimits);
  freeCUDA(cuBVHIndices);
  for (auto &alloc : BVHnodes)
    free(alloc);
}
void objectLoader::reset() {
	auto freeCUDA = [&](auto ptr) {
		if (ptr != nullptr)
			cudaFree(ptr);
	};
	freeCUDA(cuVertices);
	freeCUDA(cuTriangles);
	freeCUDA(cuTriIntersectionData);
	freeCUDA(cuTriIndices);
	freeCUDA(cuBVHLimits);
	freeCUDA(cuBVHIndices);
	for (auto &alloc : BVHnodes)
		free(alloc);
	meshes.clear();
	merged.vertices.clear();
	merged.triangles.clear();
	BVHnodes.clear();
	BVHroot = nullptr;
	triIndicesLength = 0;
	triIndices = nullptr;
	CFBVHLength = 0;
	CFBVH = nullptr;
	cuVertices = nullptr;
	cuTriangles = nullptr;
	cuTriIntersectionData = nullptr;
	cuTriIndices = nullptr;
	cuBVHLimits = nullptr;
	cuBVHIndices = nullptr;
	dirty = true;
	stored_hash = 0x0;
}

gpuBVH objectLoader::getGPUArrays() {
	return gpuBVH{ active, cuVertices, cuTriangles , cuBVHIndices , cuBVHLimits , cuTriIntersectionData,
	cuTriIndices, triIndicesLength , CFBVHLength , (int32_t)merged.vertices.size() , (int32_t)merged.triangles.size() };
}

void objectLoader::tearDownMeshes() {
	int32_t vtxCounter = merged.vertices.size();
	for (auto& t : merged.triangles) {
		auto vtx0 = merged.vertices[t.i0];
		auto vtx1 = merged.vertices[t.i1];
		auto vtx2 = merged.vertices[t.i2];

		auto n0 = math::normalize(vtx0.normal);
		auto n1 = math::normalize(vtx1.normal);
		auto n2 = math::normalize(vtx2.normal);

		auto threshold = 1.f / 12.f * CUDART_PI_F;
		//threshold = 0.f;

		auto angle0 = acosf(math::dot3(n0, t.normal));
		auto angle1 = acosf(math::dot3(n1, t.normal));
		auto angle2 = acosf(math::dot3(n2, t.normal));
		if (angle0 > threshold) {
			t.i0 = vtxCounter++;
			merged.vertices.push_back(Vertex{ vtx0.position, t.normal });
		}
		if (angle1 > threshold) {
			t.i1 = vtxCounter++;
			merged.vertices.push_back(Vertex{ vtx1.position, t.normal });
		}
		if (angle2 > threshold) {
			t.i2 = vtxCounter++;
			merged.vertices.push_back(Vertex{ vtx2.position, t.normal });
		}
	}
}