#include <MyWireLib.h>
MyWireLib Sens[2];

#define PRELOAD 0xFF06 // = 65536 - 250 decimal gives is 1000 interrupts per second
#define S0 0
#define S1 1

//vars
int16_t  ac1[2], ac2[2], ac3[2], b1[2], b2[2], mb[2], mc[2], md[2]; // Store sensor PROM values from BMP180
uint16_t ac4[2], ac5[2], ac6[2];                     // Store sensor PROM values from BMP180
const uint8_t oss = 3;                      // Set oversampling setting
const uint8_t osd = 26;                     // with corresponding oversampling delay
float T[2], P[2];                                 // Set global variables for temperature and pressure
unsigned int count = 0; // counting interrupts
unsigned int h,m,oldsec, sec = 0; // counting seconds
bool lock, enable;
int state1 = S0,state2 = S0; // or whatever is the initial state.



void setup()
{
Serial.begin(9600);
//declare pins
Sens[0].SCLpinI=2;
Sens[0].SDApinI=3;
Sens[1].SCLpinI=4;
Sens[1].SDApinI=5;
Sens[0].Soss=oss;
Sens[1].Soss=oss;

//> for test
Serial.print("SCLPIN for sensor 0: "); Serial.println(Sens[0].SCLpinI);
Serial.print("SDAPIN for sensor 0: ");Serial.println(Sens[0].SDApinI);
Serial.print("SCLPIN for sensor 1: ");Serial.println(Sens[1].SCLpinI);
Serial.print("SDAPIN for sensor 1: ");Serial.println(Sens[1].SDApinI);
//< for test
 delay(500);

 Sens[0].InitWire();
 Sens[1].InitWire();

 init_SENSOR(0);
 init_SENSOR(1);

}

void loop()
{

 int32_t b5[2];

   for(int eachsens = 0; eachsens < 2 ; eachsens++)
   {
       b5[eachsens]= temperature(eachsens);             // Read and calculate temperature (T)
       P[eachsens] = pressure(b5[eachsens],eachsens);   // Read and calculate pressure (P)

       Serial.print("Sensor ");
       Serial.print(eachsens);
       Serial.print(" => ");
       Serial.print("Temperature: ");
       Serial.print(T[eachsens], 2);
       Serial.print(" C");
       Serial.print(" | Pressure: ");
       Serial.print(P[eachsens], 2);
       Serial.println(" mbar");

   }
    delay(200);                               // Delay between each readout
}

void init_SENSOR(int sensnr)
{
  ac1[sensnr] = Sens[sensnr].Get16bitFromRegister(0xAA);
  ac2[sensnr] = Sens[sensnr].Get16bitFromRegister(0xAC);
  ac3[sensnr] = Sens[sensnr].Get16bitFromRegister(0xAE);
  ac4[sensnr] = Sens[sensnr].Get16bitFromRegister(0xB0);
  ac5[sensnr] = Sens[sensnr].Get16bitFromRegister(0xB2);
  ac6[sensnr] = Sens[sensnr].Get16bitFromRegister(0xB4);
  b1[sensnr]  = Sens[sensnr].Get16bitFromRegister(0xB6);
  b2[sensnr]  = Sens[sensnr].Get16bitFromRegister(0xB8);
  mb[sensnr]  = Sens[sensnr].Get16bitFromRegister(0xBA);
  mc[sensnr]  = Sens[sensnr].Get16bitFromRegister(0xBC);
  md[sensnr]  = Sens[sensnr].Get16bitFromRegister(0xBE);

  Serial.println("");
  Serial.print("Sensor ");  Serial.print(sensnr);  Serial.println(" calibration data:");
  Serial.print(F("AC1 = ")); Serial.println(ac1[sensnr]);
  Serial.print(F("AC2 = ")); Serial.println(ac2[sensnr]);
  Serial.print(F("AC3 = ")); Serial.println(ac3[sensnr]);
  Serial.print(F("AC4 = ")); Serial.println(ac4[sensnr]);
  Serial.print(F("AC5 = ")); Serial.println(ac5[sensnr]);
  Serial.print(F("AC6 = ")); Serial.println(ac6[sensnr]);
  Serial.print(F("B1 = "));  Serial.println(b1[sensnr]);
  Serial.print(F("B2 = "));  Serial.println(b2[sensnr]);
  Serial.print(F("MB = "));  Serial.println(mb[sensnr]);
  Serial.print(F("MC = "));  Serial.println(mc[sensnr]);
  Serial.print(F("MD = "));  Serial.println(md[sensnr]);
  Serial.println("");
}

/**********************************************
  Calcualte pressure readings
 **********************************************/
float pressure(int32_t b5, int sensnr)
{
  int32_t x1, x2, x3, b3, b6, p, UP;
  uint32_t b4, b7;

  UP = read_pressure(sensnr);  // Read raw pressure

  b6 = b5 - 4000;
  x1 = (b2[sensnr] * (b6 * b6 >> 12)) >> 11;
  x2 = ac2[sensnr] * b6 >> 11;
  x3 = x1 + x2;
  b3 = (((ac1[sensnr] * 4 + x3) << oss) + 2) >> 2;
  x1 = ac3[sensnr] * b6 >> 13;
  x2 = (b1[sensnr] * (b6 * b6 >> 12)) >> 16;
  x3 = ((x1 + x2) + 2) >> 2;
  b4 = (ac4[sensnr] * (uint32_t)(x3 + 32768)) >> 15;
  b7 = ((uint32_t)UP - b3) * (50000 >> oss);
  if(b7 < 0x80000000) { p = (b7 << 1) / b4; } else { p = (b7 / b4) << 1; } // or p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
  x1 = (p >> 8) * (p >> 8);
  x1 = (x1 * 3038) >> 16;
  x2 = (-7357 * p) >> 16;
  return (p + ((x1 + x2 + 3791) >> 4)) / 100.0f; // Return pressure in mbar
}

/**********************************************
  Read uncompensated temperature
 **********************************************/
int32_t temperature(int sensnr)
{
  int32_t x1, x2, b5, UT;
  byte tobesend[2] = {0xf4, 0x2e};
  Sens[sensnr].sendbytes(tobesend,2);

  delay(5);
  UT = Sens[sensnr].Get16bitFromRegister(0xf6);

  // Calculate true temperature
  x1 = (UT - (int32_t)ac6[sensnr]) * (int32_t)ac5[sensnr] >> 15;
  x2 = ((int32_t)mc[sensnr] << 11) / (x1 + (int32_t)md[sensnr]);
  b5 = x1 + x2;
  T[sensnr]  = (b5 + 8) >> 4;
  T[sensnr] = T[sensnr] / 10.0;                           // Temperature in celsius
  return b5;
}
/**********************************************
  Read uncompensated pressure value
 **********************************************/
int32_t read_pressure(int sensnr)
{
  int32_t value;
  byte tobesend[2] = {0xf4, (0x34 + (oss << 6))};

  Sens[sensnr].sendbytes(tobesend,2);
  delay(osd);

  value= Sens[sensnr].Get24bitFromRegister(0xf6);
  return (value); // Return value

  // initialize Timer1
cli();         // disable global interrupts
TCCR1A = 0;    // set entire TCCR1A register to 0
TCCR1B = 0;    // set entire TCCR1B register to 0
               // (as we do not know the initial  values)

// enable Timer1 overflow interrupt:
TIMSK1 |= (1 << TOIE1); //Atmega8 has no TIMSK1 but a TIMSK register
// Preload
TCNT1= PRELOAD;
TCCR1B |= (1 << CS11); // Sets bit CS11 in TCCR1B
TCCR1B |= (1 << CS10); // and CS10
//the clock source is divided by 64, i.e. one clock cycle every 64 / (16 * 10^6) = 4 * 10^(-6) = 0.000004s

// This is achieved by shifting binary 1 (0b00000001)
// to the left by CS11 or CS10 bits. This is then bitwise
// OR-ed into the current value of TCCR1B, which effectively set
// this one bit high.
// enable global interrupts:
sei();
Serial.begin(9600); // opens serial port, sets data rate to 9600 bps ready for debug.
count = 0;
h=m=sec = 0;oldsec = 10;
lock=enable=true;
}

// here is the ISR executed by the CPU when the corresponding interrupt occurs


{
  TCNT1=PRELOAD; // reload the timer preload
  if (enable) count++;
  if (count >= 1000 && lock) {
    count -= 1000; sec++;
  }
}



void loop() {
 switch (state1) {
 S0:byte tobesend[2] = {0xf4, 0x2e}; Sens[sensnr].sendbytes(tobesend,2);
timestamp = count; state1 = S1; break;
 S1:if (timestamp + 5 <= count) { state1 = S3; break;}
else { state1 = S1; break;}
 S3:UT = Sens[sensnr].Get16bitFromRegister(0xf6);
//calibration calculation
state1 = S0; break;
 }
switch (state2) {
 S0:byte tobesend[2] = {0xf4, 0x2e}; Sens[sensnr].sendbytes(tobesend,2);
timestamp = count; state2 = S1; break;
 S1:if (timestamp + 5 <= count) { state2 = S3; break;}
else { state2 = S1; break;}
 S3:UT = Sens[sensnr].Get16bitFromRegister(0xf6);
//calibration calculation
state2 = S0; break;
 }
}