#!/usr/bin/env python3
#coding:utf-8
import serial
import time
import sys
import os
import proto.flight_information_pb2 as flight_information_pb2
import logging
from utils.readConfig import Read_PUB_Config
from utils.proto_uavlink_sub_data_from_ros import Proto_msg_from_ros

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


def init_dataFormat(cfg:Read_PUB_Config):
    f1data = b'\xf1drone550pub...........\r\n'
    sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout=0.3)
    sel.write(f1data)
    readTenByte = sel.readline()
    print(readTenByte)
    Proto_msg_from_ros.sel = sel
    Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()

    rospy.Subscriber('/mavros/global_position/global', NavSatFix, Proto_msg_from_ros.callBack_gps)
    rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, Proto_msg_from_ros.callBack_compass_hdg)

if __name__ == '__main__':
    FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_PUB.yml")
    cfg = Read_PUB_Config(FilePath)

    # set log
    log_format = "%(asctime)s - %(levelname)s - %(message)s"
    formatter = logging.Formatter(log_format)

    stream_handler = logging.StreamHandler()
    stream_handler.setFormatter(formatter)
    stream_handler.setLevel(logging.DEBUG)

    file_handler = logging.FileHandler(cfg.logFileName)
    file_handler.setFormatter(formatter)
    file_handler.setLevel(logging.INFO)

    logger = logging.getLogger("__UAVLINKSUBPUB__")
    logger.setLevel(logging.INFO)
    logger.addHandler(file_handler)
    logger.addHandler(stream_handler)
    logger.info(cfg)    



    # Ros
    rosClient = cfg.ROSClientNamePub
    rospy.init_node(rosClient)
    
    # init data format
    init_dataFormat(cfg)

    try:
        rospy.spin()
    except BaseException as error:
        logger.info("End of program")
        sys.exit()