#include <stdio.h>
#include <stdlib.h> 
#include <array>
#include "pico/stdlib.h"

using namespace std;

#define POLL_PERIOD 50 //time between polls (ms)
#define MAX_STORED_VALUES 10 //number of stored values to calculate e_dot and e_dot_dot values

/*
trying to keep the code clean, so some conventions:
classes: PascalCase
function: camelCase
var names: snake_case
*/

class CustomArray {
    int* items;    
    int size;

    public:
    CustomArray() {
        size = MAX_STORED_VALUES;
        items = (int*) malloc(size * sizeof(int));
        for (int i = 0; i < size; i++ ) items[i] = 0;
    }

    void PushItem(int item) {
        for (int i = 1; i < MAX_STORED_VALUES; i++) {
            items[i] = items[i-1];
        }

        items[0] = item;
    }

    int getElement(int index) { return items[index]; }
    int getSum() {
        int sum = 0;

        for (int i = 0; i < MAX_STORED_VALUES; i++ ) {
            sum += items[i];
        }

        return sum;
    }

    ~CustomArray() {
        free(items);
    }
};

class PID {
    private:
        int kp, ki, kd, pwm_pin, thermistor_pin, index, desired_temp;
        bool on;
        CustomArray error_array;

    public:
        PID(int kp) {
            error_array = CustomArray();
            this->kp = kp;
        }

        int calulatePid( int x ) {
            int e = desired_temp-x; 

            int e_dot = (e - error_array.getElement(0)) / POLL_PERIOD;
            int e_int = error_array.getSum() * POLL_PERIOD;
        

            return e * kp + e_dot * kd + e_int * ki;
        }
};

void init() {
    gpio_init(25);
    gpio_set_dir(25, GPIO_OUT);
}

int main()
{
    stdio_init_all();


    while (true) {
        //printf("Hello, world!\n");
        gpio_put(25, 0);
        sleep_ms(250);
        gpio_put(25, 1);
        sleep_ms(250);
    }
}