#!/usr/bin/env python3

from time import sleep
import ev3dev2 as ev3
from ev3dev2.motor import Motor, SpeedPercent, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D 
from ev3dev2.sensor import INPUT_1,INPUT_2,INPUT_3,INPUT_4
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.led import Leds

# Define all inputs and outputs of the Robot
# Sensors are inputs
cs_right = ColorSensor(INPUT_4) 
cs_left = ColorSensor(INPUT_2)
# Motors are outputs
m_left = Motor(OUTPUT_A) 
m_right = Motor(OUTPUT_D)
leds = Leds()

# The motor is initialized to run counter-clockwise (gegen den Uhrzeigersinn)
m_right.polarity  = 'inversed' # use 'normal' to initialize it to run clockwise (im Uhrzeigersinn)
m_left.polarity  = 'inversed'

# Defined inputs for the given experiment / enviroment
Line = 2 # min. reflection meassured of line
Floor = 30 # max. reflection meassured of floor
max_speed_percent = 45 # maximum wanted speed percentage (highest possible value 100%)

# calculate range
range = (Floor - Line)

# while-loop: the robot drives as long as the programm is not stopped
while True: 
    
    # the detected intensity of the two colorsensors are measured
    cs_r_intensity = cs_right.reflected_light_intensity
    cs_l_intensity = cs_left.reflected_light_intensity
    # calculate percentage of intensity from given range
    cs_r_percent = (cs_r_intensity - Line) / range
    cs_l_percent = (cs_l_intensity - Line) / range
    # calculation percantage of driving speed, depending on maximum speed percentage
    m_l_speed_percent= max_speed_percent*cs_l_percent 
    m_r_speed_percent= max_speed_percent*cs_r_percent
    # make sure percentage is not higher than possible (100%) and signal it through lights
    if m_l_speed_percent > 100:
        m_l_speed_percent = 0
        leds.set_color("LEFT", "AMBER")
    else:
        leds.set_color("LEFT", "GREEN")
    if m_r_speed_percent > 100:
        m_r_speed_percent = 0
        leds.set_color("RIGHT", "AMBER")
    else:
        leds.set_color("RIGHT", "GREEN")
    # the motors drive with the calculated speed
    m_left.on(SpeedPercent(m_l_speed_percent)) 
    m_right.on(SpeedPercent(m_r_speed_percent))
    sleep(0.3)




