#include <ros/ros.h>
#include "std_msgs/String.h"
#include <std_msgs/Float64.h>
#include <sensor_msgs/BatteryState.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

// Parameters
bool isCollisionDetected = false;
bool isFinished = false;
int angleDetection = 15;
double distThreshold = 0.3;
float batThreshold = 10;
double entroThreshold = 100000000;

// Function to run when receive data from the subscribed Lidar
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& data) //
{
	isCollisionDetected = false;
	
	for (int i=0;i<angleDetection;i++){
		
		// Get data in a cone of +-15° in front of the robot 
		double firstVal = data->ranges[i];
		double lastVal = data->ranges[data->ranges.size()-i-1];
		
		// Check if one of the data is below the threshold
		if (lastVal <distThreshold && lastVal != 0.0){
			isCollisionDetected = true;
			break;
		}
		if (firstVal <distThreshold&& firstVal != 0.0){
			isCollisionDetected = true;
			break;
		}
	}
	
}

void entropyCallback(const std_msgs::Float64& entropy) //??
{
	if (entropy.data > entroThreshold){
		isFinished = true;
	}
}

void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& bat)
{
	if (bat->voltage < batThreshold){
		isFinished = true;
	}
}
	
	
int main(int argc, char** argv)
{
	ros::init(argc,argv, "autopilot");
	
	// Publish to the cmd_vel topic
	ros::NodeHandle nh;
	ros::Publisher cmdVelPub = nh.advertise<geometry_msgs::Twist>("cmd_vel",1);
	
	// Subscribe to the scan topic
	ros::Subscriber scanSub = nh.subscribe("scan",1,scanCallback); 
	
	// Subscribe to the entropy topic
	ros::Subscriber entropySub = nh.subscribe("turtlebot3_slam_gmapping/entropy",1,entropyCallback); 
	
	// Subscribe to the battery state topic
	ros::Subscriber batterySub = nh.subscribe("battery_state",1,batteryCallback); 
	
	ros::Rate loopRate(10);
    
	while(ros::ok())
	{
		geometry_msgs::Twist msg;
		
		
		if (!isFinished){
			if (isCollisionDetected){
				msg.angular.z = 1;
			}
			else{
				msg.linear.x = 1;
			}
		}
		
		cmdVelPub.publish(msg);
		ros::spinOnce();
		loopRate.sleep();
	}
	return 0;
}