'use strict';

/**
 * Production-focused discrete PID controller with modern control features:
 * - auto/manual and bumpless transfer
 * - freeze/unfreeze (hold output while optionally tracking process)
 * - derivative filtering and derivative-on-measurement/error
 * - anti-windup (clamp or back-calculation)
 * - output and integral limits
 * - output rate limiting
 * - deadband
 * - gain scheduling (array/function)
 * - feedforward and dynamic tunings at runtime
 */
class PIDController {
  constructor(options = {}) {
    const {
      kp = 1,
      ki = 0,
      kd = 0,
      sampleTime = 1000,
      derivativeFilter = 0.15,
      outputMin = Number.NEGATIVE_INFINITY,
      outputMax = Number.POSITIVE_INFINITY,
      integralMin = null,
      integralMax = null,
      derivativeOnMeasurement = true,
      setpointWeight = 1,
      derivativeWeight = 0,
      deadband = 0,
      outputRateLimitUp = Number.POSITIVE_INFINITY,
      outputRateLimitDown = Number.POSITIVE_INFINITY,
      antiWindupMode = 'clamp',
      backCalculationGain = 0,
      gainSchedule = null,
      autoMode = true,
      trackOnManual = true,
      frozen = false,
      freezeTrackMeasurement = true,
      freezeTrackError = false,
    } = options;

    this.kp = 0;
    this.ki = 0;
    this.kd = 0;

    this.setTunings({ kp, ki, kd });
    this.setSampleTime(sampleTime);
    this.setOutputLimits(outputMin, outputMax);
    this.setIntegralLimits(integralMin, integralMax);
    this.setDerivativeFilter(derivativeFilter);
    this.setSetpointWeights({ beta: setpointWeight, gamma: derivativeWeight });
    this.setDeadband(deadband);
    this.setOutputRateLimits(outputRateLimitUp, outputRateLimitDown);
    this.setAntiWindup({ mode: antiWindupMode, backCalculationGain });
    this.setGainSchedule(gainSchedule);

    this.derivativeOnMeasurement = Boolean(derivativeOnMeasurement);
    this.autoMode = Boolean(autoMode);
    this.trackOnManual = Boolean(trackOnManual);

    this.frozen = Boolean(frozen);
    this.freezeTrackMeasurement = Boolean(freezeTrackMeasurement);
    this.freezeTrackError = Boolean(freezeTrackError);

    this.reset();
  }

  setTunings({ kp = this.kp, ki = this.ki, kd = this.kd } = {}) {
    [kp, ki, kd].forEach((gain, index) => {
      if (!Number.isFinite(gain)) {
        const label = ['kp', 'ki', 'kd'][index];
        throw new TypeError(`${label} must be a finite number`);
      }
    });

    this.kp = kp;
    this.ki = ki;
    this.kd = kd;
    return this;
  }

  setSampleTime(sampleTimeMs = this.sampleTime) {
    if (!Number.isFinite(sampleTimeMs) || sampleTimeMs <= 0) {
      throw new RangeError('sampleTime must be a positive number of milliseconds');
    }

    this.sampleTime = sampleTimeMs;
    return this;
  }

  setOutputLimits(min = this.outputMin, max = this.outputMax) {
    if (!Number.isFinite(min) && min !== Number.NEGATIVE_INFINITY) {
      throw new TypeError('outputMin must be finite or -Infinity');
    }
    if (!Number.isFinite(max) && max !== Number.POSITIVE_INFINITY) {
      throw new TypeError('outputMax must be finite or Infinity');
    }
    if (min >= max) {
      throw new RangeError('outputMin must be smaller than outputMax');
    }

    this.outputMin = min;
    this.outputMax = max;
    this.lastOutput = this._clamp(this.lastOutput ?? 0, this.outputMin, this.outputMax);
    return this;
  }

  setIntegralLimits(min = this.integralMin ?? null, max = this.integralMax ?? null) {
    if (min !== null && !Number.isFinite(min)) {
      throw new TypeError('integralMin must be null or a finite number');
    }
    if (max !== null && !Number.isFinite(max)) {
      throw new TypeError('integralMax must be null or a finite number');
    }
    if (min !== null && max !== null && min > max) {
      throw new RangeError('integralMin must be smaller than integralMax');
    }

    this.integralMin = min;
    this.integralMax = max;
    this.integral = this._applyIntegralLimits(this.integral ?? 0);
    return this;
  }

  setDerivativeFilter(value = this.derivativeFilter ?? 0) {
    if (!Number.isFinite(value) || value < 0 || value > 1) {
      throw new RangeError('derivativeFilter must be between 0 and 1');
    }

    this.derivativeFilter = value;
    return this;
  }

  setSetpointWeights({ beta = this.setpointWeight ?? 1, gamma = this.derivativeWeight ?? 0 } = {}) {
    if (!Number.isFinite(beta) || !Number.isFinite(gamma)) {
      throw new TypeError('setpoint and derivative weights must be finite numbers');
    }

    this.setpointWeight = beta;
    this.derivativeWeight = gamma;
    return this;
  }

  setDeadband(value = this.deadband ?? 0) {
    if (!Number.isFinite(value) || value < 0) {
      throw new RangeError('deadband must be a non-negative finite number');
    }

    this.deadband = value;
    return this;
  }

  setOutputRateLimits(up = this.outputRateLimitUp, down = this.outputRateLimitDown) {
    if (!Number.isFinite(up) && up !== Number.POSITIVE_INFINITY) {
      throw new TypeError('outputRateLimitUp must be finite or Infinity');
    }
    if (!Number.isFinite(down) && down !== Number.POSITIVE_INFINITY) {
      throw new TypeError('outputRateLimitDown must be finite or Infinity');
    }
    if (up <= 0 || down <= 0) {
      throw new RangeError('output rate limits must be positive values');
    }

    this.outputRateLimitUp = up;
    this.outputRateLimitDown = down;
    return this;
  }

  setAntiWindup({ mode = this.antiWindupMode ?? 'clamp', backCalculationGain = this.backCalculationGain ?? 0 } = {}) {
    const normalized = String(mode || 'clamp').trim().toLowerCase();
    if (normalized !== 'clamp' && normalized !== 'backcalc') {
      throw new RangeError('anti windup mode must be "clamp" or "backcalc"');
    }
    if (!Number.isFinite(backCalculationGain) || backCalculationGain < 0) {
      throw new RangeError('backCalculationGain must be a non-negative finite number');
    }

    this.antiWindupMode = normalized;
    this.backCalculationGain = backCalculationGain;
    return this;
  }

  /**
   * Gain schedule options:
   * - null: disabled
   * - function(input, state) => { kp, ki, kd }
   * - array: [{ min, max, kp, ki, kd }, ...]
   */
  setGainSchedule(schedule = null) {
    if (schedule == null) {
      this.gainSchedule = null;
      return this;
    }

    if (typeof schedule === 'function') {
      this.gainSchedule = schedule;
      return this;
    }

    if (!Array.isArray(schedule)) {
      throw new TypeError('gainSchedule must be null, a function, or an array');
    }

    schedule.forEach((entry, index) => {
      if (!entry || typeof entry !== 'object') {
        throw new TypeError(`gainSchedule[${index}] must be an object`);
      }
      const { min = Number.NEGATIVE_INFINITY, max = Number.POSITIVE_INFINITY, kp, ki, kd } = entry;
      if (!Number.isFinite(min) && min !== Number.NEGATIVE_INFINITY) {
        throw new TypeError(`gainSchedule[${index}].min must be finite or -Infinity`);
      }
      if (!Number.isFinite(max) && max !== Number.POSITIVE_INFINITY) {
        throw new TypeError(`gainSchedule[${index}].max must be finite or Infinity`);
      }
      if (min >= max) {
        throw new RangeError(`gainSchedule[${index}] min must be smaller than max`);
      }
      [kp, ki, kd].forEach((value, gainIndex) => {
        const label = ['kp', 'ki', 'kd'][gainIndex];
        if (!Number.isFinite(value)) {
          throw new TypeError(`gainSchedule[${index}].${label} must be finite`);
        }
      });
    });

    this.gainSchedule = schedule;
    return this;
  }

  setMode(mode, options = {}) {
    if (mode !== 'automatic' && mode !== 'manual') {
      throw new Error('mode must be either "automatic" or "manual"');
    }

    const nextAuto = mode === 'automatic';
    const previousAuto = this.autoMode;
    this.autoMode = nextAuto;

    if (options && Number.isFinite(options.manualOutput)) {
      this.setManualOutput(options.manualOutput);
    }

    if (!previousAuto && nextAuto) {
      this._initializeForAuto(options);
    }

    return this;
  }

  freeze(options = {}) {
    this.frozen = true;
    this.freezeTrackMeasurement = options.trackMeasurement !== false;
    this.freezeTrackError = Boolean(options.trackError);

    if (Number.isFinite(options.output)) {
      this.setManualOutput(options.output);
    }

    return this;
  }

  unfreeze() {
    this.frozen = false;
    return this;
  }

  isFrozen() {
    return this.frozen;
  }

  setManualOutput(value) {
    this._assertNumeric('manual output', value);
    this.lastOutput = this._clamp(value, this.outputMin, this.outputMax);
    return this.lastOutput;
  }

  reset(state = {}) {
    const {
      integral = 0,
      lastOutput = 0,
      timestamp = null,
      prevMeasurement = null,
      prevError = null,
      prevDerivativeInput = null,
      derivativeState = 0,
    } = state;

    this.integral = this._applyIntegralLimits(Number.isFinite(integral) ? integral : 0);
    this.prevError = Number.isFinite(prevError) ? prevError : null;
    this.prevMeasurement = Number.isFinite(prevMeasurement) ? prevMeasurement : null;
    this.prevDerivativeInput = Number.isFinite(prevDerivativeInput) ? prevDerivativeInput : null;
    this.lastOutput = this._clamp(
      Number.isFinite(lastOutput) ? lastOutput : 0,
      this.outputMin ?? Number.NEGATIVE_INFINITY,
      this.outputMax ?? Number.POSITIVE_INFINITY
    );
    this.lastTimestamp = Number.isFinite(timestamp) ? timestamp : null;
    this.derivativeState = Number.isFinite(derivativeState) ? derivativeState : 0;

    return this;
  }

  update(setpoint, measurement, timestamp = Date.now(), options = {}) {
    if (timestamp && typeof timestamp === 'object' && options && Object.keys(options).length === 0) {
      options = timestamp;
      timestamp = Date.now();
    }

    this._assertNumeric('setpoint', setpoint);
    this._assertNumeric('measurement', measurement);
    this._assertNumeric('timestamp', timestamp);

    const opts = options || {};

    if (opts.tunings && typeof opts.tunings === 'object') {
      this.setTunings(opts.tunings);
    }

    if (Number.isFinite(opts.gainInput)) {
      this._applyGainSchedule(opts.gainInput, { setpoint, measurement, timestamp });
    }

    if (typeof opts.setMode === 'string') {
      this.setMode(opts.setMode, opts);
    }

    if (opts.freeze === true) this.freeze(opts);
    if (opts.unfreeze === true) this.unfreeze();

    if (Number.isFinite(opts.manualOutput)) {
      this.setManualOutput(opts.manualOutput);
    }

    const feedForward = Number.isFinite(opts.feedForward) ? opts.feedForward : 0;
    const force = Boolean(opts.force);

    const error = setpoint - measurement;

    if (!this.autoMode) {
      if (this.trackOnManual) {
        this._trackProcessState(setpoint, measurement, error, timestamp);
      }
      return this.lastOutput;
    }

    if (this.frozen) {
      if (this.freezeTrackMeasurement || this.freezeTrackError) {
        this._trackProcessState(setpoint, measurement, error, timestamp, {
          trackMeasurement: this.freezeTrackMeasurement,
          trackError: this.freezeTrackError,
        });
      }
      return this.lastOutput;
    }

    if (!force && this.lastTimestamp !== null && (timestamp - this.lastTimestamp) < this.sampleTime) {
      return this.lastOutput;
    }

    const elapsedMs = this.lastTimestamp === null ? this.sampleTime : (timestamp - this.lastTimestamp);
    const dtSeconds = Math.max(elapsedMs / 1000, Number.EPSILON);

    const inDeadband = Math.abs(error) <= this.deadband;
    if (inDeadband) {
      this.prevError = error;
      this.prevMeasurement = measurement;
      this.prevDerivativeInput = this.derivativeOnMeasurement
        ? measurement
        : ((this.derivativeWeight * setpoint) - measurement);
      this.lastTimestamp = timestamp;
      return this.lastOutput;
    }

    const effectiveError = error;

    const pInput = (this.setpointWeight * setpoint) - measurement;
    const pTerm = this.kp * pInput;

    const derivativeRaw = this._computeDerivative({ setpoint, measurement, error, dtSeconds });
    this.derivativeState = this.derivativeFilter === 0
      ? derivativeRaw
      : this.derivativeState + (derivativeRaw - this.derivativeState) * (1 - this.derivativeFilter);

    const dTerm = this.kd * this.derivativeState;

    const nextIntegral = this._applyIntegralLimits(this.integral + (effectiveError * dtSeconds));
    let unclampedOutput = pTerm + (this.ki * nextIntegral) + dTerm + feedForward;
    let clampedOutput = this._clamp(unclampedOutput, this.outputMin, this.outputMax);

    if (this.antiWindupMode === 'backcalc' && this.ki !== 0 && this.backCalculationGain > 0) {
      const correctedIntegral = nextIntegral + ((clampedOutput - unclampedOutput) * this.backCalculationGain * dtSeconds);
      this.integral = this._applyIntegralLimits(correctedIntegral);
    } else {
      const saturatingHigh = clampedOutput >= this.outputMax && effectiveError > 0;
      const saturatingLow = clampedOutput <= this.outputMin && effectiveError < 0;
      this.integral = (saturatingHigh || saturatingLow) ? this.integral : nextIntegral;
    }

    let output = pTerm + (this.ki * this.integral) + dTerm + feedForward;
    output = this._clamp(output, this.outputMin, this.outputMax);

    if (this.lastTimestamp !== null) {
      output = this._applyRateLimit(output, this.lastOutput, dtSeconds);
    }

    if (Number.isFinite(opts.trackingOutput)) {
      this._trackIntegralToOutput(opts.trackingOutput, { pTerm, dTerm, feedForward });
      output = this._clamp(opts.trackingOutput, this.outputMin, this.outputMax);
    }

    this.lastOutput = output;
    this.prevError = error;
    this.prevMeasurement = measurement;
    this.prevDerivativeInput = this.derivativeOnMeasurement
      ? measurement
      : ((this.derivativeWeight * setpoint) - measurement);
    this.lastTimestamp = timestamp;

    return this.lastOutput;
  }

  getState() {
    return {
      kp: this.kp,
      ki: this.ki,
      kd: this.kd,
      sampleTime: this.sampleTime,
      outputLimits: { min: this.outputMin, max: this.outputMax },
      integralLimits: { min: this.integralMin, max: this.integralMax },
      derivativeFilter: this.derivativeFilter,
      derivativeOnMeasurement: this.derivativeOnMeasurement,
      setpointWeight: this.setpointWeight,
      derivativeWeight: this.derivativeWeight,
      deadband: this.deadband,
      outputRateLimits: { up: this.outputRateLimitUp, down: this.outputRateLimitDown },
      antiWindupMode: this.antiWindupMode,
      backCalculationGain: this.backCalculationGain,
      autoMode: this.autoMode,
      frozen: this.frozen,
      integral: this.integral,
      derivativeState: this.derivativeState,
      lastOutput: this.lastOutput,
      lastTimestamp: this.lastTimestamp,
    };
  }

  getLastOutput() {
    return this.lastOutput;
  }

  _initializeForAuto(options = {}) {
    const setpoint = Number.isFinite(options.setpoint) ? options.setpoint : null;
    const measurement = Number.isFinite(options.measurement) ? options.measurement : null;
    const timestamp = Number.isFinite(options.timestamp) ? options.timestamp : Date.now();

    if (measurement !== null) {
      this.prevMeasurement = measurement;
    }
    if (setpoint !== null && measurement !== null) {
      this.prevError = setpoint - measurement;
      this.prevDerivativeInput = this.derivativeOnMeasurement
        ? measurement
        : ((this.derivativeWeight * setpoint) - measurement);
    }

    this.lastTimestamp = timestamp;

    if (this.ki !== 0 && setpoint !== null && measurement !== null) {
      const pTerm = this.kp * ((this.setpointWeight * setpoint) - measurement);
      const dTerm = this.kd * this.derivativeState;
      const trackedIntegral = (this.lastOutput - pTerm - dTerm) / this.ki;
      this.integral = this._applyIntegralLimits(Number.isFinite(trackedIntegral) ? trackedIntegral : this.integral);
    }
  }

  _trackProcessState(setpoint, measurement, error, timestamp, tracking = {}) {
    const trackMeasurement = tracking.trackMeasurement !== false;
    const trackError = Boolean(tracking.trackError);

    if (trackMeasurement) {
      this.prevMeasurement = measurement;
      this.prevDerivativeInput = this.derivativeOnMeasurement
        ? measurement
        : ((this.derivativeWeight * setpoint) - measurement);
    }

    if (trackError) {
      this.prevError = error;
    }

    this.lastTimestamp = timestamp;
  }

  _trackIntegralToOutput(trackingOutput, terms) {
    if (this.ki === 0) return;
    const { pTerm, dTerm, feedForward } = terms;
    const targetIntegral = (trackingOutput - pTerm - dTerm - feedForward) / this.ki;
    if (Number.isFinite(targetIntegral)) {
      this.integral = this._applyIntegralLimits(targetIntegral);
    }
  }

  _applyGainSchedule(input, state) {
    if (!this.gainSchedule) return;

    if (typeof this.gainSchedule === 'function') {
      const tunings = this.gainSchedule(input, this.getState(), state);
      if (tunings && typeof tunings === 'object') {
        this.setTunings(tunings);
      }
      return;
    }

    const matched = this.gainSchedule.find((entry) => input >= entry.min && input < entry.max);
    if (matched) {
      this.setTunings({ kp: matched.kp, ki: matched.ki, kd: matched.kd });
    }
  }

  _computeDerivative({ setpoint, measurement, error, dtSeconds }) {
    if (!(dtSeconds > 0) || !Number.isFinite(dtSeconds)) {
      return 0;
    }

    if (this.derivativeOnMeasurement) {
      if (this.prevMeasurement === null) return 0;
      return -(measurement - this.prevMeasurement) / dtSeconds;
    }

    const derivativeInput = (this.derivativeWeight * setpoint) - measurement;
    if (this.prevDerivativeInput === null) return 0;
    const derivativeFromInput = (derivativeInput - this.prevDerivativeInput) / dtSeconds;

    if (Number.isFinite(derivativeFromInput)) {
      return derivativeFromInput;
    }

    if (this.prevError === null) return 0;
    return (error - this.prevError) / dtSeconds;
  }

  _applyRateLimit(nextOutput, previousOutput, dtSeconds) {
    const maxRise = Number.isFinite(this.outputRateLimitUp)
      ? this.outputRateLimitUp * dtSeconds
      : Number.POSITIVE_INFINITY;
    const maxFall = Number.isFinite(this.outputRateLimitDown)
      ? this.outputRateLimitDown * dtSeconds
      : Number.POSITIVE_INFINITY;

    const lower = previousOutput - maxFall;
    const upper = previousOutput + maxRise;
    return this._clamp(nextOutput, lower, upper);
  }

  _applyIntegralLimits(value) {
    if (!Number.isFinite(value)) {
      return 0;
    }

    let result = value;
    if (this.integralMin !== null && result < this.integralMin) {
      result = this.integralMin;
    }
    if (this.integralMax !== null && result > this.integralMax) {
      result = this.integralMax;
    }
    return result;
  }

  _assertNumeric(label, value) {
    if (!Number.isFinite(value)) {
      throw new TypeError(`${label} must be a finite number`);
    }
  }

  _clamp(value, min, max) {
    if (value < min) return min;
    if (value > max) return max;
    return value;
  }
}

/**
 * Cascade PID utility:
 * - primary PID controls the outer variable
 * - primary output becomes setpoint for secondary PID
 */
class CascadePIDController {
  constructor(options = {}) {
    const {
      primary = {},
      secondary = {},
    } = options;

    this.primary = primary instanceof PIDController ? primary : new PIDController(primary);
    this.secondary = secondary instanceof PIDController ? secondary : new PIDController(secondary);
  }

  update({
    setpoint,
    primaryMeasurement,
    secondaryMeasurement,
    timestamp = Date.now(),
    primaryOptions = {},
    secondaryOptions = {},
  } = {}) {
    if (!Number.isFinite(setpoint)) {
      throw new TypeError('setpoint must be a finite number');
    }
    if (!Number.isFinite(primaryMeasurement)) {
      throw new TypeError('primaryMeasurement must be a finite number');
    }
    if (!Number.isFinite(secondaryMeasurement)) {
      throw new TypeError('secondaryMeasurement must be a finite number');
    }

    const secondarySetpoint = this.primary.update(setpoint, primaryMeasurement, timestamp, primaryOptions);
    const controlOutput = this.secondary.update(secondarySetpoint, secondaryMeasurement, timestamp, secondaryOptions);

    return {
      primaryOutput: secondarySetpoint,
      secondaryOutput: controlOutput,
      state: this.getState(),
    };
  }

  setMode(mode, options = {}) {
    this.primary.setMode(mode, options.primary || options);
    this.secondary.setMode(mode, options.secondary || options);
    return this;
  }

  freeze(options = {}) {
    this.primary.freeze(options.primary || options);
    this.secondary.freeze(options.secondary || options);
    return this;
  }

  unfreeze() {
    this.primary.unfreeze();
    this.secondary.unfreeze();
    return this;
  }

  reset(state = {}) {
    this.primary.reset(state.primary || {});
    this.secondary.reset(state.secondary || {});
    return this;
  }

  getState() {
    return {
      primary: this.primary.getState(),
      secondary: this.secondary.getState(),
    };
  }
}

module.exports = {
  PIDController,
  CascadePIDController,
};