#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor import INPUT_2
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sensor.lego import LightSensor
from ev3dev2.led import Leds
from ev3dev2.sound import Sound
import time
from math import *
PI = 3.14159265
class bod:
    x = 0
    y = 0

def xpriesecnik(x1, x2, y1, y2):
    a1 = x2.y - x1.y
    b1 = x1.x - x2.x
    c1 = -(a1*x1.x+b1*x1.x)
    a2 = y2.y - y1.y
    b2 = y1.x - y2.x
    c2 = -(a2*y1.x+b2*y1.y)
    return (b1*c2-b2*c1)/(b2*a1-b1*a2)

def ypriesecnik(x1, x2, y1, y2):
    a1 = x2.x - x1.x
    b1 = x1.y - x2.y
    c1 = -(a1*x1.y+b1*x1.y)
    a2 = y2.x - y1.x
    b2 = y1.y - y2.y
    c2 = -(a2*y1.y+b2*y1.x)
    return (b1*c2-b2*c1)/(b2*a1-b1*a2)
def degrees_to_radian(deg):
    return deg * PI / 180.0

def radian_to_degrees(deg):
    return deg * 180.0 / PI
#poznamocka urobena na HP
s1 = ColorSensor('in1')
L = LargeMotor('outA')
P = LargeMotor('outB')
naraz = MoveTank('outA', 'outB')

pole = []

rychlost = 10
cakacicas = 0.1
def line_follower(smer):
   global pole
   i = 0
   a = 0
   while s1.reflected_light_intensity > 0:
      kolko = 0
      i += 1
      #print(s1.reflected_light_intensity)
      while s1.reflected_light_intensity < 7:
         #L.on_for_degrees(speed = rychlost, degrees = 10, brake = True, block = False)
         L.on(speed = rychlost)
         a += 10
         time.sleep(cakacicas)
         #print("L")
      L.off()
      while s1.reflected_light_intensity > 6:
         #P.on_for_degrees(speed = rychlost, degrees = 10, brake = True, block = False)
         P.on(speed = rychlost)
         a += 10
         kolko += 1
         time.sleep(cakacicas)
         #print("P")      
      P.off()
      #print(".")
      if kolko > 5 and i > 3:
         break
   print("skoncil som prvu cast")
   print(L.degrees)
   print(P.degrees)
   pole.append(a/2)
   print("====")
   #for i in range(0, 3):
   #   if s1.reflected_light_intensity < 7:
   #      L.run_timed(time_sp = 20, speed_sp = smer)
   #      time.sleep(0.2)
   #   if s1.reflected_light_intensity > 6:
   #      P.run_timed(time_sp = 20, speed_sp=smer)
   #      time.sleep(0.2)

def tazis():
   global pole
   a = pole[3]
   b = pole[1]
   c = pole[2]
   A = bod()
   B = bod()
   C = bod()
   vnutro = (b*b+c*c-a*a)/(2*b*c)
   asiuholvradianoch = acos(vnutro)
   asiuholvstupnoch = radian_to_degrees(asiuholvradianoch)
   ua = asiuholvstupnoch
   vnutro = (-b*b+c*c+a*a)/(2*a*c)
   asiuholvradianoch = acos(vnutro)
   asiuholvstupnoch = radian_to_degrees(asiuholvradianoch)
   ub = asiuholvstupnoch
   vnutro = (b*b-c*c+a*a)/(2*a*b)
   asiuholvradianoch = acos(vnutro)
   asiuholvstupnoch = radian_to_degrees(asiuholvradianoch)
   uc = asiuholvstupnoch
   A.x = 0
   A.y = 0
   B.x = c
   B.y = 0
   uA = bod()
   uB = bod()
   uA.x = cos(degrees_to_radian(ua))
   uA.y = sin(degrees_to_radian(ua))
   uB.x = B.x - cos(degrees_to_radian(ub))
   uB.y = sin(degrees_to_radian(ub))
   C.x = xpriesecnik(A, uA, B, uB)
   C.y = ypriesecnik(A, uA, B, uB)
   mA = bod()
   mB = bod()
   mA.x = (B.x+C.x)/2
   mA.y = (B.y+C.y)/2
   mB.x = (A.x+C.x)/2
   mB.y = (A.y+C.y)/2
   tazisko = bod()
   tazisko.x = xpriesecnik(A, mA, B, mB)
   tazisko.y = ypriesecnik(A, mA, B, mB)
   return tazisko
print("ZACIATOK")
for i in range(0, 4):
   line_follower(1000)
   print(pole[i])
taz = tazis()
print (taz.x)
print(taz.y)
naraz.on_for_degrees(10, 10, round(taz.x))
naraz.on_for_degrees(-10, 10, 145)
naraz.on_for_degrees(10, 10, round(taz.y))
print("KONIEC")