import time
import threading
import numpy as np

from reachy_mini import ReachyMini, ReachyMiniApp
from reachy_mini.utils import create_head_pose


class TestDance1(ReachyMiniApp):

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

        reachy_mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)

        start_time = time.time()

        while not stop_event.is_set():
            t = time.time()
            elapsed_time = t - start_time

            compteur = int(elapsed_time / 2)

            antennas_offset = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))

            sinus_rot = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
            sinus_trans = 0.03 * np.sin(2 * np.pi * 0.5 * t)

            current_roll = 0.0
            current_pitch = 0.0
            current_yaw = 0.0
            current_x = 0.0
            current_y = 0.0
            current_z = 0.0

            if compteur < 3:
                current_roll = sinus_rot
            elif compteur < 6:
                current_pitch = sinus_rot
            elif compteur < 9:
                current_yaw = sinus_rot
            elif compteur < 12:
                current_x = sinus_trans
            elif compteur < 15:
                current_y = sinus_trans
            elif compteur < 18:
                current_z = sinus_trans
            else:
                start_time = time.time()
                continue

            head_pose = create_head_pose(
                x=current_x,
                y=current_y,
                z=current_z,
                roll=current_roll,
                pitch=current_pitch,
                yaw=current_yaw,
                degrees=False,
                mm=False,
            )

            reachy_mini.set_target(head=head_pose, antennas=[antennas_offset, antennas_offset])

            time.sleep(0.01)


class TestDance2(ReachyMiniApp):

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

        reachy_mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)

        start_time = time.time()

        while not stop_event.is_set():
            t = time.time()
            elapsed_time = t - start_time

            compteur = int(elapsed_time / 2)

            antennas_offset = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))

            sinus_rot = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * 4 * t))
            sinus_trans = 0.03 * np.sin(2 * np.pi * 0.5 * t)
            cosinus_trans = 0.03 * np.cos(2 * np.pi * 0.5 * t)

            current_roll = 0.0
            current_pitch = 0.0
            current_yaw = 0.0
            current_x = 0.0
            current_y = 0.0
            current_z = 0.0

            if compteur < 3:
                current_pitch = sinus_rot
                current_y = sinus_trans
            elif compteur < 6:
                current_x = sinus_trans
                current_y = cosinus_trans
            else:
                start_time = time.time()
                continue

            head_pose = create_head_pose(
                x=current_x,
                y=current_y,
                z=current_z,
                roll=current_roll,
                pitch=current_pitch,
                yaw=current_yaw,
                degrees=False,
                mm=False,
            )

            reachy_mini.set_target(head=head_pose, antennas=[antennas_offset, antennas_offset])

            time.sleep(0.01)


if __name__ == "__main__":
    print("Lancement manuel depuis le terminal...")

    with ReachyMini(media_backend="no_media") as robot:

        # 👉 Choisis ici ta danse
        # mon_app = TestDance1()
        mon_app = TestDance2()

        stop_event = threading.Event()

        try:
            mon_app.run(robot, stop_event)
        except KeyboardInterrupt:
            print("\nArrêt du programme")
            stop_event.set()