#include <vector>
#include <array>
#include <iostream>
#include <chrono>
#include <thread>
class HeatModel
{
public:
    using State = std::array<double,3>;
    HeatModel(State x0,int log_size=600)
    {
        T_hist_.reserve(log_size);
        log_size_=log_size;
        log_counter_= 0;
        T_=x0;
    };

    void initialize(double T_start,double T_amb)
    {
        T_start_ = T_start;
        T_amb_   = T_amb;
        T_Air_ = T_amb;
    }

    // Log the Temperature. Return true if log is big enough, return false if log is already full
    //you can use this as a timer (while (log(){}))
    bool log()
    {
        if (log_counter_< log_size_-1)
        {
            T_hist_.push_back(T_Air_);
            log_counter_++;
            std::cout <<"T="<<T_Air_<<"\n";
            return true;
        }
        else
        {
            T_hist_.push_back(T_Air_);
            log_counter_++;
            std::cout<<"Log exceeded Reservation Limit \n";
            return false;
        }

    }

    double get_Temp()
    {
        return T_Air_;
    }

    // You can call stepfunction with Bool Parameter(stepPWM) if you have PWM output, or with double parameter
    // if you have normalized heating Power (0,1) (stepPower)
    // Call this Function at least every 250ms (cycletime of Algortihm)
    void stepPWM(double dt, bool u)
    {
        double u_double;
        if (u)
        {
            u_double=1.0;
        }
        else
        {
            u_double=0.0;
        }
        u_double = u_double *1000.0;
        const State k1 = Func(T_, u_double);
        const State k2 = Func(addScaled(T_, k1, 0.5 * dt), u_double);
        const State k3 = Func(addScaled(T_, k2, 0.5 * dt), u_double);
        const State k4 = Func(addScaled(T_, k3, dt),        u_double);

        for (int i = 0; i < 3; ++i)
        {
            T_[i] += (dt / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]);
        }
        T_Air_=T_[0];
    }

    void stepPower(double dt, double u)
    {
        u = u *1000.0;
        const State k1 = Func(T_, u);
        const State k2 = Func(addScaled(T_, k1, 0.5 * dt), u);
        const State k3 = Func(addScaled(T_, k2, 0.5 * dt), u);
        const State k4 = Func(addScaled(T_, k3, dt),        u);

        for (int i = 0; i < 3; ++i)
        {
            T_[i] += (dt / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]);
        }
        T_Air_=T_[0];
    }
private:

    double T_start_;
    int log_size_;
    double T_Air_;
    double T_amb_;
    int log_counter_;
    std::vector<int> T_hist_;
    State T_;

    /*
    LTI System Matrices for FP56
     A =
                  x1         x2         x3
       x1  -0.007659   0.003217   0.004442
       x2    0.08422   -0.08422          0
       x3   0.005135          0  -0.005856

      B =
                 u1
       x1         0
       x2  0.008313
       x3         0

      C =
           x1  x2  x3
       y1   1   0   0

      D =
           u1
       y1   0 */

    const std::array<std::array<double, 3>, 3> A_{{
            { -0.007659,  0.003217,  0.00442 },
            {  0.08422, -0.08422,  0.0 },
            {  0.005135,  0.0, -0.005856 }
    }};
    const std::vector<double> B_{0.0,0.008313, 0.00};
    const std::vector<double> C_{1.0, 0.0, 0.0};



    static State addScaled(const State& x, const State& dx, double scale)
    {
        State out{};
        for (int i = 0; i < 3; ++i)
        {
            out[i] = x[i] + scale * dx[i];
        }
        return out;
    }


    State Func(const State& x,double u)const
    {
        State dx=matVec(A_,x);
        for (int i=0;i<3;i++)
        {
            dx[i]+=B_[i]*u;
        }
        return dx;
    }

    static State matVec(const std::array<std::array<double, 3>, 3>& M, const State& x)
    {
        State y{};
        for (int i= 0; i < 3; ++i)
        {
            double sum = 0.0;
            for (int j = 0; j < 3; ++j)
            {
                sum += M[i][j] * x[j];
            }
            y[i] = sum;
        }
        return y;
    }
};


#define SINGLE_APP 1

#if SINGLE_APP
int main(){
    HeatModel::State x0={22.0,22.0,22.0};
    int STEP_TIME=50; // dt in milliseconds
    // Heatmodel(state starting_Values, size_of_log)
    HeatModel TestModel(x0,5);
    std::vector<double> temp;
    temp.reserve(6000);
    bool running = true;
    int counter=0;
    while (running){
        TestModel.stepPower(STEP_TIME/1000.0,1.0);
        temp.push_back(TestModel.get_Temp());
        counter++;
        //logg every Minute --> simulate for 5 minutes
        if (counter >=10){
            running=TestModel.log();
            counter = 0;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(STEP_TIME));
    }
}
#endif