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

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

int kp = 10;
int ki = 100;
int kd = 1000;

int set_temp = 30;

int* e_prev;

// Perform initialisation
int* create_array(int size) {
    int* array = (int*)malloc(size * sizeof(int));

    for (int i = 0; i < size; i++) { //fill with 0s so it doesnt confuse the controller
        array[i] = 0;
    }

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

int* push_array(int* array, int value) {
    for (int i = 1; i < MAX_STORED_VALUES; i++) {
        array[i] = array[i-1];
    }

    array[0] = value;

    return array;
}

void init(void) {
    *e_prev = create_array(MAX_STORED_VALUES);
    gpio_init(PICO_DEFAULT_LED_PIN);
    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
}

void pico_set_led(bool led_on) {
    gpio_put(PICO_DEFAULT_LED_PIN, led_on);
}

int calculate_pid(int* error_array, float x) {
    int e = set_temp-x; 

    push_array(error_array, e);

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

int main() {
    //init();
    stdio_init_all();
    while (true)
    {
        /* code */
        printf("sadf");
        sleep_ms(100);
    }
    

}