//----------------------------------------------------------------------------------
// <copyright file="User_Pressure.c" company="LiquidRobotics">
//	Copyright (c) Liquid Robotics Corporation.  All rights reserved.
// </copyright>
//
// <summary>
// 	Module that manages communication with the pressure sensor
// </summary>
//
// <owner>Jim Kirklin</owner>
//---------------------------------------------------------------------------------

#include "includes.h"
#include <avr/io.h>
#include <avr/wdt.h>
#include <avr/power.h>
#include "user_app_init.h"
#include "uart.h"
#include "pib.h"
#include "timer.h"
#include "lrstat.h"
#include "spidebug.h"
#include "wdtrig.h"
#include "int_math.h"

#include "User_Health.h"




static struct 
{
    int16_t c1;         // pressure sensitivity
    int16_t c2;         // pressure offset
    int16_t c3;         // temperature coefficient of pressure sensitivity
    int16_t c4;         // temperature coefficient of pressure offset
    int16_t ut1;         // reference temperature
    int16_t c6;         // temperature coefficient of the temperature?
} pressure_cal;

static int16_t d1, d2, temp, pressure;
static int16_t dt;

//pres_meas_state_t pres_meas_state;

static void spi_outw(uint16_t data);
static uint16_t spi_inw(void);
static void read_pressure_cal(void);
static void reset_pressure_seq(void);


// send out 16 bits high byte followed by low byte
static void spi_outw(uint16_t data)
{
    volatile uint8_t tmp;

    // assume SPI is idle
    SPCR &= ~_BV(CPHA); // set clock phase to zero (data changes on falling edge)

    SPDR = data >> 8;
    loop_until_bit_is_set(SPSR, SPIF);
    SPDR = data & 0xFF;
    tmp = SPDR;             // empty the receive
    loop_until_bit_is_set(SPSR, SPIF);
    tmp = SPDR;             // empty the receive
}


// always assume SPI is idle on entry. Data comes in high-byte followed by low byte
static uint16_t spi_inw(void)
{
    uint16_t tmp;

    // assume SPI is idle...
    SPCR |= _BV(CPHA);   // clock phase = 1 (data sampled on rising edge)

    SPDR = 0;
    loop_until_bit_is_set(SPSR, SPIF);
    SPDR = 0;
    tmp = SPDR << 8;             // empty the receive
    loop_until_bit_is_set(SPSR, SPIF);
    tmp |= SPDR;             // empty the receive
    return tmp;
}



static void read_pressure_cal(void)
{
    uint16_t cal1, cal2, cal3, cal4;

    spi_outw(0x1D50);   // read cal word 1
    cal1 = spi_inw();
    spi_outw(0x1D60);   // read cal word 2
    cal2 = spi_inw();
    spi_outw(0x1D90);   // read cal word 3
    cal3 = spi_inw();
    spi_outw(0x1DA0);   // read cal word 4
    cal4 = spi_inw();

// unpack the parameters. Assume they're all positive. Go ahead and
// perform the constant math on the constants
    pressure_cal.c1 = (cal1 >> 4) + 3000;
    pressure_cal.c2 = (((cal1 & 7) << 10) | (cal2 >> 6)) + 10000;
    pressure_cal.c3 = (cal3 >> 6) + 200;
    pressure_cal.c4 = (cal4 >> 7) - 250;
    pressure_cal.ut1 = 8*(((cal2 & 63) << 6) | (cal3 & 63)) + 10000;
    pressure_cal.c6 = (cal4 & 127) + 100;
}


static void reset_pressure_seq(void)
{
    volatile uint8_t tmp;

    // this sequence resets the pressure sensor
    SPDR = 0x15;
    loop_until_bit_is_set(SPSR, SPIF);
    SPDR = 0x55;
    tmp = SPDR;
    loop_until_bit_is_set(SPSR, SPIF);
    SPDR = 0x40;
    tmp = SPDR;
    loop_until_bit_is_set(SPSR, SPIF);
    tmp = SPDR;
}


void init_pressure(void)
{

    power_spi_enable();
    power_timer1_enable();

    // set sclk and MOSI as outputs (Bit 0 must be set to avoid SPI port lockup)
    DDRB |= _BV(5) | _BV(2) | _BV(1) | _BV(0);
    PORTB |= _BV(0);    // set this bit high to avoid SPI port lockup

    // set up the SPI port. Serial clock is 500KHz maximum (/16)
    // spi mode is set up fclk/16, master operation, and clock phase 0
    SPCR = _BV(SPE) | _BV(SPR0) | _BV(MSTR) | _BV2(0, CPHA);

    TCCR1A = (1 << COM5A0); // toggle on output compare, WGM 4
    TCCR1B = (1 << WGM52);  // WGM = 4, initially stopped
    TCNT1 = 0;
    OCR1A = 244/2;

    reset_pressure_seq();
    read_pressure_cal();
}




void measure_pressure_start(void) 
{
    int16_t offs, sens;

    power_spi_enable();
    power_timer1_enable();

    // turn on the clock source
    TCCR1B |= 1;

    // send the conversion command for the temperature (D1)
    spi_outw(0x0F40);

	OSTimeDlyHMSM(0,0,0,50);

    d1 = spi_inw();
    spi_outw(0x0F20);       // setup to read D2

	OSTimeDlyHMSM(0,0,0,50);

    d2 = spi_inw();
    TCCR1A &= ~7;           // clock off

    // compute temperature
    dt = d2 - pressure_cal.ut1;
    temp = 200 + (int16_t)(((int32_t)dt * pressure_cal.c6) >> 11);

    // compute the pressure
    offs = pressure_cal.c2 + (int16_t)(((int32_t)pressure_cal.c4 * dt) >> 12);
    sens = pressure_cal.c1 + (int16_t)(((int32_t)pressure_cal.c3 * dt) >> 13);
    pressure = (int16_t)(((int32_t)sens * (d1 - offs)) >> 12) + 1000;
    power_timer1_disable();
    power_spi_disable();

}


void get_temp_pres(int16_t * t, int16_t * p)
{
    *t = temp / 10;
    *p = pressure / 10;
}

/*


#include "includes.h"




*/