# Generated by sila2.code_generator; sila2.__version__: 0.12.2
from __future__ import annotations
import asyncio
from .gripper_base import ArduinoGripper
from xarm.wrapper import XArmAPI
from typing import TYPE_CHECKING, Any, Dict, List
from sila2.server import MetadataDict

from sila2.server import MetadataDict

from ..generated.ufcontroller import (
    CloseArm_Responses,
    CloseGripper_Responses,
    InitializeArm_Responses,
    MoveJoints_Responses,
    MoveToStoredPosition_Responses,
    OpenGripper_Responses,
    ResetArm_Responses,
    SetManualMode_Responses,
    StorePosition_Responses,
    UFControllerBase,
)

"""
 Change the IP as neededy
"""
arm_ip = '192.168.1.160'

if TYPE_CHECKING:
    from ..server import Server


class UFControllerImpl(UFControllerBase):
    def __init__(self, parent_server: Server) -> None:
        gripper = ArduinoGripper()
        arm = XArmAPI(arm_ip)

        # Connect to the arm
        arm.connect()

        # Print some basic info about the arm
        print("Connected to xArm with IP: ", arm_ip)
        print("Arm State: ", arm.get_state())
        print("Current Position: ", arm.get_position())

        # Enable motion on the arm
        arm.motion_enable(enable=True)

        # Set the mode to position control
        arm.set_mode(0)

        # Set the state to be ready to receive commands
        arm.set_state(0)

        self.arm = arm
        self.gripper = gripper
        self.manual_mode_enabled = False  # Initialize manual mode state

        # Initialize position storage and periodic variables
        self.position_storage: Dict[int, List[float]] = {}
        self.periodic_variables: Dict[str, Any] = {}

        super().__init__(parent_server=parent_server)
    
    @property
    def JointAngles(self) -> List[float]:
        """Property representing the current joint angles of the arm."""
        return self.arm.get_servo_angle()

    async def monitor_joint_angles(self):
        """Background task to monitor joint angles and notify observers if changes occur."""
        while True:
            await asyncio.sleep(1)  # Poll every second
            current_angles = self.JointAngles
            if current_angles != self._current_joint_angles:
                self._current_joint_angles = current_angles
                self.notify_observers(current_angles)
                
    def notify_observers(self, joint_angles: List[float]):
        """Notify clients of changes to JointAngles."""
        # Example print statement to simulate notification
        print(f"JointAngles updated: {joint_angles}")
        # In a real implementation, you could send a message or trigger an event to notify clients

    def get_ArmIPAddress(self, *, metadata: MetadataDict) -> str:
        """Retrieve the IP address of the robotic arm."""
        return arm_ip

    def OpenGripper(self, *, metadata: MetadataDict) -> OpenGripper_Responses:
        """Open the gripper attached to the robotic arm."""
        self.gripper.send_command(command='o')
        return OpenGripper_Responses(success=True)

    def CloseGripper(self, *, metadata: MetadataDict) -> CloseGripper_Responses:
        """Close the gripper attached to the robotic arm."""
        self.gripper.send_command(command='c')
        return CloseGripper_Responses(success=True)

    def InitializeArm(self, *, metadata: MetadataDict) -> InitializeArm_Responses:
        """Initialize the robotic arm to its default state."""
        self.arm.motion_enable(enable=True)
        self.arm.set_mode(0)
        self.arm.set_state(0)
        self.arm.move_gohome()  # Move to home position
        print("Arm initialized and moved to home position.")
        return InitializeArm_Responses(success=True)

    def CloseArm(self, *, metadata: MetadataDict) -> CloseArm_Responses:
        """Disconnect and disable the robotic arm."""
        self.arm.disconnect()
        print("Arm connection closed.")
        return CloseArm_Responses(success=True)

    def ResetArm(self, *, metadata: MetadataDict) -> ResetArm_Responses:
        """Reset the robotic arm, usually after an error or emergency stop."""
        self.arm.reset(wait=True)
        print("Arm reset.")
        return ResetArm_Responses(success=True)

    def SetManualMode(self, *, metadata: MetadataDict) -> SetManualMode_Responses:
        """
        Toggle manual mode. If manual mode is off, turn it on.
        If manual mode is on, turn it off.
        """
        if not self.manual_mode_enabled:
            # Enable manual mode
            self.arm.set_mode(mode=2, detection_param=0)  # Set manual mode
            self.arm.set_state(0)  # Make the arm ready
            print("Manual mode enabled.")
            self.manual_mode_enabled = True
        else:
            # Disable manual mode
            self.arm.motion_enable(enable=True)
            self.arm.set_mode(0)  # Set to default mode
            self.arm.set_state(0)  # Make the arm ready
            print("Manual mode disabled.")
            self.manual_mode_enabled = False

        return SetManualMode_Responses(success=True)

    def MoveJoints(self, JointAngles: List[float], *, metadata: MetadataDict) -> MoveJoints_Responses:
        """Move the robotic arm to specified joint angles."""
        print("Moving to position: ", JointAngles)
        self.arm.set_servo_angle(angle=JointAngles, wait=True)
        return MoveJoints_Responses(JointAngles)

    def StorePosition(self, *, metadata: MetadataDict) -> StorePosition_Responses:
        """Store the current position of the robotic arm."""
        current_position = self.arm.get_servo_angle()
        position_id = len(self.position_storage) + 1
        self.position_storage[position_id] = current_position
        print(f"Position {position_id} stored: {current_position}")
        return StorePosition_Responses(PositionIdentifier=position_id)

    def MoveToStoredPosition(
        self, PositionIdentifier: int, *, metadata: MetadataDict
    ) -> MoveToStoredPosition_Responses:
        """Move the arm to a previously stored position using its identifier."""
        if PositionIdentifier not in self.position_storage:
            print(f"Position ID {PositionIdentifier} not found.")
            return MoveToStoredPosition_Responses(success=False)
        
        stored_position = self.position_storage[PositionIdentifier]
        self.arm.set_servo_angle(angle=stored_position, wait=True)
        print(f"Moved to stored position {PositionIdentifier}: {stored_position}")
        return MoveToStoredPosition_Responses(success=True)
