from pid import PID 
from lowpass import LowPassFilter
from yaw_controller import YawController 
import rospy

GAS_DENSITY = 2.858
ONE_MPH = 0.44704

'''
The file defines the Controller class that calculates the throttle, steering and brake inputs.
The inputs to the functions are current velocity, linear velocity, angular velocity, dbw enabled flag;
and the function computes/returns the throttle, brake and steering commands
This module uses functions from the YawController class in yaw_controller.py to calculate the steering and functions 
from PID class in PID.py to adjust the throttle depending on the velocity error. The brake torque is calculated based on the velocity error and throttle values. 
If the vehicle is stopped i.e. the reference linear velocity = 0 and the current velocity is below 0.1, the brake torque is a constant 390 N-m, 
else if the reference velocity is lower than the current velocity & the throttle input is below 0.1, the brake torque 
is calculated based on the deceleration required, the mass of the vehicle and the wheel radius
It returns the calculated values only if the dbw module is enabled, else it returns a 0 value for all commands.
'''

class Controller(object):
    def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit,
    	accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle):

        # TODO: Implement
        self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)

        kp = 0.35
        ki = 0.1
        kd = 0.
        mn = 0. # Minimum throttle value
        mx = 0.21 # Maximum throttle value
        self.throttle_controller = PID(kp, ki, kd, mn, mx)

        tau = 0.5 
        ts = 0.02 # Sample time corresponding to 50 Hz
        self.vel_lpf = LowPassFilter(tau, ts)

        self.vehicle_mass = vehicle_mass
        self.fuel_capacity = fuel_capacity
        self.brake_deadband = brake_deadband
        self.decel_limit = decel_limit
        self.accel_limit = accel_limit
        self.wheel_radius = wheel_radius
        
        self.last_time = rospy.get_time()

    def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
        # TODO: Change the arg, kwarg list to suit your needs
        # Return throttle, brake, steer

        if not dbw_enabled:
        	self.throttle_controller.reset()
        	return 0., 0., 0.

    	current_vel = self.vel_lpf.filt(current_vel)

    	steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel)

    	vel_error = linear_vel - current_vel
    	self.last_vel = current_vel

    	current_time = rospy.get_time()
    	sample_time = current_time - self.last_time
    	self.last_time = current_time

    	throttle = self.throttle_controller.step(vel_error, sample_time)
    	brake = 0

    	if linear_vel == 0. and current_vel < 0.1:
    		throttle = 0
    		brake = 390 #N-m - stop the car at the signal. Acceleration ~ 1m/s^2

        elif throttle < .1 and vel_error < 0:
            throttle = 0
            decel = max(vel_error, self.decel_limit)
            brake = abs(decel)*self.vehicle_mass*self.wheel_radius

        return throttle, brake, steering
