import threading
from reachy_mini import ReachyMini, ReachyMiniApp
from reachy_mini.utils import create_head_pose
import numpy as np
import time
from pydantic import BaseModel


class Test(ReachyMiniApp):
    # Optional: URL to a custom configuration page for the app
    # eg. "http://localhost:8042"
    custom_app_url: str | None = "http://0.0.0.0:8042"
    # Optional: specify a media backend ("gstreamer", "gstreamer_no_video", "default", etc.)
    # On the wireless, use gstreamer_no_video to optimise CPU usage if the app does not use video streaming
    request_media_backend: str | None = None

    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
        t0 = time.time()

        antennas_enabled = True
        sound_play_requested = False

        # You can ignore this part if you don't want to add settings to your app. If you set custom_app_url to None, you have to remove this part as well.
        # === vvv ===
        class AntennaState(BaseModel):
            enabled: bool

        @self.settings_app.post("/antennas")
        def update_antennas_state(state: AntennaState):
            nonlocal antennas_enabled
            antennas_enabled = state.enabled
            return {"antennas_enabled": antennas_enabled}

        @self.settings_app.post("/play_sound")
        def request_sound_play():
            nonlocal sound_play_requested
            sound_play_requested = True

        # === ^^^ ===

        # Main control loop
        while not stop_event.is_set():
            t_start = time.time()
            while time.time() - t_start < 5:
                t = time.time() - t0
                roll_deg = 30.0 * np.sin(2.0 * np.pi * 0.2 * t)
                head_pose = create_head_pose(roll=roll_deg, degrees=True)
                reachy_mini.set_target(head=head_pose)
                time.sleep(0.02)
            t_start = time.time()
            while time.time() - t_start < 5:
                t = time.time() - t0
                pitch_deg = 30.0 * np.sin(2.0 * np.pi * 0.2 * t)
                head_pose = create_head_pose(pitch=pitch_deg, degrees=True)
                reachy_mini.set_target(head=head_pose)
                time.sleep(0.02)

            t_start = time.time()
            while time.time() - t_start < 5:
                t = time.time() - t0
                yaw_deg = 30.0 * np.sin(2.0 * np.pi * 0.2 * t)
                head_pose = create_head_pose(yaw=yaw_deg, degrees=True)
                reachy_mini.set_target(head=head_pose)
                time.sleep(0.02)

            t_start = time.time()
            while time.time() - t_start < 5:
                t = time.time() - t0
                x_pos = 0.03 * np.sin(2.0 * np.pi * 0.2 * t)
                head_pose = create_head_pose(x=x_pos)
                reachy_mini.set_target(head=head_pose)
                time.sleep(0.02)

            t_start = time.time()
            while time.time() - t_start < 5:
                t = time.time() - t0
                y_pos = 0.03 * np.sin(2.0 * np.pi * 0.2 * t)
                head_pose = create_head_pose(y=y_pos)
                reachy_mini.set_target(head=head_pose)
                time.sleep(0.02)

            t_start = time.time()
            while time.time() - t_start < 5:
                t = time.time() - t0
                z_pos = 0.015 * np.sin(2.0 * np.pi * 0.2 * t)
                head_pose = create_head_pose(z=z_pos)
                reachy_mini.set_target(head=head_pose)
                time.sleep(0.02)


if __name__ == "__main__":
    app = Test()
    try:
        app.wrapped_run()
    except KeyboardInterrupt:
        app.stop()