text: /*----------------------------------------------------------------------------*/
  /* Copyright (c) FIRST 2014-2016. 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 the root directory of */
  /* the project.                                                               */
  /*----------------------------------------------------------------------------*/
  
  #pragma once
  
  #include "SafePWM.h"
  #include "CANSpeedController.h"
  #include "PIDOutput.h"
  #include "PIDSource.h"
  #include "PIDInterface.h"
  #include "HAL/CanTalonSRX.h"
  #include "MotorSafetyHelper.h"
  #include "LiveWindow/LiveWindowSendable.h"
  #include "tables/ITable.h"
  
  #include <memory>
  
  /**
   * CTRE Talon SRX Speed Controller with CAN Control
   */
  class CANTalon : public MotorSafety,
                   public CANSpeedController,
                   public ErrorBase,
                   public LiveWindowSendable,
                   public ITableListener,
                   public PIDSource,
                   public PIDInterface {
   public:
    enum FeedbackDevice {
      QuadEncoder = 0,
      AnalogPot = 2,
      AnalogEncoder = 3,
      EncRising = 4,
      EncFalling = 5,
      CtreMagEncoder_Relative = 6, //!< Cross The Road Electronics Magnetic Encoder in Absolute/PulseWidth Mode
      CtreMagEncoder_Absolute = 7, //!< Cross The Road Electronics Magnetic Encoder in Relative/Quadrature Mode
      PulseWidth = 8,
    };
    /**
     * Depending on the sensor type, Talon can determine if sensor is plugged in ot not.
     */
    enum FeedbackDeviceStatus {
      FeedbackStatusUnknown = 0,      //!< Sensor status could not be determined.  Not all sensors can do this.
      FeedbackStatusPresent = 1,      //!< Sensor is present and working okay.
      FeedbackStatusNotPresent = 2,   //!< Sensor is not present, not plugged in, not powered, etc...
    };
    enum StatusFrameRate {
      StatusFrameRateGeneral = 0,
      StatusFrameRateFeedback = 1,
      StatusFrameRateQuadEncoder = 2,
      StatusFrameRateAnalogTempVbat = 3,
      StatusFrameRatePulseWidthMeas = 4,
    };
    /**
     * Enumerated types for Motion Control Set Values.
     * When in Motion Profile control mode, these constants are paseed
     * into set() to manipulate the motion profile executer.
     * When changing modes, be sure to read the value back using getMotionProfileStatus()
     * to ensure changes in output take effect before performing buffering actions.
     * Disable will signal Talon to put motor output into neutral drive.
     *   Talon will stop processing motion profile points.  This means the buffer is
     *   effectively disconnected from the executer, allowing the robot to gracefully
     *   clear and push new traj points.  isUnderrun will get cleared.
     *   The active trajectory is also cleared.
     * Enable will signal Talon to pop a trajectory point from it's buffer and process it.
     *   If the active trajectory is empty, Talon will shift in the next point.
     *   If the active traj is empty, and so is the buffer, the motor drive is neutral and
     *   isUnderrun is set.  When active traj times out, and buffer has at least one point,
     *   Talon shifts in next one, and isUnderrun is cleared.  When active traj times out,
     *   and buffer is empty, Talon keeps processing active traj and sets IsUnderrun.
     * Hold will signal Talon keep processing the active trajectory indefinitely.
     *   If the active traj is cleared, Talon will neutral motor drive.  Otherwise
     *    Talon will keep processing the active traj but it will not shift in
     *    points from the buffer.  This means the buffer is  effectively disconnected
     *    from the executer, allowing the robot to gracefully clear and push
     *    new traj points.
     *    isUnderrun is set if active traj is empty, otherwise it is cleared.
     *    isLast signal is also cleared.
     *
     * Typical workflow:
     *   set(Disable),
     *   Confirm Disable takes effect,
     *   clear buffer and push buffer points,
     *   set(Enable) when enough points have been pushed to ensure no underruns,
     *   wait for MP to finish or decide abort,
     *   If MP finished gracefully set(Hold) to hold position servo and disconnect buffer,
     *   If MP is being aborted set(Disable) to neutral the motor and disconnect buffer,
     *   Confirm mode takes effect,
     *   clear buffer and push buffer points, and rinse-repeat.
     */
    enum SetValueMotionProfile {
      SetValueMotionProfileDisable = 0,
      SetValueMotionProfileEnable = 1,
      SetValueMotionProfileHold = 2,
    };
    /**
     * Motion Profile Trajectory Point
     * This is simply a data transer object.
     */
    struct TrajectoryPoint {
      double position; //!< The position to servo to.
      double velocity; //!< The velocity to feed-forward.
      /**
       * Time in milliseconds to process this point.
       * Value should be between 1ms and 255ms.  If value is zero
       * then Talon will default to 1ms.  If value exceeds 255ms API will cap it.
       */
      unsigned int timeDurMs;
      /**
       * Which slot to get PIDF gains.
       * PID is used for position servo.
       * F is used as the Kv constant for velocity feed-forward.
       * Typically this is hardcoded to the a particular slot, but you are free
       * gain schedule if need be.
       */
      unsigned int profileSlotSelect;
      /**
       * Set to true to only perform the velocity feed-forward and not perform
       * position servo.  This is useful when learning how the position servo
       * changes the motor response.  The same could be accomplish by clearing the
       * PID gains, however this is synchronous the streaming, and doesn't require restoing
       * gains when finished.
       *
       * Additionaly setting this basically gives you direct control of the motor output
       * since motor output = targetVelocity X Kv, where Kv is our Fgain.
       * This means you can also scheduling straight-throttle curves without relying on
       * a sensor.
       */
      bool velocityOnly;
      /**
       * Set to true to signal Talon that this is the final point, so do not
       * attempt to pop another trajectory point from out of the Talon buffer.
       * Instead continue processing this way point.  Typically the velocity
       * member variable should be zero so that the motor doesn't spin indefinitely.
       */
      bool isLastPoint;
     /**
       * Set to true to signal Talon to zero the selected sensor.
       * When generating MPs, one simple method is to make the first target position zero,
       * and the final target position the target distance from the current position.
       * Then when you fire the MP, the current position gets set to zero.
       * If this is the intent, you can set zeroPos on the first trajectory point.
       *
       * Otherwise you can leave this false for all points, and offset the positions
       * of all trajectory points so they are correct.
       */
      bool zeroPos;
    };
    /**
     * Motion Profile Status
     * This is simply a data transer object.
     */
    struct MotionProfileStatus {
      /**
       * The available empty slots in the trajectory buffer.
       *
       * The robot API holds a "top buffer" of trajectory points, so your applicaion
       * can dump several points at once.  The API will then stream them into the Talon's
       * low-level buffer, allowing the Talon to act on them.
       */
      unsigned int topBufferRem;
      /**
       * The number of points in the top trajectory buffer.
       */
      unsigned int topBufferCnt;
      /**
       * The number of points in the low level Talon buffer.
       */
      unsigned int btmBufferCnt;
      /**
       * Set if isUnderrun ever gets set.
       * Only is cleared by clearMotionProfileHasUnderrun() to ensure
       * robot logic can react or instrument it.
       * @see clearMotionProfileHasUnderrun()
       */
      bool hasUnderrun;
      /**
       * This is set if Talon needs to shift a point from its buffer into
       * the active trajectory point however the buffer is empty. This gets cleared
       * automatically when is resolved.
       */
      bool isUnderrun;
      /**
       * True if the active trajectory point has not empty, false otherwise.
       * The members in activePoint are only valid if this signal is set.
       */
      bool activePointValid;
      /**
       * The number of points in the low level Talon buffer.
       */
      TrajectoryPoint activePoint;
      /**
       * The current output mode of the motion profile executer (disabled, enabled, or hold).
       * When changing the set() value in MP mode, it's important to check this signal to
       * confirm the change takes effect before interacting with the top buffer.
       */
      SetValueMotionProfile outputEnable;
    };
    explicit CANTalon(int deviceNumber);
    explicit CANTalon(int deviceNumber, int controlPeriodMs);
    DEFAULT_MOVE_CONSTRUCTOR(CANTalon);
    virtual ~CANTalon();
  
    // PIDOutput interface
    virtual void PIDWrite(float output) override;
  
    // PIDSource interface
    virtual double PIDGet() override;
  
    // MotorSafety interface
    virtual void SetExpiration(float timeout) override;
    virtual float GetExpiration() const override;
    virtual bool IsAlive() const override;
    virtual void StopMotor() override;
    virtual void SetSafetyEnabled(bool enabled) override;
    virtual bool IsSafetyEnabled() const override;
    virtual void GetDescription(std::ostringstream& desc) const override;
  
    // CANSpeedController interface
    virtual float Get() const override;
    virtual void Set(float value, uint8_t syncGroup = 0) override;
    virtual void Reset() override;
    virtual void SetSetpoint(float value) override;
    virtual void Disable() override;
    virtual void EnableControl();
    virtual void Enable() override;
    virtual void SetP(double p) override;
    virtual void SetI(double i) override;
    virtual void SetD(double d) override;
    void SetF(double f);
    void SetIzone(unsigned iz);
    virtual void SetPID(double p, double i, double d) override;
    virtual void SetPID(double p, double i, double d, double f);
    virtual double GetP() const override;
    virtual double GetI() const override;
    virtual double GetD() const override;
    virtual double GetF() const;
    virtual bool IsModePID(CANSpeedController::ControlMode mode) const override;
    virtual float GetBusVoltage() const override;
    virtual float GetOutputVoltage() const override;
    virtual float GetOutputCurrent() const override;
    virtual float GetTemperature() const override;
    void SetPosition(double pos);
    virtual double GetPosition() const override;
    virtual double GetSpeed() const override;
    virtual int GetClosedLoopError() const;
    virtual void SetAllowableClosedLoopErr(uint32_t allowableCloseLoopError);
    virtual int GetAnalogIn() const;
    virtual void SetAnalogPosition(int newPosition);
    virtual int GetAnalogInRaw() const;
    virtual int GetAnalogInVel() const;
    virtual int GetEncPosition() const;
    virtual void SetEncPosition(int);
    virtual int GetEncVel() const;
    int GetPinStateQuadA() const;
    int GetPinStateQuadB() const;
    int GetPinStateQuadIdx() const;
    int IsFwdLimitSwitchClosed() const;
    int IsRevLimitSwitchClosed() const;
    int GetNumberOfQuadIdxRises() const;
    void SetNumberOfQuadIdxRises(int rises);
    virtual int GetPulseWidthPosition() const;
    virtual void SetPulseWidthPosition(int newpos);
    virtual int GetPulseWidthVelocity() const;
    virtual int GetPulseWidthRiseToFallUs() const;
    virtual int GetPulseWidthRiseToRiseUs() const;
    virtual FeedbackDeviceStatus IsSensorPresent(FeedbackDevice feedbackDevice)const;
    virtual bool GetForwardLimitOK() const override;
    virtual bool GetReverseLimitOK() const override;
    virtual uint16_t GetFaults() const override;
    uint16_t GetStickyFaults() const;
    void ClearStickyFaults();
    virtual void SetVoltageRampRate(double rampRate) override;
    virtual void SetVoltageCompensationRampRate(double rampRate);
    virtual uint32_t GetFirmwareVersion() const override;
    virtual void ConfigNeutralMode(NeutralMode mode) override;
    virtual void ConfigEncoderCodesPerRev(uint16_t codesPerRev) override;
    virtual void ConfigPotentiometerTurns(uint16_t turns) override;
    virtual void ConfigSoftPositionLimits(double forwardLimitPosition,
                                          double reverseLimitPosition) override;
    virtual void DisableSoftPositionLimits() override;
    virtual void ConfigLimitMode(LimitMode mode) override;
    virtual void ConfigForwardLimit(double forwardLimitPosition) override;
    virtual void ConfigReverseLimit(double reverseLimitPosition) override;
    /**
     * Change the fwd limit switch setting to normally open or closed.
     * Talon will disable momentarilly if the Talon's current setting
     * is dissimilar to the caller's requested setting.
     *
     * Since Talon saves setting to flash this should only affect
     * a given Talon initially during robot install.
     *
     * @param normallyOpen true for normally open.  false for normally closed.
     */
    void ConfigFwdLimitSwitchNormallyOpen(bool normallyOpen);
    /**
     * Change the rev limit switch setting to normally open or closed.
     * Talon will disable momentarilly if the Talon's current setting
     * is dissimilar to the caller's requested setting.
     *
     * Since Talon saves setting to flash this should only affect
     * a given Talon initially during robot install.
     *
     * @param normallyOpen true for normally open.  false for normally closed.
     */
    void ConfigRevLimitSwitchNormallyOpen(bool normallyOpen);
    virtual void ConfigMaxOutputVoltage(double voltage) override;
    void ConfigPeakOutputVoltage(double forwardVoltage,double reverseVoltage);
    void ConfigNominalOutputVoltage(double forwardVoltage,double reverseVoltage);
    /**
     * Enables Talon SRX to automatically zero the Sensor Position whenever an
     * edge is detected on the index signal.
     * @param enable      boolean input, pass true to enable feature or false to disable.
     * @param risingEdge  boolean input, pass true to clear the position on rising edge,
     *                    pass false to clear the position on falling edge.
     */
    void EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge);
    void ConfigSetParameter(uint32_t paramEnum, double value);
    bool GetParameter(uint32_t paramEnum, double & dvalue) const;
  
    virtual void ConfigFaultTime(float faultTime) override;
    virtual void SetControlMode(ControlMode mode);
    void SetFeedbackDevice(FeedbackDevice device);
    void SetStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs);
    virtual ControlMode GetControlMode() const;
    void SetSensorDirection(bool reverseSensor);
    void SetClosedLoopOutputDirection(bool reverseOutput);
    void SetCloseLoopRampRate(double rampRate);
    void SelectProfileSlot(int slotIdx);
    int GetIzone() const;
    int GetIaccum() const;
    void ClearIaccum();
    int GetBrakeEnableDuringNeutral() const;
  
    bool IsControlEnabled() const;
    bool IsEnabled() const override;
    double GetSetpoint() const override;
  
  
    /**
     * Calling application can opt to speed up the handshaking between the robot API and the Talon to increase the
     * download rate of the Talon's Motion Profile.  Ideally the period should be no more than half the period
     * of a trajectory point.
     */
    void ChangeMotionControlFramePeriod(int periodMs);
  
    /**
     * Clear the buffered motion profile in both Talon RAM (bottom), and in the API (top).
     * Be sure to check GetMotionProfileStatus() to know when the buffer is actually cleared.
     */
    void ClearMotionProfileTrajectories();
  
    /**
     * Retrieve just the buffer count for the api-level (top) buffer.
     * This routine performs no CAN or data structure lookups, so its fast and ideal
     * if caller needs to quickly poll the progress of trajectory points being emptied
     * into Talon's RAM. Otherwise just use GetMotionProfileStatus.
     * @return number of trajectory points in the top buffer.
     */
    int GetMotionProfileTopLevelBufferCount();
  
    /**
     * Push another trajectory point into the top level buffer (which is emptied into
     * the Talon's bottom buffer as room allows).
     * @param trajPt the trajectory point to insert into buffer.
     * @return true  if trajectory point push ok. CTR_BufferFull if buffer is full
     * due to kMotionProfileTopBufferCapacity.
     */
    bool PushMotionProfileTrajectory(const TrajectoryPoint & trajPt);
  
    /**
     * @return true if api-level (top) buffer is full.
     */
    bool IsMotionProfileTopLevelBufferFull();
  
    /**
     * This must be called periodically to funnel the trajectory points from the API's top level buffer to
     * the Talon's bottom level buffer.  Recommendation is to call this twice as fast as the executation rate of the motion profile.
     * So if MP is running with 20ms trajectory points, try calling this routine every 10ms.  All motion profile functions are thread-safe
     * through the use of a mutex, so there is no harm in having the caller utilize threading.
     */
    void ProcessMotionProfileBuffer();
  
    /**
     * Retrieve all status information.
     * Since this all comes from one CAN frame, its ideal to have one routine to retrieve the frame once and decode everything.
     * @param [out] motionProfileStatus contains all progress information on the currently running MP.
     */
    void GetMotionProfileStatus(MotionProfileStatus & motionProfileStatus);
  
    /**
     * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is ready for another point,
     * but the low level buffer is empty.
     *
     * Once the Motion Profile Executer sets the hasUnderrun flag, it stays set until
     * Robot Application clears it with this routine, which ensures Robot Application
     * gets a chance to instrument or react.  Caller could also check the isUnderrun flag
     * which automatically clears when fault condition is removed.
     */
    void ClearMotionProfileHasUnderrun();
  
    // LiveWindow stuff.
    void ValueChanged(ITable* source, llvm::StringRef key,
                      std::shared_ptr<nt::Value> value, bool isNew) override;
    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;
  
    // SpeedController overrides
    virtual void SetInverted(bool isInverted) override;
    virtual bool GetInverted() const override;
  
   private:
    // Values for various modes as is sent in the CAN packets for the Talon.
    enum TalonControlMode {
      kThrottle = 0,
      kFollowerMode = 5,
      kVoltageMode = 4,
      kPositionMode = 1,
      kSpeedMode = 2,
      kCurrentMode = 3,
      kMotionProfileMode = 6,
      kDisabled = 15
    };
  
    int m_deviceNumber;
    std::unique_ptr<CanTalonSRX> m_impl;
    std::unique_ptr<MotorSafetyHelper> m_safetyHelper;
    int m_profile = 0;  // Profile from CANTalon to use. Set to zero until we can
                    // actually test this.
  
    bool m_controlEnabled = true;
    ControlMode m_controlMode = kPercentVbus;
    TalonControlMode m_sendMode;
  
    double m_setPoint = 0;
    /**
     * Encoder CPR, counts per rotations, also called codes per revoluion.
     * Default value of zero means the API behaves as it did during the 2015 season, each position
     * unit is a single pulse and there are four pulses per count (4X).
     * Caller can use ConfigEncoderCodesPerRev to set the quadrature encoder CPR.
     */
    uint32_t m_codesPerRev = 0;
    /**
     * Number of turns per rotation.  For example, a 10-turn pot spins ten full rotations from
     * a wiper voltage of zero to 3.3 volts.  Therefore knowing the
     * number of turns a full voltage sweep represents is necessary for calculating rotations
     * and velocity.
     * A default value of zero means the API behaves as it did during the 2015 season, there are 1024
     * position units from zero to 3.3V.
     */
    uint32_t m_numPotTurns = 0;
    /**
     * Although the Talon handles feedback selection, caching the feedback selection is helpful at the API level
     * for scaling into rotations and RPM.
     */
    FeedbackDevice m_feedbackDevice = QuadEncoder;
  
    static const unsigned int kDelayForSolicitedSignalsUs = 4000;
    /**
     * @param devToLookup FeedbackDevice to lookup the scalar for.  Because Talon
     *                allows multiple sensors to be attached simultaneously, caller must
     *                specify which sensor to lookup.
     * @return        The number of native Talon units per rotation of the selected sensor.
     *                Zero if the necessary sensor information is not available.
     * @see ConfigEncoderCodesPerRev
     * @see ConfigPotentiometerTurns
     */
    double GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)const;
    /**
     * Fixup the sendMode so Set() serializes the correct demand value.
     * Also fills the modeSelecet in the control frame to disabled.
     * @param mode Control mode to ultimately enter once user calls Set().
     * @see Set()
     */
    void ApplyControlMode(CANSpeedController::ControlMode mode);
    /**
     * @param fullRotations   double precision value representing number of rotations of selected feedback sensor.
     *                            If user has never called the config routine for the selected sensor, then the caller
     *                            is likely passing rotations in engineering units already, in which case it is returned
     *                            as is.
     *                            @see ConfigPotentiometerTurns
     *                            @see ConfigEncoderCodesPerRev
     * @return fullRotations in native engineering units of the Talon SRX firmware.
     */
    int32_t ScaleRotationsToNativeUnits(FeedbackDevice devToLookup, double fullRotations) const;
    /**
     * @param rpm     double precision value representing number of rotations per minute of selected feedback sensor.
     *                            If user has never called the config routine for the selected sensor, then the caller
     *                            is likely passing rotations in engineering units already, in which case it is returned
     *                            as is.
     *                            @see ConfigPotentiometerTurns
     *                            @see ConfigEncoderCodesPerRev
     * @return sensor velocity in native engineering units of the Talon SRX firmware.
     */
    int32_t ScaleVelocityToNativeUnits(FeedbackDevice devToLookup, double rpm) const;
    /**
     * @param nativePos     integral position of the feedback sensor in native Talon SRX units.
     *                            If user has never called the config routine for the selected sensor, then the return
     *                            will be in TALON SRX units as well to match the behavior in the 2015 season.
     *                            @see ConfigPotentiometerTurns
     *                            @see ConfigEncoderCodesPerRev
     * @return double precision number of rotations, unless config was never performed.
     */
    double ScaleNativeUnitsToRotations(FeedbackDevice devToLookup, int32_t nativePos) const;
    /**
     * @param nativeVel     integral velocity of the feedback sensor in native Talon SRX units.
     *                            If user has never called the config routine for the selected sensor, then the return
     *                            will be in TALON SRX units as well to match the behavior in the 2015 season.
     *                            @see ConfigPotentiometerTurns
     *                            @see ConfigEncoderCodesPerRev
     * @return double precision of sensor velocity in RPM, unless config was never performed.
     */
    double ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, int32_t nativeVel) const;
  
    // LiveWindow stuff.
    std::shared_ptr<ITable> m_table;
    bool m_isInverted;
  
    HasBeenMoved m_hasBeenMoved;
  };
  