#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(int s) {
        size = s;
        items = (int*) malloc(s * sizeof(int));
        for (int i = 0; i < s; i++ )
        {
            items[i] = 0;
        }
    }
};

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

    int calculate_integral(int* array) {
        int sum = 0;

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

        sum *= POLL_PERIOD;
        return sum;
    }
    
    public:
        PID(int kp) {
            error_array = new CustomArray(MAX_STORED_VALUES);
            this.kp = kp;
        }

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

            int e_dot = (e - error_array[0]) / POLL_PERIOD;
            int e_int = calculate_integral(error_array);
        

            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);
    }
}