export interface PIDParameters {
  kP: number;
  kI: number;
  kD: number;
  calculateError?: (setpoint: number, measuredValue: number) => number;
}

export type PIDReport = {
  timeMs: number;
  measured: number;
  setpoint: number;
  output: number;
  error: number;
};

export type PIDCallback = (report: PIDReport) => any;

export class PID {
  private params: PIDParameters;
  private setpoint: number;
  private prevError: number = 0;
  private integral: number = 0;
  private lastTime: number | null = null;
  private handler: PIDCallback | null = null;

  constructor(params: PIDParameters, setpoint: number = 0) {
    this.params = Object.assign({}, params);
    this.setpoint = setpoint;
  }

  public setParameters(params: PIDParameters) {
    this.params = Object.assign({}, params);
  }

  public setSetpoint(setpoint: number): void {
    this.setpoint = setpoint;
    this.prevError = 0;
    this.integral = 0;
    this.lastTime = null;
  }

  public setCallback(handler: PIDCallback) {
    this.handler = handler;
  }

  public update(measuredValue: number): number {
    const currentTime = os.epoch("utc");

    const error = this.params.calculateError
      ? this.params.calculateError(this.setpoint, measuredValue)
      : this.setpoint - measuredValue;

    let deltaTime = 0;
    if (this.lastTime !== null) {
      deltaTime = (currentTime - this.lastTime) / 1000; // in seconds
    }

    this.integral += error * deltaTime;
    const derivative = deltaTime > 0 ? (error - this.prevError) / deltaTime : 0;

    const output =
      this.params.kP * error +
      this.params.kI * this.integral +
      this.params.kD * derivative;

    this.prevError = error;
    this.lastTime = currentTime;

    try {
      if (this.handler != null)
        this.handler({
          timeMs: currentTime,
          measured: measuredValue,
          setpoint: this.setpoint,
          output,
          error,
        });
    } catch (e) {
      print(e);
    }

    return output;
  }
}