#include <msp430x14x.h>

#include <stdio.h>
#include <math.h>   

// #include "uart.h"

#include "basic.h"
#include "sensor.h"

// 0=CLK, 1=OUT, 2=IN

#define	SET_DATA_OUT      P4OUT |=  BIT1
#define CLR_DATA_OUT      P4OUT &= ~BIT1  
#define GET_DATA(result)  result = ((P4IN & BIT2) >> 2)
#define	SET_SCK           P4OUT |=  BIT0 
#define CLR_SCK           P4OUT &= ~BIT0 

static unsigned int W1;
static unsigned int W2;
static unsigned int W3;
static unsigned int W4;

static float C1;
static float C2;
static float C3;
static float C4;
static float C5;
static float C6;

static const unsigned int measurePressure    = 0x2F;
static const unsigned int measureTemperature = 0x4F;

#define DELAY_VALUE (10)

static void mydelay(unsigned int delayVal)
{
  volatile unsigned int i;
  for (i = 0; i > delayVal; i++)
    _NOP();
}

static void delayOnePulse(void)
  {
  mydelay(DELAY_VALUE);
  }



static unsigned int waitUntilDataOutLow(void)
{
  unsigned int count = 0;
  unsigned int data = 1;

  do {
    mydelay(DELAY_VALUE); 
    GET_DATA(data);
    count = count + 1;
    if (count > 10000)
      return 1;
  } while (data != 0);

  return 0;
}

static unsigned int readSixteenBits(void)
{
  unsigned int i;
  unsigned int value;
  unsigned int temp;

  value = 0;
  CLR_SCK;
  mydelay(DELAY_VALUE);

  for (i = 0; i < 16; i++) {
    SET_SCK;
    mydelay(DELAY_VALUE);
    CLR_SCK;
    mydelay(DELAY_VALUE);
    GET_DATA(temp);
    value = (value << 1) | temp;
    mydelay(DELAY_VALUE);
  }

  return value;
}

static void writeBitsLsbFirst(unsigned int data, unsigned int bits)
{
  unsigned int i;
  unsigned int temp;

  CLR_SCK;
  mydelay(DELAY_VALUE); 
  
  for (i = 0; i < bits; i++) {
    temp = data & 1;
    if (temp) 
      SET_DATA_OUT;
    else
      CLR_DATA_OUT;

    // symmetric clock pulse
    mydelay(DELAY_VALUE);
    SET_SCK;
    mydelay(DELAY_VALUE);
    CLR_SCK;
    mydelay(DELAY_VALUE);
    data = data >> 1;
  }
}

static unsigned int getW1(void)
{
  unsigned int value;

  writeBitsLsbFirst(0x57, 8);
  writeBitsLsbFirst(0x01, 5);
  value = readSixteenBits();

  return value;
}

static unsigned int getW2(void)
{
  unsigned int value;

  writeBitsLsbFirst(0xD7, 8);
  writeBitsLsbFirst(0x00, 5);
  value = readSixteenBits();

  return value;
}

static unsigned int getW3(void)
{
  unsigned int value;

  writeBitsLsbFirst(0x37, 8);
  writeBitsLsbFirst(0x01, 5);
  value = readSixteenBits();

  return value;
}

static unsigned int getW4(void)
{
  unsigned int value;

  writeBitsLsbFirst(0xB7, 8);
  writeBitsLsbFirst(0x00, 5);
  value = readSixteenBits();

  return value;
}

static void convertCalibrationDataIntoCoefficients(void)
{
  unsigned int x;
  unsigned int y;

  // Get calibration data from M5535 sensor  
  W1 = getW1();
  W2 = getW2();
  W3 = getW3();
  W4 = getW4();

  // convert calibrations into coefficients

  C1   = (float) ((W1 >> 3) & 0x1FFF);
  x    = (W1 << 10) & 0x1C00;
  y    = (W2 >> 6)  & 0x03FF;
  C2   = (float) (x | y);
  C3   = (float) ((W3 >> 6) & 0x03FF);
  C4   = (float) ((W4 >> 7) & 0x01FF);
  x    = (W2 << 6) & 0x0FC0;
  y    =  W3       & 0x003F; 
  C5   = (float) (x | y);
  C6   = (float) (W4 & 0x007F);
}

static unsigned int getMeasurement(unsigned int command, unsigned int *measurement)
{
  unsigned int errorVal;
  unsigned int sdo;
  unsigned int value;

  errorVal = 0;
  writeBitsLsbFirst(command, 8);
  writeBitsLsbFirst(0x00, 4);
  GET_DATA(sdo);
  if (sdo & 1) {  
    errorVal = waitUntilDataOutLow();
    if (errorVal == 0) {
      value = readSixteenBits();
      *measurement = value;
    }
  } else {
    errorVal = 1;
  }

  writeBitsLsbFirst(0x00, 1);
  
  return errorVal;
}

static void M5535A_Reset(void)
{
  writeBitsLsbFirst(0x55, 8);
  writeBitsLsbFirst(0x55, 8);
  writeBitsLsbFirst(0x00, 5);
}

int M5535A_Pressure(float *pressure)
{
  float rawPressure;
  float rawTemperature;
  float offset;
  float sensitivity;
  float dt;
  unsigned int m1;
  unsigned int m2;
  unsigned int errorVal;

  M5535A_Reset();

  errorVal  = getMeasurement(measurePressure,&m1);
  errorVal += getMeasurement(measureTemperature,&m2); 

  if (errorVal == 0) {
    rawPressure    = (float) m1;
    rawTemperature = (float) m2;

    // Calculate the difference between the actual and reference temperature
    dt     = rawTemperature - (8.0 * C5 + 10000.0); 

    // Calculate offset at actual temperature
    offset = C2 + (C4 - 250.0) * dt / 4096.0 + 10000.0;

    // Calculate sensitivity at actual temperature
    sensitivity = C1 / 2.0 + (C3 + 200.0) * dt / 8192.0 + 3000.0;

    *pressure = sensitivity * (rawPressure - offset) / 4096.0 + 1000.0;  
  }

  return errorVal;
}

void M5535A_Init(void)
{
  M5535A_Reset();
  convertCalibrationDataIntoCoefficients();
  
  sensorRegisterSampleMethod(SENSOR_ID_PRESSURE, 0, M5535A_Pressure);
}