#!/usr/bin/env python
"""Test that the controller server works as expected.

Written by Tiger Sachse.
"""
import time
import json
import rospy
import random
import rostest
import httplib
import unittest
import threading
import std_msgs.msg
import geometry_msgs.msg


# Some top-level constants for this test suite.
ITERATION_RANGE = 10
COORDINATE_MAX = 1337
COORDINATE_MIN = -1337
AUTONOMOUS_TOGGLES_INTEGERS = (1, 2, 4, 8)
PACKAGE_NAME = "ezrassor_controller_server"
DEFAULT_TEST_NAME = "test_controller_server"
AUTONOMOUS_TOGGLES_TOPIC = "autonomous_toggles"
TARGET_COORDINATES_TOPIC = "target_coordinates"
POST_HEADER = {"content-type" : "application/json"}
WHEEL_INSTRUCTION_STRINGS = ("forward", "backward", "left", "right")


class Instruction():
    """Hold an instruction from the controller server."""
    def __init__(self, name):
        """Initialize this instruction with no content."""
        self.name = name
        self.__content = None
        self.__content_lock = threading.Lock()

    def update(self, content):
        """Update this instruction with new content."""
        self.__content_lock.acquire()
        self.__content = content
        self.__content_lock.release()

    def get(self):
        """Get the content from this instruction."""
        self.__content_lock.acquire()
        content = self.__content
        self.__content = None
        self.__content_lock.release()

        return content

    def is_none(self):
        """Check to see if this instruction's content is None."""
        self.__content_lock.acquire()
        is_none = self.__content == None
        self.__content_lock.release()

        return is_none


class TestControllerServer(unittest.TestCase):
    """A suite of tests that confirm the functionality of the controller server."""
    def __init__(self, *arguments, **keyword_arguments):
        """Initialize this test suite to subscribe to the server's topics."""
        super(TestControllerServer, self).__init__(*arguments, **keyword_arguments)
        rospy.init_node(DEFAULT_TEST_NAME)
        
        # Read in all necessary ROS parameters.
        self.__server_url = rospy.get_param(rospy.get_name() + "/server_url")
        self.__port = int(rospy.get_param(rospy.get_name() + "/port"))
        wheel_instructions_topic = rospy.get_param(
            rospy.get_name() + "/wheel_instructions_topic",
        )
        front_arm_instructions_topic = rospy.get_param(
            rospy.get_name() + "/front_arm_instructions_topic",
        )
        back_arm_instructions_topic = rospy.get_param(
            rospy.get_name() + "/back_arm_instructions_topic",
        )
        front_drum_instructions_topic = rospy.get_param(
            rospy.get_name() + "/front_drum_instructions_topic",
        )
        back_drum_instructions_topic = rospy.get_param(
            rospy.get_name() + "/back_drum_instructions_topic",
        )
        startup_delay = float(
            rospy.get_param(rospy.get_name() + "/startup_delay"),
        )
        self.__communication_delay = float(
            rospy.get_param(rospy.get_name() + "/communication_delay"),
        )

        self.__wheel_instruction = Instruction("wheel_instruction")
        self.__front_arm_instruction = Instruction("front_arm_instruction")
        self.__back_arm_instruction = Instruction("back_arm_instruction")
        self.__front_drum_instruction = Instruction("front_drum_instruction")
        self.__back_drum_instruction = Instruction("back_drum_instruction")
        self.__autonomous_toggles = Instruction("autonomous_toggles")
        self.__target_coordinate = Instruction("target_coordinate")

        # Subscribe to all of the controller server's output topics. Each
        # subscriber dumps data into a designated Instruction.
        rospy.Subscriber(
            wheel_instructions_topic,
            geometry_msgs.msg.Twist,
            callback=self.__wheel_instruction.update,
        )
        rospy.Subscriber(
            front_arm_instructions_topic,
            std_msgs.msg.Float32,
            callback=self.__front_arm_instruction.update,
        )
        rospy.Subscriber(
            back_arm_instructions_topic,
            std_msgs.msg.Float32,
            callback=self.__back_arm_instruction.update,
        )
        rospy.Subscriber(
            front_drum_instructions_topic,
            std_msgs.msg.Float32,
            callback=self.__front_drum_instruction.update,
        )
        rospy.Subscriber(
            back_drum_instructions_topic,
            std_msgs.msg.Float32,
            callback=self.__back_drum_instruction.update,
        )
        rospy.Subscriber(
            AUTONOMOUS_TOGGLES_TOPIC,
            std_msgs.msg.Int8,
            callback=self.__autonomous_toggles.update,
        )
        rospy.Subscriber(
            TARGET_COORDINATES_TOPIC,
            geometry_msgs.msg.Point,
            callback=self.__target_coordinate.update,
        )

        # Sleep for a bit to give ROS and all other nodes time to start up.
        time.sleep(startup_delay)

    def __post_and_sleep(self, payload):
        """Send a POST request to the server and sleep for a bit."""
        connection = httplib.HTTPConnection(self.__server_url, self.__port)
        connection.request("POST", "", json.dumps(payload), POST_HEADER)
        time.sleep(self.__communication_delay)

    def __get_random_speed(self):
        """Get a random speed from -1.0 to 1.0."""
        speed = random.random()
        if random.random() > .5:
            speed = -speed

        return speed

    def setUp(self):
        """Empty all instructions before each test."""
        self.__wheel_instruction.update(None)
        self.__front_arm_instruction.update(None)
        self.__back_arm_instruction.update(None)
        self.__front_drum_instruction.update(None)
        self.__back_drum_instruction.update(None)
        self.__autonomous_toggles.update(None)
        self.__target_coordinate.update(None)

    def test_simple_instructions(self):
        """Send a few simple instructions through the controller server."""
        FLOAT_INSTRUCTIONS = (
            self.__front_arm_instruction,
            self.__back_arm_instruction,
            self.__front_drum_instruction,
            self.__back_drum_instruction,
        )

        for instruction in FLOAT_INSTRUCTIONS:
            for iteration in range(ITERATION_RANGE):
                speed = self.__get_random_speed()
                self.__post_and_sleep({instruction.name : speed})
                self.assertFalse(instruction.is_none())
                self.assertAlmostEqual(instruction.get().data, speed, places=4)

    def test_complex_instructions(self):
        """Send some complex instructions through the controller server."""
        def build_complex_instruction():
            """Construct a complex instruction with random values."""
            instruction = {}
            instruction["wheel_instruction"] = random.choice(
                WHEEL_INSTRUCTION_STRINGS,
            )
            instruction["front_arm_instruction"] = self.__get_random_speed()
            instruction["back_arm_instruction"] = self.__get_random_speed()
            instruction["front_drum_instruction"] = self.__get_random_speed()
            instruction["back_drum_instruction"] = self.__get_random_speed()
            instruction["autonomous_toggles"] = random.choice(
                AUTONOMOUS_TOGGLES_INTEGERS,
            )
            instruction["target_coordinate"] = {
                "x" : random.randrange(COORDINATE_MIN, COORDINATE_MAX),
                "y" : random.randrange(COORDINATE_MIN, COORDINATE_MAX),
            }

            # Randomly remove one key so that the complex instruction is not
            # complete, (complex instructions do not need all fields filled).
            instruction.pop(random.choice(instruction.keys()))

            return instruction

        for iteration in range(ITERATION_RANGE):
            complex_instruction = build_complex_instruction()
            self.__post_and_sleep(complex_instruction)

            # For all of the instructions that are not None, assess their
            # validity and pop them from the complex instruction dictionary.
            if not self.__wheel_instruction.is_none():
                self.assertTrue("wheel_instruction" in complex_instruction)
                wheel_instruction = complex_instruction.pop("wheel_instruction")
                if wheel_instruction == "forward":
                    self.assertAlmostEqual(
                        self.__wheel_instruction.get().linear.x,
                        1.0,
                        places=4,
                    )
                elif wheel_instruction == "backward":
                    self.assertAlmostEqual(
                        self.__wheel_instruction.get().linear.x,
                        -1.0,
                        places=4,
                    )
                elif wheel_instruction == "left":
                    self.assertAlmostEqual(
                        self.__wheel_instruction.get().angular.z,
                        1.0,
                        places=4,
                    )
                elif wheel_instruction == "right":
                    self.assertAlmostEqual(
                        self.__wheel_instruction.get().angular.z,
                        -1.0,
                        places=4,
                    )
            if not self.__front_arm_instruction.is_none():
                self.assertTrue("front_arm_instruction" in complex_instruction)
                self.assertAlmostEqual(
                    self.__front_arm_instruction.get().data,
                    complex_instruction.pop("front_arm_instruction"),
                    places=4,
                )
            if not self.__back_arm_instruction.is_none():
                self.assertTrue("back_arm_instruction" in complex_instruction)
                self.assertAlmostEqual(
                    self.__back_arm_instruction.get().data,
                    complex_instruction.pop("back_arm_instruction"),
                    places=4,
                )
            if not self.__front_drum_instruction.is_none():
                self.assertTrue("front_drum_instruction" in complex_instruction)
                self.assertAlmostEqual(
                    self.__front_drum_instruction.get().data,
                    complex_instruction.pop("front_drum_instruction"),
                    places=4,
                )
            if not self.__back_drum_instruction.is_none():
                self.assertTrue("back_drum_instruction" in complex_instruction)
                self.assertAlmostEqual(
                    self.__back_drum_instruction.get().data,
                    complex_instruction.pop("back_drum_instruction"),
                    places=4,
                )
            if not self.__autonomous_toggles.is_none():
                self.assertTrue("autonomous_toggles" in complex_instruction)
                self.assertEqual(
                    self.__autonomous_toggles.get().data,
                    complex_instruction.pop("autonomous_toggles"),
                )
            if not self.__target_coordinate.is_none():
                self.assertTrue("target_coordinate" in complex_instruction)
                given_point = complex_instruction.pop("target_coordinate")
                received_point = self.__target_coordinate.get()
                self.assertEqual(given_point["x"], received_point.x)
                self.assertEqual(given_point["y"], received_point.y)

            # Since each discovered instruction component should have been
            # popped from the complex instruction during testing by this point,
            # ensure that the complex instruction is now empty.
            self.assertEqual(len(complex_instruction), 0)


# Start this node with some default values.
rostest.rosrun(
    PACKAGE_NAME,
    DEFAULT_TEST_NAME,
    TestControllerServer,
)
