#include <Arduino.h>
#include <SPI.h>
#include "A1335.h"

#define kNOERROR 0
#define kPRIMARYREADERROR 1
#define kEXTENDEDREADTIMEOUTERROR 2
#define kPRIMARYWRITEERROR 3
#define kEXTENDEDWRITETIMEOUTERROR 4
#define kCRCERROR 5

#define kUNABLETOCHANGEPROCESSORSTATE 6

const uint16_t ChipSelectPin = 5;
const uint16_t LEDPin = 13;

const uint32_t WRITE = 0x40;
const uint32_t READ = 0x00;
const uint32_t COMMAND_MASK = 0xC0;
const uint32_t ADDRESS_MASK = 0x3F;

unsigned long nextTime;
bool ledOn = false;

bool includeCRC = false;
void setup() {

  uint16_t unused;
   uint32_t flags;
  uint16_t angle;
  uint32_t flagsAndZeroOffset;

  SPI.begin();
  // put your setup code here, to run once:
  Serial.begin(19200);

   while (!Serial);


  pinMode(SS, OUTPUT);
  digitalWrite(SS, HIGH);

  // Make sure all of the SPI pins are
  // ready by doing a read
  //PrimaryRead(ChipSelectPin, 0x0, unused);

  // Unlock the device
  ExtendedWrite(ChipSelectPin, 0xFFFE, 0x27811F77);

  // Make sure the device is unlocked
  ExtendedRead(ChipSelectPin, 0x22, flags);
  if (!((flags & 0x0022) == 0x0020))
  {
      Serial.println("Device is not Unlocked");
  }

  // Zero the angle
  // Extended location 0x06 contains flags in the MSW and the Zero Angle values in the LSW
  // so get both and zero out ZeroAngle
  ExtendedRead(ChipSelectPin, 0x06, flagsAndZeroOffset);
  flagsAndZeroOffset = (flagsAndZeroOffset & 0xFFFF0000);
  ExtendedWrite(ChipSelectPin, 0x06, flagsAndZeroOffset);

  // Get the current angle. It is now without the ZeroAngle correction
  PrimaryRead(ChipSelectPin, 0x20, angle);

  // Copy the read angle into location 0x5C preserving the flags
  flagsAndZeroOffset = (flagsAndZeroOffset & 0xFFFF0000) | ((angle << 4) & 0x0000FFFF);
  ExtendedWrite(ChipSelectPin, 0x06, flagsAndZeroOffset);
  
  /* uint16_t angle_read;
  Serial.println("Init Complete");
  PrimaryRead(ChipSelectPin, 0x20, angle_read);
  Serial.print("Angle= ");
  Serial.println(angle_read); */


}

void loop() {
  uint16_t angle;
    uint16_t temperature;
    uint16_t fieldStrength;

  // Every second, read the angle, temperature and field strength
    if (nextTime < millis())
    {
        if (PrimaryRead(ChipSelectPin, 0x20, angle) == kNOERROR)
    {
            if (CalculateParity(angle))
            {
                Serial.print("Angle = ");
                Serial.print((float)(angle & 0x0FFF) * 360.0 / 4096.0);
                Serial.println(" Degrees");
            }
            else
            {
                Serial.println("Parity error on Angle read");
            }
    }
    else
    {
      Serial.println("Unable to read Angle");
    }

        if (PrimaryRead(ChipSelectPin, 0x28, temperature) == kNOERROR)
    {
      Serial.print("Temperature = ");
      Serial.print(((float)(temperature & 0x0FFF) / 8.0) - 273.15);
      Serial.println(" C");
    }
    else
    {
      Serial.println("Unable to read Temperature");
    }

        if (PrimaryRead(ChipSelectPin, 0x2A, fieldStrength) == kNOERROR)
    {
      Serial.print("Field Strength = ");
      Serial.print(fieldStrength & 0x0FFF);
      Serial.println(" Gauss");
    }
    else
    {
      Serial.println("Unable to read Field Strength");
    }

        nextTime = millis() + 500L;

    // Blink the LED every half second
        if (ledOn)
        {
            digitalWrite(LEDPin, LOW);
            ledOn = false;
        }
        else
        {
            digitalWrite(LEDPin, HIGH);
            ledOn = true;
        }
    }
}

}