//
// Tune PID control parameters
//
// Copyright 2026, Jakob Østergaard Hegelund <joe@unthought.net>
// -----------------------------------------------------------------------
// This file is part of FanMan.
//
// FanMan is free software: you can redistribute it and/or modify it
// under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// FanMan is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FanMan. If not, see <https://www.gnu.org/licenses/>.
// -----------------------------------------------------------------------
//

#include <exception>
#include <iostream>
#include <sstream>
#include <fstream>
#include <chrono>
#include <thread>
#include <optional>
#include <vector>
#include <deque>
#include <array>
#include <cmath>
#include <cstring>
#include <numbers>
#include <algorithm>
#include "cfgparser.hh"
#include "pid.hh"
#include "pwm.hh"

class CPUBurn {
public:
  void enable();
  void disable();
  ~CPUBurn();
private:
  std::vector<std::thread> m_threads;
  std::atomic<bool> m_quit;
  std::mutex m_entry;
  void burner();
};


int main(int argc, char **argv) try
{
  CfgParameter<std::string> temp_sensor("temp_sensor", "/sys/class/thermal/thermal_zone0/temp");
  CfgParameter<size_t> pwm_chip("pwm_chip", 0);
  CfgParameter<size_t> pwm_channel("pwm_channel", 0);
  CfgParameter<size_t> pwm_period("pwm_period", 5000); // 200kHz
  CfgParameter<double> ideal_temp("ideal_temp", 48000); // 48°C
  if (argc != 2)
    throw std::runtime_error("Expected configuration file argument");

  CfgParser cp(argv[1],
               {&temp_sensor,
                &pwm_chip,
                &pwm_channel,
                &pwm_period,
                &ideal_temp });
  cp.ignore_unknown_parameters();
  cp.reload();

  // Open temperature input
  std::ifstream tempfile(temp_sensor);

  // Open PWM
  PWMChannel pwm(pwm_chip, pwm_channel, pwm_period);
  pwm.set_dc(0);
  pwm.enable(true);
  std::cerr << "Tuning mode entered." << std::endl
            << "---------------------------------" << std::endl
            << "This test assumes the system is " << std::endl
            << "idle but will exert pressure on" << std::endl
            << "CPU by itself" << std::endl
            << std::endl;

  std::cerr << "1: Oscillation around t = " << ideal_temp << std::endl;

  //
  // Use the Åström–Hägglund method to determine Tu (time period of
  // oscillation), a (amplitude of process variable = temperature
  // variation) and b (amplitude of control output (fan duty cycle)
  // which caused it).
  //
  // We can then calculate Ku = 4b / (pi a)
  //
  // And from there we can use the Ziegler–Nichols method to calculate:
  //  Kp = 0.60 Ku
  //  Ki = 1.2 Ku / Tu
  //  Kd = 3 Ku Tu / 40
  //
  // These should give a good starting point. Anyone can tune from there.
  //

  //
  // Run two full oscillations with the middle temperature set
  // between the two extremes found
  //
  //
  // Run two full oscillations (we're cooling down at the moment)
  //
  enum class Direction { Up, Down } direction = Direction::Down;

  // We want to time when we see minimums and maximums in order to
  // determine the oscillation period
  std::chrono::steady_clock clock;
  std::vector<std::chrono::time_point<std::chrono::steady_clock>> events;

  // Use the same error history as the main fanman code
  ProcessHistory<int32_t> errorhist(10, 10*15); // 10 samps/sec, 15 secs

  // We need to track the amplitude of our process error during
  // oscillation
  double t_max = std::numeric_limits<double>::min();
  double t_min = std::numeric_limits<double>::max();

  // Temperature measurements are slightly noisy - let's require
  // temperature to be 0.2°C below or above the ideal for it to count
  const double t_hysteresis = 200;

  // We collect a certain number of total half periods (marked by
  // reaching-high or reaching-low events), but only consider a
  // certain number of last events actually good (since we're
  // adjusting the first few cycles after testing the extremes in
  // the previous step)
  const size_t n_events_total = 12;
  const size_t n_events_ignore = 4;
  static_assert(n_events_total > n_events_ignore+1);
  //
  // Burn some power
  //
  CPUBurn burner;
  burner.enable();

  while (true) {
    // If we have ten events we're done
    if (events.size() == n_events_total) break;

    // Get current temp and log the error
    { int32_t temperature;
      tempfile >> temperature;
      if (tempfile.rdstate() != std::ios_base::goodbit)
        throw std::runtime_error("Unable to read temperature");
      tempfile.seekg(0);
      errorhist.add(temperature - ideal_temp);
      std::cerr << "t=" << temperature;
    }

    // If we're past the first two cycles, record min/max e(t)
    const auto latest = errorhist.get();
    std::cerr << " t~" << latest << "                       \r";
    if (events.size() > n_events_ignore) {
      t_max = std::max(t_max, latest);
      t_min = std::min(t_min, latest);
    }

    // If we're going up and reached positive error, go down - and
    // vice versa
    switch (direction) {
    case Direction::Up: {
      if (latest > t_hysteresis && errorhist.getdiffSG9() > 0) {
        events.push_back(clock.now());
        std::cerr << "reached high (t=" << latest << ")            " << std::endl;
        pwm.set_dc(1);
        direction = Direction::Down;
      }
      break;
    }
    case Direction::Down: {
      if (latest < -t_hysteresis && errorhist.getdiffSG9() < 0) {
        events.push_back(clock.now());
        std::cerr << "reached low (t=" << latest << ")              " << std::endl;
        pwm.set_dc(0);
        direction = Direction::Up;
      }
      break;
    }
    }

    // Go easy
    { using namespace std::chrono_literals;
      std::this_thread::sleep_for(100ms);
    }
  }
  //
  // Stop burning
  //
  burner.disable();

  //
  // We have five events - calculate the median half period
  //
  std::vector<std::chrono::duration<double>> half_periods(events.size()-1);
  for (size_t i = n_events_ignore; i != events.size() - 1; ++i) {
    half_periods[i] = events[i+1] - events[i];
    std::cerr << "Half period " << i << " is " << half_periods[i] << std::endl;
  }
  std::sort(half_periods.begin(), half_periods.end());
  const double Tu = std::chrono::duration_cast<std::chrono::microseconds>
    (half_periods[half_periods.size() / 2] * 2).count() / 1E6;
  const double a = t_max - t_min;
  const double b = 1;
  const double Ku = 4 * b / (std::numbers::pi_v<double> * a);
  const double Kp = 0.6 * Ku;
  const double Ki = 1.2 * Ku / Tu;
  const double Kd = 3. * Ku * Tu / 40.;
  std::cerr << "Suggested parameters:" << std::endl
            << "--- background data ---" << std::endl
            << " Tu = " << Tu << std::endl
            << "  a = " << a << std::endl
            << "  b = " << b << std::endl
            << " Ku = " << Ku << std::endl
            << "--- PID parameters ----" << std::endl
            << " ideal_temp = " << ideal_temp << std::endl
            << " Kp = " << Kp << std::endl
            << " Ki = " << Ki << std::endl
            << " Kd = " << Kd << std::endl
            << std::endl;

  //
  // Set back cooling
  //
  std::cerr << "Re-enabling cooling..." << std::endl;
  pwm.set_dc(1);
  burner.disable();

} catch(std::exception const &e) {
  std::cerr << "FATAL: " << e.what() << std::endl;
  return 1;
}


void CPUBurn::enable()
{
  if (!m_threads.empty())
    return;
  m_quit = false;
  for (size_t i = 0; i != std::thread::hardware_concurrency(); ++i) {
    m_threads.emplace_back(std::thread(&CPUBurn::burner, this));
  }
}

void CPUBurn::disable()
{
  // We can only be in here one at a time
  std::lock_guard<std::mutex> lock(m_entry);
  m_quit = true;

  // Wait for them to quit
  for (auto &t: m_threads) {
    t.join();
  }
  m_threads.clear();
}

void CPUBurn::burner()
{
  double n = 3;
  double o = 8;
  std::vector<double> memary(1024*1024, 0.);
  while (!m_quit) {
    auto start = std::chrono::high_resolution_clock().now();
    for (size_t i = 0; i != memary.size(); ++i) {
      n = sqrt(n * n + n);
      o = n * n - o;
      memary[i] = memary[i] * o + n;
    }
    // Warm the memory bus too
    std::memcpy(&memary[0], &memary[memary.size() / 2],
                memary.size() / sizeof(memary[0]) / 2);
    auto stop = std::chrono::high_resolution_clock().now();
    // Now sleep for as long time as we spent, to reach 50%
    // utilisation
    std::this_thread::sleep_for(stop-start);
  }
}

CPUBurn::~CPUBurn()
{
  disable();
}

