/*
We're working with: LEDs, servo motor 360,  distant sensor, button and ofc the arduino.
The code should start with a status of 0 where nothing is on, no lights, no motor. Once
the distant sensor catches anything closer to 5 cm, the state will go up by one and it
will turn on the motor (now it's at state 1). Then when the button is pressed, the
state will go up again, which will turn on the LEDs (now it's at state 2), keep in mind
that the motor is still turning. Once the button is pressed again, the state will go back
to 0 and turn everything off.
*/


#include <mechButton.h>
#include <Servo.h>


#define BTN_PIN   2
#define LED_PIN   3
#define TRIG_PIN  4
#define ECHO_PIN  5
#define SERVO_PIN 6
#define MIN_DIST  158  // cm Yes, a little high. Because simulation.

int           state;
mechButton    theBtn(BTN_PIN);
mechButton    sensor(ECHO_PIN);
Servo         theServo;
bool          sensWorking;
timeObj       senseTimer(100);
unsigned long pulseSt;
float         dist;


void setup() {
  
  Serial.begin(115200);
  Serial.println("Bla bla bla.");
 
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN,LOW);
  pinMode(TRIG_PIN, OUTPUT);
  digitalWrite(TRIG_PIN,LOW);
  sensWorking = false;
  theServo.attach(SERVO_PIN);
  theServo.write(0);             
  state = 0;
  theBtn.setCallback(btnClk);
  sensor.setCallback(echoChg);
}

void servo(bool offOn) {

  if (offOn) {
    theServo.write(180); 
  } else {
    theServo.write(0); 
  }
}


void LED(bool offOn) {

  if (offOn) {
    digitalWrite(LED_PIN, HIGH); 
  } else {
     digitalWrite(LED_PIN, LOW);
  }
}


void setState(int newState) {

  switch(newState) {
    case 0 :
      servo(false);
      LED(false);
      state = 0;
    break;
    case 1 :
      Serial.print("Distance = ");
      Serial.println(dist,2);
      servo(true);
      LED(false);
      state = 1;
    break;
    case 2 :
      servo(true);
      LED(true);
      state = 2;
    break;
    default : break;
  }
}


void btnClk(void) {

  if (!theBtn.getState()) {
    switch(state) {
      case 0 : Serial.println("Don't touch that!"); break;
      case 1 : setState(2); break;
      case 2 : setState(0); 
    }
  }
}


void echoChg(void) {

  unsigned long pulseMicSec;

  if (state==0) {
    if (sensor.getState()) {
      pulseSt = micros();
      sensWorking = true;
    } else {
      pulseMicSec = micros() - pulseSt;
      pulseMicSec++;
      dist = pulseMicSec/58.0;
      if (dist<MIN_DIST) {
        setState(1);
      }
      sensWorking = false;
    }
  }
}


void loop() {
  
  idle();
  if (state==0 && !sensWorking) {
    sensWorking = true;
    digitalWrite(TRIG_PIN,HIGH);
    delayMicroseconds(15);
    digitalWrite(TRIG_PIN,LOW);
  }
}