#pragma once

#include <cmath>
#include "ugv_comms.hh"

struct LatLong {
  static constexpr float PI =
      3.1415926535897932384626433832795028841971693993751058209749445923078164062;

  static constexpr float RAD_PER_DEG = PI / 180.f;
  // Radius of earth in meters
  static constexpr float EARTH_RAD = 6372795.f;

 public:
  float latitude;
  float longitude;

  inline LatLong() : LatLong(0., 0.) {}

  inline LatLong(double latitude_, double longitude_)
      : latitude(latitude_), longitude(longitude_) {}

  inline LatLong(const ugv::comms::messages::TargetLocation &loc)
      : latitude(loc.latitude()), longitude(loc.longitude()) {}

  /**
   * Return distance from this LatLong to target, in meters
   */
  float distance_to(const LatLong &target) const;
  float bearing_toward(const LatLong &target) const;
};