#include <SPI.h>
#include <nRF24L01.h>
#include <printf.h>
#include <RF24.h>
#include <RF24_config.h>
#include <Wire.h>
#include <MPU6050.h>

//network
RF24 radio(10, 9);
const byte address[6] = "00001";    

//gyro
MPU6050 mpu;

int pitch;
int roll;
int Ppitch;
int Proll;
int pitchT = 127;
int rollT = 127;

//////////////////////
void setup() {
  Serial.begin(9600);

  Serial.println("200");
  //MPU6050, for pitch and roll values
  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }
  //RF24 network activation
  SPI.begin();
  radio.begin();
  radio.openWritingPipe(address);
  radio.stopListening();
  Serial.println("200");
}
//////////////////////

//////////////////////
void loop() {
  calcValues();
  sendInputToMaster();
  delay(25);
} 
//////////////////////

//////////////////////
void calcValues(){
  readPitchAndRoll();
}
//////////////////////

//////////////////////
void readPitchAndRoll(){
  //Get Accel vector
  Vector normAccel = mpu.readNormalizeAccel();
  //Calculate pitch and roll
  int firstPitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis*normAccel.YAxis + normAccel.ZAxis*normAccel.ZAxis))*180.0)/M_PI;
  int firstRoll = (atan2(normAccel.YAxis, normAccel.ZAxis)*180.0)/M_PI;
  pitch = map(firstPitch, -64, 64, 0, 127);
  roll = map(firstRoll, -64, 64, 0, 127);
}
//////////////////////

//////////////////////
void sendInputToMaster(){
  //Check if pitch is changed or inside bounds
  if((pitch < Ppitch-1 || pitch > Ppitch+1) && pitch >= 0 && pitch <= pitchT){
    sendMessage(1, 0, pitch); //Sender, value type, value
    //Change previous pitch value to current pitch value
    Ppitch = pitch;
    Serial.println(pitch);
  }
  
  //Check if roll is changed or inside bounds
  if((roll < Proll-1 || roll > Proll+1) && roll >= 0 && roll <= rollT){
    sendMessage(1, 1, roll); //Sender, value type, value
    //Change previous pitch value to current pitch value
    Proll = roll;
    Serial.println(roll);
  }
}
//////////////////////

//////////////////////
void sendMessage(int sender, int valueType, int value){
  //Create message
  int message = 0;
  if(sender == 1){
    if(valueType == 0){
      message = value+1000;
    }
    if(valueType == 1){
      message = value+2000;
    }
  }
  radio.write(&message, sizeof(message)); // Send
}
//////////////////////