/*
Project: Diplomová práce 2019
Purpose: Robot docking with laser scanners
Author: František Brandštýl
*/

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <laser_geometry/laser_geometry.h>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <string>
#include <math.h>
#include <laser_node/transform.h> 
using namespace std;

ros::Publisher pub0;
ros::Publisher pub1;
Eigen::Matrix4f guess;
Eigen::Matrix4f result;
pcl::PointCloud<pcl::PointXYZ>::Ptr reference(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr completeCloud(new pcl::PointCloud<pcl::PointXYZ>);
double initAngle = 0;
double initX = 0.5;
double initY = 0;
bool ready = false;


void loadReference() {
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("/home/turtlebot/work/reference.pcd", *reference) != -1)
	{
		cout << "Reference loaded. \n";

	}
	else cout << "Unable to load reference. \n";
}

void aligning() {
	pcl::PointCloud<pcl::PointXYZ> finalCloud;
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	// This is guess for the transform
	guess << cos(initAngle), -sin(initAngle), 0, initX,
		sin(initAngle), cos(initAngle), 0, initY,
		0, 0, 0, 0,
		0, 0, 0, 1;
	// Set the input source and target
	icp.setInputSource(reference); // This one is moved
	icp.setInputTarget(completeCloud);
	// Set the max correspondence distance (correspondences with higher distances will be ignored)
	icp.setMaxCorrespondenceDistance(0.5);
	// Set the maximum number of iterations (criterion 1)
	icp.setMaximumIterations(100);
	// Set the transformation epsilon (criterion 2)
	icp.setTransformationEpsilon(1e-8);
	// Set the euclidean distance difference epsilon (criterion 3)
	//icp.setEuclideanFitnessEpsilon (1);

	// Perform the alignment
	icp.align(finalCloud, guess);
	finalCloud.header.frame_id = "map";
	// Get final transform
	result = icp.getFinalTransformation();
	initAngle = -atan2(result(0, 1), result(0, 0));
	initX = result(0, 3);
	initY = result(1, 3);

	// Convert to ROS data type and publish cloud
	sensor_msgs::PointCloud2 outCloud;
	pcl::toROSMsg(finalCloud, outCloud);
	pub0.publish(outCloud);
	// Publish transform
	laser_node::transform tf;
	tf.angle = initAngle;
	tf.x = initX;
	tf.y = initY;
	tf.score = icp.getFitnessScore();
	pub1.publish(tf);
}

void scanCallback0(const sensor_msgs::PointCloud2 cloud)
{
	ready = true;
	// Convert to PCL data type
	pcl::fromROSMsg(cloud, *completeCloud);
}

int main(int argc, char** argv)
{
	// Initialize ROS
	ros::init(argc, argv, "laser_node");
	ros::NodeHandle nh("~");
	string input;
	nh.getParam("input", input);
	// Create a ROS subscriber for the input point cloud
	ros::Subscriber sub0 = nh.subscribe(input, 1, scanCallback0);
	// Create a ROS publisher for the output point cloud and transform
	pub0 = nh.advertise<sensor_msgs::PointCloud2>("alignedCloud", 1);
	pub1 = nh.advertise<laser_node::transform>("transform", 1);

	// Load reference point cloud from file
	loadReference();

	ros::Rate r(10);
	while (!ready)
	{
		ros::spinOnce();
		r.sleep();
	}

	cout << "Aligning started. \n";

	while (ros::ok())
	{
		ros::spinOnce();
		aligning();
		r.sleep();
	}
}