double acc[3];

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

int i2cspeed = 100;

void setup() {
  Serial.begin(57600);
  i2c_init;
//  nack(A4, A5);
delay(1000);
//  beginTransmission(0x68, A4, A5);
//  i2c_write(0x07, A4, A5);
//  endTransmission(A4, A5);
//  delay(1000);
  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.println(accAngleX);
  //  Serial.print("{ \"a0x\" :\"");
  //  Serial.print(acc[0]);
  //  Serial.print("\", \"a0y\" :\"");
  //  Serial.print(acc[1]);
  //  Serial.print("\", \"a0z\" :\"");
  //  Serial.println(acc[2]);

  delay(10);
}

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




void readAccelRaw(byte devaddress, int sda, int scl) {
  uint8_t xlg, xhg, ylg, yhg, zlg, zhg;
//  beginTransmission(devaddress, sda, scl);
//  i2c_write(0x3B, sda, scl);
//  endTransmission(sda, scl);
//  requestFrom(devaddress, sda, scl);
//  xhg = i2c_read(sda, scl);
//  ack(sda, scl);
//  xlg = i2c_read(sda, scl);
//  ack(sda, scl);
//  yhg = i2c_read(sda, scl);
//  ack(sda, scl);
//  ylg = i2c_read(sda, scl);
//  ack(sda, scl);
//  zhg = i2c_read(sda, scl);
//  ack(sda, scl);
//  zlg = i2c_read(sda, scl);
//  nack(sda, scl);
//  endTransmission(sda, scl);
  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(xhg);
  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;
  Serial.println(acc[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);
    uint8_t reply = i2c_read(sda, scl); nack(sda, scl);
    endTransmission(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 nack(int sda, int scl) {
  pinMode(sda, OUTPUT);
  digitalWrite(sda, HIGH);
  delayMicroseconds(i2cspeed);
  digitalWrite(scl, HIGH);
  delayMicroseconds(i2cspeed);
  digitalWrite(scl, LOW);
  digitalWrite(sda, HIGH);
}

void i2c_init(int sda, int scl) {
  pinMode(sda, OUTPUT);
  digitalWrite(sda, HIGH);
  delayMicroseconds(i2cspeed);
  pinMode(scl, OUTPUT);
  digitalWrite(scl, 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);
  i2c_write((address<<1) | 0, sda, scl);
  return i2c_readbit(sda, scl);
}

uint8_t beginTransmission(int address, int sda, int scl) {
  return beginTransmission( (uint8_t) address, sda, scl);
}

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

uint8_t requestFrom(int address, int sda, int scl)
{
    return requestFrom( (uint8_t) address, sda, scl);
}

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

void 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;
  }

}

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)
{
  delayMicroseconds(i2cspeed);
  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;
}