#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 {
    int* items;    
    int size;

    public:
    CustomArray(int s = MAX_STORED_VALUES) {
        size = s;
        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, //PID
            pwm_pin, thermistor_pin, //thermistor
            thermistor_resistance, voltage_devider_resistance, thermistor_beta, thermistor_ref_temp, //thermistor
            desired_temp, temp, //temperature
            index; //misc
        bool on;
        CustomArray error_array;

    public:
        PID(
            int index,
            int pwm_pin,
            int thermistor_pin,
            int desired_temp = 0,
            int kp = 1000,
            int ki = 100,
            int kd = 10,
            int thermistor_resistance = 100000,
            int thermistor_beta = 3950,
            int thermistor_ref_temp = 25, //C 
            int 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->voltage_devider_resistance = voltage_devider_resistance;
            this->desired_temp = desired_temp;
            this->index = index;

            measureTemp();
        }

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

        int measureTemp() {
            //adc_select_input(thermistor_pin);
            uint16_t result = adc_read();
            uint16_t v_slash_vin = 2<<15 / result;
            uint16_t r = v_slash_vin * voltage_devider_resistance / (1 / (1-v_slash_vin));

            int T = 1/(1/thermistor_beta * log(r/thermistor_resistance) + 1/thermistor_ref_temp);

            this->temp = 10;

            return this->temp;
        }

        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() {
    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) {
        printf("%f\n", test_pid.getTemp());
        //printf("asdf");
        sleep_ms(500);
    }
}