#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include "hardware/pwm.h"
#include "hardware/clocks.h"

// I2C defines
// This example will use I2C0 on GPIO8 (SDA) and GPIO9 (SCL) running at 400KHz.
// Pins can be changed, see the GPIO function select table in the datasheet for information on GPIO assignments
#define I2C_PORT i2c0
#define I2C_SDA 8
#define I2C_SCL 9

#define CO2_PIN 5

void measure_freqs(void) {
    uint f_pll_sys = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_PLL_SYS_CLKSRC_PRIMARY);
    uint f_pll_usb = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_PLL_USB_CLKSRC_PRIMARY);
    uint f_rosc = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_ROSC_CLKSRC);
    uint f_clk_sys = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_SYS);
    uint f_clk_peri = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_PERI);
    uint f_clk_usb = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_USB);
    uint f_clk_adc = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_ADC);
    uint f_clk_rtc = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_RTC);

    printf("pll_sys  = %dkHz\n", f_pll_sys);
    printf("pll_usb  = %dkHz\n", f_pll_usb);
    printf("rosc     = %dkHz\n", f_rosc);
    printf("clk_sys  = %dkHz\n", f_clk_sys);
    printf("clk_peri = %dkHz\n", f_clk_peri);
    printf("clk_usb  = %dkHz\n", f_clk_usb);
    printf("clk_adc  = %dkHz\n", f_clk_adc);
    printf("clk_rtc  = %dkHz\n", f_clk_rtc);

    // Can't measure clk_ref / xosc as it is the ref
}

float measure_co2(float *max, float *c) {
    // Only the PWM B pins can be used as inputs.
    assert(pwm_gpio_to_channel(CO2_PIN) == PWM_CHAN_B);
    uint slice_num = pwm_gpio_to_slice_num(CO2_PIN);


    // Count once for every 2000 cycles the PWM B input is high
    pwm_config cfg = pwm_get_default_config();
    // pwm_config_set_wrap(&cfg, 64000);
    pwm_config_set_clkdiv_mode(&cfg, PWM_DIV_B_HIGH);
    pwm_config_set_clkdiv(&cfg, 2000);
    pwm_init(slice_num, &cfg, false);
    pwm_set_counter(slice_num, 0);
    gpio_set_function(CO2_PIN, GPIO_FUNC_PWM);

    pwm_set_enabled(slice_num, true);
    sleep_ms(1004);
    pwm_set_enabled(slice_num, false);
    float count = pwm_get_counter(slice_num);
    float counting_rate = clock_get_hz(clk_sys) / 2000.0;
    float max_possible_count = counting_rate * 1.002;
    //return count;
    *max = max_possible_count;
    *c = count;
    return 2000.0 * count / max_possible_count;

    // co2 = 2000.0*(TH-2ms)/(TH+TL-4ms)
}

// I2C reserves some addresses for special purposes. We exclude these from the scan.
// These are any addresses of the form 000 0xxx or 111 1xxx
bool reserved_addr(uint8_t addr) {
    return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
}

void i2cscan()
{
        printf("\nI2C Bus Scan\n");
    printf("   0  1  2  3  4  5  6  7  8  9  A  B  C  D  E  F\n");

    for (int addr = 0; addr < (1 << 7); ++addr) {
        if (addr % 16 == 0) {
            printf("%02x ", addr);
        }

        // Perform a 1-byte dummy read from the probe address. If a slave
        // acknowledges this address, the function returns the number of bytes
        // transferred. If the address byte is ignored, the function returns
        // -1.

        // Skip over any reserved addresses.
        int ret;
        uint8_t rxdata;
        if (reserved_addr(addr))
            ret = PICO_ERROR_GENERIC;
        else
            ret = i2c_read_blocking(I2C_PORT, addr, &rxdata, 1, true);

        printf(ret < 0 ? "." : "@");
        printf(addr % 16 == 15 ? "\n" : "  ");
    }


}

int measure_i2c_co2(float *co2, float *temp, float *rh) {
    uint8_t has_measurement[] = {0xE4, 0xB8};
    uint8_t read_measurement[] = {0xEC, 0x05};
    uint8_t data[9];
    int count;

    *co2 = 0;
    *temp = 0;
    *rh = 0;

    if ((count = i2c_write_blocking(I2C_PORT, 0x62, has_measurement, 2, false)) != 2) {
        *co2 = count;
        return 1;
    }

    if ((count = i2c_read_blocking(I2C_PORT, 0x62, data, 3, false)) != 3 ||
        (data[2] == 0 && data[1] == 0 && data[0] & 0x7 == 0)) {

        *co2 = count;
        printf("    data: %b %b %b", data[2], data[1], data[0]);
        return 2;
    }

    if ((count = i2c_write_blocking(I2C_PORT, 0x62, read_measurement, 2, false)) != 2) {
        *co2 = count;
        return 3;
    }

    if ((count = i2c_read_blocking(I2C_PORT, 0x62, data, 9, false)) != 9) {
        *co2 = count;
        return 4;
    }

    *co2 = data[0] * 256.0 + data[1];
    *temp = -45.0 + 175.0 * (data[3] * 256.0 + data[4]) / 0xFFFF;
    *rh = 100 * (data[6] * 256.0 + data[7]) / 0xFFFF;

    return 0;
}

int main()
{
    stdio_usb_init();

    // I2C Initialisation. Using it at 100Khz.
    i2c_init(I2C_PORT, 400*1000);

    gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
    gpio_set_function(I2C_SCL, GPIO_FUNC_I2C);
//    gpio_init(CO2_PIN);
 //   gpio_set_dir(CO2_PIN, GPIO_OUT);

    gpio_pull_up(I2C_SDA);
    gpio_pull_up(I2C_SCL);
    // For more examples of I2C use see https://github.com/raspberrypi/pico-examples/tree/master/i2c

    // get clock of RPI

    int slice = pwm_gpio_to_slice_num(0);
    pwm_config config = pwm_get_default_config();

//bi_decl(bi_2pins_with_func(PICO_DEFAULT_I2C_SDA_PIN,
//PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C));

    int w = i2c_write_blocking(I2C_PORT, 0x62, (const uint8_t*)"\x21\xb1", 2, false);
    sleep_ms(5000);
    printf("bytes written on init: %d\n", w);


bool on = false;
    while (true) {
    // measure_freqs();aa

    float c, m;
    float co2 = measure_co2(&m, &c);
    // i2cscan();
        printf("Hello CO2: %f   %f / %f\n", co2, c, m);


/*
    float temp1, h1;


    float temp2, h2;
*/
float temp, rh;
int r = measure_i2c_co2(&co2, &temp, &rh);
printf("  %d -- ac. CO2: %f    T: %f    RH: %f\n", r, co2, temp, rh);


        //gpio_put(CO2_PIN, on);
        //printf("pin is %d\n", (int)on);
        //on = !on;
        sleep_ms(5000);
    }
}