#! /usr/bin/env python3

from clearpath_onav_api_examples_lib.ros_node import RosNode
from clearpath_onav_api_examples_lib.waypoint import Waypoint
from clearpath_onav_api_examples_lib.mission import Mission
import time


class SimpleMission1(RosNode):
    """Create and run a simple mission with 3 waypoints.

    Our goal is to get to the waypoint (50.1095255, -97.3192484)
    through the waypoints (50.1094938, -97.3191085) and (50.1095100, -97.3192000).

    The coordinates used in this example are based on the cpr_agriculture_gazebo
    world and should be updated to match the location in which the user's
    robot will be operating.
    """

    def __init__(self):
        """Initialize the mission details and server connection."""

        RosNode.__init__(self, 'simple_mission_1')
        waypoints = [
            Waypoint("A", "uuid-waypoint-1", 50.1094938, -97.3191085),
            Waypoint("B", "uuid-waypoint-2", 50.1095100, -97.3192000),
            Waypoint("C", "uuid-waypoint-3", 50.1095255, -97.3192484),
        ]
        self.mission = Mission("Mission 1", "uuid-mission-1", waypoints)

    def run(self):
        """Execute the mission."""

        if not self.mission.startMission():
            return False
        while not self.mission.isMissionComplete():
            time.sleep(1.0)
        return self.mission.getMissionSuccess()


if __name__ == '__main__':
    if SimpleMission1().run():
        print("Mission completed successfully")
    else:
        print("Mission failed")
