#define FWDPWM 9          // left motor PWM
#define REVPWM 10         // right motor PWM
#define HEADLIGHT 11      // pin control for headlight
#define FANS 12           // pin control for fans

#define A_PIN 1         // accelerometer analog input // was0
#define A_HEADLIGHT 0   // accelerometer analog input, for headlight
#define A_HEADLIGHT2 2   // accelerometer analog input, for headlight
#define G_PIN 5         // gyro analog input // was 4
#define S_PIN 6         // steering analog input

#define A_ZERO 331      // approx. 1.5[V] * 1024[LSB/V] //was 341
#define G_ZERO 247     // approx. 1.23[V] * 1024[LSB/V] // was 253
#define S_ZERO 766      // approx. 2.5[V] * 1024[LSB/V]

#define A_GAIN 0.932    // [deg/LSB]
#define G_GAIN 1.466    // [deg/s/LSB]
#define S_GAIN 0.25     // [LSB/LSB] (AAAHHHHHHH WHAT?)

#define DT 0.03         // [s/loop] loop period
#define A 0.962         // complementary filter constant

#define KP 40.0          // proportional controller gain [LSB/deg/loop] KP was .5
#define KD 0.002          // derivative controller gain [LSB/deg/loop]

float angle = 0.0;      // [deg]
float rate = 0.0;       // [deg/s]
float output = 0.0;     // [LSB] (100% voltage to motor is 255LSB)
float absoutput = 0.0;
signed int headlight_x;
signed int headlight_y;
signed int headlight_integrator = 0;


void setup()
{
  // Make sure all motor controller pins start low.
  digitalWrite(FWDPWM, LOW);
  digitalWrite(REVPWM, LOW);
  
  // Set all motor control pins to outputs.
  pinMode(FWDPWM, OUTPUT);
  pinMode(REVPWM, OUTPUT);
  pinMode(HEADLIGHT, OUTPUT);
  pinMode(FANS, OUTPUT);
  
  // switch to 15.625kHz PWM
  TCCR1B &= ~0x07;
  TCCR1B |= 0x01;   // no prescale
  TCCR1A &= ~0x03;  // 9-bit PWM
  TCCR1A |= 0x02;
  
  Serial.begin(19200);
}

void loop()
{

  signed int accel_raw = 0;
  signed int gyro_raw = 0;
  signed int output_forward = 0;
  signed int output_reverse = 0;
  
  
  // Read in the raw accelerometer, gyro, and steering singals.
  // Offset for zero angle/rate.
  accel_raw = (signed int) analogRead(A_PIN) - A_ZERO;
  
  gyro_raw = G_ZERO - (signed int) analogRead(G_PIN);

  
  // Scale the gyro to [deg/s].
  rate = (float) gyro_raw * G_GAIN;
  
  // Complementarty filter.
  angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN;
 
  
  //Serial.println(angle);
  
  // PD controller.
  output = angle * KP + rate * KD;
  
  // Clip as float (to prevent wind-up).
  if(output < -255.0) { output = -255.0; } 
  if(output > 255.0) { output = 255.0; }
  
  
  if(output <= 0)
    {

      analogWrite(FWDPWM, 0);
      absoutput = abs(output);
      analogWrite(REVPWM, absoutput);
    }
  if(output >= 0)
    {

      analogWrite(REVPWM, 0);
      analogWrite(FWDPWM, output);
      
    }
    
  // Add Headlight function, turn on headlight when not moving and tilting board clockwise, off counterclockwise
       headlight_x= analogRead(A_HEADLIGHT);
          headlight_y= analogRead(A_PIN);
          if (headlight_x > 400)
          {
            if (headlight_y <360)     
              headlight_integrator = headlight_integrator+1;
      
          }
          if (headlight_x < 280)
          {
              if (headlight_y <360)
             {
              headlight_integrator = headlight_integrator-1;
              }
          }
   if(headlight_integrator > 40)   {digitalWrite(HEADLIGHT, HIGH);}
   if(headlight_integrator < 10)   {digitalWrite(HEADLIGHT, LOW); }
   // Bound so you dont get negatives
   if(headlight_integrator < 2) { headlight_integrator = 2; } 
   if(headlight_integrator > 100.0) {headlight_integrator = 100.0; }
    
    
   // Delay for consistent loop rate.
  delay(30);
}