text: #ifndef FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
  #define FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
  
  #include <memory>
  
  #include "frc971/control_loops/control_loop.h"
  #include "frc971/control_loops/state_feedback_loop.h"
  
  namespace frc971 {
  namespace control_loops {
  namespace testing {
  class WristTest_NoWindupPositive_Test;
  class WristTest_NoWindupNegative_Test;
  };
  
  // Note: Everything in this file assumes that there is a 1 cycle delay between
  // power being requested and it showing up at the motor.  It assumes that
  // X_hat(2, 1) is the voltage being applied as well.  It will go unstable if
  // that isn't true.
  
  template<int kNumZeroSensors>
  class ZeroedJoint;
  
  // This class implements the CapU function correctly given all the extra
  // information that we know about from the wrist motor.
  template<int kNumZeroSensors>
  class ZeroedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
   public:
    ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop,
                            ZeroedJoint<kNumZeroSensors> *zeroed_joint)
        : StateFeedbackLoop<3, 1, 1>(loop),
          zeroed_joint_(zeroed_joint),
          voltage_(0.0),
          last_voltage_(0.0) {
    }
  
    // Caps U, but this time respects the state of the wrist as well.
    virtual void CapU();
  
    // Returns the accumulated voltage.
    double voltage() const { return voltage_; }
  
    // Returns the uncapped voltage.
    double uncapped_voltage() const { return uncapped_voltage_; }
  
    // Zeros the accumulator.
    void ZeroPower() { voltage_ = 0.0; }
   private:
    ZeroedJoint<kNumZeroSensors> *zeroed_joint_;
  
    // The accumulated voltage to apply to the motor.
    double voltage_;
    double last_voltage_;
    double uncapped_voltage_;
  };
  
  template<int kNumZeroSensors>
  void ZeroedStateFeedbackLoop<kNumZeroSensors>::CapU() {
    const double old_voltage = voltage_;
    voltage_ += U(0, 0);
  
    uncapped_voltage_ = voltage_;
  
    // Do all our computations with the voltage, and then compute what the delta
    // is to make that happen.
    if (zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY) {
      if (Y(0, 0) >= zeroed_joint_->config_data_.upper_limit) {
        voltage_ = std::min(0.0, voltage_);
      }
      if (Y(0, 0) <= zeroed_joint_->config_data_.lower_limit) {
        voltage_ = std::max(0.0, voltage_);
      }
    }
  
    const bool is_ready =
        zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY;
    double limit = is_ready ?
        12.0 : zeroed_joint_->config_data_.max_zeroing_voltage;
  
    // Make sure that reality and the observer can't get too far off.  There is a
    // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
    // against last cycle's voltage.
    if (X_hat(2, 0) > last_voltage_ + 2.0) {
      //X_hat(2, 0) = last_voltage_ + 2.0;
      voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
      LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
    } else if (X_hat(2, 0) < last_voltage_ -2.0) {
      //X_hat(2, 0) = last_voltage_ - 2.0;
      voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
      LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
    }
  
    voltage_ = std::min(limit, voltage_);
    voltage_ = std::max(-limit, voltage_);
    U(0, 0) = voltage_ - old_voltage;
    LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
    LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));
  
    last_voltage_ = voltage_;
  }
  
  
  // Class to zero and control a joint with any number of zeroing sensors with a
  // state feedback controller.
  template<int kNumZeroSensors>
  class ZeroedJoint {
   public:
    // Sturcture to hold the hardware configuration information.
    struct ConfigurationData {
      // Angle at the lower hardware limit.
      double lower_limit;
      // Angle at the upper hardware limit.
      double upper_limit;
      // Speed (and direction) to move while zeroing.
      double zeroing_speed;
      // Speed (and direction) to move while moving off the sensor.
      double zeroing_off_speed;
      // Maximum voltage to apply when zeroing.
      double max_zeroing_voltage;
      // Deadband voltage.
      double deadband_voltage;
      // Angles where we see a positive edge from the hall effect sensors.
      double hall_effect_start_angle[kNumZeroSensors];
    };
  
    // Current position data for the encoder and hall effect information.
    struct PositionData {
      // Current encoder position.
      double position;
      // Array of hall effect values.
      bool hall_effects[kNumZeroSensors];
      // Array of the last positive edge position for the sensors.
      double hall_effect_positions[kNumZeroSensors];
    };
  
    ZeroedJoint(StateFeedbackLoop<3, 1, 1> loop)
        : loop_(new ZeroedStateFeedbackLoop<kNumZeroSensors>(loop, this)),
          last_good_time_(::aos::monotonic_clock::min_time()),
          state_(UNINITIALIZED),
          error_count_(0),
          zero_offset_(0.0),
          capped_goal_(false) {}
  
    // Copies the provided configuration data locally.
    void set_config_data(const ConfigurationData &config_data) {
      config_data_ = config_data;
    }
  
    // Clips the goal to be inside the limits and returns the clipped goal.
    // Requires the constants to have already been fetched.
    double ClipGoal(double goal) const {
      return ::std::min(config_data_.upper_limit,
                        std::max(config_data_.lower_limit, goal));
    }
  
    // Updates the loop and state machine.
    // position is null if the position data is stale, output_enabled is true if
    // the output will actually go to the motors, and goal_angle and goal_velocity
    // are the goal position and velocities.
    double Update(const ::aos::monotonic_clock::time_point monotonic_now,
                  const ZeroedJoint<kNumZeroSensors>::PositionData *position,
                  bool output_enabled, double goal_angle, double goal_velocity);
  
    // True if the code is zeroing.
    bool is_zeroing() const { return state_ == ZEROING; }
  
    // True if the code is moving off the hall effect.
    bool is_moving_off() const { return state_ == MOVING_OFF; }
  
    // True if the state machine is uninitialized.
    bool is_uninitialized() const { return state_ == UNINITIALIZED; }
  
    // True if the state machine is ready.
    bool is_ready() const { return state_ == READY; }
  
    // Returns the uncapped voltage.
    double U_uncapped() const { return loop_->U_uncapped(0, 0); }
  
    // True if the goal was moved to avoid goal windup.
    bool capped_goal() const { return capped_goal_; }
  
    // Timestamp
    static const double dt;
  
    double absolute_position() const { return loop_->X_hat(0, 0); }
  
   private:
    friend class ZeroedStateFeedbackLoop<kNumZeroSensors>;
    // Friend the wrist test cases so that they can simulate windeup.
    friend class testing::WristTest_NoWindupPositive_Test;
    friend class testing::WristTest_NoWindupNegative_Test;
  
    static constexpr ::aos::monotonic_clock::duration kRezeroTime;
  
    // The state feedback control loop to talk to.
    ::std::unique_ptr<ZeroedStateFeedbackLoop<kNumZeroSensors>> loop_;
  
    ConfigurationData config_data_;
  
    ::aos::monotonic_clock::time_point last_good_time_;
  
    // Returns the index of the first active sensor, or -1 if none are active.
    int ActiveSensorIndex(
        const ZeroedJoint<kNumZeroSensors>::PositionData *position) {
      if (!position) {
        return -1;
      }
      int active_index = -1;
      for (int i = 0; i < kNumZeroSensors; ++i) {
        if (position->hall_effects[i]) {
          if (active_index != -1) {
            LOG(ERROR, "More than one hall effect sensor is active\n");
          } else {
            active_index = i;
          }
        }
      }
      return active_index;
    }
    // Returns true if any of the sensors are active.
    bool AnySensorsActive(
        const ZeroedJoint<kNumZeroSensors>::PositionData *position) {
      return ActiveSensorIndex(position) != -1;
    }
  
    // Enum to store the state of the internal zeroing state machine.
    enum State {
      UNINITIALIZED,
      MOVING_OFF,
      ZEROING,
      READY,
      ESTOP
    };
  
    // Internal state for zeroing.
    State state_;
  
    // Missed position packet count.
    int error_count_;
    // Offset from the raw encoder value to the absolute angle.
    double zero_offset_;
    // Position that gets incremented when zeroing the wrist to slowly move it to
    // the hall effect sensor.
    double zeroing_position_;
    // Last position at which the hall effect sensor was off.
    double last_off_position_;
  
    // True if the zeroing goal was capped during this cycle.
    bool capped_goal_;
  
    // Returns true if number is between first and second inclusive.
    bool is_between(double first, double second, double number) {
      if ((number >= first || number >= second) &&
          (number <= first || number <= second)) {
        return true;
      }
      return false;
    }
  
    DISALLOW_COPY_AND_ASSIGN(ZeroedJoint);
  };
  
  template <int kNumZeroSensors>
  constexpr ::aos::monotonic_clock::duration
      ZeroedJoint<kNumZeroSensors>::kRezeroTime = ::std::chrono::seconds(2);
  
  template <int kNumZeroSensors>
  /*static*/ const double ZeroedJoint<kNumZeroSensors>::dt = 0.01;
  
  // Updates the zeroed joint controller and state machine.
  template <int kNumZeroSensors>
  double ZeroedJoint<kNumZeroSensors>::Update(
      const ::aos::monotonic_clock::time_point monotonic_now,
      const ZeroedJoint<kNumZeroSensors>::PositionData *position,
      bool output_enabled, double goal_angle, double goal_velocity) {
    // Uninitialize the bot if too many cycles pass without an encoder.
    if (position == NULL) {
      LOG(WARNING, "no new pos given\n");
      error_count_++;
    }
    if (error_count_ >= 4) {
      output_enabled = false;
      LOG(WARNING, "err_count is %d so disabling\n", error_count_);
  
      if (monotonic_now > kRezeroTime + last_good_time_) {
        LOG(WARNING, "err_count is %d (or 1st time) so forcing a re-zero\n",
            error_count_);
        state_ = UNINITIALIZED;
        loop_->Reset();
      }
    }
    if (position != NULL) {
      last_good_time_ = monotonic_now;
      error_count_ = 0;
    }
  
    // Compute the absolute position of the wrist.
    double absolute_position;
    if (position) {
      absolute_position = position->position;
      if (state_ == READY) {
        absolute_position -= zero_offset_;
      }
      loop_->Y << absolute_position;
      if (!AnySensorsActive(position)) {
        last_off_position_ = position->position;
      }
    } else {
      // Dead recon for now.
      absolute_position = loop_->X_hat(0, 0);
    }
  
    switch (state_) {
      case UNINITIALIZED:
        LOG(DEBUG, "UNINITIALIZED\n");
        if (position) {
          // Reset the zeroing goal.
          zeroing_position_ = absolute_position;
          // Clear the observer state.
          loop_->X_hat << absolute_position, 0.0, 0.0;
          loop_->ZeroPower();
          // Set the goal to here to make it so it doesn't move when disabled.
          loop_->R = loop_->X_hat;
          // Only progress if we are enabled.
          if (::aos::joystick_state->enabled) {
            if (AnySensorsActive(position)) {
              state_ = MOVING_OFF;
            } else {
              state_ = ZEROING;
            }
          }
        }
        break;
      case MOVING_OFF:
        LOG(DEBUG, "MOVING_OFF\n");
        {
          // Move off the hall effect sensor.
          if (!::aos::joystick_state->enabled) {
            // Start over if disabled.
            state_ = UNINITIALIZED;
          } else if (position && !AnySensorsActive(position)) {
            // We are now off the sensor.  Time to zero now.
            state_ = ZEROING;
          } else {
            // Slowly creep off the sensor.
            zeroing_position_ -= config_data_.zeroing_off_speed * dt;
            loop_->R << zeroing_position_, -config_data_.zeroing_off_speed, 0.0;
            break;
          }
        }
      case ZEROING:
        LOG(DEBUG, "ZEROING\n");
        {
          int active_sensor_index = ActiveSensorIndex(position);
          if (!::aos::joystick_state->enabled) {
            // Start over if disabled.
            state_ = UNINITIALIZED;
          } else if (position && active_sensor_index != -1) {
            state_ = READY;
            // Verify that the calibration number is between the last off position
            // and the current on position.  If this is not true, move off and try
            // again.
            const double calibration =
                position->hall_effect_positions[active_sensor_index];
            if (!is_between(last_off_position_, position->position, 
                            calibration)) {
              LOG(ERROR, "Got a bogus calibration number.  Trying again.\n");
              LOG(ERROR,
                  "Last off position was %f, current is %f, calibration is %f\n",
                  last_off_position_, position->position,
                  position->hall_effect_positions[active_sensor_index]);
              state_ = MOVING_OFF;
            } else {
              // Save the zero, and then offset the observer to deal with the
              // phantom step change.
              const double old_zero_offset = zero_offset_;
              zero_offset_ =
                  position->hall_effect_positions[active_sensor_index] -
                  config_data_.hall_effect_start_angle[active_sensor_index];
              loop_->X_hat(0, 0) += old_zero_offset - zero_offset_;
              loop_->Y(0, 0) += old_zero_offset - zero_offset_;
            }
          } else {
            // Slowly creep towards the sensor.
            zeroing_position_ += config_data_.zeroing_speed * dt;
            loop_->R << zeroing_position_, config_data_.zeroing_speed, 0.0;
          }
          break;
        }
  
      case READY:
        LOG(DEBUG, "READY\n");
        {
          const double limited_goal = ClipGoal(goal_angle);
          loop_->R << limited_goal, goal_velocity, 0.0;
          break;
        }
  
      case ESTOP:
        LOG(DEBUG, "ESTOP\n");
        LOG(WARNING, "have already given up\n");
        return 0.0;
    }
  
    // Update the observer.
    loop_->Update(position != NULL, !output_enabled);
  
    LOG(DEBUG, "X_hat={%f, %f, %f}\n",
        loop_->X_hat(0, 0), loop_->X_hat(1, 0), loop_->X_hat(2, 0));
  
    capped_goal_ = false;
    // Verify that the zeroing goal hasn't run away.
    switch (state_) {
      case UNINITIALIZED:
      case READY:
      case ESTOP:
        // Not zeroing.  No worries.
        break;
      case MOVING_OFF:
      case ZEROING:
        // Check if we have cliped and adjust the goal.
        if (loop_->uncapped_voltage() > config_data_.max_zeroing_voltage) {
          double dx = (loop_->uncapped_voltage() -
                       config_data_.max_zeroing_voltage) / loop_->K(0, 0);
          zeroing_position_ -= dx;
          capped_goal_ = true;
        } else if(loop_->uncapped_voltage() < -config_data_.max_zeroing_voltage) {
          double dx = (loop_->uncapped_voltage() +
                       config_data_.max_zeroing_voltage) / loop_->K(0, 0);
          zeroing_position_ -= dx;
          capped_goal_ = true;
        }
        break;
    }
    if (output_enabled) {
      double voltage = loop_->voltage();
      if (voltage > 0) {
        voltage += config_data_.deadband_voltage;
      } else if (voltage < 0) {
        voltage -= config_data_.deadband_voltage;
      }
      if (voltage > 12.0) {
        voltage = 12.0;
      } else if (voltage < -12.0) {
        voltage = -12.0;
      }
      return voltage;
    } else {
      return 0.0;
    }
  }
  
  }  // namespace control_loops
  }  // namespace frc971
  
  #endif  // FRC971_CONTROL_LOOPS_ZEROED_JOINT_H_
  