#ifndef MARSHCONNECTOR_H
#define MARSHCONNECTOR_H

#include "godot_cpp/classes/node.hpp"
#include "godot_cpp/classes/packet_peer_udp.hpp"
#include "godot_cpp/classes/timer.hpp"
#include "godot_cpp/variant/quaternion.hpp"
#include "godot_cpp/variant/transform3d.hpp"
#include "godot_cpp/variant/vector2.hpp"
#include "godot_cpp/variant/vector3.hpp"
#include "mavlink/mavlink_types.h"

namespace godot {

class MarshConnector : public Node {
  GDCLASS(MarshConnector, Node)

protected:
  static void _bind_methods();

public:
  MarshConnector();
  ~MarshConnector();

  void _ready() override;
  void _process(double delta) override;

  // Returns the error from put_packet
  Error send_heartbeat();
  // Handle MAVLink messages fully contained in data
  void receive_data(const PackedByteArray &data);
  // Set parameter value with some validation
  Error set_parameter(const String &id, float value);
  // Get current parameter value
  float get_parameter(const String &id);

  // Get current state of the aircraft
  Transform3D get_aircraft();
  // Get normalized (-1 to 1) cyclic position, where X is right, Y is pitch up
  Vector2 get_cyclic();
  // Get normalized (0 to 1) collective position, positive to climb
  float get_collective();
  // Get normalized (-1 to 1) pedals position, positive turn right
  float get_pedals();

  // Is there a connection to MARSH Manager
  bool get_manager_connected();
  // Is receiving data from flight model
  bool get_model_connected();

  // Called by timer
  void manager_timeout();
  // Called by timer
  void model_timeout();

  // Convert from global coordinates to local position x=north, y=east
  // taking into account current configuration.
  // Note that 32-bit float type is not exact enough to store global position
  Vector2 local_meters_from_global_degrees(double latitude, double longitude);

  static Vector3 godot_location_from_mavlink(float north_meters,
                                             float east_meters,
                                             float alt_meters);
  static Quaternion godot_rotation_from_mavlink(float roll_rad, float pitch_rad,
                                                float yaw_rad);

private:
  // Handle corresponding MAVLink message received.
  // The functions ensure the type is correct; message_t is used, only
  // to keep the amount of code required in the header small.
  void handle_sim_state(mavlink_message_t message);
  void handle_local_position(mavlink_message_t message);
  void handle_attitude(mavlink_message_t message);
  void handle_param(mavlink_message_t message);
  void handle_manual_control(mavlink_message_t message);
  void handle_heartbeat(mavlink_message_t message);

  void receive_model_data();
  void receive_location(Vector3 location);
  void receive_rotation(Quaternion rotation);

  Error send_message(mavlink_message_t message);

  // Do not specify specific values, to ensure PARAM_COUNT is correct
  // Not an enum class on purpose, to make use more convenient, it's scoped
  // inside the class namespace anyway
  enum Parameter {
    // FOG_DENSITY,
    NAV_OFS_HDG,
    NAV_OFS_X,
    NAV_OFS_Y,
    LOCAL_FRAME_LAT,
    LOCAL_FRAME_LON,
    PARAM_COUNT,
  };
  float parameters[PARAM_COUNT] = {0.0, 0.0, 0.0, 0.0, 0.0};
  const String parameter_names[PARAM_COUNT] = {
      // "FOG_DENSITY",
      "NAV_OFS_HDG",     "NAV_OFS_X",       "NAV_OFS_Y",
      "LOCAL_FRAME_LAT", "LOCAL_FRAME_LON",
  };

  // TODO: Interpolate with some delay
  Vector3 last_location;
  Quaternion last_rotation;
  Vector4 last_controls;

  double time_passed;
  Timer *heartbeat_timer;
  PacketPeerUDP *socket;

  bool manager_connected;
  Timer *manager_timer;
  bool model_connected;
  Timer *model_timer;
};

} // namespace godot

#endif // MARSHCONNECTOR_H