/*
    ESS1995 communications test

    i2c version

    http://arduino.cc/playground/Code/ATMELTWI

    i2c library seems to require a 'register address' - -ESS1995 doesn't need this?
    it certainly doesn't define a register in control byte 1 of write message
*/

#include "electic.h"

extern "C" void __cxa_pure_virtual()
{
  cli();
  for (;;);
}

int main(){

    init(); //-- init Arduino library - delay, PWM setup messes with timers, millis(), micros() etc don't work without it

    Wire.begin();


    pinMode(CS,OUTPUT);
    digitalWrite(CS,LOW); //turn off DMM, maybe it doesn't like waiting around

    setupBlueTooth();

/*
    pinMode(DATA_NEW,INPUT);
    pinMode(SCLK,OUTPUT);
    pinMode(BTC,INPUT);

    pinMode(OUTPUT1, OUTPUT);
    pinMode(OUTPUT2, OUTPUT);

    digitalWrite(OUTPUT2,HIGH);
*/
    digitalWrite(CS,HIGH); //select DMM

    uint8_t WB1,WB2,WB3,WB4;
/*
    uint8_t WB1 = (CB_SHBP << 7) | (CB_function << 3) | (CB_VARCrange);
    uint8_t WB2 = (CB_BUZfreq << 5) | (CB_ADCrate << 3) | (CB_FREQrange);
    uint8_t WB3 = (CB_ACmode << 7) | (CB_EXTdiode << 4) | (CB_DCfs << 3) | (CB_LPFbw << 1) | (CB_FREQinprest);
    uint8_t WB4 = (CB_PEAKmode << 7) | (CB_PCALmode << 6) | (CB_OPconf << 2) | (CB_ADPext);
*/
    Wire.beginTransmission(WRITE_ID);
    Wire.send(WB1);
    Wire.send(WB2);
    Wire.send(WB3);
    Wire.send(WB4);
    //default reads
    Wire.endTransmission();


    uint16_t delayMillis=250;

    //send 1st write to initiate readings - not necessary - what is default mode?




/*
writeDMM();
    delay(delayMillis);
    startWrite();
    writeStartBit();
    writeByte(WRITE_ID);
    writeByte(WB1);
    writeByte(WB2);
    writeByte(WB3);
    writeByte(WB4);
    writeStopBit(); //is ACK already the same as stop bit?
    */







    //variables to read
    uint8_t DB_STATUS;
    /*
    uint8_t DB_ASIGN;
    uint8_t DB_BSIGN;
    uint8_t DB_PMAX;
    uint8_t DB_PMIN;
    uint8_t DB_BTS;
    uint8_t DB_ALARM;
    uint8_t DB_HF;
    uint8_t DB_LF;
    uint8_t DB_LDUTY;
    uint8_t DB_STA;
    uint8_t DB_F_FIN;
    */
    uint32_t DB_D0;
    uint16_t DB_D1;
    uint32_t DB_D2;
    uint32_t DB_D3;

    //delay(delayMillis);
    //writeDMM();

    while(1) {
        if (digitalRead(DATA_NEW)==HIGH){ //DMM is ready to send data
            Wire.beginTransmission(READ_ID);
            if (digitalRead(DATA_NEW)==LOW){ //ID confirmed
                for (int i=0;i<10;i++) {
                    data[i] = Wire.receive();
                }
                Wire.endTransmission();

                //parse data
                DB_STATUS=data[0];
                /*
                DB_ASIGN=data[0]>>7;
                DB_BSIGN=(data[0]>>6) & 0b01;
                DB_PMAX=(data[0]>>5) & 0b01;
                DB_PMIN=(data[0]>>4) & 0b01;
                DB_BTS=(data[0]>>2) & 0b11;
                DB_ALARM=data[0]& 0b01;
                DB_HF=data[1]>>7;
                DB_LF=(data[1]>>6) & 0b01;
                DB_LDUTY=(data[1]>>5) & 0b01;
                DB_STA=((data[1]>>3) & 0b10)|((data[0]>>1) & 0b01);
                DB_F_FIN=(data[1]>>3) & 0b01;
                */
                DB_D0=(((uint32_t)( data[1] & 0b111))<<16)|(((uint32_t)data[2])<<8)|data[3];
                DB_D1=(((uint16_t)data[4])<<2)|((data[5] & 0b11000000)>>6);
                DB_D2=(((uint32_t)( data[5] & 0b111111))<<13)|(((uint32_t) data[6] )<<5)|(((uint32_t)( data[7] & 0b11111000))>>3);
                DB_D3=(((uint32_t)( data[7] & 0b111))<<16)|(((uint32_t) data[8])<<8)|data[9];


                while (Serial.available() > 0) {
                    // get incoming byte:
                    uint8_t sampleCode = Serial.read();
                    switch (sampleCode) {
                        case 1:
                            delayMillis=1000;
                            break;
                        case 2:
                            delayMillis=250;
                            break;
                        case 3:
                            delayMillis=50;
                            break;
                    }
                }


                if (digitalRead(BTC)){
                    if (!connected) connected=true;
                    // data packet :
                    // ID (2 byte)
                    // no of bytes (max 255)
                    // data bytes
                    Serial.write((uint8_t*)&ELECTIC_ID,2);
                    Serial.write((uint8_t*)&packetSize,1);
                    Serial.write((uint8_t*)&DB_STATUS,1);
                    Serial.write((uint8_t*)&DB_D0,4);
                    Serial.write((uint8_t*)&DB_D1,2);
                }
                else {
                    if (connected) {
                        advertise();
                        connected=false;
                    }
                }
            }
        }

        delay(delayMillis);
        digitalWrite(OUTPUT1,LOW);
        digitalWrite(OUTPUT2,LOW);
        //delay(delayMillis);
    }
}