/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.
 */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/
#pragma once

#include "SensorBase.h"
#include "Counter.h"
#include "Task.h"
#include "PIDSource.h"
#include "LiveWindow/LiveWindowSendable.h"
#include <atomic>

#include "HAL/cpp/priority_mutex.h"

#include <memory>

class DigitalInput;
class DigitalOutput;

/**
 * Ultrasonic rangefinder class.
 * The Ultrasonic rangefinder measures absolute distance based on the round-trip
 * time
 * of a ping generated by the controller. These sensors use two transducers, a
 * speaker and
 * a microphone both tuned to the ultrasonic range. A common ultrasonic sensor,
 * the Daventech SRF04
 * requires a short pulse to be generated on a digital channel. This causes the
 * chirp to be
 * emmitted. A second line becomes high as the ping is transmitted and goes low
 * when
 * the echo is received. The time that the line is high determines the round
 * trip distance
 * (time of flight).
 */
class Ultrasonic : public SensorBase,
                   public PIDSource,
                   public LiveWindowSendable {
 public:
  enum DistanceUnit { kInches = 0, kMilliMeters = 1 };

  DEPRECATED(
      "Raw pointers are deprecated; prefer either specifying the channel "
      "numbers as integers or passing shared_ptrs.")
  Ultrasonic(DigitalOutput *pingChannel, DigitalInput *echoChannel,
             DistanceUnit units = kInches);

  DEPRECATED(
      "References are deprecated; prefer either specifying the channel numbers "
      "as integers or passing shared_ptrs.")
  Ultrasonic(DigitalOutput &pingChannel, DigitalInput &echoChannel,
             DistanceUnit units = kInches);

  Ultrasonic(std::shared_ptr<DigitalOutput> pingChannel,
             std::shared_ptr<DigitalInput> echoChannel,
             DistanceUnit units = kInches);
  Ultrasonic(uint32_t pingChannel, uint32_t echoChannel,
             DistanceUnit units = kInches);
  virtual ~Ultrasonic();

  void Ping();
  bool IsRangeValid() const;
  static void SetAutomaticMode(bool enabling);
  double GetRangeInches() const;
  double GetRangeMM() const;
  bool IsEnabled() const { return m_enabled; }
  void SetEnabled(bool enable) { m_enabled = enable; }

  double PIDGet() override;
  void SetPIDSourceType(PIDSourceType pidSource) override;
  void SetDistanceUnits(DistanceUnit units);
  DistanceUnit GetDistanceUnits() const;

  void UpdateTable() override;
  void StartLiveWindowMode() override;
  void StopLiveWindowMode() override;
  std::string GetSmartDashboardType() const override;
  void InitTable(std::shared_ptr<ITable> subTable) override;
  std::shared_ptr<ITable> GetTable() const override;

 private:
  void Initialize();

  static void UltrasonicChecker();

  static constexpr double kPingTime =
      10 * 1e-6;  ///< Time (sec) for the ping trigger pulse.
  static const uint32_t kPriority =
      64;  ///< Priority that the ultrasonic round robin task runs.
  static constexpr double kMaxUltrasonicTime =
      0.1;  ///< Max time (ms) between readings.
  static constexpr double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;

  static Task m_task;  // task doing the round-robin automatic sensing
  static Ultrasonic *m_firstSensor;  // head of the ultrasonic sensor list
  static std::atomic<bool> m_automaticEnabled; // automatic round robin mode
  static priority_mutex m_mutex;  // synchronize access to the list of sensors

  std::shared_ptr<DigitalOutput> m_pingChannel;
  std::shared_ptr<DigitalInput> m_echoChannel;
  bool m_allocatedChannels;
  bool m_enabled;
  Counter m_counter;
  Ultrasonic *m_nextSensor;
  DistanceUnit m_units;

  std::shared_ptr<ITable> m_table = nullptr;
};