import pymavlink.mavutil as utility
import pymavlink.dialects.v20.all as dialect
import argparse
import pika
import time
def main():
    # MQ

    connection_parameters = pika.ConnectionParameters('localhost')

    connection = pika.BlockingConnection(connection_parameters)

    channel = connection.channel()

    channel.queue_declare(queue="letterbox")

    parser = argparse.ArgumentParser()

    parser.add_argument('--comm',
                        dest='comm',
                        help='np. --comm udpin:127.0.0.1:14560 1 (0-> ARM, 1 -> DISARM) 1 (RELAY NUMBER)',
                        type=str,
                        nargs=3)

    #args = parser.parse_args()
    args, unknown = parser.parse_known_args()

    print("New client created")
    channel.basic_publish(exchange="", routing_key="letterbox", body="@Connected to query".encode())

    comm_str = args.comm[0]
    print(comm_str)
    RELAY_FLAG = args.comm[1]
    RELAY_NO = args.comm[1]
    # connect to vehicle
    client_message = ("@Connecting_to_drone")
    channel.basic_publish(exchange="", routing_key="letterbox", body=client_message.encode())
    print("@Connecting_to_drone")
    vehicle = utility.mavlink_connection(device=comm_str)
    # wait for a heartbeat
    vehicle.wait_heartbeat()

    # inform user
    msg ="@Connected to drone:", vehicle.target_system, ", component:", vehicle.target_component
    client_message = str(msg)
    channel.basic_publish(exchange="", routing_key="letterbox", body=client_message.encode())
    print(msg)

    if RELAY_FLAG == 0:

        # create set relay command
        command = dialect.MAVLink_command_long_message(target_system=vehicle.target_system,
                                                       target_component=vehicle.target_component,
                                                       command=dialect.MAV_CMD_DO_SET_RELAY,
                                                       confirmation=0,
                                                       param1=RELAY_NO,  # relay pin instance number, starts from 0, 0 = first relay
                                                       param2=0,  # 1 = ON, 0 = OFF
                                                       param3=0,
                                                       param4=0,
                                                       param5=0,
                                                       param6=0,
                                                       param7=0)
        # send command to the vehicle
        vehicle.mav.send(command)

        # inform user
        msg = f"@Relay {} set to {}:".format(RELAY_NO,"OFF")
        print(msg)
        client_message = str(msg)
        channel.basic_publish(exchange="", routing_key="letterbox", body=client_message.encode())

        time.sleep(1)

        # create set relay command
        command = dialect.MAVLink_command_long_message(target_system=vehicle.target_system,
                                                       target_component=vehicle.target_component,
                                                       command=dialect.MAV_CMD_DO_SET_RELAY,
                                                       confirmation=0,
                                                       param1=RELAY_NO,  # relay pin instance number, starts from 0, 0 = first relay
                                                       param2=1,  # 1 = ON, 0 = OFF
                                                       param3=0,
                                                       param4=0,
                                                       param5=0,
                                                       param6=0,
                                                       param7=0)
        # send command to the vehicle
        vehicle.mav.send(command)
        # inform user
        msg = f"@Relay {} set to {}:".format(RELAY_NO,"ON")
        print(msg)
        client_message = str(msg)
        channel.basic_publish(exchange="", routing_key="letterbox", body=client_message.encode())


    if RELAY_FLAG == 1:
        # create set relay command
        command = dialect.MAVLink_command_long_message(target_system=vehicle.target_system,
                                                       target_component=vehicle.target_component,
                                                       command=dialect.MAV_CMD_DO_SET_RELAY,
                                                       confirmation=0,
                                                       param1=RELAY_NO,
                                                       # relay pin instance number, starts from 0, 0 = first relay
                                                       param2=1,  # 1 = ON, 0 = OFF
                                                       param3=0,
                                                       param4=0,
                                                       param5=0,
                                                       param6=0,
                                                       param7=0)
        # send command to the vehicle
        vehicle.mav.send(command)
        # inform user
        msg = f"@Relay {} set to {}:".format(RELAY_NO, "ON")
        print(msg)
        client_message = str(msg)
        channel.basic_publish(exchange="", routing_key="letterbox", body=client_message.encode())

        time.sleep(1)

        # create set relay command
        command = dialect.MAVLink_command_long_message(target_system=vehicle.target_system,
                                                       target_component=vehicle.target_component,
                                                       command=dialect.MAV_CMD_DO_SET_RELAY,
                                                       confirmation=0,
                                                       param1=RELAY_NO,
                                                       # relay pin instance number, starts from 0, 0 = first relay
                                                       param2=0,  # 1 = ON, 0 = OFF
                                                       param3=0,
                                                       param4=0,
                                                       param5=0,
                                                       param6=0,
                                                       param7=0)
        # send command to the vehicle
        vehicle.mav.send(command)
        # inform user
        msg = f"@Relay {} set to {}:".format(RELAY_NO, "OFF")
        print(msg)
        client_message = str(msg)
        channel.basic_publish(exchange="", routing_key="letterbox", body=client_message.encode())


if __name__ == '__main__':
    main()