import threading
import time

import numpy as np
from reachy_mini import ReachyMiniApp
from reachy_mini.reachy_mini import ReachyMini
from scipy.spatial.transform import Rotation as R


class ExampleApp(ReachyMiniApp):
    def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
        t0 = time.time()
        while not stop_event.is_set():
            t = time.time() - t0

            target = np.deg2rad(30) * np.sin(2 * np.pi * 0.5 * t)

            yaw = target
            head = np.eye(4)
            head[:3, :3] = R.from_euler("xyz", [0, 0, yaw], degrees=False).as_matrix()

            reachy_mini.set_position(head=head, antennas=np.array([target, -target]))

            time.sleep(0.01)
            # if more than one second since last ping, print ping
            if t % 1 < 0.01:
                print("Ping")