#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_D,OUTPUT_A,  OUTPUT_C, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_2,INPUT_1,INPUT_3,INPUT_4
from ev3dev2.sensor.lego import TouchSensor,ColorSensor
from ev3dev2.led import Leds
from ev3dev2.sound import Sound
import time


ts = TouchSensor(INPUT_3)
l_right = ColorSensor(INPUT_2)
l_left = ColorSensor(INPUT_4)

sound = Sound()

#tank_drive = MoveTank(OUTPUT_A, OUTPUT_D)

drive_left = LargeMotor(OUTPUT_A)
drive_right = LargeMotor(OUTPUT_D)



leds = Leds()

time.sleep(0.1)
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "RED")
time.sleep(0.1)
leds.set_color("LEFT", "RED")
leds.set_color("RIGHT", "GREEN")
time.sleep(0.1)
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "GREEN")
time.sleep(0.1)

tolerance = 4 #aprox difference between black and white
is_running = False
Kp = 10
Ki = 0
Kd = 0

limit = 300
speed = 200

dt = 200


integral = 0
previous_error = 0

sound.speak('Hello world')
try:
	while True:
		if(ts.is_pressed):
			is_running = not is_running
			time.sleep(0.3)
		if(not is_running):
			drive_right.stop()
			drive_left.stop()
			continue

		error = l_left.reflected_light_intensity - l_right.reflected_light_intensity
		integral += (error * dt)
		derivative = (error - previous_error) / dt

		u=(Kp*error) + (Ki * integral) + (Kd * derivative)
		print(u)
		previous_error = error
		# u>0 -> turn right
		# u<0 -> turn left


		#run
		if(u>=0):#bright from the left side
				#turn right
			drive_left.run_forever(speed_sp=min(speed+u,limit))
			drive_right.run_forever(speed_sp=max(speed-u,-limit))
		else:#bright from the right side
				  #turn left
			drive_left.run_forever(speed_sp=max(speed+u,-limit))
			drive_right.run_forever(speed_sp=min(speed-u,limit))





		# if((l_left.reflected_light_intensity - tolerance) > l_right.reflected_light_intensity):#needs to turn left
		# 	drive_left.run_forever(speed_sp=100)
		# 	drive_right.run_forever(speed_sp=-100)
		# elif((l_left.reflected_light_intensity + tolerance) < l_right.reflected_light_intensity):#needs to turn right
		# 	drive_left.run_forever(speed_sp=-100)
		# 	drive_right.run_forever(speed_sp=100)
		# else:
		# 	drive_left.run_forever(speed_sp=200)
		# 	drive_right.run_forever(speed_sp=200)




except KeyboardInterrupt:
	drive_right.stop()
	drive_left.stop()
print('hey')
	# if ts.
	#is_pressed:
	# 	sound.speak('intensity of light is ' + str(l_left.reflected_light_intensity()))

	# if ts.is_pressed:
	# 	tank_drive.on_for_rotations(SpeedPercent(-25), SpeedPercent(25), 10)
	# 	tank_drive.run_forever()
	# else:
	# 	tank_drive.stop()