from functools import partial

from ingeniamotion import MotionController
from ingeniamotion.enums import OperationMode


class Model:
    def __init__(self):
        self.stop = False

    def notify_exception(self, exc) -> None:
        self.stop = True

    @staticmethod
    def notify_send_data(position_set_point):
        position_set_point.value += 1


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()
    model = Model()
    position_set_point = mc.capture.pdo.create_pdo_item(
        "CL_POS_SET_POINT_VALUE", value=0
    )
    # Create a TPDO map item
    actual_position = mc.capture.pdo.create_pdo_item("CL_POS_FBK_VALUE")
    # Create the RPDO and TPDO maps
    rpdo_map, tpdo_map = mc.capture.pdo.create_pdo_maps([position_set_point], [actual_position])
    mc.capture.pdo.set_pdo_maps_to_slave(rpdo_map, tpdo_map)
    mc.capture.pdo.subscribe_to_exceptions(model.notify_exception)
    mc.capture.pdo.subscribe_to_send_process_data(
        partial(model.notify_send_data, position_set_point)
    )
    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)
    try:
        while not model.stop:
            pass
    except BaseException:
        pass
    mc.capture.pdo.unsubscribe_to_exceptions(model.notify_exception)
    mc.capture.pdo.unsubscribe_to_send_process_data(partial(model.notify_send_data, position_set_point))
    # 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 = "169.254.52.79"
    ethercat_slave_id = 1
    dictionary_path = "C://Program Files (x86)//MotionLab3//_internal//resources//dictionaries//den-s-net-e_eoe_2.6.1.xdf"
    main(network_interface_ip, ethercat_slave_id, dictionary_path)