#include <Servo.h>

Servo markerServo;          // servo with attached marker
int pos = 0;                // variable to store the servo position

const int led = 13;         // led for on or off indication of machine
const int motor1 = 5;
const int motor2 = 6;
int val3;                   // vibration frequency for motor 1
int val4;                   // vibration frequency for motor 2

/* motors need transisters. flat side to right. top - to ground on motor
 middle - to 500 resister, to arduino. bottom - to ground.
 */

const int photoCell = A0;
int sensorValue;            // analog read of the photo cell

const int button = 2;       // resistors, small to pin, large to ground
int buttonState;
int val;                    // debounce
int val2;                   // debounce
int lightMode = 0;

void setup() {
  pinMode(led, OUTPUT);
  pinMode(motor1, OUTPUT);
  pinMode(motor2, OUTPUT);

  pinMode(photoCell, INPUT);
  Serial.begin(9600);

  pinMode(button, INPUT);

  markerServo.attach(9);    // attaches the servo to pin 9
}

void loop() {
  buttonState = digitalRead(button);
  Serial.print(val);
  Serial.print(" ");
  Serial.print(val2);
  Serial.print("   ");
  Serial.println(sensorValue);

  sensorValue = analogRead(photoCell);

  val = digitalRead(button);        // debounce
  delay(5);                         // debounce
  val2 = digitalRead(button);       // debounce
  if (val == val2) {                // debounce
    if(val != buttonState) {        // debounce
      if(val == LOW) {              // debounce
        if(lightMode == 0) {
          lightMode = 1;
        }
        else {
          if(lightMode == 1) {
            lightMode = 0;
          }
        }
      }
    }
    buttonState = val;
  }

  if (lightMode == 0) {                               // off
    digitalWrite(led, LOW);
    analogWrite(motor1, 0);
    analogWrite(motor2, 0);
    for(pos = 0; pos < 90; pos += 180) {    
      markerServo.write(pos);
    }
  }
  if (lightMode == 1) {                               // on
    digitalWrite(led, HIGH);                          // led is indicator, turn on
    for(pos = 90; pos >= 0; pos -= 180) {
      markerServo.write(pos);
      if(sensorValue > 600 && sensorValue < 1000) {
        val3 = map(sensorValue, 650, 930, 255, 0);
        val4 = map(sensorValue, 660, 930, 255, 0);
        analogWrite(motor1, val3);
        analogWrite(motor2, val4);
      }
    }
  }
}