// Dumentation: https://github.com/spothings/auto-switch

#define WAITINGKITCHEN 6000
#define WAITINGBATHROOM 100
#define DISTANCEULTRASONIC 60

const unsigned short int
    PIR = 2,
    TRIG = 3,
    ECHO = 4,
    KITCHEN = 5,
    BATHROOM = 6;

unsigned short int
    waitingKitchen,
    waitingBathroom;

bool
    statusPir,
    statusKitchen,
    statusBathroom;

void setup()
{
  Serial.begin(9600);
  pinMode(PIR, INPUT);
  pinMode(ECHO, INPUT);
  pinMode(TRIG, OUTPUT);
  pinMode(KITCHEN, OUTPUT);
  pinMode(BATHROOM, OUTPUT);

  statusKitchen = false;
  statusBathroom = false;
}

void loop()
{
  // Kitchen
  bool pir = digitalRead(PIR);
  if (waitingKitchen <= 0)
  {
    KitchenLamp(pir);
  }
  else
  {
    if (statusPir != pir)
    {
      statusPir = pir;
      waitingKitchen = WAITINGKITCHEN;
    }
    else if (!pir)
    {
      waitingKitchen--;
    }
  }

  // Bathroom
  unsigned short int ultrasonic = HCSR04(TRIG, ECHO, "cm");
  if (waitingBathroom <= 0)
  {
    if (ultrasonic < DISTANCEULTRASONIC)
    {
      Relay(BATHROOM, !statusBathroom);
      statusBathroom = !statusBathroom;
      waitingBathroom = WAITINGBATHROOM;
    }
  }
  else
  {
    waitingBathroom--;
  }

  SerialMonitor(pir, ultrasonic);
}

void SerialMonitor(int _pir, int _ultrasinic)
{
  Serial.print(_pir);
  Serial.print("\t");
  Serial.print(waitingKitchen);
  Serial.print("\t");
  Serial.print(statusKitchen);
  Serial.print(" | ");
  Serial.print(_ultrasinic);
  Serial.print("\t");
  Serial.print(waitingBathroom);
  Serial.print("\t");
  Serial.print(statusBathroom);
  Serial.print("\n");
}

void KitchenLamp(int _pir)
{
  statusKitchen = _pir;
  Relay(KITCHEN, statusKitchen);
  if (_pir)
  {
    waitingKitchen = WAITINGKITCHEN;
  }
}

void Relay(int _pin, bool _status) {
  if (_status) {
    digitalWrite(_pin, HIGH);
  } else {
    digitalWrite(_pin, LOW);
  }
}

int Ultrasonic(int _tring, int _echo, String _unit) {
  digitalWrite(_tring, LOW);
  delayMicroseconds(2);
  digitalWrite(_tring, HIGH);
  delayMicroseconds(10);
  digitalWrite(_tring, LOW);
  long duration = pulseIn(_echo, HIGH);

  if (_unit == "in") {
    return duration / 74 / 2;
  } else if (_unit == "cm"){
    return duration / 29.1 / 2;
  } else {
    return 0;
  }
}

int HCSR04(int _tring, int _echo, String _unit) {
  return Ultrasonic(_tring, _echo, _unit);
}

int HYSRF05(int _tring, int _echo, String _unit) {
  return Ultrasonic(_tring, _echo, _unit);
}