import math

def calculate_acceleration(speed, acc, V_MAX=11):
    sigma = 2.5
    mu = V_MAX / 2
    return acc * (1 - 0.5 * (1 + (math.erf((abs(speed) - mu) / (math.sqrt(2 * sigma**2))))))
    
def calculate_friction(speed, f, V_MAX=11):
    # TODO hier stimmt was nicht
    sigma = 4
    mu = V_MAX / 2
    return (f/2) * (1 + (math.erf((abs(speed) - mu) / (math.sqrt(2 * sigma**2)))))

class Iot_car():

    V_MAX = 11 #m/s
    A_MAX = 45 # degree
    
    acc = 2.6  # Max acceleration of the car (per sec.)
    dec = 4.5  # Max deceleration of the car (per sec.)
    frict = -1  # max friction
    angle_acc = 300  # max change of angle (per sec.)

    is_testmode_activated = True
    is_testmode_servo_active = False
    
    speed_cur = 0
    speed_last = 0
    angle_cur = 0
    speed_cruise_control = 0 # m/s but only positive
    
    motor = None
    streeting = None

    def __init__(self, testmode=True):
        self.is_testmode_activated = testmode
        if not testmode:
            import servo_ctrl
            self.motor = Motor(1)
            self.streeting = Steering(2)
    
    # for keyboard control     
    def __testmode_accelerate(self):
        calc_acc = calculate_acceleration(self.speed_cur, self.acc)
        self.speed_last = self.speed_cur
        self.speed_cur = min(self.V_MAX, self.speed_cur + calc_acc)
        self.is_testmode_servo_active = True
        
    def __testmode_decelerate(self):
        calc_dec = calculate_acceleration(self.speed_cur, self.dec)
        self.speed_last = self.speed_cur
        self.speed_cur = max(-1 * self.V_MAX, self.speed_cur - calc_dec)
        self.is_testmode_servo_active = True
    
    def __accelerate(self, speed):
        self.speed_last = self.speed_cur
        self.speed_cur = speed
        self.motor.set_speed(speed)
        
    def accelerate(self, speed=0, direct=False):
        if direct and self.is_testmode_activated: # Mouse testmode
            self.last = self.speed_cur
            self.speed_cur = speed
        elif self.is_testmode_activated:
            if speed > 0:
                self.__testmode_accelerate()
            elif speed < 0:
                self.__testmode_decelerate()
        else:
            self.__accelerate(speed)
            
    # for wii remote
    def cruise_control_increase(self):
        s = min(self.V_MAX, self.speed_cruise_control + 2.75)
        if s < 0:
            self.speed_cruise_control = 0
        else:
            self.speed_cruise_control = s
        
    def cruise_control_decrease(self):
        s = min(self.V_MAX, self.speed_cruise_control - 2.75)
        if s < 0:
            self.speed_cruise_control = 0
        else:
            self.speed_cruise_control = s
            
    def def cruise_control_reset(self):
        self.speed_cruise_control = 0
        
    def activate_cruise_control_forward(self):
        self.speed_last = self.speed_cur
        self.speed_cur = self.speed_cruise_control
        self.is_testmode_servo_active = True
        if not is_testmode_activated:
            self.__accelerate(self.speed_cruise_control)
            
    def activate_cruise_control_backward(self):
        self.speed_last = self.speed_cur
        self.speed_cur = -1 * self.speed_cruise_control
        self.is_testmode_servo_active = True
        if not is_testmode_activated:
            self.__accelerate(-1 * self.speed_cruise_control)
            
    def deactivate_cruise_control(self):
        self.speed_last = self.speed_cur
        self.speed_cur = 0
        self.is_testmode_servo_active = False
        if not is_testmode_activated:
            self.__accelerate(0)
            
    def reset_streeting(self):
        self.set_angle(0)
          
    def streeting_right(self, angle=0):
        if angle > 0:
            a = angle
        else:
            a = min(self.A_MAX, self.angle_cur + self.angle_acc)
        self.set_angle(a)
        
    def streeting_left(self, angle=0):
        if angle < 0:
            a = angle
        else:
            a = max(-1 * self.A_MAX, self.angle_cur - self.angle_acc)
        self.set_angle(a)
          
    def set_angle(self, angle):
        self.angle_cur = angle
        if not self.is_testmode_activated:
            self.streeting.set_angle(angle)

    def stop(self):
        self.speed_last = self.speed_cur
        self.speed_cur = 0
        self.angle_cur = 0
        
    def symulate(self):
        if self.is_testmode_activated:
            if not self.is_testmode_servo_active:
                self.speed_last = self.speed_cur
                self.speed_cur = min(0, self.speed_cur - calculate_friction(self.speed_cur, self.frict))