#include <Servo.h>
Servo rightVictor;// create servo object to control a servo
Servo leftVictor;
int buttonPin1 = 8; //button 1 pin
int leftVictorPin = 10;
int rightVictorPin = 11; // twelve servo objects can be created on most boards
const int XMIN = 4611;
const int XMID = 4900;
const int XMAX = 5191;

const int xIn = 7;              // X output
const int yIn = 6;              // Y output
int buttonState1 = 0;
int maxX = 0;
int maxY = 0;
bool start = true;
int minX;
int minY;
int tilt = 90;
int xTilt = XMID;
int yTilt;

void setup() {
  Serial.begin(9600);
  // initialize the pushbutton pin as an input:
  pinMode(buttonPin1, INPUT);
  rightVictor.attach(rightVictorPin);  // attaches the servo on pin 10 to the servo object
  leftVictor.attach(leftVictorPin);
}

int getXTilt() {return pulseIn(xIn, HIGH);} // Read X pulse
int getYTilt() {return pulseIn(yIn, HIGH);} // Read X pulse

void runMotor(int value)
{
  rightVictor.write(value);
  leftVictor.write(value);
}

int scale(int value){return (pow((value - 4900), 3)) / 280000 + 90;} //map(value, XMIN,XMAX,0,179);}// //turns accelerometer # into motor #

void valueTest()
{
  for (int i = 90; i <= 180; i++)
  {
    runMotor(i);
    Serial.println(i);
    delay(500);
  }
  for (int i = 180; i >= 0; i--)
  {
    runMotor(i);
    Serial.println(i);
    delay(500);
  }

}
void accelerate(int currentVal)
{
  for (int i = currentVal; i < 180; i++)
  {
    runMotor(i);
    delay(50);
  }
}

void slow(int currentVal)
{
  Serial.println("SLOWING");
  buttonState1 = digitalRead(buttonPin1);
  if (currentVal < XMID)
    for (int i = currentVal; i <= XMID; i += 4)
    {
      xTilt = i;
      tilt = scale(xTilt);
      runMotor(tilt);
      delay(10);
      debug();
      if (buttonState1 == HIGH)
        break;
    }
  else if (currentVal > XMID)
    for (int i = currentVal; i >= XMID; i -= 4)
    {
      xTilt = i;
      tilt = scale(xTilt);
      runMotor(tilt);
      delay(10);
      debug();
      if (buttonState1 == HIGH)
        break;
    }
  else
  {
    debug();
    runMotor(90);
  }
}

void debug()
{
  Serial.print("Tilt= ");
  Serial.print(tilt);
  Serial.print("\t");
  Serial.print("xTilt= ");
  Serial.print(xTilt);
  Serial.print("\t");
  Serial.print("buttonstate= ");
  Serial.print(buttonState1);
  Serial.println();
}

void loop() {
  //valueTest();
  buttonState1 = digitalRead(buttonPin1);
  debug();
  if (buttonState1 == HIGH)
  {
    xTilt = getXTilt();
    tilt = scale(xTilt);//map(xTilt, XMIN,XMAX,0,179);
    runMotor(tilt);
  }
  else
    slow(xTilt);
}

void accellDebug() {
  int pulseX = getXTilt();
  int pulseY = getYTilt();
  if (start)
  {
    minX = pulseX;
    minY = pulseY;
    start = false;
  }
  if (pulseX < minX)
    minX = pulseX;
  if (pulseY < minY)
    minY = pulseY;
  if (pulseX > maxX)
    maxX = pulseX;
  if (pulseY > maxY)
    maxY = pulseY;
  Serial.println(rightVictor.read());

  Serial.print(pulseX);         // Display X and Y values
  Serial.print("\t");
  Serial.print(pulseY);
  Serial.print("\t");
  Serial.print("X Max = ");
  Serial.print(maxX);
  Serial.print("\t");
  Serial.print("Y Max = ");
  Serial.print(maxY);
  Serial.print("\t");
  Serial.print("X Min = ");
  Serial.print(minX);
  Serial.print("\t");
  Serial.print("Y Min = ");
  Serial.print(minY);
  Serial.println();
  delay(20);
}