#include "webnav.h"

int main(int argc, char *argv[])
{
   WebNavData wnd;
   webnav_init("nav-config.cfg", &wnd);

   // Initialize mission and GPS comms.
   // Run mission with 5 second GPS updates.
   // 
   if (0 == webnav_init("nav-config.cfg", &wnd))
   {
      int count = 0;
      while (1)
      {
         if (0 == webnav_run(&wnd))
         {
            float distance, bearing;
            webnav_get_distance(&wnd, &distance);
            webnav_get_bearing(&wnd, &bearing);
            printf("Count is %d, Target position is %.1f m at a heading of %.1f degrees\n",
               count, distance, bearing);

            if (++count > 1)
            {
               printf("Push it to the next waypoint...\n");
               Waypoint wp;
               webnav_get_waypoint(&wnd, &wp);
               webnav_pos_update(&wnd, wp.latitude, wp.longitude);
               count = -1;
            }
         }
         sleep(1);
      }
   }
   webnav_close(&wnd);

   return 0;
}