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

// ---------- SPI pin setup ----------
#define SPI_PORT spi0
#define PIN_MISO 16
#define PIN_CS   17
#define PIN_SCK  18
#define PIN_MOSI 19

// ---------- MPU-style registers ----------
#define REG_WHO_AM_I     0x75
#define REG_ACCEL_XOUT_H 0x3B

static inline void cs_select() {
  gpio_put(PIN_CS, 0);
}

static inline void cs_deselect() {
  gpio_put(PIN_CS, 1);
}

// Write one register
static void mpu_write_reg(uint8_t reg, uint8_t value) {
  uint8_t buf[2];
  buf[0] = reg & 0x7F;   // bit7 = 0 for write
  buf[1] = value;

  cs_select();
  spi_write_blocking(SPI_PORT, buf, 2);
  cs_deselect();
}

// Read one register
static uint8_t mpu_read_reg(uint8_t reg) {
  uint8_t tx[2];
  uint8_t rx[2];

  tx[0] = reg | 0x80;    // bit7 = 1 for read
  tx[1] = 0x00;          // dummy byte to clock data out

  cs_select();
  spi_write_read_blocking(SPI_PORT, tx, rx, 2);
  cs_deselect();

  // rx[0] is garbage/previous byte, rx[1] is the register value
  return rx[1];
}

// Read multiple sequential registers
static void mpu_read_bytes(uint8_t start_reg, uint8_t *out, size_t len) {
  cs_select();

  uint8_t cmd = start_reg | 0x80;  // read bit set
  spi_write_blocking(SPI_PORT, &cmd, 1);

  for (size_t i = 0; i < len; i++) {
    uint8_t tx = 0x00;
    uint8_t rx = 0x00;
    spi_write_read_blocking(SPI_PORT, &tx, &rx, 1);
    out[i] = rx;
  }

  cs_deselect();
}

static int16_t be_to_i16(uint8_t hi, uint8_t lo) {
  return (int16_t)((hi << 8) | lo);
}

int main() {
  stdio_init_all();
  sleep_ms(1500); // give serial monitor time

  printf("Starting fake MPU SPI test...\n");

  // SPI init
  spi_init(SPI_PORT, 1000 * 1000); // 1 MHz
  spi_set_format(SPI_PORT, 8, SPI_CPOL_0, SPI_CPHA_0, SPI_MSB_FIRST);

  gpio_set_function(PIN_MISO, GPIO_FUNC_SPI);
  gpio_set_function(PIN_SCK,  GPIO_FUNC_SPI);
  gpio_set_function(PIN_MOSI, GPIO_FUNC_SPI);

  gpio_init(PIN_CS);
  gpio_set_dir(PIN_CS, GPIO_OUT);
  gpio_put(PIN_CS, 1);

  sleep_ms(100);

  // Optional: wake the chip if you later emulate PWR_MGMT_1
  // mpu_write_reg(0x6B, 0x00);

  // Check identity register
  uint8_t who = mpu_read_reg(REG_WHO_AM_I);
  printf("WHO_AM_I = 0x%02X\n", who);

  while (true) {
    uint8_t raw[6];
    mpu_read_bytes(REG_ACCEL_XOUT_H, raw, 6);

    int16_t ax = be_to_i16(raw[0], raw[1]);
    int16_t ay = be_to_i16(raw[2], raw[3]);
    int16_t az = be_to_i16(raw[4], raw[5]);

    printf("AX=%d  AY=%d  AZ=%d\n", ax, ay, az);
    sleep_ms(500);
  }
}