/*----------------------------------------------------------------------------*/
/* 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.  */
/*----------------------------------------------------------------------------*/

#include "Counter.h"
#include "AnalogTrigger.h"
#include "DigitalInput.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "WPIErrors.h"

/**
 * Create an instance of a counter object.
 * This creates a ChipObject counter and initializes status variables
 * appropriately
 *
 * The counter will start counting immediately.
 * @param mode The counter mode
 */
void Counter::InitCounter(Mode mode) {
  int32_t status = 0;
  m_counter = initializeCounter(mode, &m_index, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));

  SetMaxPeriod(.5);

  HALReport(HALUsageReporting::kResourceType_Counter, m_index, mode);
}

/**
 * Create an instance of a counter where no sources are selected.
 * They all must be selected by calling functions to specify the upsource and
 * the downsource
 * independently.
 *
 * The counter will start counting immediately.
 */
Counter::Counter() {
  InitCounter();
}

/**
 * Create an instance of a counter from a Digital Source (such as a Digital
 * Input).
 * This is used if an existing digital input is to be shared by multiple other
 * objects such
 * as encoders or if the Digital Source is not a Digital Input channel (such as
 * an Analog Trigger).
 *
 * The counter will start counting immediately.
 * @param source A pointer to the existing DigitalSource object. It will be set
 * as the Up Source.
 */
Counter::Counter(DigitalSource *source) {
  InitCounter();
  SetUpSource(source);
  ClearDownSource();
}

/**
 * Create an instance of a counter from a Digital Source (such as a Digital
 * Input).
 * This is used if an existing digital input is to be shared by multiple other
 * objects such
 * as encoders or if the Digital Source is not a Digital Input channel (such as
 * an Analog Trigger).
 *
 * The counter will start counting immediately.
 * @param source A reference to the existing DigitalSource object. It will be
 * set as the Up Source.
 */
Counter::Counter(DigitalSource &source) {
  InitCounter();
  SetUpSource(&source);
  ClearDownSource();
}

/**
 * Create an instance of a Counter object.
 * Create an up-Counter instance given a channel.
 *
 * The counter will start counting immediately.
 * @param channel The DIO channel to use as the up source. 0-9 are on-board,
 * 10-25 are on the MXP
 */
Counter::Counter(int32_t channel) {
  InitCounter();
  SetUpSource(channel);
  ClearDownSource();
}

/**
 * Create an instance of a Counter object.
 * Create an instance of a simple up-Counter given an analog trigger.
 * Use the trigger state output from the analog trigger.
 *
 * The counter will start counting immediately.
 * @param trigger The pointer to the existing AnalogTrigger object.
 */
Counter::Counter(AnalogTrigger *trigger) {
  InitCounter();
  SetUpSource(trigger->CreateOutput(kState));
  ClearDownSource();
  m_allocatedUpSource = true;
}

/**
 * Create an instance of a Counter object.
 * Create an instance of a simple up-Counter given an analog trigger.
 * Use the trigger state output from the analog trigger.
 *
 * The counter will start counting immediately.
 * @param trigger The reference to the existing AnalogTrigger object.
 */
Counter::Counter(AnalogTrigger &trigger) {
  InitCounter();
  SetUpSource(trigger.CreateOutput(kState));
  ClearDownSource();
  m_allocatedUpSource = true;
}

/**
 * Create an instance of a Counter object.
 * Creates a full up-down counter given two Digital Sources
 * @param encodingType The quadrature decoding mode (1x or 2x)
 * @param upSource The pointer to the DigitalSource to set as the up source
 * @param downSource The pointer to the DigitalSource to set as the down source
 * @param inverted True to invert the output (reverse the direction)
 */

Counter::Counter(EncodingType encodingType, DigitalSource *upSource,
                 DigitalSource *downSource, bool inverted) {
  if (encodingType != k1X && encodingType != k2X) {
    wpi_setWPIErrorWithContext(
        ParameterOutOfRange,
        "Counter only supports 1X and 2X quadrature decoding.");
    return;
  }
  InitCounter(kExternalDirection);
  SetUpSource(upSource);
  SetDownSource(downSource);
  int32_t status = 0;

  if (encodingType == k1X) {
    SetUpSourceEdge(true, false);
    setCounterAverageSize(m_counter, 1, &status);
  } else {
    SetUpSourceEdge(true, true);
    setCounterAverageSize(m_counter, 2, &status);
  }

  wpi_setErrorWithContext(status, getHALErrorMessage(status));
  SetDownSourceEdge(inverted, true);
}

/**
 * Delete the Counter object.
 */
Counter::~Counter() {
  SetUpdateWhenEmpty(true);
  if (m_allocatedUpSource) {
    delete m_upSource;
    m_upSource = nullptr;
  }
  if (m_allocatedDownSource) {
    delete m_downSource;
    m_downSource = nullptr;
  }

  int32_t status = 0;
  freeCounter(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
  m_counter = nullptr;
}

/**
 * Set the upsource for the counter as a digital input channel.
 * @param channel The DIO channel to use as the up source. 0-9 are on-board,
 * 10-25 are on the MXP
 */
void Counter::SetUpSource(int32_t channel) {
  if (StatusIsFatal()) return;
  SetUpSource(new DigitalInput(channel));
  m_allocatedUpSource = true;
}

/**
 * Set the up counting source to be an analog trigger.
 * @param analogTrigger The analog trigger object that is used for the Up Source
 * @param triggerType The analog trigger output that will trigger the counter.
 */
void Counter::SetUpSource(AnalogTrigger *analogTrigger,
                          AnalogTriggerType triggerType) {
  if (StatusIsFatal()) return;
  SetUpSource(analogTrigger->CreateOutput(triggerType));
  m_allocatedUpSource = true;
}

/**
 * Set the up counting source to be an analog trigger.
 * @param analogTrigger The analog trigger object that is used for the Up Source
 * @param triggerType The analog trigger output that will trigger the counter.
 */
void Counter::SetUpSource(AnalogTrigger &analogTrigger,
                          AnalogTriggerType triggerType) {
  SetUpSource(&analogTrigger, triggerType);
}

/**
 * Set the source object that causes the counter to count up.
 * Set the up counting DigitalSource.
 * @param source Pointer to the DigitalSource object to set as the up source
 */
void Counter::SetUpSource(DigitalSource *source) {
  if (StatusIsFatal()) return;
  if (m_allocatedUpSource) {
    delete m_upSource;
    m_upSource = nullptr;
    m_allocatedUpSource = false;
  }
  m_upSource = source;
  if (m_upSource->StatusIsFatal()) {
    CloneError(m_upSource);
  } else {
    int32_t status = 0;
    setCounterUpSource(m_counter, source->GetChannelForRouting(),
                       source->GetAnalogTriggerForRouting(), &status);
    wpi_setErrorWithContext(status, getHALErrorMessage(status));
  }
}

/**
 * Set the source object that causes the counter to count up.
 * Set the up counting DigitalSource.
 * @param source Reference to the DigitalSource object to set as the up source
 */
void Counter::SetUpSource(DigitalSource &source) { SetUpSource(&source); }

/**
 * Set the edge sensitivity on an up counting source.
 * Set the up source to either detect rising edges or falling edges or both.
 * @param risingEdge True to trigger on rising edges
 * @param fallingEdge True to trigger on falling edges
 */
void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge) {
  if (StatusIsFatal()) return;
  if (m_upSource == nullptr) {
    wpi_setWPIErrorWithContext(
        NullParameter,
        "Must set non-nullptr UpSource before setting UpSourceEdge");
  }
  int32_t status = 0;
  setCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Disable the up counting source to the counter.
 */
void Counter::ClearUpSource() {
  if (StatusIsFatal()) return;
  if (m_allocatedUpSource) {
    delete m_upSource;
    m_upSource = nullptr;
    m_allocatedUpSource = false;
  }
  int32_t status = 0;
  clearCounterUpSource(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Set the down counting source to be a digital input channel.
 * @param channel The DIO channel to use as the up source. 0-9 are on-board,
 * 10-25 are on the MXP
 */
void Counter::SetDownSource(int32_t channel) {
  if (StatusIsFatal()) return;
  SetDownSource(new DigitalInput(channel));
  m_allocatedDownSource = true;
}

/**
 * Set the down counting source to be an analog trigger.
 * @param analogTrigger The analog trigger object that is used for the Down
 * Source
 * @param triggerType The analog trigger output that will trigger the counter.
 */
void Counter::SetDownSource(AnalogTrigger *analogTrigger,
                            AnalogTriggerType triggerType) {
  if (StatusIsFatal()) return;
  SetDownSource(analogTrigger->CreateOutput(triggerType));
  m_allocatedDownSource = true;
}

/**
 * Set the down counting source to be an analog trigger.
 * @param analogTrigger The analog trigger object that is used for the Down
 * Source
 * @param triggerType The analog trigger output that will trigger the counter.
 */
void Counter::SetDownSource(AnalogTrigger &analogTrigger,
                            AnalogTriggerType triggerType) {
  SetDownSource(&analogTrigger, triggerType);
}

/**
 * Set the source object that causes the counter to count down.
 * Set the down counting DigitalSource.
 * @param source Pointer to the DigitalSource object to set as the down source
 */
void Counter::SetDownSource(DigitalSource *source) {
  if (StatusIsFatal()) return;
  if (m_allocatedDownSource) {
    delete m_downSource;
    m_downSource = nullptr;
    m_allocatedDownSource = false;
  }
  m_downSource = source;
  if (m_downSource->StatusIsFatal()) {
    CloneError(m_downSource);
  } else {
    int32_t status = 0;
    setCounterDownSource(m_counter, source->GetChannelForRouting(),
                         source->GetAnalogTriggerForRouting(), &status);
    wpi_setErrorWithContext(status, getHALErrorMessage(status));
  }
}

/**
 * Set the source object that causes the counter to count down.
 * Set the down counting DigitalSource.
 * @param source Reference to the DigitalSource object to set as the down source
 */
void Counter::SetDownSource(DigitalSource &source) { SetDownSource(&source); }

/**
 * Set the edge sensitivity on a down counting source.
 * Set the down source to either detect rising edges or falling edges.
 * @param risingEdge True to trigger on rising edges
 * @param fallingEdge True to trigger on falling edges
 */
void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge) {
  if (StatusIsFatal()) return;
  if (m_downSource == nullptr) {
    wpi_setWPIErrorWithContext(
        NullParameter,
        "Must set non-nullptr DownSource before setting DownSourceEdge");
  }
  int32_t status = 0;
  setCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Disable the down counting source to the counter.
 */
void Counter::ClearDownSource() {
  if (StatusIsFatal()) return;
  if (m_allocatedDownSource) {
    delete m_downSource;
    m_downSource = nullptr;
    m_allocatedDownSource = false;
  }
  int32_t status = 0;
  clearCounterDownSource(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Set standard up / down counting mode on this counter.
 * Up and down counts are sourced independently from two inputs.
 */
void Counter::SetUpDownCounterMode() {
  if (StatusIsFatal()) return;
  int32_t status = 0;
  setCounterUpDownMode(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Set external direction mode on this counter.
 * Counts are sourced on the Up counter input.
 * The Down counter input represents the direction to count.
 */
void Counter::SetExternalDirectionMode() {
  if (StatusIsFatal()) return;
  int32_t status = 0;
  setCounterExternalDirectionMode(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Set Semi-period mode on this counter.
 * Counts up on both rising and falling edges.
 */
void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
  if (StatusIsFatal()) return;
  int32_t status = 0;
  setCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Configure the counter to count in up or down based on the length of the input
 * pulse.
 * This mode is most useful for direction sensitive gear tooth sensors.
 * @param threshold The pulse length beyond which the counter counts the
 * opposite direction.  Units are seconds.
 */
void Counter::SetPulseLengthMode(float threshold) {
  if (StatusIsFatal()) return;
  int32_t status = 0;
  setCounterPulseLengthMode(m_counter, threshold, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Get the Samples to Average which specifies the number of samples of the timer
 * to
 * average when calculating the period. Perform averaging to account for
 * mechanical imperfections or as oversampling to increase resolution.
 * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
 */
int Counter::GetSamplesToAverage() const {
  int32_t status = 0;
  int32_t samples = getCounterSamplesToAverage(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
  return samples;
}

/**
 * Set the Samples to Average which specifies the number of samples of the timer
 * to
 * average when calculating the period. Perform averaging to account for
 * mechanical imperfections or as oversampling to increase resolution.
 * @param samplesToAverage The number of samples to average from 1 to 127.
 */
void Counter::SetSamplesToAverage(int samplesToAverage) {
  if (samplesToAverage < 1 || samplesToAverage > 127) {
    wpi_setWPIErrorWithContext(
        ParameterOutOfRange,
        "Average counter values must be between 1 and 127");
  }
  int32_t status = 0;
  setCounterSamplesToAverage(m_counter, samplesToAverage, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Read the current counter value.
 * Read the value at this instant. It may still be running, so it reflects the
 * current value. Next
 * time it is read, it might have a different value.
 */
int32_t Counter::Get() const {
  if (StatusIsFatal()) return 0;
  int32_t status = 0;
  int32_t value = getCounter(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
  return value;
}

/**
 * Reset the Counter to zero.
 * Set the counter value to zero. This doesn't effect the running state of the
 * counter, just sets
 * the current value to zero.
 */
void Counter::Reset() {
  if (StatusIsFatal()) return;
  int32_t status = 0;
  resetCounter(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Get the Period of the most recent count.
 * Returns the time interval of the most recent count. This can be used for
 * velocity calculations
 * to determine shaft speed.
 * @returns The period between the last two pulses in units of seconds.
 */
double Counter::GetPeriod() const {
  if (StatusIsFatal()) return 0.0;
  int32_t status = 0;
  double value = getCounterPeriod(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
  return value;
}

/**
 * Set the maximum period where the device is still considered "moving".
 * Sets the maximum period where the device is considered moving. This value is
 * used to determine
 * the "stopped" state of the counter using the GetStopped method.
 * @param maxPeriod The maximum period where the counted device is considered
 * moving in
 * seconds.
 */
void Counter::SetMaxPeriod(double maxPeriod) {
  if (StatusIsFatal()) return;
  int32_t status = 0;
  setCounterMaxPeriod(m_counter, maxPeriod, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Select whether you want to continue updating the event timer output when
 * there are no samples captured.
 * The output of the event timer has a buffer of periods that are averaged and
 * posted to
 * a register on the FPGA.  When the timer detects that the event source has
 * stopped
 * (based on the MaxPeriod) the buffer of samples to be averaged is emptied.  If
 * you
 * enable the update when empty, you will be notified of the stopped source and
 * the event
 * time will report 0 samples.  If you disable update when empty, the most
 * recent average
 * will remain on the output until a new sample is acquired.  You will never see
 * 0 samples
 * output (except when there have been no events since an FPGA reset) and you
 * will likely not
 * see the stopped bit become true (since it is updated at the end of an average
 * and there are
 * no samples to average).
 * @param enabled True to enable update when empty
 */
void Counter::SetUpdateWhenEmpty(bool enabled) {
  if (StatusIsFatal()) return;
  int32_t status = 0;
  setCounterUpdateWhenEmpty(m_counter, enabled, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Determine if the clock is stopped.
 * Determine if the clocked input is stopped based on the MaxPeriod value set
 * using the
 * SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the device (and
 * counter) are
 * assumed to be stopped and it returns true.
 * @return Returns true if the most recent counter period exceeds the MaxPeriod
 * value set by
 * SetMaxPeriod.
 */
bool Counter::GetStopped() const {
  if (StatusIsFatal()) return false;
  int32_t status = 0;
  bool value = getCounterStopped(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
  return value;
}

/**
 * The last direction the counter value changed.
 * @return The last direction the counter value changed.
 */
bool Counter::GetDirection() const {
  if (StatusIsFatal()) return false;
  int32_t status = 0;
  bool value = getCounterDirection(m_counter, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
  return value;
}

/**
 * Set the Counter to return reversed sensing on the direction.
 * This allows counters to change the direction they are counting in the case of
 * 1X and 2X
 * quadrature encoding only. Any other counter mode isn't supported.
 * @param reverseDirection true if the value counted should be negated.
 */
void Counter::SetReverseDirection(bool reverseDirection) {
  if (StatusIsFatal()) return;
  int32_t status = 0;
  setCounterReverseDirection(m_counter, reverseDirection, &status);
  wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

void Counter::UpdateTable() {
  if (m_table != nullptr) {
    m_table->PutNumber("Value", Get());
  }
}

void Counter::StartLiveWindowMode() {}

void Counter::StopLiveWindowMode() {}

std::string Counter::GetSmartDashboardType() const { return "Counter"; }

void Counter::InitTable(ITable *subTable) {
  m_table = subTable;
  UpdateTable();
}

ITable *Counter::GetTable() const { return m_table; }