from threading import Thread
from multiprocessing import Process, Manager, Queue
import time

# Selfmade Libraries
import Controller as ctr
import RobotState as rs

# Global Variables
import GlobalVariables as gv


def robot_control():
    # Connection
    if gv.robotCommunication != None:
        gv.robotCommunication.start()

    # Start Position
    rs.initPos()
    time.sleep(1)

    ############################## Main Loop ##############################
    tick = 0.05
    next_time = time.time()

    while gv.control_pause == False:
        next_time += tick

        if gv.emote == "wave":
            rs.initPos()
            time.sleep(0.3)
            rs.wave_emote()
            gv.emote = None
            gv.robot_state = "idle"
            continue

        # Robot
        if gv.robot_state == "idle":
            rs.initPos()
            time.sleep(0.2)
        elif gv.robot_state == "walking":
            rs.walking()

        # Simulation
        if gv.shared_sim != None:
            gv.shared_sim.step()

        sleep_time = next_time - time.time()
        if sleep_time > 0:
            time.sleep(sleep_time)


if __name__ == "__main__":
    controller_queue = Queue()
    robot_queue = Queue()

    controller_thread = Thread(target=ctr.controller)
    controller_thread.start()
    robot_control_thread = Thread(target=robot_control)
    robot_control_thread.start()