#include <hidef.h>

#include "derivative.h"
#include "MC9S12XS128.h"

/* laziness */
#define null16 0x0000
#define null8 0x00

/* x, y, z enum */
typedef enum { X, Y, Z } axis;

/* gets magnetic field gauss val from lsm
 * over spi & parses into passed int ptr
 */
void read_axis(axis x, int *hi, int *lo) {

  switch(x) {

    case X:

      SPI0DR = 0x8900;
      while(!SPI0SR_SPTEF);
      *hi = SPI0DRL;

      SPI0DR = 0x8800;
      while(!SPI0SR_SPTEF);
      *lo = SPI0DRL;

      break;

    case Y:

      SPI0DR = 0x8B00;
      while(!SPI0SR_SPTEF);
      *hi = SPI0DRL;

      SPI0DR = 0x8A00;
      while(!SPI0SR_SPTEF);
      *lo = SPI0DRL;

      break;

    case Z:

      SPI0DR = 0x8D00;
      while(!SPI0SR_SPTEF);
      *hi = SPI0DRL;

      SPI0DR = 0x8C00;
      while(!SPI0SR_SPTEF);
      *lo = SPI0DRL;

      break;
  }
}

/* parses 8 bit HI/LO pairs into x/y/z words */
void parse_raw(int in[], long set[]) {

  set[0] = (in[0] << 8) + in[1];
  set[1] = (in[2] << 8) + in[3];
  set[2] = (in[4] << 8) + in[5];
}

void configure_lsm() {

  /* wait for SPI ready */
  while(!SPI0SR_SPTEF);

  /* fill transfer reg */
  SPI0DR = 0x2474;

  /* wait for transfer */
  while(!SPI0SR_SPTEF);
}

void configure_spi() {

  /* reroute spi I/O */
  MODRR_MODRR4 = 1;

  /* configure SPI */
  SPI0CR1_SSOE = 1; /* enable slave select */
  SPI0CR1_MSTR = 1; /* enable master */
  SPI0CR1_CPOL = 0; /* clock polarity */
  SPI0CR1_SPE = 1; /* enable system */
  SPI0CR1_LSBFE = 1; /* set endian-ness */

  SPI0CR2_XFRW = 1; /* set 16 bit width */
  SPI0CR2_MODFEN = 1; /* enable slave select */
}

void main(void) {

    /* SDA :: SPI in :: PM2 :: white
     * SDO :: SPI out :: PM4 :: blue
     * SCL :: clock :: PM5 :: yellow
     * CS :: select :: PM3 :: green
     */

    /* positional vars & init */
    long coords[3] = { null16, null16, null16 };
    int coord8[6] = { null8, null8, null8, null8, null8, null8 };

  /* configure SPI */
  configure_spi();

  /* configure LSM */
  configure_lsm();

  /* latch intrs.  */
  EnableInterrupts;

  for(;;) {

    /* read x hi/lo */
    read_axis(X, &coord8[0], &coord8[1]);

    /* read y hi/lo */
    read_axis(Y, &coord8[2], &coord8[3]);

    /* read z hi/lo */
    read_axis(Z, &coord8[4], &coord8[5]);;

    /* parse raw bytes into x/y/z vars */
    parse_raw(coord8, coords);
  }
}