#!/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
from std_msgs.msg import Float64
from std_msgs.msg import Header
from mavros_msgs.srv import ParamGet
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range
from geometry_msgs.msg import Vector3

# test proto
import random

class fakeGps():
    def __init__(self):
        self.latitude = 0.0
        self.longitude = 20
        self.altitude = 30

class fake_hdg():
    def __init__(self):
        self.data = 40
# test json
class state():
    def __init__(self):
        self.mode = "test"




def init_dataFormat(cfg):
    if cfg.msg_format == "Proto":
        utils.Proto_msg_from_ros.client = client
        utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
        utils.Proto_msg_from_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
        utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
        utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
        utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
    elif cfg.msg_format == "Json":
        utils.Json_msg_from_ros.client = client
        utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
        utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
        utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
    else:
        print("msg_format not found")

def on_message(client, userdata, msg):
    pass

def on_connect(self, userdata, flags, rc):
    print("Connected with result code " + str(rc))

def on_publish(self, userdata, mid):
    pass


if __name__ == '__main__':
    # Read Config
    cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
    client = utils.MQTTClient(cfg.MQTTClientNamePub)

    # Setting Config
    init_dataFormat(cfg)

    # Mqtt
    client.on_connect = on_connect
    client.on_message = on_message
    client.on_publish = on_publish
    client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
    client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
    client.loop_start()

    #test proto
    gps = fakeGps()
    hdg = fake_hdg()
    state = state()
    while True:
        #test proto
        gps.latitude = random.uniform(0, 10)
        gps.longitude = random.uniform(0, 10)
        gps.altitude = random.uniform(0, 10)
        hdg.data = random.uniform(0, 10)

        utils.Proto_msg_from_ros.callBack_gps(gps)
        utils.Proto_msg_from_ros.callBack_compass_hdg(hdg)
        time.sleep(0.25)
        #test json
        # utils.Json_msg_from_ros.callBack_gps(gps)
        # utils.Json_msg_from_ros.callBack_compass_hdg(hdg)
        # utils.Json_msg_from_ros.callBack_state(state)
        # time.sleep(1)

    # Ros
    # rosClient = cfg.ROSClientNamePub
    # rospy.init_node(rosClient)
    # subscriber = rospy.Subscriber('/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
    # subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)


    # rospy.spin()