from control.eunomia import Eunomia
from control.dice import Dice
from control.warrior import Warrior
import numpy


class Zeus:
    """Class of robot Control

    Attributes:
        warriors: Warrior() list with the information of the robots.
        nWarriors: Robots in play.
        robotsSpeed: interface speed list.
        actions: Instance of class Eunomia() that calculates what is needed to find de robots velocities.
        translate: Instance of class Dice() that calculates the robots velocities.
    """

    def __init__(self):
        self.warriors = []
        self.nWarriors = 0
        self.robotsSpeed = []

        self.actions = Eunomia()
        self.translate = Dice()
        print("Zeus summoned")

    def updateSpeeds(self, robots):
        print("[Zeus] New speeds:")

        for robot in robots:
            self.robotsSpeed.append(robot)

    def setup(self, nWarriors):
        """Zeus first movements

        This method must be called before using Zeus properly.
        Here is instantiated the Eunomia() and Dice() as well as the amount nWarriors of Warriors to be used.

        Args:
            nWarriors: Num of warriors in game

        Returns:

        """
        self.actions.setup()
        self.nWarriors = nWarriors

        for i in range(0, nWarriors):
            self.warriors.append(Warrior())
            self.robotsSpeed.append(0.0)

        print("Zeus is set up")
        return self

    def reset(self):
        self.translate.reset()

    def getVelocities(self, strategia):
        """Zeus main method

        Recebe dados retornados pela estratégia e gera uma lista de Warrior() em getWarriors.
        Essa lista é encaminhada para o controlRoutine onde serão feitos as chamadas de action.run
        e translate.run para cada warrior in game e, ao final dessa rotina, é obtido uma lista com
        as velocidades de cada roda dos warrios que serão enviadas para generateOutput afim de se obter
        uma lista de dicionários com esses valores.

        Args:
            strategia (list): Lista de dicionários com as informações geradas pelo Strategy

        Returns:
            list: Informações a serem passadas para Comunicação(Hermes)

        """

        self.warriors = self.getWarriors(strategia)
        velocities = self.generateOutput(self.controlRoutine(), strategia)
        return velocities

    def getWarriors(self, strategia):
        """Transforms a list of dictionaries into a Warrior() list.

        Seta os atributos do object Warrior() baseado nas informações passadas pela estratégia. Ações que podem ser escolhidas:
        - {
            "command": "goTo",
            "data": {
                "obstacles": [(x, y)] # opcional - se passado, desviar de tais obstaclos
                "pose": { "position": (x, y), "orientation": θ radianos },
                "target": {
                    "position": (x, y),
                    "orientation": θ radianos | (x, y)  # opcional - pode ser uma orientação final ou uma posição de lookAt
                    },
                "velocity": X m/s,  # opcional - se passado, sem before, é a velocidade constante / com before é velocidade padrão
                "before": X s  # se passado sem o velocity, usa a velocidade máxima do robô como teto
            }
        }
        - {
            "command": "spin",
            "data": { "velocity": X m/s, "direction": "clockwise" | "counter" }
        }
        - {
            "command": "lookAt",
            "data": {
                "pose": {
                    "position": (x, y),  # opcional - é passado se o target for um ponto
                    "orientation": θ radianos
                },
                "target": θ radianos | (x, y)
            }
        }
        - {
            "command": stop,
            "data": {before: 0}
        }

        Args:
            strategia (list): List of dictionaries with information generated by Strategy0

        Returns:
            list: Object Warrior() list

        Raises:
            ValueError:

        """

        warriors = []
        if type(strategia) is not list or \
                len(strategia) != self.nWarriors:
            raise ValueError("Invalid data object received.")

        for i in range(0, self.nWarriors):
            if type(strategia[i]) is not dict:
                raise ValueError("Invalid data object received.")

            if ("command" in strategia[i]) is False or \
                    ("data" in strategia[i]) is False:
                raise ValueError("Invalid data object received.")

            warriors.append(Warrior())

        for x in range(0, len(strategia)):
            if strategia[x]["command"] is not "goTo" and \
                    strategia[x]["command"] is not "spin" and \
                    strategia[x]["command"] is not "lookAt" and \
                    strategia[x]["command"] is not "stop":
                raise ValueError("Invalid command.")

            warriors[x].action.append(strategia[x]["command"])
            info = strategia[x]["data"]

            if strategia[x]["command"] == "goTo":
                warriors[x].position = numpy.asarray(info["pose"]["position"], dtype=float)
                warriors[x].orientation = float(info["pose"]["orientation"])

                warriors[x].target = numpy.asarray(info["target"]["position"], dtype=float)
                if type(info["target"]["orientation"]) is tuple:
                    warriors[x].targetOrientation = numpy.asarray(info["target"]["orientation"], dtype=float)
                else:
                    warriors[x].targetOrientation = float(info["target"]["orientation"])

                if "velocity" in info:
                    warriors[x].vMax = numpy.asarray(info["velocity"], dtype=float)
                else:
                    warriors[x].vMax = self.robotsSpeed[x]

                if "before" in info:
                    warriors[x].before = float(info["before"])

                if "obstacles" in info:
                    warriors[x].obstacles = numpy.asarray(info["obstacles"], dtype=float)
                    warriors[x].obstaclesSpeed = numpy.asarray(info["obstaclesSpeed"], dtype=float)

            elif strategia[x]["command"] == "spin":
                warriors[x].action.append(info["direction"])

                if "velocity" in info:
                    warriors[x].vMax = numpy.asarray(info["velocity"], dtype=float)
                else:
                    warriors[x].vMax = self.robotsSpeed[x]

            elif strategia[x]["command"] == "lookAt":
                warriors[x].orientation = float(info["pose"]["orientation"])

                if "velocity" in info:
                    warriors[x].vMax = numpy.asarray(info["velocity"], dtype=float)
                else:
                    warriors[x].vMax = self.robotsSpeed[x]

                if type(info["target"]) is not tuple:
                    warriors[x].targetOrientation = float(info["target"])
                    warriors[x].action.append("orientation")
                else:
                    warriors[x].position = numpy.asarray(info["pose"]["position"], dtype=float)
                    warriors[x].target = numpy.asarray(info["target"], dtype=float)
                    warriors[x].action.append("target")

            elif strategia[x]["command"] == "stop":
                warriors[x].vMax = 0.0
                warriors[x].before = float(info["before"])

            warriors[x].backward = self.warriors[x].backward
            warriors[x].front = self.warriors[x].front
            warriors[x].velAcc = self.warriors[x].velAcc

        return warriors

    def controlRoutine(self):
        """Action and Translate call

        Flow of calculations to generate the PWM's that will be passed to the communication.
        Actions performs the calculations based on the commands set by the Strategy.
        Translate takes the data generated by Actions and calculates the final speed of each wheel.

        Returns:
            list: Velocities list of each wheel of the robots returned by Translate.

        """

        velocities = []
        for warrior in self.warriors:
            if len(warrior.action) > 0:
                velocities.append(self.translate.run(self.actions.run(warrior)))

        return velocities

    def generateOutput(self, velocities, strategia):
        """Generation of output data

        This method generates list of dictionaries with the speeds of each robot.

        Args:
            velocities (list): List with the speeds of each wheels of each robot.

        Returns:
            list: List of dictionary ready to be sent to the Communication(Hermes).

        """
        output = []
        for x in range(0, self.nWarriors):
            output.append(
                {
                    "robotLetter": strategia[x]["robotLetter"],
                    "vLeft": velocities[x][0],
                    "vRight": velocities[x][1],
                    "vector": velocities[x][2]
                }
            )

        return output
