#!/usr/bin/env python3

from planet import Planet, Direction
from enum import Enum
from time import sleep

"""
Class to hold the MQTT client

Documentation:
On start do the following (make part of __init__):
(1) subscribe to explorer/<N>
(2) publish to explorer/<N>: SYN ready

Next receive the planet name and starting coordinates
(1) receive from explorer/<N>: ACK <Name> <X>,<Y>
(2) subscribe to planet/<Name>

on_message:
Called when a message has been received on a topic that the client subscribes to and the message does not match an existing topic filter callback. Use message_callback_add() to define a callback that will be called for specific topic filters. on_message will serve as fallback when none matched.
client
    the client instance for this callback
userdata
    the private user data as set in Client() or user_data_set()
message
    an instance of MQTTMessage. This is a class with members topic, payload, qos, retain.


mqtt message Documentation:
 |  Members:
 |
 |  topic : String. topic that the message was published on.
 |  payload : String/bytes the message payload.
 |  qos : Integer. The message Quality of Service 0, 1 or 2.
 |  retain : Boolean. If true, the message is a retained message and not fresh.
 |  mid : Integer. The message id.
"""


def str2dir(string):
    return {
        'N': Direction.NORTH,
        'E': Direction.EAST,
        'S': Direction.SOUTH,
        'W': Direction.WEST
    }.get(string)


def dir2str(direction):
    return {
        Direction.NORTH: 'N',
        Direction.EAST: 'E',
        Direction.SOUTH: 'S',
        Direction.WEST: 'W'
    }.get(direction)


def str2tuple(string_a, string_b):
    return (eval(string_a), eval(string_b))


def tupel2str(int_a, int_b):
    return str(int_a) + ',' + str(int_b)


class Command(str, Enum):
    SEND = "SYN "     # SYN is short for synchronize
    RECEIVE = "ACK "  # ACK is short for acknowledge
    READY = "ready"
    PATH = "path "
    TARGET = "target "
    COMPLETE = "exploration completed!"
    NOTICE = "notice"


class Mode(str, Enum):
    EXPLORE = "explore"
    GOTOSTATION = "gotostation"
    TARGET = "target"
    COMPLETE = "complete"


class Communication:
    """ Initializes communication module, connect to server, subscribe, etc. """
    def __init__(self, mqtt_client):
        # THESE TWO VARIABLES MUST NOT BE CHANGED
        self.msg_queue = []
        self.planetname = None
        self.planet = Planet()
        self.client = mqtt_client
        self.client.on_message = self.on_message
        self.uid = '122'
        passwd = '7KMuWPT2UE'
        self.client.username_pw_set(self.uid, password=passwd)
        try:
            self.client.connect('robolab.inf.tu-dresden.de', port=8883)
        except OSError:
            print("ERROR: check your network connection.")
        self.subscribetochannel("explorer/" + self.uid)
        self.encode_message(Command.READY, None)
        self.client.loop_start()
        self._status = Mode.EXPLORE
        self.navto = []
        self.isfirstrun = True
        self.target = None

    def status(self):
        return self._status

    def update(self, odometry):  # TODO: FINISH
        if(self.isfirstrun):
            self.isfirstrun = False
        elif(odometry is not None):  # use odometry to send data
            self.encode_message(Command.PATH, ((self.planet.getcurnode(), self.planet.getcurdir()), ((self.planet.getcurnode()[0] + odometry[0][0], self.planet.getcurnode()[1] + odometry[0][1]), (odometry[1] - 180) % 360), odometry[2]))
        elif(self._status is Mode.GOTOSTATION or self._status is Mode.TARGET):
            self.encode_message(Command.PATH, ((self.navto[0][0], self.navto[0][1]), (self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][0], self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][1]), self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][2]))
            # self.encode_message(Command.PATH, ((self.navto[0][0], self.navto[0][1]),        (self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][0], self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][1]), self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][2]))
        self.process_messages()  # this should update curnode and curdir

    def process_edges(self, edges):
            edges = self.planet.checkedges(edges)
            if(edges == [] and self._status is Mode.EXPLORE):
                if(self.planet.unexedge == []):
                    self.encode_message(Command.COMPLETE, None)
                    self._status = Mode.COMPLETE
                    self.process_messages()
                else:
                    self.navto = next(filter(lambda x: x is not None, map(lambda edge: self.planet.shortest_path(self.planet.getcurnode(), edge), self.planet.unexedge)))
                    self._status = Mode.GOTOSTATION
            elif(edges != []):
                self.navto = [(self.planet.getcurnode(), edges[0])]

    def getdir(self):
        return ((self.planet.getcurnode(), self.planet.getcurdir()))

    # THIS FUNCTIONS SIGNATURE MUST NOT BE CHANGED
    """ Handles the callback if any message arrived """
    def on_message(self, client, data, message):
        self.msg_queue.append(message)

    def subscribetochannel(self, channelname):
        self.client.subscribe(channelname, qos=1)

    def encode_message(self, msgtype, msgdata):
        if(msgtype == Command.READY):
            self.msg_queue.append(("explorer/" + self.uid, Command.SEND + Command.READY))
        if(msgtype == Command.TARGET):
            self.msg_queue.append(("explorer/" + self.uid, Command.SEND + Command.TARGET + "reached!"))
        if(msgtype == Command.COMPLETE):
            self.msg_queue.append(("explorer/" + self.uid, Command.SEND + Command.COMPLETE))
        if(msgtype == Command.PATH):
            self.msg_queue.append(("planet/" + self.planetname, Command.SEND + Command.PATH + self.pathtostring(msgdata[0], msgdata[1], msgdata[2])))

    def process_messages(self):
        while(self.msg_queue != []):
            for messages in self.msg_queue:
                if(type(messages) == tuple):
                    print("Outgoing Message is:", messages)
                    self.client.publish(messages[0], payload=messages[1], qos=1)
                elif(messages.payload.decode('utf-8').startswith(Command.RECEIVE)):
                    print("Incoming Message is:", messages.payload.decode('utf-8'))
                    self.comexec(messages.payload.decode('utf-8')[4:])
            self.msg_queue.clear()
            sleep(3)

    def comexec(self, message):
        if not any([message.startswith(instruction) for instruction in Command]):  # is planet name and starting position
            [self.planetname, startnode] = message.rsplit(' ')
            self.subscribetochannel("planet/" + self.planetname)
            self.planet.setcurnode(str2tuple(*startnode.rsplit(',')))
            # Fake Message send and add entry to set start as blocked
            self.encode_message(Command.PATH, [(self.planet.getcurnode(), Direction.SOUTH), (self.planet.getcurnode(), Direction.SOUTH), -1])
            self.planet.add_path((self.planet.getcurnode(), Direction.SOUTH), (self.planet.getcurnode(), Direction.SOUTH), -1)
        elif message.startswith(Command.PATH):
            self.stringtopath(*message[5:].rsplit(' '))
        elif message.startswith(Command.TARGET):
            self.target = str2tuple(*message[7:].rsplit(','))
            self.navto = self.planet.shortest_path(self.planet.getcurnode(), str2tuple(*message[7:].rsplit(',')))
            if(self.navto is not None):
                self._status = Mode.TARGET
        elif message.startswith(Command.NOTICE):
            print(message)

    def stringtopath(self, start, target, block, weight):
        snode = (str2tuple(*start[:-2].rsplit(',')), str2dir(start[-1:]))
        tnode = (str2tuple(*target[:-2].rsplit(',')), str2dir(target[-1:]))
        self.planet.add_path(snode, tnode, int(weight))
        if(snode[0] == self.planet.getcurnode() or self.planet.getcurnode is None):
            self.planet.setcurnode(tnode[0])
            self.planet.setcurdir((tnode[1] - 180) % 360)

    def pathtostring(self, start, target, blocked):
        if(blocked == -1):
            path = "blocked"
        else:
            path = "free"
        return tupel2str(*start[0]) + ',' + dir2str(start[1]) + ' ' + tupel2str(*target[0]) + ',' + dir2str(target[1]) + ' ' + path