/*
1 Push buttons: Velocity of drawing servo
1 Switch: ON/OFF
*/

#include <Servo.h>

const int servoDrawPin = 4;
const int servoMovePin = 8;
const int switchPin = 3;
const int buttonPin = 2;
const int ledPin =  13;
const int upperDraw = 180;
const int lowerDraw = 0;
const int upperMove = 160;
const int lowerMove = 30;

int delayDraw = 10.0;
int delayMove = 50.0;

int posDraw = 90;
int posMove = 90;
int switchState = 0;
int buttonState = 0;
boolean toggleDraw = false;
boolean toggleMove = false;
unsigned long lastMillisDraw = 0.0;
unsigned long lastMillisMove = 0.0;
  
Servo servoDraw;
Servo servoMove;

void setup() 
{
  servoDraw.attach(servoDrawPin);
  servoMove.attach(servoMovePin);
  pinMode(ledPin, OUTPUT);      
  pinMode(switchPin, INPUT);     
  pinMode(buttonPin, INPUT);     
  servoDraw.write(posDraw);
  servoMove.write(posMove);
  //posDraw = servoDraw.read();
  //posMove = servoMove.read();
  lastMillisDraw = millis();
  lastMillisMove = millis();
}

void loop()
{
  switchState = digitalRead(switchPin);
  buttonState = digitalRead(buttonPin);
  
  // if switch is turned on
  if (switchState == HIGH) 
  {     
    digitalWrite(ledPin, HIGH);  
    
    // TODO: check buttonState and get time between two button press actions
    //       set new delay for draw = difference / 10 
        
    unsigned long deltaMillisDraw = millis() - lastMillisDraw;
    unsigned long deltaMillisMove = millis() - lastMillisMove;
    
    toggleDraw = toogle(posDraw, toggleDraw, upperDraw, lowerDraw);
    toggleMove = toogle(posMove, toggleMove, upperMove, lowerMove);
    
    if (deltaMillisDraw >= delayDraw)
    {
      if (toggleDraw)
        posDraw += 1;
      else 
        posDraw -= 1;
        
      servoDraw.write(posDraw); 
      lastMillisDraw = millis();      
    }
    if (deltaMillisMove >= delayMove)
    {
      if (toggleMove)
        posMove += 1;
      else
        posMove -= 1;
        
      servoMove.write(posMove); 
      lastMillisMove = millis();
    }
  } 
  // if switch is turned off
  else 
    digitalWrite(ledPin, LOW); 
}

boolean toogle(int p, int t, int up, int low)
{
  if ((p == up) || (p == low))
    t = !t;
    
  return t;
}