"""Sample Webots controller for highway driving benchmark."""

from vehicle import Driver
import os

def clear():os.system('cls')

def control(l_angle, ll_angle, lll_angle):
    frontDistance = sensors['front'].getValue()
    frontRange = sensors['front'].getMaxValue()
    f = frontDistance/frontRange

    f_r_0_Distance = sensors['front right 0'].getValue()
    f_r_0_Range = sensors['front right 0'].getMaxValue()
    f_r_1_Distance = sensors['front right 1'].getValue()
    f_r_1_Range = sensors['front right 1'].getMaxValue()
    f_r_2_Distance = sensors['front right 2'].getValue()
    f_r_2_Range = sensors['front right 2'].getMaxValue()
    r_Distance = sensors['right'].getValue()
    r_Range = sensors['right'].getMaxValue()
    
    f_r_0 = f_r_0_Distance/f_r_0_Range
    f_r_1 = f_r_1_Distance/f_r_1_Range
    f_r_2 = f_r_2_Distance/f_r_2_Range
    r = r_Distance/r_Range
    
    
    f_l_0_Distance = sensors['front left 0'].getValue()
    f_l_0_Range = sensors['front left 0'].getMaxValue()
    f_l_1_Distance = sensors['front left 1'].getValue()
    f_l_1_Range = sensors['front left 1'].getMaxValue()
    f_l_2_Distance = sensors['front left 2'].getValue()
    f_l_2_Range = sensors['front left 2'].getMaxValue()
    l_Distance = sensors['left'].getValue()
    l_Range = sensors['left'].getMaxValue()
    
    f_l_0 = f_l_0_Distance/f_l_0_Range
    f_l_1 = f_l_1_Distance/f_l_1_Range
    f_l_2 = f_l_2_Distance/f_l_2_Range
    l = l_Distance/l_Range
    
    
    maxSpeed = 90
    if r < 1 or l < 1:
        # maxSpeed =  83
        K = 1
    if f_r_2 < 1 or f_l_2 < 1:
        # maxSpeed =  75
        K = 1.1
    if f_r_1 < 1 or f_l_1 < 1:
        # maxSpeed =  70
        K = 1.2
    if f_r_0 < 1 or f_l_0 < 1:
        # maxSpeed =  65
        K = 1.3    
    if f < 1:
        # maxSpeed = 60
        K = 1.4
   
    speed = maxSpeed * (0.5*f + 0.15*(f_r_0 + f_l_0) + 0.1*(f_r_1 + f_l_1))
    driver.setCruisingSpeed(speed)
    # brake if we need to reduce the speed
    speedDiff = driver.getCurrentSpeed() - speed
    if speedDiff > 0:
        driver.setBrakeIntensity(min(K*speedDiff / speed, 1))
    else:
        driver.setBrakeIntensity(0)

    
    kf=0.1
    kr0=0.25
    kr1=0.3
    kr2=0.4
    kr=0.4
    kl0=0.25
    kl1=0.3
    kl2=0.4
    kl=0.4
    
    fr = kr0*f_r_0 + kr1*f_r_1 + kr2*f_r_2 + kr*r
    fl = kl0*f_l_0 + kl1*f_l_1 + kl2*f_l_2 + kl*l

    angle = 0.12*(fr-fl)

    # if f == 1 and f_r_0 == 1 and f_l_0 == 1:
        # driver.setSteeringAngle(0.0)
    if f < 1 and (kr0*f_r_0 + kr1*f_r_1 + kr2*f_r_2)-(kl0*f_l_0 + kl1*f_l_1 + kl2*f_l_2) > 0:
        fl = kl0*f_l_0 + kl1*f_l_1 + kl2*f_l_2
        fr = kr0*f_r_0 + kr1*f_r_1 + kr2*f_r_2 + 1.1*kr*r
        angle = 0.12*(fr-fl) 
    elif f < 1 and (kr0*f_r_0 + kr1*f_r_1 + kr2*f_r_2)-(kl0*f_l_0 + kl1*f_l_1 + kl2*f_l_2) < 0:
        fr = kr0*f_r_0 + kr1*f_r_1 + kr2*f_r_2
        fl = kl0*f_l_0 + kl1*f_l_1 + kl2*f_l_2 + 1.1*kl*l
        angle = 0.12*(fr-fl) 
    # elif f < 1 and angle == 0:
    #     angle = angle + 0.1


    # if abs(angle-l_angle) > 0.08:
    #     driver.setSteeringAngle((angle+l_angle)/2)
    # else:
    average = (angle+l_angle+ll_angle + lll_angle) / 4
    if abs(average) <= 0.002:
        angle = average
        driver.setSteeringAngle(angle)
    elif average > 0.002 and average <= 0.20:
        angle = average
        driver.setSteeringAngle(angle)
    elif average > 0.20:
        angle = 0.20
        driver.setSteeringAngle(angle)
    elif -average > 0.002 and -average <= 0.20:
        angle = average
        driver.setSteeringAngle(angle)
    elif -average > 0.20:
        angle = -0.20
        driver.setSteeringAngle(angle)
    # elif angle <
    #     driver.setSteeringAngle(angle)
    # elif angle > 0.2:
    #     driver.setSteeringAngle(0.2)
    # else:
    #     driver.setSteeringAngle(-0.2)

    # if angle > 0.03:
    #     driver.setSteeringAngle(abs(angle)** 0.5)
    # elif angle < -0.03:
    #     driver.setSteeringAngle(-abs(angle)** 0.5)
    # else:
    #     driver.setSteeringAngle(0.0)
    print("angle = " + str(angle))
    print("v = " + str(speed))

    return angle

# name of the available distance sensors
sensorsNames = [
    'front',
    'front right 0',
    'front right 1',
    'front right 2',
    'front left 0',
    'front left 1',
    'front left 2',
    'rear',
    'rear left',
    'rear right',
    'right',
    'left']
sensors = {}

driver = Driver()
driver.setSteeringAngle(0.0)  # go straight

# get and enable the distance sensors
for name in sensorsNames:
    sensors[name] = driver.getDevice('distance sensor ' + name)
    sensors[name].enable(10)

# get and enable the GPS
gps = driver.getDevice('gps')
gps.enable(10)

# get the camera
camera = driver.getDevice('camera')
# uncomment those lines to enable the camera
camera.enable(50)
camera.recognitionEnable(50)
clear()
oldangle = 0.0
olderangle = 0.0
olderestangle = 0.0
while driver.step() != -1:
    olderestangle = olderangle
    olderangle = oldangle
    oldangle = control(oldangle, olderangle, olderestangle)