#include <stdio.h>
#include <stdlib.h>

int main(int argc, char* argv[])
{
  //
  // Send NAV data for camera to include in images
  //
  // Gather data from Navigation
  // Assemble into NAV packet for Cathx
  // Push it
  //
  char navdata[150];
  char *nav_fmt = "$PNAVALL,"                       // preamble
                  "%4d%02d%02dT%02d%02d%02d.%03dZ," // ISO 8601 time
                  "A,"                              // valid data
                  "%02.7f,"                         // lat,degrees,zero-padded
                  "%03.7f,"                         // long,deg,zero-padded
                  "%.2f,"                           // depth,meters,not padded
                  "%.2f,"                           // alt,meters,not padded
                  "%.3f,"                           // surge,m/s,not padded
                  "%.3f,"                           // sway,m/s,not padded
                  "%.3f,"                           // heave,m/s,not padded
                  "%03.2f,"                         // roll,deg,padded
                  "%03.2f,"                         // pitch,deg,padded
                  "%03.2f,"                         // yaw,deg,padded
                  "%03.2f,"                         // roll-rate,deg/s,padded
                  "%03.2f,"                         // pitch-rate,deg/s,padded
                  "%03.2f,"                         // yaw-rate,deg/s,padded
                  "*";                              // end of nav data

  unsigned int year, month, day, hour, min, sec, millis;
  float lat, lon, depth, alt, surge, sway, heave;
  float roll, pitch, yaw, roll_r, pitch_r, yaw_r;
  char  checksum;

  // Test data
  year = 2015, month = 11, day = 11;
  hour = 14, min = 23, sec = 27, millis = 3;
  lat = 19.123; lon = -61.0345; depth = 123.45; alt = 345.6;
  surge = heave = 4.05; sway = 0. - surge;
  roll = pitch = yaw = 234.5;
  roll_r = pitch_r = yaw_r = 67.08;

  sprintf(navdata, nav_fmt,
          year, month, day, hour, min, sec, millis,
          lat, lon, depth, alt, surge, sway, heave,
          roll, pitch, yaw, roll_r, pitch_r, yaw_r);
  printf("%s\n", navdata);

  // Calculate checksum
  int i = 1;
  for (checksum = 0; navdata[i] != '*'; i++)
    checksum ^= navdata[i];

  printf("%s length %d\n", navdata, i);
  // variable i is now index to '*', so place checksum
  // after that followed by cr and lf
  //
  navdata[++i] = checksum;
  navdata[++i] = 0x0d;
  navdata[++i] = 0x0a;
}