double acc[3];

/****************/

int i2cspeed = 100;

void setup() {
  Serial.begin(57600);
  delay(1000);
  endTransmission(A4, A5);
  delay(100);
  initializeAccel(0x68, A4,A5);
  delay(100);
  Serial.println("init accel");

}



void loop() {
  readAccelRaw(0x68, A4, A5);
  double accAngleX = atan(acc[0] / sqrt(pow(acc[0], 2) + pow(acc[2], 2 ))) / (PI / 180);
  double accAngleY = atan(acc[1] / sqrt(pow(acc[0], 2) + pow(acc[2], 2 ))) / (PI / 180);

  Serial.println(accAngleY);
  //  Serial.print("{ \"a0x\" :\"");
  //  Serial.print(accel[0]);
  //  Serial.print("\", \"a0y\" :\"");
  //  Serial.print(accel[1]);
  //  Serial.print("\", \"a0z\" :\"");
  //  Serial.println(accel[2]);

  delay(10);
}

/********** ACCEL *****************/

int initializeAccel(byte devaddress, int sda, int scl) {

  beginTransmission(devaddress, sda, scl);
  i2c_write(0x6B, sda, scl);
  i2c_write(0x00, sda, scl);
  endTransmission(sda, scl);
};




void readAccelRaw(byte devaddress, int sda, int scl) {
  uint8_t xlg, xhg, ylg, yhg, zlg, zhg;
  xhg = getReading(devaddress, 0x3B, sda, scl);
  xlg = getReading(devaddress, 0x3C, sda, scl);
  yhg = getReading(devaddress, 0x3D, sda, scl);
  ylg = getReading(devaddress, 0x3E, sda, scl);
  zhg = getReading(devaddress, 0x3F, sda, scl);
  zlg = getReading(devaddress, 0x40, sda, scl);

//  Serial.println((int)yhg);
  int accelraw[3];
  accelraw[0] = (xhg << 8 | xlg);
  accelraw[1] = (yhg << 8 | ylg);
  accelraw[2] = (zhg << 8 | zlg);
  acc[0] =  accelraw[0]  / 16384.0;
  acc[1] = accelraw[1]  / 16384.0;
  acc[2] = accelraw[2]  / 16384.0;
}


uint8_t getReading(uint8_t devaddress, uint8_t regaddress, int sda, int scl) {
  beginTransmission(devaddress, sda, scl);
  i2c_write(regaddress, sda, scl);
  endTransmission(sda, scl);
  requestFrom(devaddress, sda, scl);
  endTransmission(sda, scl);
  uint8_t reply = i2c_read(sda, scl);
  ack(sda, scl);
  endTransmission(sda, scl);
  return reply;
}

void ack(int sda, int scl) {
  pinMode(sda, OUTPUT);
  digitalWrite(sda, LOW);
  delayMicroseconds(i2cspeed);
  digitalWrite(scl, HIGH);
  delayMicroseconds(i2cspeed);
  digitalWrite(scl, LOW);
  digitalWrite(sda, HIGH);
}

void i2c_start(int sda, int scl) {
  pinMode(sda, OUTPUT);
  digitalWrite(sda, LOW);
  delayMicroseconds(i2cspeed);
  pinMode(scl, OUTPUT);
  digitalWrite(scl, LOW);
}

void i2c_stop(int sda, int scl) {
  pinMode(scl, OUTPUT);
  digitalWrite(scl, HIGH);
  delayMicroseconds(i2cspeed);
  pinMode(sda, OUTPUT);
  digitalWrite(sda, HIGH);
}


uint8_t beginTransmission(uint8_t address, int sda, int scl) {
  i2c_start(sda, scl);
  uint8_t rc = i2c_write((address << 1) | 0, sda, scl);
  return rc;
}

uint8_t requestFrom(uint8_t address, int sda, int scl)
{
  i2c_start(sda, scl);
  uint8_t rc = i2c_write((address << 1) | 1, sda, scl); // set read bit
  return rc;
}

uint8_t endTransmission(int sda, int scl)
{
  i2c_stop(sda, scl);
  return 0;
}

uint8_t i2c_write( uint8_t c , int sda, int scl)
{
  for ( uint8_t i = 0; i < 8; i++) {
    i2c_writebit( c & 128 , sda, scl);
    c <<= 1;
  }

  return i2c_readbit(sda, scl); //ACK
}

uint8_t i2c_read(int sda, int scl)
{
  uint8_t res = 0;

  for ( uint8_t i = 0; i < 8; i++) {
    res <<= 1;
    res |= i2c_readbit(sda, scl);
    //Serial.println(res);
  }
  return res;
}

void i2c_writebit( uint8_t c, int sda, int scl )
{
  delayMicroseconds(i2cspeed);

  if ( c > 0 ) {
    pinMode(sda, OUTPUT);
    digitalWrite(sda, HIGH);
  } else {
    pinMode(sda, OUTPUT);
    digitalWrite(sda, LOW);
  }
  delayMicroseconds(i2cspeed);

  pinMode(scl, OUTPUT);
  digitalWrite(scl, HIGH);
  delayMicroseconds(i2cspeed);

  pinMode(scl, OUTPUT);
  digitalWrite(scl, LOW);
  delayMicroseconds(i2cspeed);

}

//
uint8_t i2c_readbit(int sda, int scl)
{

  pinMode(sda, INPUT);
  digitalWrite(scl, LOW);
  delayMicroseconds(i2cspeed);
  digitalWrite(scl, HIGH);
  delayMicroseconds(i2cspeed);
  uint8_t i2creadbit = digitalRead(sda);
  digitalWrite(scl, LOW);

  delayMicroseconds(i2cspeed);

  return i2creadbit;
}