import contextlib

from ingeniamotion import MotionController
from ingeniamotion.enums import OperationMode
from ingeniamotion.exceptions import IMTimeoutError


def main(interface_ip, slave_id, dict_path):
    """Establish a FSoE connection, deactivate the STO and
    move the motor."""
    mc = MotionController()
    # Configure error channel
    mc.fsoe.subscribe_to_errors(lambda error: print(error))
    # Connect to the servo drive
    mc.communication.connect_servo_ethercat_interface_ip(interface_ip, slave_id, dict_path)
    current_operation_mode = mc.motion.get_operation_mode()
    # Set the Operation mode to Velocity
    mc.motion.set_operation_mode(OperationMode.VELOCITY)
    # Create and start the FSoE master handler
    mc.fsoe.create_fsoe_master_handler()
    mc.fsoe.configure_pdos(start_pdos=True)
    # Wait for the master to reach the Data state
    mc.fsoe.wait_for_state_data(timeout=10)
    # Deactivate the SS1
    mc.fsoe.ss1_deactivate()
    # Deactivate the STO
    mc.fsoe.sto_deactivate()
    # Wait for the STO to be deactivated
    while mc.fsoe.check_sto_active():
        pass
    # Enable the motor
    mc.motion.motor_enable()
    # Wait for the motor to reach a certain velocity (10 rev/s)
    target_velocity = 10
    mc.motion.set_velocity(target_velocity)
    with contextlib.suppress(IMTimeoutError):
        mc.motion.wait_for_velocity(velocity=target_velocity, timeout=5)
    # Disable the motor
    mc.motion.motor_disable()
    # Activate the SS1
    mc.fsoe.sto_activate()
    # Activate the STO
    mc.fsoe.sto_activate()
    # Stop the FSoE master handler
    mc.fsoe.stop_master(stop_pdos=True)
    # Restore the operation mode
    mc.motion.set_operation_mode(current_operation_mode)
    # Disconnect from the servo drive
    mc.communication.disconnect()


if __name__ == "__main__":
    # Modify these parameters according to your setup
    network_interface_ip = "192.168.2.1"
    ethercat_slave_id = 1
    dictionary_path = "safe_dict.xdf"
    main(network_interface_ip, ethercat_slave_id, dictionary_path)
