#include <Servo.h> 
 
Servo drive;
 
int drive_pin = 9;
int orient_pin = 0;
int orient = 0;
int old_orient = -1;
int want_orient = 0;
int drive_signal = 0;
int old_drive_signal = 0;
int diff = 0;
int default_diff;
int strength = 0;
int slower = 0;
int ledPin = 12;
 
void setup() 
{ 
  drive.attach(drive_pin);
  pinMode(ledPin, OUTPUT);
  pinMode(13, OUTPUT);
  want_orient = analogRead(orient_pin);
  strength = 0; // 50
  default_diff = 5;
  orient = analogRead(orient_pin);
  old_orient = orient;
  //Serial.begin(9600);
} 
 
void loop() 
{ 
  slower = slower+2;
  if(slower > 30) {
    slower = 0;
    want_orient = want_orient + 1;
  }
  
  if(want_orient < 0) { want_orient = 1023; }
  if(want_orient > 1023) { want_orient = 0; }
  
  // crude ADC smoothing
  orient = (old_orient + analogRead(orient_pin))/2; 
  old_orient = orient;
  
  diff = find_diff(orient, want_orient);
  
  //if(orient < want_orient) { digitalWrite(ledPin, HIGH); }
  //else { digitalWrite(ledPin, LOW); }
  
  //want_orient = 100;
  
  if(diff > 10) {
    digitalWrite(ledPin, LOW);
    strength = strength +1; 
  } else if (diff < -10) {
    digitalWrite(ledPin, LOW);
    strength = strength -1;
  } else {
    digitalWrite(ledPin, HIGH);
  }
  
  strength = constrain(strength, 0, 100);
  drive_signal = 91 - strength/10 - constrain(diff,-20,20)/2;
  
  // This is to try and get around the weird gap
  if(want_orient < 10 || want_orient > 1013) {
    drive_signal = old_drive_signal;
 // } else if(orient < 10 || orient > 1013) {
 //   drive_signal = old_drive_signal;
  } else {
    old_drive_signal = drive_signal;
  }
  
  drive.write(drive_signal); // had to choose some polarity...
  //Serial.println(diff, DEC);
  delay(15);
} 

// Express the difference between a to b, accounting for the wrap around
// a and b are both 0-1023
int find_diff(int a, int b) {
  int c;
  c = b - a;
  if(c < - 511) {
    c = c + 1023;
  } else if (c > 512) {
    c = c - 1023;
  }
  
  if(abs(c) > 100) { 
    digitalWrite(13, HIGH);
  } else {
    digitalWrite(13, LOW);
  }
  return c;
}