import paho.mqtt.client as mqtt
import json
import base64
import socket
import datetime
import signal
import argparse

RTCM3_PREAMBLE = b'\xd3'

# Specify the host name and port number of the broker here. 
MQTTBROKER_HOSTNAME = "localhost"
MQTTBROKER_PORT = 8883

# The port number on which in the rtkrcv is publishing the RTCM3 messages. 
RTKRCV_SRV_HOST = "localhost"
RTKRCV_SRV_PORT = 11235

# The topic for the messages. 
MQTT_RTCM3_TOPIC = "rtcm3"

# The client ID used for publish
MQTT_RTCM3_RELAY_ID = "rtcm3relay"

class MQTTRelay:
    mqtt_client = None
    sock = None

    """
    create an MQTT client that connects to the broker 
    """
    def __init__(self, mqtthost, mqttport):
        self.mqtt_client = mqtt.Client(client_id=MQTT_RTCM3_RELAY_ID, clean_session=True, userdata=None, transport='tcp')
        self.mqtt_client.on_connect = self._mqtt_connect 
        self.mqtt_client.on_disconnect = self._mqtt_disconnect
        self.mqtt_client.connect(mqtthost, mqttport)
        print("Connected to MQTT Broker")
        self.sock = None

        # Specify the signal handlers. 
        signal.signal(signal.SIGINT, self.handle_exit)
        signal.signal(signal.SIGTERM, self.handle_exit)
        return

    """
    Debug messages to show when connected
    """    
    def _mqtt_connect(self, client, obj, flags, rc):
        return

    """
    Do a clean disconnect
    """
    def _mqtt_disconnect(self, client, userdata, rc=0):
        print ("Disconnecting from the MQTT broker")
        client.loop_stop()
        return

    # """
    # Publish a message with the given topic .. Made it inline. 
    # """
    # def publish_rtcm3(self, rtcm3_frame):
    #     self.mqtt_client.publish(topic=MQTT_RTCM3_TOPIC, 
    #             payload=json.dumps({'rtcm3':rtcm3_frame, 'timestamp': datetime.datetime.now().timestamp()}))
    #     return


    def handle_exit(self, signum, frame):
        # Disconnect the client
        self.sock.close()
        self.mqtt_client.disconnect()
        return

# """
# Read the log file and publish each RTCM3 frame in an individual message. 
# """        
# def rtcm3_relay_file(fname, publisher):
#     print("Reading File Now")
#     cnt = 0
#     with open(fname, mode="rb") as bfile:
#         # skip the first few bytes which are newline characters.
#         data = bfile.read(1)
#         while data != RTCM3_PREAMBLE:
#             data = bfile.read(1)
#         rtcm3_frame =  bytes()
#         rtcm3_frame += base64.encodebytes(data)
#         while data != b"":
#             cnt = cnt + 1
#             data = bfile.read(1)
#             print(data)
#             if data == RTCM3_PREAMBLE:
#                 print("Found preamble")
#                 utf8_rtcm3_frame = rtcm3_frame.decode('utf-8')
#                 publisher.publish_rtcm3(utf8_rtcm3_frame)
#                 rtcm3_frame = bytes()
#             rtcm3_frame += base64.encodebytes(data)
#             print(cnt)
#     return True

   
    """
    Connect to the tcp server created by rtknavi/rtklib.
    Read the rtcm3 frames, and publish each frame as a message.  
    """
    def rtcm3_relay_socket(self, srv_host, srv_port):
        buff_size = 4096
        rtcm3_frame = bytes()
        first_preamble = True
        try:
            self.sock = socket.create_connection((srv_host, srv_port))
            print ("Connected to RTKRCV tcp port. Relaying the RTCM3 messages now")
            while True:
                data = self.sock.recv(4096)
                data = [bytes(data[i:i+1]) for i in range(len(data))]
                for i in range(len(data)):
                    data_byte = data[i]
                    # This needs to be optimized. Currently, we create a message each time the preamble is seen. 
                    # The correct way would be to fetch the length, and encode the payload up to the length. 
                    if data_byte == RTCM3_PREAMBLE:
                        if first_preamble is False:
                            self.mqtt_client.publish(topic=MQTT_RTCM3_TOPIC,
                                                      json.dumps({'rtcm3':base64.b64encode(rtcm3_frame).decode('utf-8'),
                                                                  'timestamp': datetime.datetime.now().timestamp()}))
                            #utf8_rtcm3_frame = base64.b64encode(rtcm3_frame).decode('utf-8')
                            #self.publish_rtcm3(utf8_rtcm3_frame)
                        first_preamble = False    
                        rtcm3_frame = bytes()
                    rtcm3_frame += data_byte
        except IOError:
            pass
        except Exception as e:
            print(e)
        return True


def main(mqtthost, mqttport, rtkhost, rtkport):
    mqtt_relay = MQTTRelay(mqtthost, mqttport)
    mqtt_relay.rtcm3_relay_socket(rtkhost, rtkport)
    print ("Quitting")

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument('-r','--rtktcphost',
                        help="hostname where the rtrcv is exporting rtcm3 over tcp",
                        type=str)
    parser.add_argument('-p', '--rtktcpport',
                        help="port number where the rtrcv is exporting rtcm3 over tcp",
                        type=int)
    
    parser.add_argument('-b','--mqtthost',
                        help="hostname of the MQTT broker",
                        type=str)
    parser.add_argument('-m', '--mqttport',
                        help="port number of the MQTT broker",
                        type=int)
    args = parser.parse_args()
    if None in [ args.rtktcphost, args.rtktcpport, args.mqtthost, args.mqttport ]:
        parser.print_help()
    else:
        main(args.mqtthost, args.mqttport, args.rtktcphost, args.rtktcpport)