/*
  For sensor information:
  https://www.ti.com/lit/ds/symlink/tmag5170-q1.pdf?ts=1605622801161&ref_url=https%253A%252F%252Fwww.ti.com%252Fproduct%252FTMAG5170-Q1%253FkeyMatch%253DTMAG5170-Q1%2B%2526tisearch%253DSearch-EN-everything%2526usecase%253DGPN
  Circuit:
  TMAG5170
  DRDY: pin ____
  CS: pin 13
  MOSI: pin 4
  MISO: pin 2
  SCK: pin 3
*/
#include <SPI.h>

//Sensor's memory register addresses:
const int DEVICE_CONFIG = 0x00; //0x0 is used for writes, should really handle this in the read command
const int SENSOR_CONFIG = 0x01;   
const int SYSTEM_CONFIG = 0x02; // this is where we can switch to the 12bit XY sampling mode                                      
const int TEST_CONFIG = 0x0F;
const int X_CH_RESULT = 0x89; //0x8 is used for reads, should really handle this in the write command
const int Y_CH_RESULT = 0x8A;
const int Z_CH_RESULT = 0x8B;
const int TEMP_RESULT = 0x8C;
const int range = 50;
const int chipSelectX = 13;
// chipSelectY needs to be done in registers because it itsn't broken out 
//int whichDevice


void setup() {
  int a;
  Serial.begin(9600);
  //while (!Serial); //hold setup while serial starts up
  pinMode(chipSelectX, OUTPUT); // Set the X axis CS to output
  //PORT->Group[0].DIRSET.reg = (1 << 18); // Set the Y axis CS to output (not broken out on trinket)
  
  delay(200);
  SPI.begin();  // start the SPI library:
  //Configure sensor with syntax (address,dataA, dataB,command+CRC)

  // Configure the X-axis Sensor
  for( a = 0; a < 100; a = a + 1 ){
    writeRegister(TEST_CONFIG, 0x00, 0x04, 0x07); // from TI support - write a 0x0F000407 which disables CRC in the test config addres //https://e2e.ti.com/support/sensors/f/1023/t/937812
    delay(5);
    writeRegister(SENSOR_CONFIG, 0x19, 0xEA, 0x00);  //0x1 = SENSOR_CONFIG: Configure X,Y, and Z RANGE to be +/-100mT, as well as activating them (they default to off)
    delay(5);
    //writeRegister(SYSTEM_CONFIG, 0x00, 0x00, 0x00);  //0x2 = SYSTEM_CONFIG: This is where we can get the special 2 channels in one 32bit comm setting, to activate XZ you want 0b00000000, 0b10000000, and for ZY 0b00000000, 0b11000000
    //delay(50);
    writeRegister(DEVICE_CONFIG, 0b00110001, 0x20, 0x00); // 0x0 = DEVICE_CONFIG: Set to 8x averaging + no temp coefficient + set to active measure mode + disable temp stuff
    delay(5); // give the sensor time to set up:
  }

  /*
  // Configure the Y-axis Sensor
  PORT->Group[0].OUTCLR.reg = (1 << 18);
  for( a = 0; a < 100; a = a + 1 ){
    writeRegister(TEST_CONFIG, 0x00, 0x04, 0x07); // from TI support - write a 0x0F000407 which disables CRC in the test config addres //https://e2e.ti.com/support/sensors/f/1023/t/937812
    delay(1);
    writeRegister(SENSOR_CONFIG, 0x19, 0xEA, 0x00);  //0x1 = SENSOR_CONFIG: Configure X,Y, and Z RANGE to be +/-100mT, as well as activating them (they default to off)
    delay(1);
    //writeRegister(SYSTEM_CONFIG, 0x00, 0x00, 0x00);  //0x2 = SYSTEM_CONFIG: This is where we can get the special 2 channels in one 32bit comm setting, to activate XZ you want 0b00000000, 0b10000000, and for ZY 0b00000000, 0b11000000
    //delay(50);
    writeRegister(DEVICE_CONFIG, 0b00110001, 0x20, 0x00); // 0x0 = DEVICE_CONFIG: Set to 8x averaging + no temp coefficient + set to active measure mode + disable temp stuff
    delay(1); // give the sensor time to set up:
  PORT->Group[0].OUTSET.reg = (1 << 18);
  }
  */
}

void loop()
{ 
  // Read X and Z, do math, then print
  int16_t xChannel = readRegister(X_CH_RESULT,0x00, 0x00, 0x00);
  signed short xValue = xChannel + 80; //100/(32768);
  //Serial.print(xValue);
  //Serial.print(", ");
  delay(1);
  int16_t zChannel = readRegister(Z_CH_RESULT,0x00, 0x00, 0x00);
  signed short zValue = zChannel + 50; //*100/(32768);
  //Serial.println(zValue);
  delay(1);
  float mx = (float)xValue / 1.0;
  float mz = (float)zValue / 1.0;
  float position = conversionMath(mx, mz);
  Serial.println(position);
  /*
  PORT->Group[0].OUTCLR.reg = (1 << 18);
  xChannel = readRegister(X_CH_RESULT,0x00, 0x00, 0x00);
  xValue = xChannel + 80; //100/(32768);
  Serial.print(xValue);
  Serial.print(", ");
  delay(1);
  zChannel = readRegister(Z_CH_RESULT,0x00, 0x00, 0x00);
  zValue = zChannel + 50; //*100/(32768);
  Serial.println(zValue);
  delay(1);
  PORT->Group[0].OUTSET.reg = (1 << 18);
  */
}

//READ COMMAND, returns an unsigned 16 bit int
unsigned int readRegister(byte thisRegister, byte thisValueA, byte thisValueB, byte thisCommand)
{
  byte inByte = 0x0;           // incoming byte from the SPI
  int16_t result = 0;   // result to return
  unsigned char bytesToRead = 2;
  byte dataToSend = thisRegister; // Previously concatinated the address and commmand, but we won't do this
  digitalWrite(chipSelectX, LOW);
  SPI.beginTransaction(SPISettings(100000, MSBFIRST, SPI_MODE0));
    SPI.transfer(dataToSend); // sending address
    result = SPI.transfer(thisValueA);
    result = result << 8;   // shift the first byte, then get the second byte:
    inByte = SPI.transfer(thisValueB);
    SPI.transfer(thisCommand);
    //Serial.println(thisValueB);
    result = result | inByte;   // combine the byte you just got with the previous one:
    //Serial.println(result);
  SPI.endTransaction();
  digitalWrite(chipSelectX, HIGH);
  return (result);
}

//WRITE COMMAND, returns nothing
void writeRegister(byte thisRegister, byte thisValueA, byte thisValueB, byte thisCommand) //we've rolled command and CRC into one byte
{ 
  digitalWrite(chipSelectX, LOW);
  SPI.beginTransaction(SPISettings(100000, MSBFIRST, SPI_MODE0));
    // when we set an SPI.transfer to a variable it will fill up with what's coming in on MISO
    byte writeResult = SPI.transfer(thisRegister); // we concatinated above, now we are sending the complete address
    writeResult = SPI.transfer(thisValueA);  // thisValue is really 16 bits, but we've chopped it to send in chunks of 8 here
    writeResult = SPI.transfer(thisValueB);
    writeResult = SPI.transfer(thisCommand);
  SPI.endTransaction();
  digitalWrite(chipSelectX, HIGH);
}

float conversionMath(float mx, float mz)
{
  float output;
  const float scaleZ = 1.004;
  const float translateZ = 383;
  const float globalTranslate = -252;
  const float amplitude = 11417;
  const float phaseShift = 0;
  const float polePitch = 4;
  const float jumpCheck = 1.5;
  const float RC = 0.35;  // RC value for our low pass RC filter approximation
  // Static Variables
  static float jumpTally = 0;
  static float atangentPrev = 0; // ^ This one I want to persist between the current instance of the function runs and the previous, so harder to fix
  
  // Start of the conversion process
  static float mxFil = mx; // Initialize both of these variables once off of the current mx value, then update below
  static float mzFil = mz;
  mxFil = (mx - mxFil) * RC + mxFil + globalTranslate;
  mzFil = ((mz - mzFil) * RC + mzFil + globalTranslate) * scaleZ + translateZ;
  float mxNorm = mxFil / amplitude;
  float mzNorm = (mzFil / amplitude) + (sin(phaseShift) * mxFil) / cos(phaseShift);
  float atangent = 0.5 * polePitch / 3.14159 * atan(mxNorm / mzNorm);
  // Here we are going to zero the signal as we have started at some arbitrary location along the pole pitch from 0-2.
  static float atanZero = atangent; // Initialize here after we set atangnet, will only initialize once
  atangent = atangent - atanZero;
  // Now we need to check if we have made a complete circle and jumped from 360 deg to 0, we will check for a jump in both directions
  if (atangent - atangentPrev < jumpCheck)
  {
    jumpTally = jumpTally + 2;
  }
  if (atangent - atangentPrev > -jumpCheck)
  {
    jumpTally = jumpTally - 2;
  }
  atangentPrev = atangent;
  output = atangent + jumpTally;
  return output;
}