#pragma once

#include <esp_log.h>
#include <esp_timer.h>

#include "MadgwickAHRS.h"
#include "config.pb.h"
#include "lat_long.hh"
#include "pid_controller.hh"
#include "ugv_io.hh"

namespace ugv {

namespace comms {
class CommsClass;
}

class DisplayClass;

using ugv::comms::CommsClass;
using ugv::comms::messages::UGV_State;
using ugv::io::IOClass;

extern "C" void UGV_TickTimeout(void *arg);

void UpdateLocationFromGPS(comms::messages::Location &location,
                           const io::GpsData &gps_data);

class UGV {
 private:
  CommsClass *comms;
  IOClass *io;
  DisplayClass *display;
  esp_timer_handle_t timer_handle;

  LatLong target_;
  config::Config conf_;
  PIDController angle_controller_;
  Madgwick ahrs_;

  io::Inputs inputs_;
  io::Outputs outputs_;
  int64_t last_print_;
  UGV_State current_state_;
  UGV_State next_state_;
  float yaw_;
  float pitch_;
  float roll_;
  io::MpuData last_mpu_;
  int64_t last_noise_;
  float accel_noise_accum_;
  float gyro_noise_accum_;
  float accel_noise_;
  float gyro_noise_;
  bool is_still_;
  float last_left_;
  float last_right_;
  bool did_miss_mpu_;

  void UpdateAHRS();
  void DoDebugPrint();
  void ReadComms();

 public:
  explicit UGV();

  static config::Config DefaultConfig();

  void SetConfig(const config::Config &conf);
  void SetTarget(LatLong target);

  void Init();

  void OnTick();
};

extern UGV *the_ugv;

void Start(void);

}  // namespace ugv