#include "mbed.h"

SPI         sensor(p5, p6, p7);
PwmOut      clk(p21);
Serial      pc(USBTX, USBRX);
uint16_t    word1 = 0;
uint16_t    word2 = 0;
uint16_t    word3 = 0;
uint16_t    word4 = 0;

uint16_t    c1 = 0;
uint16_t    c2 = 0;
uint16_t    c3 = 0;
uint16_t    c4 = 0;
uint16_t    c5 = 0;
uint16_t    c6 = 0;

uint16_t    d1 = 0;
uint16_t    d2 = 0;

uint16_t    ut1 = 0;
int         dT = 0;
int         temp = 0;
int         off = 0;

uint16_t   sens = 0;
uint16_t   x = 0;
uint32_t   p = 0;
uint8_t     i = 0;
int main()
{
    clk.write(0.5f);
    clk.period_us(30.517);
    sensor.frequency(500000);
    sensor.format(8,1);
    //reset
    pc.printf("%x ",sensor.write(0x15));
    pc.printf("%x ",sensor.write(0x55));
    pc.printf("%x\r\n\n",sensor.write(0x40));

    //word1
    pc.printf("%x ",sensor.write(0x1D));
    pc.printf("%x \r\n\n",sensor.write(0x50));
    word1 = sensor.write(0x00)<<8;
    word1 = word1 | sensor.write(0x00);

    pc.printf("Word1: %x\r\n\n",word1);

    //word2
    pc.printf("%x ",sensor.write(0x1D));
    pc.printf("%x\r\n",sensor.write(0x60));
    word2 = sensor.write(0x00)<<8;
    word2 = word2 | sensor.write(0x00);

    pc.printf("Word2: %x\r\n",word2);

    //word3
    pc.printf("%x ",sensor.write(0x1D));
    pc.printf("%x\r\n\n",sensor.write(0x90));
    word3 = sensor.write(0x00)<<8;
    word3 = word3 | sensor.write(0x00);

    pc.printf("Word3: %x\r\n\n",word3);

    //word4
    pc.printf("%x ",sensor.write(0x1D));
    pc.printf("%x\r\n\n",sensor.write(0xA0));
    word4 = sensor.write(0x00)<<8;
    word4 = word4 | sensor.write(0x00);

    pc.printf("Word4: %x\r\n\n",word4);


    c1 = word1 >> 1;
    pc.printf("C1: %x\r\n\n",c1);

    c2 = (word3 & 0x3F) << 6;
    c2 = c2 | (word4 & 0x3F);
    pc.printf("C2: %x\r\n\n",c2);

    c3 = (word4 & 0xFFC0) >> 6;
    pc.printf("C3: %x\r\n\n",c3);

    c4 = (word3 & 0xFFC0) >> 6;
    pc.printf("C4: %x\r\n\n",c4);

    c5 = (word1 & 0x0001) << 10;
    c5 = ((word2 & 0xFFC0) >> 6) | c5;
    pc.printf("C5: %x\r\n\n",c5);


    c6 = word2 & 0x3F;
    pc.printf("C6: %x\r\n\n",c6);

    while (1) {
        //Read digital pressure value from MS5534A
        sensor.write(0x0F);
        sensor.write(0x40);
        wait_ms(36);
        d1 = (sensor.write(0x00)<<8);
        d1 = d1 | sensor.write(0x00);
        //pc.printf("d1: %x\r\n\n",d1);

        //Read digital temperature value from MS5534A
        sensor.write(0x0F);
        sensor.write(0x20);
        wait_ms(36);
        d2 = (sensor.write(0x00)<<8);
        d2 = d2 | sensor.write(0x00);
        //pc.printf("d2: %i\r\n\n",d2);

        //Calculate calibration temperature
        ut1 = 8*c5+20224;
        //pc.printf("UT1: %i\r\n\n", ut1);

        dT = d2 - ut1;
        //pc.printf("dT: %i\r\n\n",dT);
        //Actual temperature:
        temp += 200 + dT *(c6+50)/1024;



        off = c2*4 + ((c4-512)*dT)/4096;

        sens = c1 + (c3*dT)/1024 + 24576;

        x = (sens * (d1-7168))/16384 - off;

        p += x*100/32 + 250*100;



        i++;
        if(i == 25) {
            pc.printf("Temperatur: %i\r\n\n",temp/25);
            pc.printf("Druck: %i\r\n\n",p/25);
            i = 0;
            p = 0;
            temp = 0;
            //wait_ms(500);
        }
    }
}