#include "PioAsmBusReader.pio.h"

#define PIN_BUS_0 2
#define BUS_BIT_COUNT (busreader_bus_size)
#define PIN_DATA_PUSH_CLOCK 18
#define PIN_ADDRESS_INPUT_ENABLE 19
#define PIN_DATA_OUTPUT_ENABLE 20
#define PIN_SIDESET_START PIN_DATA_PUSH_CLOCK

#define STATE_MACHINE_BUS_READER 0

extern "C" void watch_ram_access(volatile void *NeoRomAddress);
volatile bool core0Initialized = false;
volatile uint16_t NeoROM[64*1024];

static void printHexValue(int val)
{
  Serial1.print("0x");
  if (val < (1<<4))
    Serial1.print("0");
  if (val < (1<<8))
    Serial1.print("0");
  if (val < (1<<12))
    Serial1.print("0");
  Serial1.print(val, HEX);
}

static void pio0_programs_init()
{
  // Set pin's GPIO function (connect PIO to the pad)
  for (int p = PIN_BUS_0; p < PIN_BUS_0 + BUS_BIT_COUNT; ++p )
  {
    pio_gpio_init(pio0, p);
  }
  pio_gpio_init(pio0, PIN_DATA_PUSH_CLOCK);
  pio_gpio_init(pio0, PIN_ADDRESS_INPUT_ENABLE);

  // State machine 0: Bus Reader => Set pins function
  pio_add_program(pio0, &busreader_program);
  pio_sm_config c = busreader_program_get_default_config(busreader_program.origin);
  sm_config_set_in_pins(&c, PIN_BUS_0);
  sm_config_set_out_pins(&c, PIN_BUS_0, BUS_BIT_COUNT);
  sm_config_set_sideset_pins(&c, PIN_SIDESET_START);
  //sm_config_set_clkdiv_int_frac(&c, 65535, 0);

  // Load our configuration, and jump to the start of the program
  pio_sm_init(pio0, STATE_MACHINE_BUS_READER, busreader_program.origin, &c);

  // Force pin directions
  pio_sm_set_consecutive_pindirs(pio0, STATE_MACHINE_BUS_READER, PIN_BUS_0, BUS_BIT_COUNT, false);
  pio_sm_set_consecutive_pindirs(pio0, STATE_MACHINE_BUS_READER, PIN_DATA_PUSH_CLOCK, 2, true);

  // Start the state machine!
  pio_sm_set_enabled(pio0, STATE_MACHINE_BUS_READER, true);
}

void setup()
{
  Serial1.begin(115200);
  pinMode(PIN_DATA_OUTPUT_ENABLE, INPUT_PULLUP);

  // Fill ROM image
  for (int i = 0; i < 64*1024; ++i)
  {
      NeoROM[i] = i;
  }

  // Start Core1
  core0Initialized = true;

  // Start PIO
  pio0_programs_init();

  // Simulate core 1 code...
  watch_ram_access(NeoROM);
}

void loop()
{
  /*
  static int cnt = 0;

  
//  uint32_t address = pio_sm_get_blocking(pio0, 0);




  while (((*(uint32_t *)((void *)0x50200004)) & 256) != 0) delay(1);
  uint32_t address = *(uint32_t *)(((void *)0x50200020));





  //Serial1.print(cnt++);
  //Serial1.print(" => Received address: ");
  //printHexValue(address);
  //Serial1.println();

  //int val = address;
  //uint32_t val = NeoROM[address];




  uint32_t val = *(NeoROM + address);



  //Serial1.print("Sending data: ");
  //printHexValue(val);
  //Serial1.println();

  //pio_sm_put_blocking(pio0, 0, val);



  *(uint32_t *)((void *)0x50200010) = val;
  */
}