#!/usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from std_msgs.msg import String, Header
from ackermann_msgs.msg import AckermannDriveStamped


import numpy as np
from sklearn.decomposition import PCA

"""
Author: Ariel Anders
This program tells the robot to follow a wall.

Edited by: Winter Guerra
"""

class WallFollower():
    def __init__(self):
        '''
        Init the node. See safety.py for a good idea of what to do.
        '''


        # Init subscribers and publishers
        self.sub = rospy.Subscriber("/scan", LaserScan,\
                self.lidarCB, queue_size=1)

        self.pub = rospy.Publisher("/vesc/low_level/ackermann_cmd_mux/input/safety",\
                AckermannDriveStamped, queue_size =1 )

        rospy.loginfo("Safety node initialized")
        self.count = 0


    def lidarCB(self, msg):
        '''
        This callback is called everytime the laserscanner sends us data.
        This is about 40hz. The message received is a laserscan message
        '''
        angles = np.linspace(-2./3 * np.pi, -np.pi/6, 405)
        distances = np.array(msg.ranges)
        distances = distances[0:405]
        valid_indices = msg.range_min < distances < msg.range_max
        rect_dist = distances[valid_indices]
        rect_ang = angles[valid_indices]

        cart_x = rect_dist*np.cos(rect_ang)
        cart_y = rect_dist*np.sin(rect_ang)

        cart = np.column_stack((cart_x, cart_y))
        pca = PCA(n_components=2)
        pca.fit(cart)
        wall_angle = np.arctan(pca.components_[0, 1]/pca.components_[0, 0])

        # vector to median point, use median point to compute distance
        median_vec = np.array([np.median(cart_x), np.median(cart_y)])
        wall_vector = pca.components_[0]
        wall_vector /= np.linalg.norm(wall_vector)

        # vector represneting the perpendicular to wall
        perp_vector = (1 - median_vec.dot(wall_vector)) * median_vec
        distance = np.linalg.norm(perp_vector)

        self.follow_wall(distance, wall_angle)


if __name__=="__main__":
    # Tell ROS that we're making a new node.
    rospy.init_node("Wall_Follower_Node")

    # Init the node
    WallFollower()

    # Don't let this script exit while ROS is still running
    rospy.spin()