#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <stdlib.h>

// For rand() and RAND_MAX
using namespace std;
const float contact_distance = 2;
const float security_distance = 0.9;
const float * distances;
float angle_incr;
float angle_min;

void processScanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    distances = &msg->ranges[0];
    angle_incr = msg->angle_increment;
    angle_min = msg->angle_min;
}

int getIndex(double angle)
{
    return floor((angle-angle_min)/angle_incr);
}

double getAngle(int idx)
{
    return angle_min + idx*angle_incr;
}

bool someRight()
{
    float right = distances[getIndex(-1.57)];
    if (right < contact_distance) {
        return true;
    } else {
        return false;
    }
}

bool someFront()
{
    for(int i = getIndex(-0.25); i < getIndex(0.25); ++i)
    {
        if (distances[i] < security_distance) {
            return true;
        }
    }
    return false;
}

int main(int argc, char **argv)
{
    // Initialize the ROS system and become a node.
    ros::init( argc , argv , "exploring") ;

    ROS_INFO_STREAM("Robotic explorer node running and initialized! Let's have some fun!");

    ros::NodeHandle nh;

    // Create a subscriber object
    ros::Subscriber sub = nh.subscribe("/base_scan", 1000, processScanCallback);

    // Create a publisher object
    ros::Publisher pub = nh.advertise <geometry_msgs::Twist>("/cmd_vel" , 1000) ;

    // Seed the random number generator.
    srand ( time (0) ) ;

    // Loop at 2Hz until the node is shut down.
    ros::Rate rate (2) ;
    ros::Time begin = ros::Time::now();

    while ( begin.toSec() == 0 )
      begin = ros::Time::now();

    double ellapsed_time = 0;
    // Checks if we found for the first time a wall.
    bool pared = false;

    while ( ( ros::ok () ) && ( ellapsed_time < 60*5 ) )
    {
        // Create and fill in the message. The other four
        // fields , which are ignored by stage, default to 0.

        geometry_msgs::Twist msg;

        // Wait until it 's time for another iteration .
        rate.sleep () ;
        ros::spinOnce(); // give time to receive /base_scan messages

        // Bug algorithm:
        // Once we found a wall for the first time* (we find it going all straight until we find somthing in fornt of us)
        // 1. Go straight
        // 2. If we found something in front of us, turn left.
        // 3. Otherwise, if we don't have the wall at our right, turn right.
        // 4. Go 1.

        msg.linear.x = 1.0;
        msg.angular.z = 0;

        if (pared) {
            if (someFront()) {
                msg.linear.x = 0;
                msg.angular.z = 2.35;
            } else {
                if (!someRight()) {
                    msg.linear.x = 0;
                    msg.angular.z = -2.35;
                }
            }
        } else {
            if (someFront())
            {
                pared = true;
            }
        }

        // Publish the message.

        pub.publish(msg) ;

        // Send a message to rosout with the details .
        ROS_INFO_STREAM("Sending random velocity command: "
        << " linear=" << msg.linear.x
        << " angular=" << msg.angular.z ) ;

        ros::Time current = ros::Time::now();
        ellapsed_time = (current - begin).toSec();
        ROS_INFO_STREAM("Ellpased time: " << ellapsed_time );
    }
}