import numpy as np
from ArduinoCommunication import ArduinoCommunication
from EspCommunication import ESP32Communication
from simulation import Simulation
import DataTypes as dt
import kinematics as kin
import config as cfg

# Globale Variablen
robotCommunication = None
shared_sim = None
emote = None
display_text = ""

if cfg.sim:
    shared_sim = Simulation()
else:
    if cfg.arduinoConnection:
        robotCommunication = ArduinoCommunication()
    else:
        robotCommunication = ESP32Communication()

vector_dirmov = [0, 0, 0]  # Direction Movement [vx, vy, omega]
current_rad: dt.RadArray  # Current Rad
current_pos: dt.PosArray  # Current Position

# current_deg: dt.DegArray  # Current Degrees
# legarray: dt.DegArray  # Working Leg Array
# target_pos: dt.PosArray  # Target Position

robot_state = "idle"  # Current State
# Init for 6 Legs
#leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
# Init for 4 Legs
leg_state = np.array(["step", "drag", "drag", "drag", "drag", "drag"])
control_pause: bool = False

# Initilize mit Start Position
#init_deg: dt.DegArray = dt.DegArray(
#    [
#        [90, 30, 115],
#        [90, 30, 115],
#        [90, 30, 115],
#        [90, 150, 65],
#        [90, 150, 65],
#        [90, 150, 65],
#    ]
#)
#init_deg: dt.DegArray = dt.DegArray(
#    [
#        [90, 30, 95],
#        [90, 30, 95],
#        [90, 30, 95],
#        [90, 150, 85],
#        [90, 150, 85],
#        [90, 150, 85],
#    ]
#)
init_deg: dt.DegArray = dt.DegArray(
    [
        [90, 45, 140],
        [90, 45, 140],
        [90, 45, 140],
        [90, 135, 40],
        [90, 135, 40],
        [90, 135, 40],
    ]
)

init90_deg: dt.DegArray = dt.DegArray(
    [
        [90, 90, 90],
        [90, 90, 90],
        [90, 90, 90],
        [90, 90, 90],
        [90, 90, 90],
        [90, 90, 90],
    ]
)

center_points: dt.PosArray = kin.ikpyForward(init_deg.to_rad())