/*
 
 PIO frequency measurement
 2025-10-21 mchris
 
 https://www.mikrocontroller.net/topic/580993#7954679
 PIO Assembler:
 https://wokwi.com/tools/pioasm
 
 */
#include "fmes.pio.h"            // Includes the compiled PIO assembly program for blinking
#include <hardware/gpio.h>
#include <hardware/pio.h>
#include "hardware/clocks.h"

#define TESTPIN 16                    // Macro for frequency input

// function to translate
#if defined(ARDUINO_ARCH_MBED)
#include <pinDefinitions.h>
static int translate_pin(int pin) {
  return digitalPinToPinName(pin);
}
#else
static int translate_pin(int pin) {
  return pin;
}
#endif

static PIO pio;
static int sm;

static inline void fmes_program_init(PIO pio, uint sm, uint pin)
{
  pio_gpio_init(pio, pin);

  // Set pin direction: input
  pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false);

  pio_sm_config c = fmes_program_get_default_config(0);
  sm_config_set_in_pins(&c, pin); // for WAIT, IN
  sm_config_set_jmp_pin(&c, pin); // for jump

  // don't join FIFO's
  sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);

  // always run at sysclk (125 MHz default)
  sm_config_set_clkdiv(&c, 1.0);

  pio_sm_init(pio, sm, 0, &c);

  // Use SDK function to enable state machine
  pio_sm_set_enabled(pio, sm, true);
}

void setup() {
  Serial.begin(115200);

  //while (!Serial) delay(10);
  //Serial.println("start");

  //gpio_init(TESTPIN);                    // Initialize the GPIO pin
  //gpio_set_dir(TESTPIN, GPIO_IN);        // Set the TESTPIN as input (standard GPIO)
  pinMode(TESTPIN,INPUT);
  
  // Assign pio to pio0
  pio = pio0;

  // Claim an unused state machine
  sm = pio_claim_unused_sm(pio, true);

  int gpio_pin = translate_pin(TESTPIN);

  pio_add_program(pio, &fmes_program);

  // Initialize PIO program on the claimed state machine
  fmes_program_init(pio, sm, gpio_pin);

  Serial.printf("Using PIO instance %p, state machine %d on pin %d\n", pio, sm, gpio_pin);
}

static inline int32_t pio_read(PIO pio, uint sm)
{
  int32_t value = -1;
  uint32_t ints;

  uint8_t count = pio_sm_get_rx_fifo_level(pio, sm);

  ints = save_and_disable_interrupts();
  if (count > 0)
    value = pio_sm_get_blocking(pio, sm);
  else value = -1;
  restore_interrupts(ints);

  return value;
}

float get_freq()
{
  //count = self.sm.get()        # Read count from FIFO
  uint32_t count = pio_read(pio, sm);

  const int cyclesPerLoop = 3;
  const int correctionCycles = 1;
  uint32_t sigCycles = count * cyclesPerLoop - correctionCycles;
  float f;
  uint32_t fcpu = clock_get_hz(clk_sys);
  //Serial.print("fcpu:");Serial.println(fcpu);
  if (sigCycles != 0)  f = fcpu / (sigCycles);
  else f = -1;
  return f;
}

void loop()
{
  Serial.print("click ");
  Serial.println(get_freq());

  delay(200);
}