Location.hpp

#ifndef LGL_LOCATION_HPP
#define LGL_LOCATION_HPP

#include <iostream>


namespace jafar {
  namespace lgl {

    struct NavLocation {

      int pos_x; 
      int pos_y;
      int pos_z;

      // Triplet for UTM position
      double utm_x;
      double utm_y;
      double utm_z;

      // Location jauge about wapoint checking
      // Safe radius meters around that point
      // -1.0 means it is not set
      // else any value >= 0.0 is in meter around the waypoint.
      float safeRadius;

      // Default constructor
      NavLocation():pos_x(0),pos_y(0),pos_z(0),utm_x(0.0),utm_y(0.0),utm_z(0.0),safeRadius(-1.0) {}
      // (X,Y) constructor
      NavLocation(int in_xxx, int in_yyy):
      pos_x(in_xxx),pos_y(in_yyy),pos_z(0),utm_x(0.0),utm_y(0.0),utm_z(0.0),safeRadius(-1.0) {}
      // (X,Y,Z) constructor
      NavLocation(int in_xxx, int in_yyy, int in_zzz):
      pos_x(in_xxx),pos_y(in_yyy),pos_z(in_zzz),utm_x(0.0),utm_y(0.0),utm_z(0.0),safeRadius(-1.0) {}

      // Setters
      void setSafeRadius(float sr) { safeRadius = sr; }
      void setPos(int in_xxx, int in_yyy) {pos_x=in_xxx;pos_y=in_yyy;}
      void setUTMPos(double in_xxx, double in_yyy, double _z) {
        utm_x = in_xxx;
        utm_y = in_yyy;
        utm_z = _z;
      }

      void updateUTMPos(
          double utmXRoot,
          double utmYRoot,
          double cellXRes,
          double cellYRes
        )
      {
        utm_x = utmXRoot + ((double)(pos_x)/2.0) * cellXRes;
        // using minus sign for pos_y since pos_y is negative to suite image frames.
        utm_y = utmYRoot + ((double)(pos_y)/2.0) * cellYRes;
        //utm_z = 0.0;
      }

      int x() const {return pos_x;}
      int y() const {return pos_y;}

      bool operator==(const  NavLocation &rhs) const
      {
        return (pos_x == rhs.pos_x) && (pos_y == rhs.pos_y);
      }
      
      bool operator!=(const  NavLocation &rhs) const
      {
        return (pos_x != rhs.pos_x) || (pos_y != rhs.pos_y);
      }

      NavLocation& operator=(const NavLocation& loc) {
        pos_x=loc.pos_x;
        pos_y=loc.pos_y;
        utm_x = loc.utm_x;
        utm_y = loc.utm_y;
        utm_z = loc.utm_z;
        safeRadius = loc.safeRadius;
        return *this;
      }

    };

    std::ostream& operator<<(std::ostream& out, const NavLocation &loc);

  }
}

#endif /* LGL_LOCATION_HPP */