#pragma once
#include <system_time.hpp>

#include <string>
#include <string_view>
#include <cmath>

/**
 * This class implements the Second Order Filtered PID Algorithm described in
 *
 *   Åström, Karl Johan, Tore Hägglund, and Karl J. Astrom.
 *   Advanced PID control. Vol. 461. Research Triangle Park:
 *   ISA-The Instrumentation, Systems, and Automation Society, 2006.
 *
 * See page 428 for a pseudo-code implementation and page 418 for derivation.
 */

using systime::DateTimeStruct;

class AH_PID
{
  public:
	typedef struct state_t
	{
		float y_sp; // setpoint
		float y_meas; // measured process variable
		float y1; // filter state_1
		float y2; // filter state_2
		float v_rev; // computed nominal output
		float u_rev; // output subject to saturation constraints
		float I; // Integral term with anti-windup protection
	} state_t;

	struct gains_t
	{
		float K;
		float Td;
		float Ti;
		float Tf;
		float Tt;
		float b = 1.0; // initialize to 1
		float limOutput_max;
		float limOutput_min;
	};

	typedef struct parameters_t
	{
		float h;
		float p1;
		float p2;
		float p3;
		float p4;
		float p5;
	} parameters_t;

	// Values used for logging. state should be initialized to refer to the
	// state_ member during initialization.
	typedef struct logVals_t
	{
		state_t state; // not modifyable from logVal struct
		parameters_t params;
		DateTimeStruct timeStamp;
	} logVals_t;

  public:
	AH_PID(std::string id);
	~AH_PID() = default;

	// Getters
	const state_t getState() const;
	const gains_t getGains() const;
	const parameters_t getParams() const;

	// Setters
	void setGains(const gains_t g);
	void resetState()
	{
		state_ = {};
	}

	// Compute state and output
	logVals_t update(float dt_sec, const float setpoint, const float measurement);

  private:
	// Using limits in gains_, will ensure sat_output obeys saturation limits.
	// Returns true of output is saturated.
	bool limitOutput(const float output_desired, float& sat_output);

	// Compute controller parameters
	void computeParams(const float samplePeriod_s);

	// Zero-initialization for all data structs
	state_t state_ = {};
	gains_t gains_ = {};
	parameters_t param_ = {};

	// From orig. C# code
	AH_PID::gains_t default_gains_ = { 
		.K = 80,
		.Td = 10,
		.Ti = 50e3,
		.Tf = 10,
		.Tt = 100,
		.limOutput_max = 3000.0 / 60.0,
		.limOutput_min = -3000.0 / 60.0
	 };
	const std::string moduleID_;
};