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

GAS_DENSITY = 2.858
ONE_MPH = 0.44704


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.3#0.3
        ki = 0.1 #0.1
        kd = 0. #0
        mn = 0. #min throttle 
        mx = 0.2 #max throttle 0.2
        self.throttle_controller = PID(kp,ki,kd,mn,mx)

        #filter out high freq noise in velocity
        tau = 0.5 # 1/(2*pi*tau) =cutoff freq
        ts = 0.02 # sample time 
        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_enable, linear_vel, angular_vel):
        # TODO: Change the arg, kwarg list to suit your needs
        # Return throttle, brake, steer

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

        current_vel = self.vel_lpf.filt(current_vel)

        # # rospy.logwarn("Auglar velocity: {0}".format(current_angular_vel))
        # rospy.logwarn("Target velocity: {0}".format(linear_vel))
        # rospy.logwarn("Target angular velocity: {0}".format(angular_vel))
        # rospy.logwarn("Current velocity: {0}".format(current_vel))
        # rospy.logwarn("Filtered velocity: {0}".format(self.vel_lpf.get()))

        steering = self.yaw_controller.get_steering(linear_vel,angular_vel,current_vel)
        #TODO may add dampening to steering to make less wandering


        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 =400 #newton*meter to hold the car in place if stopped at a light acceleration ~1m/s^2

        elif throttle < 0.1 and vel_error < 0: #current velocity is faster than target velocity, need brake 
        	throttle = 0
        	decel = max(vel_error, self.decel_limit) # decelleration rate
        	brake = abs(decel) * self.vehicle_mass * self.wheel_radius # Torque N*m 

        return throttle, brake, steering
