#!/usr/bin/env python3
#coding:utf-8
import paho.mqtt.client as mqtt
import os
import sys
import time
import utils
import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2
import rospy
from std_msgs.msg import String

def init_dataFormat(cfg):
    if cfg.msg_format == "Proto":
        utils.Proto_msg_to_ros.rate = rospy.Rate(10)
        utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
        utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
        
        utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
        utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
        
        client.on_message = utils.Proto_msg_to_ros.on_message

        utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
        utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
    elif cfg.msg_format == "Json":
        utils.Json_msg_to_ros.rate = rospy.Rate(10)
        utils.Json_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
        utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
        
        client.on_message = utils.Json_msg_to_ros.on_message
        utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
        utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
    else:
        print("msg_format not found")

# def on_message(client, userdata, msg):
#     utils.Json_msg_to_ros.ros_pub(msg)

def on_connect(self, userdata, flags, rc):
    print("Connected with result code " + str(rc))
    client.subscribe(cfg.Flight_Information_topicToMqtt)


def on_publish(self, userdata, mid):
    pass


if __name__ == '__main__':
    # Read Config
    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml")
    # Read Config
    cfg = utils.MQTT_ROS_Config(FilePath)
    print(cfg)
   
    # Mqtt
    client = utils.MQTTClient(cfg.MQTTClientNameSub)
    client.on_connect = on_connect
    client.on_message = utils.Json_msg_to_ros.on_message
    client.on_publish = on_publish
    client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
    

    # Ros
    rospy.init_node(cfg.ROSClientNameSub)
    # initialize
    init_dataFormat(cfg)

    client.loop_start()
    rospy.spin()