from inputs import *
from controllables import *
from sensors import *
from scriptruntime import *
from ports import *
from color import *
import asyncio
from math import sin, cos, asin, acos, atan, sqrt, degrees, radians, pi, atan2
import time
from time import sleep

# Начальные данные
stream_keys = []
Legs = []
keys_list = ['w', 'a', 's', 'd', 'q', 'e', 'r', 'f', 'z', 'x', 'g', 'c', 'v', "1", "2", "o", "p", "i", "u"]
Angle = -90
Test = False

Leg_angle = 0
pov_angle = 0
Visota_angle = 25
regim = 0
piston = 0


def init():
    # инициализация робота
    global leg_FL, leg_FR, leg_BL, leg_BR, Legs, hand, screen
    # обьявление ног
    leg_FL = Leg(LED(31), ServoMotor(35), Piston(28), Piston(26), Piston(27), Piston(25), ServoMotor(30),
                 ServoMotor(29), ServoMotor(24), ServoMotor(44), DCMotor(43), DCMotor(42))
    leg_FR = Leg(LED(15), ServoMotor(32), Piston(12), Piston(10), Piston(11), Piston(9), ServoMotor(13), ServoMotor(14),
                 ServoMotor(8), ServoMotor(45), DCMotor(39), DCMotor(38))
    leg_BL = Leg(LED(23), ServoMotor(34), Piston(20), Piston(18), Piston(19), Piston(17), ServoMotor(21),
                 ServoMotor(22), ServoMotor(16), ServoMotor(46), DCMotor(41), DCMotor(40))
    leg_BR = Leg(LED(6), ServoMotor(33), Piston(4), Piston(3), Piston(5), Piston(2), ServoMotor(1), ServoMotor(0),
                 ServoMotor(7), ServoMotor(47), DCMotor(36), DCMotor(37))
    # реверс моторов
    leg_FR.invers = True
    leg_BL.invers = True
    leg_FR.stopa.invers_poz = True
    leg_BR.stopa.invers_poz = True
    leg_FR.gaz.invers = True
    leg_BR.gaz.invers = True
    # главный экран
    screen = TextScreen(48)

    # обьявление манипулятора
    class hand():
        # экран манипулятора
        screen = TextScreen(55)
        # обьявление звеньев руки
        osnov = Lokot(ServoMotor(49), ServoMotor(50), 6)
        lokot = Lokot(ServoMotor(51), ServoMotor(52), 13)
        kist = Lokot(ServoMotor(53), ServoMotor(54), 13)
        # обьявление данных для руки
        znach = 0
        x = 0
        y = 0
        z = 0
        angle = 0
        screen.size = 60
        
        
        def polojenie(pangle, pturn, langle, lturn, kangle, ktunr):
            hand.osnov.spin_osnov(pturn)
            hand.osnov.spin_sgib(pangle)
            hand.lokot.spin_osnov(lturn)
            hand.lokot.spin_sgib(langle)
            hand.kist.spin_osnov(ktunr)
            hand.kist.spin_sgib(kangle)
        
        def pologUP(up):
            if up:
                hand.znach += 1
            else:
                hand.znach -= 1
            hand.znach = round(limiter(hand.znach, 0, 4))
            hand.polog()
        
        def polog():
            if hand.znach == 0:
                hand.polojenie(0,0,0,0,0,0)
            elif hand.znach == 1:
                hand.polojenie(-35,0,90,0,0,0)
            elif hand.znach == 2:
                hand.polojenie(-35,180,90,0,0,0)
            elif hand.znach == 3:
                hand.polojenie(-35,360,90,0,0,0)
            elif hand.znach == 4:
                hand.polojenie(-90,360,-90,90,0,0)
            else:
                print(f"ошибка значения: {hand.znach}")


        def screen_update():
            '''if hand.znachenie == 1:
                hand.screen.text = (
                    f"""
                x = {hand.x}
                """)
            elif hand.znachenie == 2:
                hand.screen.text = (
                    f"""
                y = {hand.y}
                """)
            elif hand.znachenie == 3:
                hand.screen.text = (
                    f"""
                z = {hand.z}
                """)
            elif hand.znachenie == 4:
                hand.screen.text = (
                    f"""
                угол инс. = {hand.angle}
                """)
            elif hand.znachenie == 0:
                hand.screen.text = (
                    f"""x = {hand.x}
                y = {hand.y}
                z = {hand.z}
                угол инс. = {hand.angle}""")'''
            pass

        '''def OZK(x_v, y_v, z_v, orient_alf):  # pos = [x, y, z]
            try:
                l1 = 6
                l2 = 13
                l3 = 13
                l4 = 3

                x_v += 0.000001
                y_v += 0.000001
                z_v += 0.000001

                alf1 = atan2(y_v, x_v)

                z_v = z_v - l1
                z = z_v + l4 * cos(orient_alf)

                k_v = sqrt(x_v ** 2 + y_v ** 2)
                k = k_v - l4 * sin(orient_alf)

                d_v = sqrt(k_v ** 2 + z_v ** 2)
                d = sqrt(k ** 2 + z ** 2)

                v1 = atan2(z, k)

                v2 = acos((d ** 2 + l2 ** 2 - l3 ** 2) / (2 * d * l2))

                alf2 = pi / 2 - (atan2(z, k) + acos((d ** 2 + l2 ** 2 - l3 ** 2) / (2 * d * l2)))
                alf3 = pi - (acos((-(d ** 2) + l2 ** 2 + l3 ** 2) / (2 * l2 * l3)))

                alf_vspom = pi - (alf2 + alf3)
                alf4 = alf_vspom - orient_alf

                alf1 = round(degrees(alf1), 1)
                alf2 = round(degrees(alf2), 1)
                alf3 = round(degrees(alf3), 1)
                alf4 = round(degrees(alf4), 1)

                if Test:
                    print("манипулятор")
                    print(f"Alf1 = {alf1}  Alf2 = {alf2}  Alf3 = {alf3}  Alf4 = {alf4}")
                sleep(.1)
                hand.osnov.spin_osnov(alf1)
                hand.osnov.spin_sgib(alf2)
                hand.lokot.spin_sgib(alf3)
                hand.kist.spin_sgib(alf4)
                if Test:
                    print("")
            except:
                print("ошибка положения манипулятора")
                sleep(0.1)
                pass'''

    print("инициализация закончена")


# создание переменных клавиш
for key in keys_list:
    if key.isdigit():
        # клавиши цифр
        exec(f"n{key} = False")
        stream_keys.append(Input.stream(key))
    else:
        # клавиши обычные
        exec(f"{key} = False")
        stream_keys.append(Input.stream(key))


def key_update():
    # функция обновления клавиш
    global keys_list
    if Test:
        print("обновление клавиш")
    for idx, stream_key in enumerate(stream_keys):
        Key = keys_list[idx]
        for event in stream_key:
            if event == ButtonState.DOWN:
                if Key.isdigit():
                    exec(f"n{Key} = True", globals())
                    if Test:
                        print(f"n{Key} = нажали")
                    pass
                else:
                    exec(f"{Key} = True", globals())
                    if Test:
                        print(f"{Key} = нажали")
                    pass
            elif event == ButtonState.UP:
                if Key.isdigit():
                    exec(f"n{Key} = False", globals())
                    if Test:
                        print(f"n{Key} = Отжали")
                    pass
                else:
                    exec(f"{Key} = False", globals())
                    if Test:
                        print(f"{Key} = Отжали")
                    pass
    if Test:
        print("")


class Lokot():
    # класс изгиба руки
    def __init__(self, motor1, motor2, dlin):
        self.motor1 = motor1
        self.motor2 = motor2
        self.dlin = dlin

    def spin_osnov(self, angle):
        self.motor1.spin_to_degrees(angle)

    def spin_sgib(self, angle):
        self.motor2.spin_to_degrees(angle)


class Gaz():
    # класс движения стопы
    def __init__(self, motor1, motor2):
        self.motor1 = motor1
        self.motor2 = motor2
        self.invers = False

    def go(self, power):
        if self.invers:
            self.motor1.spin(power)
            self.motor2.spin(power)
        else:
            self.motor1.spin(power * -1)
            self.motor2.spin(power * -1)

    def stop(self):
        self.motor1.stop()
        self.motor2.stop()


class Stopa():
    # класс вращения стопы
    def __init__(self, motor_nap, motor_poz):
        self.motor_nap = motor_nap
        self.invers_nap = False
        self.motor_poz = motor_poz
        self.invers_poz = False
        self.popravka = 0

    def spin_to(self, angle):
        if self.invers_nap:
            self.motor_nap.spin_to_degrees(angle + self.popravka)
        else:
            self.motor_nap.spin_to_degrees(angle * -1 + self.popravka)

    def poz_to(self, angle):
        if self.invers_poz:
            self.motor_poz.spin_to_degrees(angle)
        else:
            self.motor_poz.spin_to_degrees(angle * -1)


class Visota():
    # класс высоты ноги
    def __init__(self, motor1, motor2):
        self.motor1 = motor1
        self.motor2 = motor2

    def spin_to(self, angle):
        self.motor1.spin_to_degrees(angle)
        self.motor2.spin_to_degrees(angle * -1)


class Leg():
    # класс длинны ноги
    global Legs

    def __init__(self, led, motor, piston_UL, piston_UR, piston_DL, piston_DR, visota_motor1, visota_motor2,
                 stopa_motor_nap, stopa_motor_poz, gaz_motor1, gaz_motor2):
        global Legs
        self.motor = motor
        self.visota = Visota(visota_motor1, visota_motor2)
        self.led = led
        self.piston_UL = piston_UL
        self.piston_UR = piston_UR
        self.piston_DL = piston_DL
        self.piston_DR = piston_DR
        self.stopa = Stopa(stopa_motor_nap, stopa_motor_poz)
        self.gaz = Gaz(gaz_motor1, gaz_motor2)
        self.invers = False
        Legs.append(self)

    def spin_to(self, angle):
        if self.invers:
            self.motor.spin_to_degrees(angle)
        else:
            self.motor.spin_to_degrees(angle * -1)

    def stop_piston(self):
        self.piston_UR.stop()
        self.piston_UL.stop()
        self.piston_DR.stop()
        self.piston_DL.stop()

    def move_to(self, dlina):
        self.piston_UL.move_to(dlina)
        self.piston_UR.move_to(dlina)
        self.piston_DL.move_to(dlina)
        self.piston_DR.move_to(dlina)


class sittings():
    standart_angle = -90
    piston_speed = 0.05
    visota_speed = 0.1
    backand_speed = 1
    leg_napr_speed = 0.1
    Angle_speed = 5
    pov_angle_speed = 3


def coordinate_to_degrees(x, y):  # функция для преобразования координат в углы от оси X (0 ~ 360)
    x += 0.00001  # это делается для того, чтобы избежать ошибки деления нуля в случае x == 0
    angle = None
    if x >= 0 and y >= 0:
        angle = degrees(atan(y / x))
    elif x < 0 and y >= 0:
        angle = 180 + degrees(atan(y / x))
    elif x < 0 and y < 0:
        angle = 180 + degrees(atan(y / x))
    elif x >= 0 and y < 0:
        angle = 360 + degrees(atan(y / x))
    return round(angle, 1)


def start():
    # начальная позиция робота
    for leg in Legs:
        leg.visota.spin_to(Visota_angle)
        leg.spin_to(Leg_angle + 45)
        leg.stopa.poz_to(90)

    print("Стартовое положение принято")


def main():
    # функция обьявляющая запуск
    print('шаблон скрипта прикреплен к ' + MicroController.self().name())
    print("Программа запущена")


def angle_flip(angle):
    # переворот угла
    smoothed_angle_degrees = angle - 360 if angle > 180 else angle
    return smoothed_angle_degrees


def update():
    # функция выполняющася постоянно
    global Leg_angle, Angle, Legs
    leg_BL.stopa.popravka = -1 * angle_flip(leg_BL.motor.degrees()) - pov_angle
    leg_BR.stopa.popravka = -1 * angle_flip(leg_BR.motor.degrees()) - pov_angle
    leg_FL.stopa.popravka = -1 * angle_flip(leg_FL.motor.degrees()) + pov_angle
    leg_FR.stopa.popravka = -1 * angle_flip(leg_FR.motor.degrees()) + pov_angle

    for i in Legs:
        i.stopa.spin_to(Angle)
        i.spin_to(Leg_angle + 45)
        i.move_to(piston)
        i.visota.spin_to(Visota_angle)


def limiter(x, min_val, max_val):
    # функция ограничивающая значения
    if x < min_val:
        return min_val
    elif x > max_val:
        return max_val
    else:
        return x


def limit():
    # функция проверяющая ограничения пораметров
    global Leg_angle, pov_angle, piston, Angle, Visota_angle
    Leg_angle = round(limiter(Leg_angle, -65, +65), 2)
    pov_angle = round(limiter(pov_angle, -90, +90), 2)
    piston = round(limiter(piston, 0, 1), 4)
    Angle = round(Angle, 2)
    Visota_angle = round(limiter(Visota_angle, -50, 60), 1)
    


def screen_update():
    # функция обновления экрана
    screen.text = (
        f"""
угол стопы = {Angle}
угол ноги = {Leg_angle}
длинна ноги = {round(piston * 100)}%
высота ноги = {Visota_angle}


""")


# основная программа
main()
init()
start()

print("Цикл программы запущен")
while True:
    T1 = time.time()

    # основная функция управления
    key_update()

    if p:
        pass

    elif o:
        hand.pologUP(True)
        time.sleep(0.5)
    elif i:
        hand.pologUP(False)
        time.sleep(0.5)

    if n1:
        regim = 0
    elif n2:
        regim = 1

    if w:
        leg_BL.gaz.go(1)
        leg_BR.gaz.go(1)
        leg_FL.gaz.go(1)
        leg_FR.gaz.go(1)
    elif s:
        leg_BL.gaz.go(-1)
        leg_BR.gaz.go(-1)
        leg_FL.gaz.go(-1)
        leg_FR.gaz.go(-1)
    else:
        leg_BL.gaz.stop()
        leg_BR.gaz.stop()
        leg_FL.gaz.stop()
        leg_FR.gaz.stop()

    if g:
        Angle = sittings.standart_angle

    if c:
        for i in Legs:
            piston += sittings.piston_speed
    if v:
        for i in Legs:
            piston -= sittings.piston_speed

    if z:
        Visota_angle -= sittings.visota_speed
    elif x:
        Visota_angle += sittings.visota_speed

    if pov_angle < 0:
        pov_angle += sittings.backand_speed
    elif pov_angle > 0:
        pov_angle -= sittings.backand_speed

    if f:
        Leg_angle -= sittings.leg_napr_speed
    elif r:
        Leg_angle += sittings.leg_napr_speed

    if regim == 0:
        if a:
            Angle -= sittings.Angle_speed
        elif d:
            Angle += sittings.Angle_speed
        elif e:
            pov_angle -= sittings.pov_angle_speed
        elif q:
            pov_angle += sittings.pov_angle_speed
    elif regim == 1:
        if d:
            pov_angle -= sittings.pov_angle_speed
        elif a:
            pov_angle += sittings.pov_angle_speed
        elif q:
            Angle -= sittings.Angle_speed
        elif e:
            Angle += sittings.Angle_speed

    limit()
    update()
    screen_update()
    hand.screen_update()
    # hand.OZK(hand.x,hand.y,hand.z, hand.angle)

    T2 = time.time()
    ttw = 1 / 50 - (T2 - T1)
    if ttw > 0:
        if Test:
            print(f"свободное время = {ttw * 100}mc")
            print()
        time.sleep(ttw)

    else:
        print("Полундра")
        print(f"Отставание = {ttw * 100} mc")
        print()
