#include <Servo.h>
Servo myservo;
int angles = 180;
int deg_val[181];
int prev_value;
int current_angle;

void setup() {
  // put your setup code here, to run once:
  myservo.attach(3);
  pinMode(A0, INPUT);
  Serial.begin(9600);

  myservo.write(full_scan());
  prev_value = analogRead(A0);
}

void loop() {
  // put your main code here, to run repeatedly:
  if(prev_value > analogRead(A0)){
    Serial.print(prev_value);
    Serial.print(" > ");
    Serial.println(analogRead(A0));
    
    range_scan(30);
    myservo.write(current_angle);
    prev_value = analogRead(A0);
  }else{
    prev_value = analogRead(A0);
    }
  delay(100);

}

int full_scan(){
  Serial.println("Starting Full Scan");
  int max_index = 0;
  for(int pos = 0; pos <=angles; pos+=1){
    myservo.write(pos);
    delay(10);

    deg_val[pos] = analogRead(A0);
  }

  //checks for highest value
  for(int pos = 0; pos <=angles; pos+=1){
    if(deg_val[max_index] < deg_val[pos]){
      max_index = pos;
    }
  }
  delay(100);
  current_angle = max_index;
  return max_index;
}

int range_scan(int range){
  Serial.println("Starting Range Scan");
  int max_index = 0;

  //new values for degree + 30 degrees
  for(int i = 1; i <=range; i+=1){
    myservo.write(current_angle+i);
    delay(100);
    deg_val[myservo.read()] = analogRead(A0);
  }

  myservo.write(current_angle); // Resets the Servo
  
  //new values for degree - 30 degrees
  for(int j = 1; j <= range; j+=1){
    myservo.write(current_angle-j);
    delay(100);
    deg_val[myservo.read()] = analogRead(A0);
    
  }

  myservo.write(current_angle); // Resets the Servo

  //checks for highest value
  for(int pos = 0; pos <=angles; pos+=1){
    if(deg_val[max_index] < deg_val[pos]){
      Serial.print("New Highest Value: ");
      Serial.println(deg_val[pos]);
      max_index = pos;
    }
    delay(10);
  }
  Serial.pprint("Highest value is at(degrees): ");
  Serial.println(max_index);
  current_angle = max_index;
  return current_angle;
}