esp-fc

#pragma once

#include "Device/InputDevice.hpp"
#include "Rc/Crsf.h"
#include "TelemetryManager.h"

// https://github.com/CapnBry/CRServoF/blob/master/lib/CrsfSerial/crsf_protocol.h
// https://github.com/AlessioMorale/crsf_parser/tree/master
// https://github.com/betaflight/betaflight/blob/master/src/main/rx/crsf.c

namespace Espfc::Device::Input {

class InputCRSF : public InputDevice
{
public:
  enum CrsfState
  {
    CRSF_ADDR,
    CRSF_SIZE,
    CRSF_TYPE,
    CRSF_DATA,
    CRSF_CRC
  };

  InputCRSF();

  int begin(Stream::ReadWritable* serial, TelemetryManager* telemetry);
  virtual InputStatus update() override;
  virtual uint16_t get(uint8_t i) const override;
  virtual void get(uint16_t* data, size_t len) const override;
  virtual size_t getChannelCount() const override;
  virtual bool needAverage() const override;

  void print(char c) const;
  void parse(Rc::CrsfMessage& frame, int d);

private:
  void reset();
  void apply(const Rc::CrsfMessage& msg);
  void applyLinkStats(const Rc::CrsfMessage& msg);
  void applyChannels(const Rc::CrsfMessage& msg);
  void applyMspReq(const Rc::CrsfMessage& msg);

  static constexpr size_t CHANNELS = 16;
  static constexpr size_t TELEMETRY_INTERVAL = 20000;

  Stream::ReadWritable* _serial;
  TelemetryManager* _telemetry;
  CrsfState _state;
  uint8_t _idx;
  bool _new_data;
  Rc::CrsfMessage _frame;
  uint16_t _channels[CHANNELS];
  uint32_t _telemetry_next;
  Connect::MspMessage _msg;
};

} // namespace Espfc::Device::Input