import asyncio
# asyncio is used for asynchronous programming, allowing for non-blocking operations.
from ag_motioncontrol import MotionController
# Imports the MotionController class from the ag_motioncontrol library, which is likely
# responsible for communicating with the motion control hardware or simulator.

connection = None
# Initializes the connection variable to None, indicating no specific connection is set.
#connection = "serialport2=/dev/ttyACM1"
# This commented-out line shows an example of how a serial port connection might be configured
# for communication with physical hardware (e.g., a motion controller connected via serial).
api = MotionController(simulation=True, manual_transport_config=connection, 
                     debug_log_level=0)
# Creates an instance of the MotionController class.
# - simulation=True: This crucial parameter indicates that the code will run in a simulation mode.
#   No actual hardware interaction will occur, which is useful for testing and development.
# - manual_transport_config=connection: Configures the communication transport. Since 'connection' is None,
#   it likely defaults to a simulated transport.
# - debug_log_level=0: Sets the verbosity of debug logging. 0 usually means minimal or no debug output.

async def actiongraph_test():         
    # Defines an asynchronous function to orchestrate the motion control test.
    test_movements = {
        "sequential": [
            # The 'sequential' key indicates that the movements in this list will be executed one after another.
            {
                "parallel": [
                    # The 'parallel' key indicates that the movements within this list will be executed concurrently.
                    {
                        "sequential": [
                            # Another level of sequential movements, likely for a specific axis or group of axes.
                            {
                                "position": 0.1,
                                "axis": 1,
                                "T": 2
                                # Move axis 1 to position 0.1 over a duration of 2 time units.
                            },
                            {
                                "position": 0.1,
                                "axis": 2,
                                # Move axis 2 to position 0.1 (duration might be default or controlled elsewhere).
                            },
                        ]
                    },
                    {
                        "position": 0.1,
                        "axis": 3,
                        # Move axis 3 to position 0.1.
                    }

                ]
            },
            {
                "parallel": [
                    # Another set of parallel movements to be executed after the first sequential block.
                    {
                        "sequential": [
                            {
                                "position": 0,
                                "axis": 3,
                                # Move axis 3 back to position 0.
                            },
                            {
                                "position": 0,
                                "axis": 2,
                                "T": 2
                                # Move axis 2 back to position 0 over a duration of 2 time units.
                            },
                        ]
                    },
                    {
                        "position": 0,
                        "axis": 1,
                        # Move axis 1 back to position 0.
                    }

                ],
            }
        ],
        "limits": {
            # Defines limits for the motion system.
            "max_acceleration": 0.5
            # Sets the maximum allowed acceleration to 0.5 units/s^2 (assuming time is in seconds).
        }
    }

    api.start()
    # Initializes the motion controller, preparing it for operation.
    try:             
        await api.wait_for_ready()
        # Waits asynchronously until the motion controller reports that it is ready to accept commands.
#         await asyncio.sleep(1000)
        # This line is commented out, but it would have introduced a 1000-second delay.
        for i in range(1, 4):
            await api.set_pos_feedback_frequency(i, 50)
            # Sets the frequency at which position feedback is requested for each axis (1 to 3) to 50 Hz.
            
        #await asyncio.gather(*[api.do_homing(i, speed=0.001) for i in range(1, 3)])
        # This commented-out line would have initiated a homing procedure for axes 1 and 2 concurrently at a low speed.

        await asyncio.gather(*[api.set_coordinate_transform(i, -1, 0, triggered_homing_speed=0.01) for i in range(1, 4)])
        # Concurrently sets a coordinate transformation for each axis (1 to 4). The parameters -1 and 0 likely define
        # the transformation, and 'triggered_homing_speed' might be relevant if this transformation is related to homing.
        # Note: The loop iterates from 1 to 4, suggesting there might be 4 axes, although the 'test_movements' dictionary
        # primarily refers to axes 1, 2, and 3.
        #await asyncio.gather(*[api.do_homing(i, speed=0.01) for i in range(1, 4)])
        # Another commented-out line for initiating homing for all axes (1 to 4) concurrently.

        for  _ in range(4):
            # This loop executes the defined 'test_movements' sequence 4 times.
            await api.move(test_movements)
            # Sends the complex trajectory defined in 'test_movements' to the motion controller for execution.
            for i in range(1, 4):
                print(f"Axes {i} position: {api.get_axis_pos(i)}")
                # Queries and prints the current position of each axis (1 to 3) after each movement.
            await asyncio.sleep(2)
            # Introduces a 2-second delay after each execution of the full 'test_movements' sequence.
    
    finally:
        await api.stop()
        # Ensures that the motion controller is stopped and any resources are released, even if errors occur.



t = asyncio.create_task(actiongraph_test())
# Creates an asynchronous task to run the 'actiongraph_test' function without blocking the main execution.
await t
# Awaits the completion of the asynchronous task 't', effectively running the motion control sequence.