/*
    ESS1995 communications test

    proof of concept


*/

#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

    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





    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
            digitalWrite(OUTPUT1,HIGH);
            digitalWrite(OUTPUT2,LOW);
            startWrite();
            writeStartBit();
            writeByte(READ_ID);
            if (digitalRead(DATA_NEW)==LOW){ //ID confirmed
                digitalWrite(OUTPUT2,HIGH);
                startRead();
                for (int i=0;i<10;i++) {
                    data[i] = readByte();
                }
                //stop bit comes from the MPU? does seem to be necessary.
                startWrite();
                writeStopBit();

                //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);
    }
}
void startWrite() {
    pinMode(SDATA,OUTPUT);
    digitalWrite(SCLK, HIGH);
}
void startRead() {
    pinMode(SDATA,INPUT);
    digitalWrite(SCLK, HIGH);
}
void SCLKcycle_W() {
    //insert a high clock cycle, normally LOW
    delayMicroseconds(START_DELAY);
    digitalWrite(SCLK, HIGH);
    delayMicroseconds(CLOCK_DELAY);
    digitalWrite(SCLK, LOW);
    delayMicroseconds(START_DELAY);
}
void SCLKcycle_R() {
    //insert a low clock cycle, normally HIGH
    delayMicroseconds(START_DELAY);
    digitalWrite(SCLK, LOW);
    delayMicroseconds(CLOCK_DELAY);
    digitalWrite(SCLK, HIGH);
    delayMicroseconds(START_DELAY);
}
void writeByte(uint8_t data){

    for (uint8_t i=0,bit=7;i<8;i++,bit--) {
        digitalWrite(SDATA,(data>>bit)&0b01);
        SCLKcycle_W();
    }
    digitalWrite(SDATA,HIGH); //ACK/ stopbit should be HIGH?
    SCLKcycle_W(); //ACK INTERVAL
}
void writeStopBit(){
    digitalWrite(SDATA,LOW); //prepare SDATA
    delayMicroseconds(START_DELAY);
    digitalWrite(SCLK,HIGH);
    delayMicroseconds(START_DELAY);
    digitalWrite(SDATA,HIGH); //stop bit
}
void writeStartBit(){
    digitalWrite(SDATA, HIGH); //should be anyway
    digitalWrite(SCLK, HIGH);
    delayMicroseconds(START_DELAY);
    digitalWrite(SDATA,LOW); //start bit
    delayMicroseconds(START_DELAY);
    digitalWrite(SCLK, LOW);
    delayMicroseconds(START_DELAY);
}
uint8_t readByte(){
    uint8_t data=0;
    uint8_t thisdata;
    //delayMicroseconds(START_DELAY);
    //digitalWrite(SCLK, HIGH); //load first bit
    delayMicroseconds(START_DELAY);
    for (uint8_t i=0,bit=7;i<8;i++,bit--) {
        thisdata=digitalRead(SDATA);
        data = data | (thisdata<<bit);
        digitalWrite(OUTPUT1,thisdata);
        SCLKcycle_R();
    }
    SCLKcycle_R(); //ACK
    return data;
}
void writeDMM(){
    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);

    startWrite();
    writeStartBit();
    writeByte(WRITE_ID);
    writeByte(WB1);
    writeByte(WB2);
    writeByte(WB3);
    writeByte(WB4);
    writeStopBit();

}