#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank, MediumMotor
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 vypis(self):
        print(self.x)
        print(self.y)


def degrees_to_radian(deg):
    return deg * PI / 180.0

def radian_to_degrees(deg):
    return deg * 180.0 / PI

def stred(aa, bb):
   return [(aa[0]+bb[0])/2, (aa[1]+bb[1])/2]

def kolmica(aa, bb):
   v = [aa[0]-bb[0], aa[1]-bb[1]]
   k = [v[1], -v[0]] 
   return k

def priesecnikmoj(aa, va, bb, vb):
    # a+ va*s , b + vb *t sa musia pretinat
    # rovnost pre zlozku 0,1 reprezentuje sustavu rovnic
    # a[0] + va[0]*t = b[0] + vb[0]*t  / * va[1]
    # a[1] + va[1]*t = b[1] + vb[1]*t  / * va[0]
    # ---------------------------------------------
    # a[0]*va[1] + va[0]*va[1]*t = b[0]*va[1] + vb[0]*t*va[1]       (1)
    # a[1]*va[0] + va[1]*va[0]*t = b[1]*va[0] + vb[1]*t*va[0]       (2)
    # ---------------------------------------------
    # (1) - (2)
    # ---------------------------------------------
    # a[1]*va[0] - a[0]*va[1] = b[1]*va[0] - b[0]*va[1] + t*(vb[1]*va[0] - vb[0]*va[1])    / - (b[1]*va[0] - b[0]*va[1])
    # ---------------------------------------------
    # a[1]*va[0] - a[0]*va[1] - (b[1]*va[0] - b[0]*va[1]) = t*(vb[1]*va[0] - vb[0]*va[1])  / /(vb[1]*va[0] - vb[0]*va[1])
    # ---------------------------------------------
    # (a[1]*va[0] - a[0]*va[1] - (b[1]*va[0] - b[0]*va[1])) / (vb[1]*va[0] - vb[0]*va[1]) = t
    t = (aa[1]*va[0] - aa[0]*va[1] - (bb[1]*va[0] - bb[0]*va[1])) / (vb[1]*va[0] - vb[0]*va[1])
    return [bb[0] + vb[0]*t, bb[1] + vb[1]*t]
def stredopisanej (A, B, C):
    S_ab = stred(A,B)
    K_ab = kolmica(A,B)
    S_ac = stred(A,C)
    K_ac = kolmica(A,C)
    print( "Stred : %f %f , Kolmica = %f %f \n" % ( S_ab[0], S_ab[1], K_ab[0], K_ab[1] ))
    print( "Stred : %f %f , Kolmica = %f %f \n" % ( S_ac[0], S_ac[1], K_ac[0], K_ac[1] ))
    return priesecnikmoj(S_ab, K_ab, S_ac, K_ac)
#a = eval(input())
#b = eval(input())
#c = eval(input())

def opisana():
   a = pole[2]
   b = pole[3]
   c = pole[1]
   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 = - cos(degrees_to_radian(ub))
   uB.y = sin(degrees_to_radian(ub))
   c = priesecnikmoj([A.x, A.y], [uA.x, uA.y], [B.x, B.y], [uB.x, uB.y])
   C.x = c[0]
   C.y = c[1]
   A.vypis()
   B.vypis()
   C.vypis()
   m = stredopisanej([A.x, A.y], [B.x, B.y], [C.x, C.y])
   S = bod()
   S.x = m[0]
   S.y = m[1]
   return S

s1 = ColorSensor('in1')
s2 = ColorSensor('in2')
L = LargeMotor('outA')
P = LargeMotor('outB')
M = MediumMotor('outC')
naraz = MoveTank('outA', 'outB')

pole = []

rychlost = 10
cakacicas = 0.1
def line_follower(smer):
   L.reset()
   P.reset()
   global pole
   i = 0
   a = 0.0
   while s1.reflected_light_intensity > 0:
      kolko = 0
      i += 1
      while s1.reflected_light_intensity < 7:
         L.on(speed = rychlost)
         time.sleep(cakacicas)
      L.off()
      if i == 1:
         a = L.degrees
      while s1.reflected_light_intensity > 6:
         P.on(speed = rychlost)
         kolko += 1
         time.sleep(cakacicas)
      P.off()
      if kolko > 5 and i > 3:
         break
   print("skoncil som prvu cast")
   print(L.degrees)
   print(a)
   pole.append(L.degrees-a)
   print("====")

print("ZACIATOK")

M.on(speed=10,brake=False,block=False)
time.sleep(0.5)
M.off()
for i in range(0, 4):
   line_follower(1000)
   print(pole[i])
taz = opisana()
print(taz.x)
print(taz.y)
naraz = MoveTank('outA', 'outB')
s2 = ColorSensor('in2')
naraz.on_for_degrees(10, -10, 30)
L.on(5)
P.on(-5)
while s2.reflected_light_intensity < 7:
   print(s2.reflected_light_intensity)
L.off()
P.off()
naraz.on_for_degrees(-5, 5, 20)
naraz.on_for_degrees(10, 10, taz.x)
naraz.on_for_degrees(-10, 10, 145)
naraz.on_for_degrees(10, 10, taz.y-200)
M.on(speed=-10,brake=False,block=False)
time.sleep(0.5)
M.off()
print("KONIEC")