// Copyright 2020 The Fuchsia Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "src/media/audio/lib/clock/pid_control.h"

#include <lib/syslog/cpp/macros.h>

#include <cmath>

#include <gtest/gtest.h>

namespace media::audio::clock {
namespace {

class PidControlTest : public testing::Test {
 protected:
  static void VerifyProportionalOnly(const double pFactor) {
    auto control = PidControl({.proportional_factor = pFactor});
    control.Start(zx::time(100));

    control.TuneForError(zx::time(110), 50);
    EXPECT_FLOAT_EQ(control.Read(), 50 * pFactor);

    control.TuneForError(zx::time(125), -10);
    EXPECT_FLOAT_EQ(control.Read(), -10 * pFactor);

    control.TuneForError(zx::time(130), 20);
    EXPECT_FLOAT_EQ(control.Read(), 20 * pFactor);
  }

  static void VerifyIntegralOnly(const double iFactor) {
    auto control = PidControl({.integral_factor = iFactor});
    auto expected = 0.0;

    zx::time previous_time{0};
    control.Start(previous_time);

    auto tune_time = previous_time + zx::duration(10);
    // curr_err=50, dur=10: accum_err+=500
    control.TuneForError(tune_time, 50);

    // From this, we expect error to change by 50*t*I
    expected += 50.0 * iFactor * (tune_time - previous_time).get();
    EXPECT_FLOAT_EQ(control.Read(), expected);

    previous_time = tune_time;
    tune_time += zx::duration(15);
    // curr_err=-100, dur=15: accum_err-=1500 (now -1000)
    control.TuneForError(tune_time, -100);

    // From this, we expect error to change by -100*t*I
    expected += -100.0 * iFactor * (tune_time - previous_time).get();
    EXPECT_FLOAT_EQ(control.Read(), expected);

    previous_time = tune_time;
    tune_time += zx::duration(25);
    // curr_err=40, dur=25: accum_err+=1000 (now 0)
    control.TuneForError(tune_time, 40);

    // From this, we expect error to change by 0*t*I -- to be zero!
    expected += 40.0 * iFactor * (tune_time - previous_time).get();
    EXPECT_FLOAT_EQ(control.Read(), expected);
    EXPECT_FLOAT_EQ(expected, 0.0);
  }

  static void VerifyDerivativeOnly(double dFactor) {
    auto control = PidControl({.derivative_factor = dFactor});
    double error, previous_error;
    double error_rate;

    zx::time previous_time, tune_time{0};
    control.Start(tune_time);

    previous_time = tune_time;
    tune_time += zx::duration(10);
    previous_error = 0;
    error = 50;
    // curr_err=50; prev_err=0; delta_err=50; dur=10; err_rate=50/10
    control.TuneForError(tune_time, error);

    error_rate = (error - previous_error) / (tune_time - previous_time).get();
    // Reset error to 0 at t=10, from here we expect error to change by 5
    EXPECT_FLOAT_EQ(control.Read(), dFactor * error_rate);

    previous_time = tune_time;
    tune_time += zx::duration(5);
    previous_error = error;
    error = 15;
    // curr_err=20; prev_err=50; delta_err=-30; dur=5; err_rate=-30/5
    control.TuneForError(tune_time, error);

    error_rate = (error - previous_error) / (tune_time - previous_time).get();
    // Now we expect error to change by -6
    EXPECT_FLOAT_EQ(control.Read(), dFactor * error_rate);

    previous_time = tune_time;
    tune_time += zx::duration(20);
    previous_error = error;
    error = 30;
    // curr_err=30; prev_err=20; delta_err=10; dur=20; err_rate=10/20
    control.TuneForError(tune_time, error);

    error_rate = (error - previous_error) / (tune_time - previous_time).get();
    // Now we expect error to change by 0.5
    EXPECT_FLOAT_EQ(control.Read(), dFactor * error_rate);
  }

  static void SmoothlyChaseToClockRate(const int32_t rate_adjust_ppm,
                                       const uint32_t num_iterations_limit) {
    // These PID factors were determined experimentally, from manual tuning and rule-of-thumb
    constexpr double kPfactor = 0.1;
    constexpr double kIfactor = kPfactor * 2 / ZX_MSEC(20);
    constexpr double kDfactor = kPfactor * ZX_MSEC(20) / 16;

    auto control = PidControl({.proportional_factor = kPfactor,
                               .integral_factor = kIfactor,
                               .derivative_factor = kDfactor});

    constexpr auto kIterationTimeslice = zx::msec(10);
    const auto ref_rate = static_cast<double>(1'000'000 + rate_adjust_ppm) / 1'000'000;

    zx::time rate_change_mono_time = zx::time(0) + zx::sec(1);
    zx::time rate_change_ref_time = zx::time(0) + zx::sec(11);

    control.Start(zx::time(0));
    control.TuneForError(rate_change_mono_time, 0);

    auto num_iterations = 0u;
    uint32_t first_accurate_prediction = UINT32_MAX;
    uint32_t consecutive_prediction = UINT32_MAX;
    bool previous_prediction_accurate = false;

    zx::time previous_ref_time = rate_change_ref_time;
    for (zx::time mono_time = rate_change_mono_time + zx::msec(10);
         mono_time < (zx::time(0) + zx::sec(2)); mono_time += kIterationTimeslice) {
      ++num_iterations;

      auto predict_ppm = static_cast<int64_t>(round(control.Read()));
      predict_ppm = std::clamp<int64_t>(predict_ppm, -1000, +1000);

      if (predict_ppm == rate_adjust_ppm) {
        if (previous_prediction_accurate && consecutive_prediction > num_iterations) {
          consecutive_prediction = num_iterations;
          break;
        }
        previous_prediction_accurate = true;
        if (first_accurate_prediction > num_iterations) {
          first_accurate_prediction = num_iterations;
        }
      } else {
        previous_prediction_accurate = false;
      }

      zx::time predict_ref_time =
          previous_ref_time + (kIterationTimeslice * (1'000'000 + predict_ppm)) / 1'000'000;
      zx::time ref_time =
          rate_change_ref_time + zx::duration((mono_time - rate_change_mono_time).get() * ref_rate);

      control.TuneForError(mono_time, (ref_time - predict_ref_time).get());
      previous_ref_time = predict_ref_time;
    }

    EXPECT_LE(first_accurate_prediction, num_iterations_limit - 3)
        << "PidControl took too long to initially settle";
    EXPECT_LE(consecutive_prediction, num_iterations_limit)
        << "PidControl took too long to finally settle";
  }
};

// Validate default ctor sends parameters of 0
TEST_F(PidControlTest, Static) {
  auto control = PidControl();
  EXPECT_EQ(control.Read(), 0);

  control.Start(zx::time(100));
  EXPECT_EQ(control.Read(), 0);

  control.TuneForError(zx::time(125), 500);
  EXPECT_EQ(control.Read(), 0);
}

// If only Proportional, after each Tune we predict exactly that that error.
TEST_F(PidControlTest, Proportional) {
  VerifyProportionalOnly(1.0);
  VerifyProportionalOnly(0.5);
  VerifyProportionalOnly(0.01);
}

// If only Integral, after each Tune we predict based on accumulated error over time
TEST_F(PidControlTest, Integral) {
  VerifyIntegralOnly(1.0);
  VerifyIntegralOnly(0.2);
  VerifyIntegralOnly(0.001);
}

// If only Derivative, after each Tune we predict based on the change in error
TEST_F(PidControlTest, Derivative) {
  VerifyDerivativeOnly(1.0);
  VerifyDerivativeOnly(4.0);
  VerifyDerivativeOnly(0.0001);
}

// Start sets the control's initial time, resetting other vlaues to zero.
TEST_F(PidControlTest, NoStart) {
  auto control = PidControl({.derivative_factor = 1.0});

  // start_time is implicitly 0, so control.Read will base its extrapolation(time=300)
  // across the previous tuning which had a duration of 150, thus control=(150-0)/(150-0) = 1.
  control.TuneForError(zx::time(150), 150);
  EXPECT_EQ(control.Read(), 1);

  control.Start(zx::time(100));
  // start_time is 100, so control.Read will base its extrapolation(time=300)
  // across the previous tuning which had a duration of 50, thus control=(150-0)/(150-100) = 3.
  control.TuneForError(zx::time(150), 150);
  EXPECT_EQ(control.Read(), 3);
}

// Briefly validate PI with literal values
TEST_F(PidControlTest, ProportionalIntegral) {
  auto control = PidControl({.proportional_factor = 1.0, .integral_factor = 1.0});

  control.Start(zx::time(0));
  // Expect 0, was 50: curr_err_=50, dur=10: accum_err+=500 (now 500)
  control.TuneForError(zx::time(10), 50);

  // From this  we expect error (50+500)=550
  EXPECT_EQ(control.Read(), 550);

  // Expect 550, was 500: curr_err=-50, dur=15: accum_err-=750 (now -250)
  control.TuneForError(zx::time(25), -50);

  // From this, we expect error -50-250)=-300
  EXPECT_EQ(control.Read(), -300);

  // Expect -300, was -250: curr_err=50, dur=25: accum_err+=1250 (now 1000)
  control.TuneForError(zx::time(50), 50);

  // From this, we expect error 50+1000=1050
  EXPECT_EQ(control.Read(), 1050);
}

// Briefly validate full PID with literal values
TEST_F(PidControlTest, FullPid) {
  auto control =
      PidControl({.proportional_factor = 1.0, .integral_factor = 1.0, .derivative_factor = 1.0});

  control.Start(zx::time(0));
  // curr_err_=50, dur=10: accum_err+=500 (now 500)
  // prev_err=0; delta_err=50; err_rate=50/10=5
  control.TuneForError(zx::time(10), 50);

  // Now expect error 50+500+5
  EXPECT_EQ(control.Read(), 555);  // 50 + 500 + 5

  // curr_err=-200 (for example, expected output 600 but actual 400), dur=10:
  // accum_err-=2000 (now -1500) prev_err=50; delta_err=-250; err_rate=-250/10=-25
  control.TuneForError(zx::time(20), -200);

  // Now expect error -200-1500-25
  EXPECT_EQ(control.Read(), -1725);  // -200 + -1500 - 25

  // curr_err=50, dur=25: accum_err+=1250 (now -250)
  // prev_err=-200; delta_err=250; err_rate= 250/25=10
  control.TuneForError(zx::time(45), 50);

  // Now expect error 50-1000+10
  EXPECT_EQ(control.Read(), -190);  // 50 - 250 + 10
}

TEST_F(PidControlTest, RealWorld) {
  SmoothlyChaseToClockRate(+1, 6);
  SmoothlyChaseToClockRate(-1, 6);

  SmoothlyChaseToClockRate(+10, 10);
  SmoothlyChaseToClockRate(-10, 10);

  SmoothlyChaseToClockRate(+100, 20);
  SmoothlyChaseToClockRate(-100, 20);

  SmoothlyChaseToClockRate(+950, 55);
  SmoothlyChaseToClockRate(-950, 55);
}

}  // namespace
}  // namespace media::audio::clock