import numpy as np
from ArduinoCommunication import ArduinoCommunication
from EspCommunication import ESP32Communication
from simulation import Simulation
import DataTypes as dt
import config as cfg
from robot_init import init_deg, init90_deg, init_pos, center_points

# 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
## Shared init pose definitions are imported from robot_init.py