#include <math.h>

const int xpin = A0;      // accelerometer x-axis analog pin
const int ypin = A1;      // accelerometer y-axis analog pin
const int Ratepin = A2;   // gyro rate pin

double AdcRx;
double AdcRy;
double Rx;
double Ry;
double Rate;
double gyroInitial = 0;
double RxInitial = 0;
double RyInitial = 0;
double ArcSineX;
double ArcSineY;
double GyroRate;
  double FinalAngle = 0;

#define M_PI 3.14159265358979323846

void setup()
{
  Serial.begin(115200);
  RxInitial = analogRead(xpin);
  RyInitial = analogRead(ypin);

  for(int i=0; i<100; i++){
    gyroInitial += analogRead(Ratepin);
  }
  gyroInitial /= 100;
}
void loop()
{
  AdcRx = analogRead(xpin) - RxInitial;
  Rx = (AdcRx * 5.0 / 1023);
  ArcSineX = (asin(Rx) * (180 / M_PI));
  
  AdcRy = analogRead(ypin) - RyInitial;
  Ry = (AdcRy * 5.0 / 1023);
  ArcSineY = (asin(Ry) *(180 / M_PI));
  
  Rate = analogRead(Ratepin) - gyroInitial;
  GyroRate = (Rate * 5.0 / 1023) / 0.025;
  
  FinalAngle = 0.98 * (FinalAngle * GyroRate) + 0.02 * ArcSineY;
  
  
  Serial.print("X: ");
  Serial.print(analogRead(xpin));
  Serial.print("\t\t");
  
  Serial.print("Y: ");
  Serial.print(analogRead(ypin));
  Serial.print("\t\t");
  
  Serial.print("Angle X: ");
  Serial.print(ArcSineX);
  Serial.print("\t\t");
  
  Serial.print("Angle Y: ");
  Serial.print(ArcSineY);
  Serial.print("\t\t"); 
  
  Serial.print("Gyro Rate: ");
  Serial.print(GyroRate);
  Serial.print("\t\t");
  
  Serial.print("Final Angle: ");
  Serial.println(FinalAngle);
  Serial.println();
  delay(100);
}