/******************************************************************************
  Witch Stirring Controller

  Monitors left and right motion sensors.
  Upon motion detection:
    Stop stirring and nodding.
    Activate greeting.
    Turn head to the side.
    Wait
    Reset head to straight ahead. Resume stirring and nodding.
 ****************************************************************************/
#include "SoundPlayer.h"  //Class to play the sound clips sequentially.
#include "WitchEnum.h"  //Enumeration of the different states available
#include <Arduino.h>
#define DISABLE_COMPLEX_FUNCTIONS     // Activating this disables the SINE, CIRCULAR, BACK, ELASTIC, BOUNCE and PRECISION easings. Saves up to 1850 bytes program memory.
#define MAX_EASING_SERVOS 1
#include "ServoEasing.hpp"
#include "PinDefinitionsAndMore.h"

//PINS
//pins 0, 1 are TX/RX
//nano pins A0-A5 can be used as digital pins, A6-A7 connect be used as digital pins.

//motion sensors
//This is when looking at the witch.
#define S_LEFT_MOTION 7
#define S_RIGHT_MOTION 8

//output
#define O_HEAD_TURNING_PWM 9
#define O_NODDING_RELAY 10
#define O_STIRRING_RELAY 11

//servo defines
#define SERVO_SPEED_FAST 90  //degree per second
#define SERVO_SPEED_SLOW 30  //degree per second

#define HEAD_ANGLE_LEFT 30
#define HEAD_ANGLE_CENTER 90
#define HEAD_ANGLE_RIGHT 130

//Holds the current state
StateEnum currentState;
//holds the time that the current state was entered into
unsigned long currentStateStartedAt;

//Timing, all in milliseconds
int LEFT_TURN_DURATION = 6000; //The time it takes to turn to the left
int RIGHT_TURN_DURATION = 6000; //ms, max time the lid can take to close

//Timing, all in milliseconds
int LOOKING_TO_THE_SIDE_TIMEOUT = 5000; //This is how long the witch takes to turn and stare to the side.
int JUST_TURNED_TIMEOUT = 9000; //min time that the witch will look straight after jush having looked to the side

//for the morse code
int DOT = 130;
int DASH = DOT*3;

//Sound 
/* Play the greetings 1 at a time, in order.
 * Use 6 analog pins.
    #define PIN_A0   (14)
    #define PIN_A1   (15)
    #define PIN_A2   (16)
    #define PIN_A3   (17)
    #define PIN_A4   (18)
    #define PIN_A5   (19)
    #define PIN_A6   (20) On Nano cannot be used for digital out
    #define PIN_A7   (21) On Nano cannot be used for digital out
 */
//A0-A7 are consts that refer to the #defines above in the pins_arduino.h library
//We only have 7 sounds. 
//These are the pins that will be used to trigger the audio.
//Pin 3 will be used since A6-A7 cannot produce digital output on the Arduino Nano.
int soundPinArray[7] = {A0,A1,A2,A3,A4,A5,3};
SoundPlayer SoundClipPlayer = SoundPlayer(soundPinArray, sizeof(soundPinArray)/sizeof(int));

//The servo object.
//Uses pin 9 by default.
ServoEasing Servo1;

/******************************************************************************
 * The setup function runs once when you press reset or power on the board
 *****************************************************************************/
void setup() {
  Serial.begin(9600); //this starts the serial monitor

  //input
  pinMode(S_LEFT_MOTION, INPUT);  //high = active
  pinMode(S_RIGHT_MOTION, INPUT);  //high = active

  //output
  pinMode(LED_BUILTIN, OUTPUT);  //pin 13
  digitalWrite(LED_BUILTIN, LOW);

  pinMode(O_NODDING_RELAY, OUTPUT);
  pinMode(O_STIRRING_RELAY, OUTPUT); //high = active
  digitalWrite(O_NODDING_RELAY, HIGH);
  digitalWrite(O_STIRRING_RELAY, HIGH);

  //Initialize the audio pins as digital output pins
  SoundClipPlayer.Initialize();

  //servo intialize, //center the head
  Servo1.attach(SERVO1_PIN, HEAD_ANGLE_CENTER);  //SERVO1_PIN = D9
  //Servo1.setEasingType(EASE_CUBIC_OUT);
  delay(4000); // Wait for servo to reach start position.

  //set the witch state to straight
  SetState(ST_STRAIGHT);

  Serial.println("Setup: Complete --------------------------------------");
}//setup

/******************************************************************************
 * The loop function runs over and over again forever.
 * When motion is detected, turn the head and play a greeting.
 *****************************************************************************/
void loop() {
  //State Machine

  //ST_STRAIGHT: The witch is looking straight ahead stirring her cauldron.
  //Left and right is relative to a person looking at the witch head-on.
  if (currentState == ST_STRAIGHT) {
    if (IsLeftMotion()) {  //any non-zero number evaluates to TRUE, HIGH evaluates to true
      Serial.println("LeftMotion Detected");
      Stirring(false);
      Nodding(false);
      SoundClipPlayer.PlayNextSound();
      SetState(ST_TURNING);
      HeadTurn(HEAD_ANGLE_LEFT, SERVO_SPEED_FAST);  //blocking call, do this last
    }
    else if (IsRightMotion()) {  //any non-zero number evaluates to TRUE, HIGH evaluates to true
        Serial.println("RightMotion Detected");
        Stirring(false);
        Nodding(false);
        SoundClipPlayer.PlayNextSound();
        SetState(ST_TURNING);
        HeadTurn(HEAD_ANGLE_RIGHT, SERVO_SPEED_FAST);  //blocking call, do this last
    }
  }

  //TURNING: The witch is turning to the side. 
  //Since the method to turn the head is a blocking call, this code will only
  //be reached once the witch has finished turning.
  //If enough time has passed with the witch looking to the side, resume stirring and 
  //start turning the head back to straight ahead.
  else if (currentState == ST_TURNING) {
    if(IsTimeoutReached(LOOKING_TO_THE_SIDE_TIMEOUT)) {  //If the witch has been looking to the side for long enough
      Stirring(true);
      SetState(ST_STRAIGHTENING);
      HeadTurn(HEAD_ANGLE_CENTER, SERVO_SPEED_SLOW);
    }
  }

  //ST_STRAIGHTENING: Since the servo call is blocking, we will reach this code
  //when the which is back to looking straight ahead again. Resume nodding.
  else if (currentState == ST_STRAIGHTENING) {
      Nodding(true);
      SetState(ST_JUST_TURNED);
  }

  //ST_JUST_TURNED: The witched just looked to the side. Stay looking forward for a minimum
  //of a few seconds (even if motion is detected).
  else if (currentState == ST_JUST_TURNED) {
    if(IsTimeoutReached(JUST_TURNED_TIMEOUT)) {
      SetState(ST_STRAIGHT);
      Serial.println("State: Straight");
    }
  }

  //Error state
  else {
    Serial.println("In the default");
    BlinkSOS(LED_BUILTIN);
  } //switch
}//loop

/******************************************************************************
 * Set the state.
 * Set the start time for this state.
 *****************************************************************************/
void SetState(StateEnum newState)
{
  //Serial.print("SetState: ");
  //Serial.print(StateEnumStringArray[currentState]);
  //Serial.print(" => ");
  //Serial.println(StateEnumStringArray[newState]);

  currentStateStartedAt = millis();
  currentState = newState;
}

/******************************************************************************
 * Check if a specific amount of time has passed since the current state was entered.
 * Checks the current time vs. the 'currentStateStartedAt' global variable.
 * Return true if the timeout amount of time has passed.
 *
 * int timeout: This is the amount of time to check for in milliseconds.
 ******************************************************************************/
bool IsTimeoutReached(int timeout)
{
  int timeRunning = millis() - currentStateStartedAt;

  //Serial.println("IsTimeoutReached(): elapsed: " + String(timeRunning) + " of " + String(timeout));
  if (timeRunning >= timeout) {
    return true;
  }
  else {
    return false;
  }
}

/******************************************************************************
 * If motion is detected, turn the head to the right or left.
 * 
 * int angleToTurn: The angle to move the head to. 90 degrees is straight ahead.
 * int speed: in degrees per second.
 *****************************************************************************/
void HeadTurn(int angleToTurn, int speed)
{
    Servo1.easeTo(angleToTurn, speed);
}

/******************************************************************************
 * Stop or start the stirring motor.
 *
 * bool input: If true, activate stirring. If false, stop stirring.
 *****************************************************************************/
void Stirring(bool input)
{
  SetPin(O_STIRRING_RELAY, input);
}

/******************************************************************************
 * Stop or start the nodding motor.
 *
 * bool input: If true, activate nodding. If false, stop nodding.
 *****************************************************************************/
void Nodding(bool input)
{
  SetPin(O_NODDING_RELAY, input);
}

/******************************************************************************
 * Change an output pin to HIGH or LOW
 *
 * Note: This sample code could toggle a pin, based on the pin value: digitalWrite(outPin,!digitalRead(outPin));
 *
 * int pin: The pin to change the output value of
 * int input: If true, set the pin value to HIGH, else if false LOW 
 *****************************************************************************/
void SetPin(int pin, bool input)
{
  if(input) {
    digitalWrite(pin, HIGH);
  }
  else {
    digitalWrite(pin, LOW);
  }
}

/******************************************************************************
 * Check if motion has been detected.
 * Returns true if motion detected.
 *
 * sensor HIGH = active
 *****************************************************************************/
bool IsLeftMotion()
{
  return digitalRead(S_LEFT_MOTION);
}

/******************************************************************************
 * Check if motion has been detected.
 * Returns true if motion detected.
 *
 * sensor HIGH = active
 *****************************************************************************/
bool IsRightMotion()
{
  return digitalRead(S_RIGHT_MOTION);
}

/******************************************************************************
 * Blink SOS
 * If the state machine has entered an error state.
 *
 * The length of a dot is 1 time unit.
 * A dash is 3 time units.
 * The space between symbols (dots and dashes) of the same letter is 1 time unit.
 * The space between letters is 3 time units.
 * The space between words is 6 time units.
 * https://www.infoplease.com/encyclopedia/science/engineering/electrical/morse-code
 * https://www.codebug.org.uk/learn/step/541/morse-code-timing-rules/
 *****************************************************************************/
void BlinkSOS(int pin) {
  flash3(pin, DOT);  //S
  flash3(pin, DASH); //O
  flash3(pin, DOT);  //S
  delay(DASH); //space between words is 6 time units. There was a dash at the end of the letter, so add one more.
}

/******************************************************************************
 * Flash an 'S' or and 'O'.
 *
 * These are each 3 flashes.
 * S = ...
 * O = ---
 *
 * int pin: pin to output on
 * int dotOrDash: send 3 dots or dashes
 *****************************************************************************/
void flash3(int pin, int dotOrDash) {
  for (int i=0;i<3;i++) {
    digitalWrite(pin,HIGH);
    delay(dotOrDash);
    digitalWrite(pin,LOW);
    delay(DOT);  //space between symbols (dots and dashes) of the same letter is 1 time unit.
  }
  delay(DASH-DOT);  //between letters is 3 time units. We just waited a dot, so subtract that.
}