//
// hello.MT6835.RP2040.ino
//
// Magnetic angle encoder serial hello-world
//
// Quentin Bolsee 3/15/2026
//
// This work may be reproduced, modified, distributed,
// performed, and displayed for any purpose, but must
// acknowledge this project. Copyright is retained and
// must be preserved. The work is provided as is; no
// warranty is provided, and users accept all liability.
//
#include "SPI.h"

#define PIN_CSN 1
#define PIN_CAL_EN 1

#define TAU (6.28318530718f) // 2 PI

#define MT6835_RESOLUTION  2097152
#define MT6835_OP_ANGLE    0b1010
#define MT6835_REG_ANGLE1  0x003

SPISettings spi_settings(1000000, MSBFIRST, SPI_MODE3);

uint32_t get_angle_21bits() {
  uint8_t data[6];
  data[0] = (MT6835_OP_ANGLE << 4);
  data[1] = MT6835_REG_ANGLE1;
  data[2] = 0;
  data[3] = 0;
  data[4] = 0;
  data[5] = 0;
  SPI.beginTransaction(spi_settings);
  digitalWrite(PIN_CSN, LOW);
  SPI.transfer(data, 6);
  digitalWrite(PIN_CSN, HIGH);
  SPI.endTransaction();

  // 21 bit value
  return (data[2] << 13) | (data[3] << 5) | (data[4] >> 3);
}

void setup() {
  pinMode(PIN_CAL_EN, OUTPUT);
  digitalWrite(PIN_CAL_EN, LOW);

  pinMode(PIN_CSN, OUTPUT);
  digitalWrite(PIN_CSN, HIGH);
  SPI.begin();

  Serial.begin();
}

void loop() {
  if (Serial.available()) {
    Serial.read();

    uint32_t angle_raw = get_angle_21bits();
    float angle_rad = ((float)angle_raw / MT6835_RESOLUTION) * TAU;
    Serial.printf("%.6f\n", angle_rad);
  }
}