/* Copyright (c) 2015, Ian Sutton <ian@kremlin.cc>

 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */
 
 /* this is a library for the arduino due that controls the LSM303D accelerometer 
  * & magnometer. it will not work on other arduino models, it uses API calls 
  * exclusive to newer ARM-based arduinos.
  * 
  * steps to recreate:
  *
  * !! be especially careful of the 5V output in on the due's SPI header. it is a high !!
  * !! current output & will cook anything nearby. refer to this diagram before making !!
  * !! any connections: http://uglyman.kremlin.cc/quick/due-pinout-web.png            !!
  *
  * - wire SPI ports on the LSM to their respective pins on the due's SPI header (not
  *   ICSP!)
  *
  * - wire INT2 to digital pin 53 and the LSM's chip select/slave select port to
  *   pin 10 above the pwm module.
  *
  * - ground the SPI module, arduino, and LSM together, preferably alone. i had to use
  *   an audio isolation amplifier to provide a quiet enough ground to support the
  *   unusually high SPI baud rate i use here, discussed later.
  *
  * - wire LSM's Vin to the arduino's 3.3V supply. leave Vdd floating */

#include <SPI.h>

/* slave select pin */
#define LSM_CS 10

/* wire to INT2 which latches to a magnometer-read-ready signal */
#define MAGNO_RDY_PIN 53

/* SPI clock divider (84MHz divided by this equals SPI frequency) */
#define SPI_CLK_DIV 3

/* frequency of temperature sensor in Hz, set in lsm_config() */
#define TEMP_FREQ 100

/* struct representing magnometer values at shared instant of time */
struct magno_point {
  
  signed short x;
  signed short y;
  signed short z;
};

/* hands off */
const byte BAD_REGS[8] = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x0E, 0x10, 0x11 };

/* these are set on successful completion of their respective funcs */
bool SERIAL_CONFIGURED = 0;
bool SPI_CONFIGURED = 0;
bool SPI_OK = 0;
bool LSM_CONFIGURED = 0;

/* don't brick the LSM */
bool matches_badreg(byte addr) {

  short i;
  i = 0;

  for (; i < 8; i++)
    if (addr == BAD_REGS[i])
      return true;

  return false;
}

/* fired when critical exception encountered, doesn't return */
void killspin(String msg) {

  Serial.println(msg);
  Serial.println("exception occurred or assertion failed. no-operation until reset");
  while (1);
}

/* reads n sequential bytes starting @ addr returns byte array of size n
 * returned array must be free()'d manually! */
byte *spi_multiread(byte addr, short n) {

  byte *ret, first;
  short cnt;

  if (matches_badreg(addr))
    killspin("tried to read from a reserved, internal register!");

  if (!SPI_CONFIGURED || !SPI_OK)
    killspin("tried to read over SPI before it was configured or tested!");

  /* addresses are 6 bits wide */
  if (addr > 63)
    killspin("invalid lsm register address");

  /* set msb to read, 2nd msb for multibyte op, and append 6 bit address field */
  first = 128 + 64 + addr;
  cnt   = 0;

  /* free this! */
  ret = (byte *) calloc(n, 1);

  /* kickoff read operation */
  SPI.transfer(LSM_CS, first, SPI_CONTINUE);

  for (; cnt < n - 1; cnt++)
    ret[cnt] = SPI.transfer(LSM_CS, first, SPI_CONTINUE);

  ret[n - 1] = SPI.transfer(LSM_CS, first, SPI_LAST);

  return ret;
}

/* reads & returns 1 byte @ addr */
byte spi_read(byte addr) {

  byte first, ret;

  if (matches_badreg(addr))
    killspin("tried to read from a reserved, internal register!");

  if (!SPI_CONFIGURED || !SPI_OK)
    killspin("tried to read over SPI before it was configured or tested!");

  /* addresses are 6 bits wide */
  if (addr > 63)
    killspin("invalid lsm register address");

  /* set msb to read and append 6 bit address field */
  first = 128 + addr;

  /* perform 16 bit SPI exchange as per LSM datasheet's specification */
  SPI.transfer(LSM_CS, first, SPI_CONTINUE);
  ret = SPI.transfer(LSM_CS, 0x00, SPI_LAST);

  return ret;
}

/* write single byte @ addr */
void spi_write(byte addr, byte data) {

  byte first;

  if (matches_badreg(addr))
    killspin("tried to write to a reserved, internal register!");

  if (!SPI_CONFIGURED || !SPI_OK)
    killspin("tried to write over SPI before it was configured or tested!");

  /* addresses are 6 bits wide */
  if (addr > 63)
    killspin("INVALID ADDRESS");

  /* set 6 bit address field */
  first = addr;

  /* perform 16 bit SPI exchange as per LSM datasheet's specification */
  SPI.transfer(LSM_CS, first, SPI_CONTINUE);
  SPI.transfer(LSM_CS, data, SPI_LAST);
}

void spi_config() {

  SPI.begin(LSM_CS);

  /* my arduino due has a 84 MHz cpu which is divided here to provide the
   * SPI baud rate. the LSM's data sheet purports the maximum SPI frequency
   * is 10MHz, however i've found that it works fine up to 28MHz, which is
   * the frequency set below (84 MHz / 3 = 28 MHz) */
  SPI.setClockDivider(LSM_CS, SPI_CLK_DIV);

  /* LSM is big endian */
  SPI.setBitOrder(LSM_CS, MSBFIRST);

  /* clock is active-low, exchange occurs on clock's first falling edge
   * CPOL = 1, CKE = 0 */
  SPI.setDataMode(LSM_CS, SPI_MODE3);

  SPI_CONFIGURED = 1;
}

/* read & check immutable device ID reg a number of times to guarantee LSM
 * slave is responding and capable of handling master's SPI clock freq.
 * then, write reg & read back to test writing */
boolean spi_test() {

  bool read_ok, write_ok;
  byte i;

  i = 0;
  read_ok = true;
  write_ok = true;

  if (!SPI_CONFIGURED)
    killspin("tried to test SPI before it was configured!");

  /* cheat a little here */
  SPI_OK = 1;

  /* test reading */
  Serial.print(" [READ: ");

  for (; i < 100; i++)
    if (spi_read(0x0F) != 0x49)
      read_ok = false;

  if (read_ok)
    Serial.print("OK, WRITE: ");
  else
    Serial.print("FAIL, WRITE: ");

  /* test writing with */
  i = 0;
  for (; i < 100; i++) {

    spi_write(0x17, i);

    /* uncomment for write debugging
    Serial.print("WROTE ");
    Serial.print(i);
    Serial.print(" GOT ");
    Serial.println(spi_read(0x17));*/

    if (spi_read(0x17) != i)
      write_ok = false;
  }

  /* write back datasheet-defined default value to test
   * register we used (OFFSET_X_L_M) */
  if (write_ok)
    spi_write(0x17, 0x00);

  /* finish up */
  if (write_ok)
    Serial.print("OK] ");
  else
    Serial.print("FAIL] ");

  if (read_ok && write_ok) {

    SPI_OK = 1;
    return true;

  } else {

    SPI_OK = 0;
    Serial.println(":: failed!");
    return false;
  }
}

void lsm_config() {

  if (!SPI_CONFIGURED || !SPI_OK)
    killspin("tried to configure lsm before spi was configured & tested");

  /* set 16 bit 2's comp. magnetic field offset values for x, y, z.
   * these default to zero as correct offset values depend on your geographical
   * location. you can find the current magnetic field strength at your coords
   * using NOAA's database: http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
   *
   * in the WMM model, the significant values are north comp (z offset), east comp
   * (x offset), and vertical comp (y offset). you can ignore the 'change/year' and
   * 'uncertainty' values
   *
   * you must translate the given tesla values into corresponding gauss equivalents
   * and scale them according to the range specified later in this function. you will
   * usually use the +2:-2 gauss scale. here is an example conversion for 
   * latitude 43° 2' 14" N, longitude 76° 7' 36" W (near syracuse university in
   * syracuse, NY 13210), 0 meters above sea level, taken on february 16th, 2015 at 07:32 UTC:
   *
   * [x] :: -4,129.9 nanoteslas :: -0.041299 gauss
   * [y] :: 49,817.8 nanoteslas ::  0.498178 gauss
   * [z] :: 18,755.9 nanoteslas ::  0.187559 gauss
   *
   * next, we need to fit these values into our 4-gauss scale (spanning from +2 gauss
   * to -2 gauss) in the context of a 16-bit signed number. to do this, take the gauss value
   * and divide it by 2. take this number, and multiply it by 2^15 - 1. round to closest integer.
   * finally, multiply this value by -1 as the offset value combined with the sensed value should
   * result in zero. negatives should be expressed as two's complement. here are the offsets derived
   * from the previous values:
   *
   * [x] ::     677 :: 0x02A5
   * [y] :: -16,324 :: 0xC03C
   * [z] ::  -6,146 :: 0xE7FE
   */ 
  const byte x_lo_offset = 0xA5;
  const byte x_hi_offset = 0x02;
  
  const byte y_lo_offset = 0x3C;
  const byte y_hi_offset = 0xC0;
  
  const byte z_lo_offset = 0xFE;
  const byte z_hi_offset = 0xE7;
  
  spi_write(0x16, x_lo_offset);
  spi_write(0x17, x_hi_offset);
  
  spi_write(0x18, y_lo_offset);
  spi_write(0x19, y_hi_offset);
  
  spi_write(0x1A, z_lo_offset);
  spi_write(0x1B, z_hi_offset);
  
  /* latch magnometer-ready to INT2 output pin */
  spi_write(0x23, 0x04);

  /* enable temperature sensor,
   * select high magnetic resolution,
   * select 100Hz sensor rate */
  spi_write(0x24, 0xF4);

  /* set full scale of magnetometer to +2:-2 gauss as
   * earth's field is usually between +0.65:-0.65 G */
  spi_write(0x25, 0x00);
  
  /* switch on magnometer, to continuous conversion mode */
  spi_write(0x26, 0x00);

  LSM_CONFIGURED = 1;
}

/* returns a size-n array of readings from temperature sensor. function waits
 * between reads for a time equal to the period length of the sensor as to avoid
 * multiple reads of an un-updated value. this is a hack to get around the fact
 * this chip does not have a TEMP_READY bit in a status register like the magnometer
 * or accelerometer do.
 * caller must free() returned pointer */
signed short *pull_temp_values(int n) {

  signed short *ret;
  int sensor_period, i;
  byte *temp_pair, *sync_pair_i, *sync_pair_f;

  /* period length in milliseconds of temperature sensor refresh */
  sensor_period = 1000 / TEMP_FREQ; 

  i = 0;
  ret = (signed short *) calloc(n, 2);

  sync_pair_i = sync_pair_f = spi_multiread(0x05, 2);

  /* spin until sensor cranks */
  while (sync_pair_i == sync_pair_f)
    sync_pair_f = spi_multiread(0x05, 2);
    
  /* wait until mid-period to read as to avoid edge-case duplicates */
  delay(sensor_period / 2);

  for (; i < n; i++) {
    temp_pair = spi_multiread(0x05, 2);
    ret[i] = word(temp_pair[1], temp_pair[0]);
    free(temp_pair);
    delay(sensor_period);
  }
  
  free(sync_pair_i);
  free(sync_pair_f);

  return ret;
}

/* returns a magno_point struct from passed 48 bit input taken from magno sensors */
struct magno_point parse_raw_magno_data(byte *in) {
  
  struct magno_point ret;
  
  ret.x = word(in[1], in[0]);
  ret.y = word(in[3], in[2]);
  ret.z = word(in[5], in[4]);
  
  return ret;
}

/* returns a size-n array of readings from magnometer. result contains 3 words
 * describing felt magnetic field strengths x, y, z directions. function polls INT2 
 * (latched to magnometer-ready signal) until it goes high before reading from sensor.
 * this guarantees each member in returned array is a genuine, non-repeat value from a 
 * single magnometer sensor cycle
 * caller must free() returned pointer */
struct magno_point *pull_magno_values(int n) {
  
  struct magno_point *ret;
  int i;
  
  i = 0;
  ret = (struct magno_point *) calloc(n, sizeof(struct magno_point));
  
  for(; i < n; i++) {
    
    /* spin until sensors are fresh */
    while(digitalRead(MAGNO_RDY_PIN) != 1);
    
    ret[i] = parse_raw_magno_data(spi_multiread(0x08, 6));
  }
  
  return ret;
}

void setup() {

  /* set up magno. read ready signal */
  pinMode(MAGNO_RDY_PIN, INPUT);
  
  /* this is about as fast as you can get on the serial console */
  Serial.begin(115200);
  SERIAL_CONFIGURED = 1;

  /* spi init */
  Serial.print("configuring spi..");
  spi_config();
  Serial.println("done");

  /* spi check */
  Serial.print("testing spi..");
  if (spi_test())
    Serial.println("done");
  else
    killspin("SPI test failed. perhaps the device is not wired correctly or your frequency is too high");

  /* lsm init */
  Serial.print("configuring lsm..");
  lsm_config();
  Serial.println("done");
}

/* demonstration of realtime temperature stream. does not return */
void stream_temp() {

  for(;;) {
    signed short *temp_vals;
    temp_vals = pull_temp_values(100);

    for (int i = 0; i < 100; i++) {
      if(!(i % 20) && i)
        Serial.print('\n');
      else if(i)
        Serial.print(" ");
      
      Serial.print(temp_vals[i], DEC);
    }
  
    Serial.println("\n--------------------------------------------------------------------------------");
  
    free(temp_vals);
  }
}

/* demonstration of realtime magnometer stream. does not return */
void stream_magno() {
  
  struct magno_point *read_buf;

  read_buf = pull_magno_values(100);
  
  for(int i = 0; i < 100; i++) {
    
    Serial.print("X: ");
    Serial.println(read_buf[i].x, DEC);
    Serial.print("Y: ");
    Serial.println(read_buf[i].y, DEC);
    Serial.print("Z: ");
    Serial.println(read_buf[i].z, DEC);
    Serial.println("---------");
  }
  
  free(read_buf);
}

void loop() {
  
  stream_temp();
  //stream_magno();
}