/*
 * Filename:    main.cpp
 *
 * Description: device to turn out lights in the bathroom based on a 
 *              PIR sensor.
 * 
 * Version:     
 * Created:     Sat Jan 12 22:54:52 2019
 * Revision:    None
 * Author:      Rachel Fae Fox (foxiepaws),fox@foxiepa.ws
 *
 */

#include <Servo.h>

enum states { inactive = 0, active, waiting };

const int led = 11;
const int pir = 6;
const int power = 5;
const int servo = 9 ;

#define MOTION_HEURUSTIC 5
#define MILLIS 1000
#define POWER_TIME (6 * MILLIS)
#define CHECK_POWER_TIME (3 * MILLIS)

int motion = 0;

elapsedMillis motionTimer; // motion Timeout
elapsedMillis powerTimer;  // Power Timeout
elapsedMillis cPowerTimer; // checkPower timeout

Servo lightSwitch;
#define SERVO_RESTING_POSITION 90
#define SERVO_SWITCH_PLATE_MOTION 20
#define SERVO_SLEW_TIME 120
#define SERVO_SLEW_DEG  60
#define SERVO_SLEW_RATE (SERVO_SLEW_TIME/SERVO_SLEW_DEG)
#define SERVO_MIN 0
#define SERVO_MAX 180

enum states state = inactive;
int powerState = 0;
int wantedPowerState = 0;
int servoPosition = 0;
int servoDir = 1;
// Saturation of servo position.
int servoSat ( int pos ) {
    if (pos >= SERVO_MAX) {
  return SERVO_MAX;
    } else if (pos <= SERVO_MIN) {
  return SERVO_MIN;
    } else {
  return pos;
    }
}

// function to solve a delay for a move, given a slew rate
// this is to avoid running into a bad move where a new command
// is sent before the servo has had time to make its move.
void servoDelay(int oldpos, int newpos) {
    int delayTime = abs(oldpos - newpos) * SERVO_SLEW_RATE;
    delay(delayTime);
}

void servoMove ( int pos ) {
    int oldPos = servoPosition;
    servoPosition = servoSat(pos);
    lightSwitch.write(servoPosition);
    servoDelay(oldPos, servoPosition);
}

void servoAdd ( int amount ) {
    servoMove(servoPosition + amount );
}

void setup() {
    pinMode(led, OUTPUT);
    pinMode(pir, INPUT);
    pinMode(power, INPUT);
    lightSwitch.attach(servo);
    servoMove(SERVO_RESTING_POSITION);
}


void evalPowerState() {
    int actualPowerState = digitalRead(power);

    switch (powerState) {
  case 0: // power off
      if (actualPowerState == 1) {
    powerState = 1;
    break;
      }
      break;
  case 1: // power on
      if (actualPowerState == 0) {
    powerState = 0;
    break;
      }
      if (wantedPowerState == 0) {
    servoDir = -1;
    servoMove(SERVO_RESTING_POSITION + SERVO_SWITCH_PLATE_MOTION);
    servoMove(SERVO_RESTING_POSITION);
    cPowerTimer = 0;
    powerState = 2;
    break;
      }
      break;
  case 2: // transitional;
      if (wantedPowerState == 0 and actualPowerState == 0) {
        powerState = 0;
        break;
      }
      if (wantedPowerState == 0 and actualPowerState == 1 and cPowerTimer >= CHECK_POWER_TIME) {
    servoMove(SERVO_RESTING_POSITION + (servoDir * SERVO_SWITCH_PLATE_MOTION));
    servoMove(SERVO_RESTING_POSITION);
    servoDir *= -1;
    cPowerTimer = 0;
    break;
      }     
      if (wantedPowerState == 1 and actualPowerState == 1) {
    powerState = 1;
    break;
      }
      break;
  default:
      break;
    }
}

void evalMainState() {
    switch (state) {
  case inactive:
      // return to inactive state if the light gets turned off.
      if (powerState == 1) {
        state = active;
        break;
      }
  break;
  case active:
      digitalWrite(led,HIGH);
      // return to inactive state if the light gets turned off.
      if (powerState == 0) {
        state = inactive;
        break;
      }
      if (powerState == 2) {
    wantedPowerState = 1;
      }
      if (digitalRead(pir)) {
        motionTimer = 0;
      }
      // if the pir has not detected motion for 5 seconds change state
      if (!digitalRead(pir) and motionTimer >= MILLIS *5) {
    state = waiting;
    // reset the power timer to start counting 
    powerTimer = 0;
    break;
      }
  break;
  case waiting:
      // return to inactive state if the light gets turned off.
      // blinky while timer is active, doubling up the motion timer use.
      if(motionTimer >= 1000)
    digitalWrite(led,HIGH);
      if(motionTimer > 2000) {
    digitalWrite(led,LOW);
    motionTimer = 0;
      }
      if (powerState == 0) {
        state = inactive;
        break;
      }
      if (digitalRead(pir)) {
    state = active;
    break;
      }
      if (powerTimer >= POWER_TIME ) {
    wantedPowerState = 0;
    state = inactive;
    break;
      } 
  break;
  default:
      
  break;
    }
}

void loop () {
    evalMainState();
    evalPowerState();
}