#include <SPI.h>
#include <RF24.h>
const int MPU = 0x68; // MPU6050 I2C address
float accAngleX, accAngleY;
float accAngleErrorX, accAngleErrorY;
float accX, accY, accZ;

const int i2cspeed = 10;

RF24 radio(9, 10);

int angles[2];

const uint64_t pipe = 0xE8E8F0F0E1LL;

void setup() {
  pinMode(A5, OUTPUT);
  pinMode(A4, OUTPUT);
  Serial.begin(57600);
  i2c_stop(A4, A5);
  radio.begin();
  radio.openWritingPipe(pipe);
  Serial.println("init accel");

  delay(1000);
}
void loop() {
  readAccelRaw(MPU, A4, A5);

  accAngleX = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2 ))) / (PI / 180);
  accAngleY = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2 ))) / (PI / 180);

  Serial.print("X: ");
  Serial.print(accAngleX);
  Serial.print(" | Y: ");
  Serial.println(accAngleY);
  angles[0] = (int) accAngleX;
  angles[1] = (int) accAngleY;
  radio.write(angles, sizeof(angles));
}

void readAccelRaw(byte devaddress, int sda, int scl) {
  uint8_t xlb, xhb, ylb, yhb, zlb, zhb;
  //  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);
  xhb = getReading(devaddress, 0x3B, sda, scl);
  xlb = getReading(devaddress, 0x3C, sda, scl);
  yhb = getReading(devaddress, 0x3D, sda, scl);
  ylb = getReading(devaddress, 0x3E, sda, scl);
  zhb = getReading(devaddress, 0x3F, sda, scl);
  zlb = getReading(devaddress, 0x40, sda, scl);
  accX =  (xhb << 8 | xlb)  / 16384.0;
  accY = (yhb << 8 | ylb)  / 16384.0;
  accZ = (zhb << 8 | zlb)  / 16384.0;
}

uint8_t getReading(uint8_t devaddress, uint8_t regaddress, int sda, int scl) {
  beginTransmission(devaddress, sda, scl);
  i2c_write(regaddress, sda, scl);
  i2c_readbit(sda, scl);
  endTransmission(sda, scl);
  requestFrom(devaddress, sda, scl);
  uint8_t reply = i2c_read(sda, scl);
  nack(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);

  digitalWrite(scl, HIGH);
}


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

  digitalWrite(scl, LOW);
}

void i2c_stop(int sda, int scl) {
  pinMode(sda, OUTPUT);
  digitalWrite(scl, LOW);
  delayMicroseconds(i2cspeed);
  digitalWrite(sda, LOW);
  delayMicroseconds(i2cspeed);
  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 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 endTransmission(int sda, int scl)
{
  i2c_stop(sda, scl);
  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);
    if (i < 7)
      c <<= 1;
  }
  //return i2c_readbit(sda, scl);

}

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);


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


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