#include <stdio.h>
#include <stdlib.h> 
#include <array>
#include "pico/stdlib.h"
#include <math.h>
#include "hardware/gpio.h"
#include "hardware/adc.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
#define REFERENCE_VOLTAGE 3.3 //reference voltage for the adc converter

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

//thermistor equation is R=R_0*e^(1/T-1/T_0)

class CustomArray {
    float* items;    
    int size;

    public:
    CustomArray(int s = MAX_STORED_VALUES) {
        size = s;
        items = (float*) 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;
    }

    float getElement(int index) { return items[index]; }
    float getSum() {
        float 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, //PID
            pwm_pin, thermistor_pin, //thermistor
            index //misc
        ;
        float desired_temp, temp, //temperature
            thermistor_resistance, voltage_devider_resistance, thermistor_beta, thermistor_ref_temp, //thermistor
            adc_conversion_factor
        ;
        bool on;
        CustomArray error_array;

    public:
        PID(
            int index,
            int pwm_pin,
            int thermistor_pin,
            float desired_temp = 0,
            int kp = 1000,
            int ki = 100,
            int kd = 10,
            float thermistor_resistance = 100000,
            float thermistor_beta = 3950,
            float thermistor_ref_temp = 25, //C 
            float voltage_devider_resistance = 100000
        )  {
            error_array = CustomArray();

            this->kp = kp;
            this->ki = ki;
            this->kd = kd;
            this->thermistor_pin = thermistor_pin;
            this->thermistor_resistance = thermistor_resistance;
            this->thermistor_beta = thermistor_beta;
            this->thermistor_ref_temp = thermistor_ref_temp + 273;
            this->voltage_devider_resistance = voltage_devider_resistance;
            this->desired_temp = desired_temp;
            this->index = index;
            this->adc_conversion_factor = 3.3f / (1 << 12);

            measureTemp();
        }

        float getTemp() {
            return this->temp;
        }

        float measureTemp() {
            adc_select_input(thermistor_pin);
            uint16_t result = adc_read();
            float v = (float)result * adc_conversion_factor; //calculate ratio of measured voltage and max voltage
            float r = v / (3.3f - v) * voltage_devider_resistance;

            float T = 1/(1/thermistor_beta * log(r/thermistor_resistance) + 1/thermistor_ref_temp) - 273;

            this->temp = T;
            return T;
        }
           
        int calulatePid(float desired = -100) {
            if (desired != -100) desired_temp = desired;
            float e = desired_temp-temp; 

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

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

void init() {
    stdio_init_all();
    adc_init();

    gpio_init(25);
    gpio_init(13);
    gpio_set_dir(25, GPIO_OUT);
    gpio_set_dir(13, GPIO_OUT);

    adc_gpio_init(27);
}

int main()
{
    init();

    PID test_pid = PID(0, 6, 1);

    gpio_put(13, 1);

    while (true) {
        float val = test_pid.measureTemp();
        float pid = test_pid.calulatePid(80);
        printf("temp: %f\npid: %f", val, pid); 
        sleep_ms(500);
    }
}