#!/usr/bin/env python

"""
regression tests for mavwp.py
"""

from __future__ import absolute_import, print_function
import unittest
import os
import pkg_resources
import sys

os.environ["MAVLINK20"] = "1"

from pymavlink import mavwp
from pymavlink import mavutil

class MAVWPTest(unittest.TestCase):

    """
    Class to test mavwp
    """

    def __init__(self, *args, **kwargs):
        """Constructor, set up some data that is reused in many tests"""
        super(MAVWPTest, self).__init__(*args, **kwargs)

    def makewp(self, seq):
        return mavutil.mavlink.MAVLink_mission_item_int_message(
            1, # wp.target_system,
            1, # wp.target_component,
            0, # wp.seq,
            0, # wp.frame,
            0, # wp.command,
            0, # wp.current,
            1, # wp.autocontinue,
            0, # wp.param1,
            0, # wp.param2,
            0, # wp.param3,
            0, # wp.param4,
            0, # int(wp.x*1.0e7),
            0, # int(wp.y*1.0e7),
            seq, # wp.z
        )

    def make_wps(self):
        # create waypoints
        count = 4
        waypoints = []
        for i in range(count):
            waypoints.append(self.makewp(i))

        return waypoints

    def test_add_remove(self):
        """Test we can add/remove waypoints to/from mavwp"""
        loader = mavwp.MAVWPLoader()
        self.assertEqual(loader.mav_mission_type(),
                         mavutil.mavlink.MAV_MISSION_TYPE_MISSION)

        waypoints = self.make_wps()

        # make sure basic addition works
        for i in range(len(waypoints)):
            self.assertEqual(loader.count(), i)
            loader.add(waypoints[i])
            self.assertEqual(loader.count(), i+1)

        self.assertEqual(loader.wp(0).seq, 0)
        self.assertEqual(loader.wp(1).seq, 1)
        self.assertEqual(loader.wp(2).seq, 2)
        self.assertEqual(loader.wp(0).z, 0)
        self.assertEqual(loader.wp(1).z, 1)
        self.assertEqual(loader.wp(2).z, 2)
        # short test for the new item() method:
        self.assertEqual(loader.item(0).z, 0)
        self.assertEqual(loader.item(1).z, 1)
        self.assertEqual(loader.item(2).z, 2)


        # remove a middle one, make sure things get renumbered
        loader.remove(waypoints[0])
        self.assertEqual(loader.wp(0).seq, 0)
        self.assertEqual(loader.wp(1).seq, 1)
        self.assertEqual(loader.wp(2).seq, 2)
        self.assertEqual(loader.wp(0).z, 1)
        self.assertEqual(loader.wp(1).z, 2)
        self.assertEqual(loader.wp(2).z, 3)

        loader.clear()
        self.assertEqual(loader.count(), 0)

    def test_insert(self):
        """Test we can insert waypoints into mavwp"""
        loader = mavwp.MAVWPLoader()
        waypoints = self.make_wps()

        loader.add(waypoints[0])
        loader.add(waypoints[2])
        self.assertEqual(loader.wp(0).seq, 0)
        self.assertEqual(loader.wp(1).seq, 1)
        self.assertEqual(loader.wp(0).z, 0)
        self.assertEqual(loader.wp(1).z, 2)
        loader.insert(1, waypoints[3])
        self.assertEqual(loader.count(), 3)
        self.assertEqual(loader.wp(0).seq, 0)
        self.assertEqual(loader.wp(1).seq, 1)
        self.assertEqual(loader.wp(2).seq, 2)
        self.assertEqual(loader.wp(0).z, 0)
        self.assertEqual(loader.wp(1).z, 3)
        self.assertEqual(loader.wp(2).z, 2)

    def test_set(self):
        """Test we set waypoints in mavwp"""
        loader = mavwp.MAVWPLoader()
        waypoints = self.make_wps()

        loader.add(waypoints[0])
        loader.add(waypoints[2])
        self.assertEqual(loader.wp(0).seq, 0)
        self.assertEqual(loader.wp(1).seq, 1)
        self.assertEqual(loader.wp(0).z, 0)
        self.assertEqual(loader.wp(1).z, 2)
        loader.set(waypoints[3], 1)
        self.assertEqual(loader.count(), 2)
        self.assertEqual(loader.wp(0).seq, 0)
        self.assertEqual(loader.wp(1).seq, 1)
        self.assertEqual(loader.wp(0).z, 0)
        self.assertEqual(loader.wp(1).z, 3)

        # setting at the end of the list extends the list:
        loader.set(waypoints[1], 2)

    def test_add_latlonalt(self):
        '''test add_latlonalt method'''
        loader = mavwp.MAVWPLoader()
        waypoints = self.make_wps()

        loader.add(waypoints[0])
        loader.add(waypoints[2])

        loader.add_latlonalt(5, 6, 7, terrain_alt=True)
        loader.add_latlonalt(10, 11, 12, terrain_alt=False)

        wp1 = loader.wp(2)
        self.assertEqual(wp1.x, 5)
        self.assertEqual(wp1.y, 6)
        self.assertEqual(wp1.z, 7)
        self.assertEqual(wp1.frame, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT)

        wp1 = loader.wp(3)
        self.assertEqual(wp1.x, 10)
        self.assertEqual(wp1.y, 11)
        self.assertEqual(wp1.z, 12)
        self.assertEqual(wp1.frame, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT)

    def test_save(self):
        '''test load/save in default format'''
        loader = mavwp.MAVWPLoader()
        waypoints = self.make_wps()
        for wp in waypoints:
            loader.add(wp)

        outfile = "test_wp-wp.txt"
        loader.save(outfile)

        loader2 = mavwp.MAVWPLoader()
        loader2.load(outfile)

        outfile2 = "test_wp-wp2.txt"
        loader.save(outfile2)

        self.assertEqual(loader.count(), loader2.count())

        for i in range(loader.count()):
            self.assertEqual(loader.wp(i).z, loader2.wp(i).z)

    def test_wp_is_loiter(self):
        '''test is_loiter method'''
        loader = mavwp.MAVWPLoader()

        wp = self.makewp(1)
        wp.command = mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS
        loader.add(wp)

        wp = self.makewp(2)
        wp.command = mavutil.mavlink.MAV_CMD_NAV_WAYPOINT
        loader.add(wp)

        wp = self.makewp(2)
        wp.command = mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH
        loader.add(wp)

        self.assertTrue(loader.wp_is_loiter(0))
        self.assertFalse(loader.wp_is_loiter(1))
        self.assertFalse(loader.wp_is_loiter(2))

        assert True

    def test_is_location_command(self):
        loader = mavwp.MAVWPLoader()
        self.assertFalse(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH))
        self.assertTrue(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT))
        self.assertTrue(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS))

class RallyTest(unittest.TestCase):
    '''tests functions related to loading waypoints and transfering them
    via the mission-item-protocol'''
    def test_rally_load(self):
        '''test loading rally points from old RALLY style file'''
        loader = mavwp.MissionItemProtocol_Rally()
        self.assertEqual(loader.mav_mission_type(),
                         mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        self.assertTrue(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT))

        # test loading a QGC WPL 110 file:
        rally_filepath = pkg_resources.resource_filename(__name__, "rally-110.txt")
        loader.load(rally_filepath)
        self.assertEqual(loader.count(), 2)
        self.assertEqual(loader.wp(0).command, mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT)

        # test loading tradition "RALLY" style format:
        rally_filepath = pkg_resources.resource_filename(__name__, "rally.txt")
        loader.load(rally_filepath)
        self.assertEqual(loader.count(), 2)
        self.assertEqual(loader.wp(0).command, mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT)

class FenceTest(unittest.TestCase):
    def test_fence_load(self):
        '''test loading rally points from old RALLY style file'''
        loader = mavwp.MissionItemProtocol_Fence()
        self.assertEqual(loader.mav_mission_type(),
                         mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        self.assertTrue(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION))

        # test loading a QGC WPL 110 file:
        fence_filepath = pkg_resources.resource_filename(__name__,
                                                         "fence-110.txt")
        loader.load(fence_filepath)
        self.assertEqual(loader.count(), 10)
        self.assertEqual(loader.wp(3).command, mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION)

        # test loading tradition lat/lng-pair style format:
        fence_filepath = pkg_resources.resource_filename(__name__,
                                                         "fence.txt")
        loader.load(fence_filepath)
        # there are 6 lines in the file - one return point, four fence
        # points and a "fence closing point".  We don't store the
        # fence closing point.
        self.assertEqual(loader.count(), 5)
        self.assertEqual(loader.wp(0).command, mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT)
        self.assertEqual(loader.wp(3).command, mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION)

if __name__ == '__main__':
    unittest.main()