# Global Variables
import GlobalVariables as gv

# main.py
import time
import threading

from DataTypes import ControlIntent
from RobotState.RobotState import RobotContext
from Controller import controller_loop
from RobotState.idle import IdleState
from RobotState.walking import WalkingState

import DataTypes as dt

def main():
    intent = ControlIntent()
    ctx = RobotContext()

    ctx.center_points = dt.PosArray([
        [50, 50, -80],
        [50,-50, -80],
        [0, 70, -80],
        [0,-70, -80],
        [-50,50, -80],
        [-50,-50,-80]
    ])
    ctx.current_pos = ctx.center_points.copy()

    controller_thread = threading.Thread(
        target=controller_loop,
        args=(intent,),
        daemon=True
    )
    controller_thread.start()

    states = {
        "idle": IdleState(),
        "walking": WalkingState()
    }

    current = states["idle"]
    current.on_enter(ctx)

    last = time.perf_counter()

    while not intent.quit:
        now = time.perf_counter()
        dt_s = now - last
        last = now

        next_state = current.update(ctx, intent, dt_s)
        if next_state:
            current.on_exit(ctx)
            current = states[next_state]
            current.on_enter(ctx)

        time.sleep(0.01)

if __name__ == "__main__":
    main()