#!/usr/bin/python

import sys
import time
import math
import numpy as np
import rospy
from std_msgs.msg import Float32
from geometry_msgs.msg import Twist

class ControlRobot(object):
    def __init__(self):

        self.lane_error_sub = rospy.Subscriber('/detect_lane/lane_error', Float32, self.update_error)
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.cmd_vel_msg = Twist()
        
        self.Kp = 0.0122
        self.Ki = 0.0
        self.Kd = 0.0013
        self._input = np.nan
        self.last_error = 0.0
        self.p_error, self.i_error, self.d_error = 0.0, 0.0, 0.0
        self.last_time = rospy.get_rostime().to_sec()

        rospy.on_shutdown(self.shutdown_hook)

    def update_error(self, data):

        self._input = data.data
        self.current_error = self._input if not math.isnan(self._input) else None
        now = rospy.get_rostime().to_sec()
        delta = now - self.last_time if now - self.last_time else 0.1

        if self.current_error is not None:

            self.p_error = self.Kp * self.current_error
            self.i_error = self.Ki * (self.current_error + self.last_error) * delta
            self.d_error = self.Kd * (self.current_error - self.last_error) / delta
            self.last_error = self.current_error
            
            angular_z = self.p_error + self.d_error + self.i_error
            self.cmd_vel_msg.linear.x = min(0.2 * ((1 - abs(self.current_error) / 103**2.2)), 0.2)
            self.cmd_vel_msg.angular.z = max(angular_z, -2.0) if angular_z < 0 else min(angular_z, 2.0)
            self.cmd_vel_pub.publish(self.cmd_vel_msg)
        else:
            pass

    def shutdown_hook(self):
        
        self.cmd_vel_msg.linear.x = 0.0
        self.cmd_vel_msg.angular.z = 0.0
        self.cmd_vel_pub.publish(self.cmd_vel_msg)

def main(args):
    rospy.init_node('control_robot', anonymous=True)
    ControlRobot()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

if __name__ == '__main__':
    main(sys.argv)