#define GYRO_PITCH  0
#define ACEL_X      2
#define ACEL_Z      3
#define FILTERAMT  10

struct Gyro1DKalman
{
  float x_angle,x_bias;
  float P_00,P_01,P_10,P_11;	
  float Q_angle, Q_gyro;
  float R_angle;
} filterPitch;

float deltaTime, startTime, lastTime;

int gyro_vals[FILTERAMT];
int acelx_vals[FILTERAMT];
int acelz_vals[FILTERAMT];
int filterIndex = 0;

float GYRO_PITCH_NEUTRAL = 773;
float ACEL_X_NEUTRAL = 544;
float ACEL_Z_NEUTRAL = 553;

float calGyroPitch = 0, calAccelX = 0;
float calCount = 0;

float lastAngle = 0.0;
float P = 0.0;
float I = 0.0;
float D = 0.0;

// Initializing the struct
void init_Gyro1DKalman(struct Gyro1DKalman *filterdata, float Q_angle, float Q_gyro, float R_angle)
{
  filterdata->Q_angle = Q_angle;
  filterdata->Q_gyro  = Q_gyro;
  filterdata->R_angle = R_angle;
}


// Kalman predict
void ars_predict(struct Gyro1DKalman *filterdata, const float dotAngle, const float dt)
{
  filterdata->x_angle += dt * (dotAngle - filterdata->x_bias);

  filterdata->P_00 +=  - dt * (filterdata->P_10 + filterdata->P_01) + filterdata->Q_angle * dt;
  filterdata->P_01 +=  - dt * filterdata->P_11;
  filterdata->P_10 +=  - dt * filterdata->P_11;
  filterdata->P_11 +=  + filterdata->Q_gyro * dt;
}


// Kalman update
float ars_update(struct Gyro1DKalman *filterdata, const float angle_m)
{
  const float y = angle_m - filterdata->x_angle;

  const float S = filterdata->P_00 + filterdata->R_angle;
  const float K_0 = filterdata->P_00 / S;
  const float K_1 = filterdata->P_10 / S;
	
  filterdata->x_angle +=  K_0 * y;
  filterdata->x_bias  +=  K_1 * y;
	
  filterdata->P_00 -= K_0 * filterdata->P_00;
  filterdata->P_01 -= K_0 * filterdata->P_01;
  filterdata->P_10 -= K_1 * filterdata->P_00;
  filterdata->P_11 -= K_1 * filterdata->P_01;

  return filterdata->x_angle;
}


void setup()
{
  analogReference(EXTERNAL);
  Serial.begin(9600);
  
  pinMode(3,OUTPUT);
  pinMode(5,OUTPUT);
  
  for(int i=0;i= FILTERAMT)
    filterIndex = 0;
    
  if(millis() - startTime < 5000)
  {
    // calibrating
    
    digitalWrite(13,HIGH);
    
    calAccelX += analogRead(ACEL_X);
    calGyroPitch += analogRead(GYRO_PITCH);
    calCount++;
    
    ACEL_X_NEUTRAL = calAccelX / calCount;
    GYRO_PITCH_NEUTRAL = calGyroPitch / calCount;
    
    digitalWrite(13,LOW);
    
    return;
  }
    
  if(deltaTime >= 20)
  {
    float gyro_avg = 0;
    for(int i=0;i windupGuard)
      I = windupGuard;
    if(I < -windupGuard)
      I = -windupGuard;
      
    float pid = P+I+D;
      
    if(pid > 255)
      pid = 255;
    if(pid < -255)
      pid = -255;
        
    Serial.print(pitchAngle);
    Serial.print(" | ");
    Serial.print(tmp * (180/PI));
    Serial.print(" | ");
    Serial.println(pid);
      
    if (abs(pitchAngle) < 60)
    {
      if(pid > 0)
      {
        digitalWrite(3,LOW);
        digitalWrite(5,LOW);
        analogWrite(3,pid);
      }
      else if(pid < 0)
      {
        digitalWrite(3,LOW);
        digitalWrite(5,LOW);
        analogWrite(5,-pid);
      } else {
        digitalWrite(3,LOW);
        digitalWrite(5,LOW);
      }
    } else
      I = 0;
        
    //Serial.print(pid);
    //Serial.print(" | ");
    //Serial.println(pitchAngle);
      
    lastAngle = pitchAngle;
      
    lastTime = millis();
  }
}