#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/timer.h"

#include "quadrature_decoder.pio.h"

#define DATA_WORDS 1000

//
// ---- quadrature encoder interface example
//
// the PIO program reads phase A/B of a quadrature encoder and increments or
// decrements an internal counter to keep the current absolute step count
// updated. At any point, the main code can query the current count by using
// the quadrature_encoder_*_count functions. The counter is kept in a full
// 32 bit register that just wraps around. Two's complement arithmetic means
// that it can be interpreted as a 32-bit signed or unsigned value, and it will
// work anyway.
//
// As an example, a two wheel robot being controlled at 100Hz, can use two
// state machines to read the two encoders and in the main control loop it can
// simply ask for the current encoder counts to get the absolute step count. It
// can also subtract the values from the last sample to check how many steps
// each wheel as done since the last sample period.
//
// One advantage of this approach is that it requires zero CPU time to keep the
// encoder count updated and because of that it supports very high step rates.
//

int main()
{
    // Base pin to connect the A phase of the encoder.
    // The B phase must be connected to the next pin
    const uint PIN_AB = 9;

    stdio_init_all();

    PIO pio = pio0;
    int sm = pio_claim_unused_sm(pio, true);

    // we don't really need to keep the offset, as this program must be loaded
    // at offset 0
    printf("Loaded program, using sm %d, gpio %d\n", sm, PIN_AB);
    pio_add_program(pio, &QuadratureDecoder_program);
    gpio_set_input_enabled(PIN_AB, true);
    gpio_set_input_enabled(PIN_AB + 1, true);
    QuadratureDecoder_program_init(pio, sm, PIN_AB, 0);

    // pio_sm_config_xfer(pio, sm, PIO_DIR_FROM_SM, 8, 1);

    // struct for time and position
    typedef struct
    {
        union
        {
            struct
            {
                uint16_t pos;
                uint16_t time;
            };
            uint32_t value;
        };
    } time_pos_t;

    uint16_t last_time = 0;
    while (1)
    {
        const uint buff_len = 16;
        time_pos_t buff[buff_len];
        int level = pio_sm_get_rx_fifo_level(pio, sm);
        if (level == 0)
        {
            continue;
        }

        // int res = pio_sm_xfer_data(pio, sm, PIO_DIR_FROM_SM, sizeof(buff), buff);
        for (int i = 0; i < level; i++)
        {
            buff[i].value = pio_sm_get_blocking(pio, sm);
        }

        uint16_t time = buff[level - 1].time;
        uint16_t pos = buff[level - 1].pos;

        uint16_t delta_time = time - last_time;
        printf("pos=%u\ttime=%u\tdelta_time=%u\tcount=%u\n", pos, time, delta_time, level);
        last_time = time;
        sleep_ms(100);
    }
}