#! usr#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import tf.transformations
import matplotlib.pyplot as plt
import random
import math
import time

#PLOTTING THE ENVIRONMENT!!!
global mapPointsX # global variable for x coordinates of points
mapPointsX = []
global mapPointsY # global variable for y coordinates of points
mapPointsY = []

def plot_map():
    # Plots points (X, Y) from turtlebot odometry
    global mapPointsX
    global mapPointsY

    print('mapPoints range: ', len(mapPointsX))
    plt.plot(mapPointsY,mapPointsX, 'ro')
    plt.axis([-3,3,-3,3])
    plt.show()

def shutdown():

    # stop turtlebot
    rospy.loginfo("Stop TurtleBot")

    # a default Twist has linear.x of 0 and angular.z of 0.  So it'll stop TurtleBot
    pub.publish(Twist())
    # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script
    rospy.sleep(1)
    plot_map() # plot the map

class RobotMap:
    global mapPointsX
    global mapPointsY
    def __init__(self):
        self.position = None
        self.angle = None
        self.lidarPoints = None


    def callback_lidar(self, msg):
        # stores the lidarPoints
        self.lidarPoints = msg.ranges


    def callback_odom(self, msg):

        self.position = msg.pose.pose.position # stores the position from odometry
        quaternion = (
        msg.pose.pose.orientation.x,
        msg.pose.pose.orientation.y,
        msg.pose.pose.orientation.z,
        msg.pose.pose.orientation.w)
        euler = tf.transformations.euler_from_quaternion(quaternion) # translates the quaternion orientation of turtlebot to euler
        self.yaw = euler[2]
        print("position:",self.position)
        print('yaw: ', self.yaw)

        self.store_data()

    def store_data(self):
        # mapPoints.append([self.position.x, self.position.y])
        for i in range(360):
            x = self.position.x + self.lidarPoints[i] *math.cos(math.radians(i+self.yaw))
            y = self.position.y + self.lidarPoints[i] *math.sin(math.radians(i+self.yaw))
            y = -1 * y # this is done for the plot to be consistent with the coordinates of turtlebot
            mapPointsX.append(x)
            mapPointsY.append(y)
        # print('position: ',len(mapPoints))

#MOVEMENT OF ROBOT!!!



def callback(msg):
    print('=========================')
    movements = [[0.0, -1*math.pi/2], [0.0,math.pi/2], [-0.3, 0.0]]  #linear and angular velocities for turning right, left, and moving back, respectively



    print('s1 [0]')
    print(msg.ranges[0])  #distance at angle 0, which is directly in front of robot

    print('s2 [90]')
    print(msg.ranges[90])  #distance at angle 90

    print('s3 [180]')
    print(msg.ranges[180])  #distance at angle 180

    print('s4 [270]')
    print(msg.ranges[270])  #distance at angle 270

    print('s5 [359]')
    print(msg.ranges[359])  #distance at angle 359

    if msg.ranges[0] < msg.ranges[180]:     #CHECK FRONT AND BACK, REVERSE IF BACK > FRONT
        move.linear.x = -0.3
        move.angular.z = 0.0

    elif msg.ranges[0] > msg.ranges[180]:   #CHECK FRONT AND BACK, FORWARD IF FRONT > BACK
        move.linear.x = 0.3
        move.angular.z = 0.0

    elif msg.ranges[90] < msg.ranges[270]:  #CHECK SIDES, TURN RIGHT IF RIGHT > LEFT
        move.linear.x = 0.0
        move.angular.z = -1 * math.pi/2

    elif msg.ranges[90] > msg.ranges[270]:  #CHECK SIDES, TURN LEFT IF LEFT > RIGHT
        move.linear.x = 0.0
        move.angular.z = math.pi/2

    elif msg.ranges[0] - msg.ranges[180] <= 0.3 and msg.ranges[90] - msg.ranges[270] <= 0.3:    #CHECK ALL DISTANCES
        move.linear.x = 0.0
        move.angular.z = 0.0
        rospy.signal_shutdown()


    pub.publish(move)




rospy.init_node('DP_World3x3')
rospy.loginfo("To STOP turtlebot Ctrl + C")
rospy.on_shutdown(shutdown)

robotMap = RobotMap()

sub = rospy.Subscriber('/scan', LaserScan, callback)
sub2 = rospy.Subscriber('/scan', LaserScan, robotMap.callback_lidar)
sub3 = rospy.Subscriber('/odom', Odometry, robotMap.callback_odom)

pub = rospy.Publisher('/cmd_vel', Twist)
move = Twist()
rospy.spin()