// MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START *
#define dbg(myFixedText, variableName) \
  Serial.print( F(#myFixedText " "  #variableName"=") ); \
  Serial.println(variableName);

#define dbgi(myFixedText, variableName,timeInterval) \
  { \
    static unsigned long intervalStartTime; \
    if ( millis() - intervalStartTime >= timeInterval ){ \
      intervalStartTime = millis(); \
      Serial.print( F(#myFixedText " "  #variableName"=") ); \
      Serial.println(variableName); \
    } \
  }

#define dbgc(myFixedText, variableName) \
  { \
    static long lastState; \
    if ( lastState != variableName ){ \
      Serial.print( F(#myFixedText " "  #variableName" changed from ") ); \
      Serial.print(lastState); \
      Serial.print( F(" to ") ); \
      Serial.println(variableName); \
      lastState = variableName; \
    } \
  }

#define dbgcf(myFixedText, variableName) \
  { \
    static float lastState; \
    if ( lastState != variableName ){ \
      Serial.print( F(#myFixedText " "  #variableName" changed from ") ); \
      Serial.print(lastState); \
      Serial.print( F(" to ") ); \
      Serial.println(variableName); \
      lastState = variableName; \
    } \
  }
// MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END *

#include <Servo.h>

//define states and working functions
//myServo1 states
const byte START_TO10 = 0;          //move Motor to 10 degrees
const byte MOVETO90   = 1;      //move Motor towards 90 degrees
const byte MOVEBKTO0  = 2;  //move Motor back to 0 degrees
const byte MOVETO180  = 3;    //move Motor to 180 if reaches 3 degrees
const byte STOP_AT180 = 4;       //stop Motor at 180 degrees


const char stateNames[][16] = {
  "START_TO10",
  "MOVETO90",
  "MOVEBKTO0",
  "MOVETO180",
  "STOP_AT180"
};


// We want to start in state "START"
byte currentStateVar = START_TO10;

// constants won't change
// the momentary switch (Touch Sensor) we use to change the motor angle
const int TOUCH_SENSOR1_PIN = 7;        // Arduino pin connected to touch sensor1 's pin

Servo myServo1;                         // create servo object to control a myServo1 motor

//define motor controller pins
const int myServo1_PIN = 11;            // Arduino pin connected to servo motor1 's pin

// variables will change:
int pos = 0;                            // the current angle of servo motor
int lastTouchState;                    // the previous state of touch sensor
int currentTouchState;                 // the current state of touch sensor


void setup() {
  // All motor control pins
  pinMode(TOUCH_SENSOR1_PIN, INPUT);    //Arduino pin connected to touch sensor pin
  myServo1.attach(myServo1_PIN);        // attaches the servo on pin 11 to the servo object

  lastTouchState    = currentTouchState;     //save last state of touch sensor
  currentTouchState = digitalRead(TOUCH_SENSOR1_PIN);        //read new state of touch sensor

  Serial.begin(115200);
}


void startMotor() {
  //move Motor to 0 degrees
  pos = 0;
  myServo1.write(pos);
  delay(15);  //delay for 15 micro seconds
}

void turnMotor90() {
  //move Motor towards 90 degrees

  while (pos = 0 && pos < 90) {
    (pos += 1);         // goes from 0 degrees to 90 degrees in steps of 1 degree
    //{
    myServo1.write(pos);    // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 10ms for the servo to reach the position
  }
}


void turnMotorBack0() {
  //move Motor back to 0 degrees

  while (pos = 90 && pos >= 1) {
    (pos -= 1);          // goes from 90 degrees to 0 degrees in steps of 1 degree
    //{
    myServo1.write(pos);    // tell servo to go to position in variable 'pos'
    delay(10);                       // waits 10ms for the servo to reach the position
  }

  currentTouchState = digitalRead(TOUCH_SENSOR1_PIN);        //read new state of touch sensor
  if (currentTouchState == HIGH) {
    //The sensor is touched
    //set motor state depending on current state
    if (pos > 0) {            //sensor was touched on time so rotation continues
      currentStateVar = MOVETO90;
    }
    else if (pos <= 0) {         //sensor was not touched on time so rotation stops
      currentStateVar = MOVETO180;
    }

  }
}


void turnMotor180() {
  //move Motor to 180 if reaches 0 degrees
  pos = 180;
  myServo1.write(pos);
  delay(50);  //delay for half microsecond
}

void stopMotor() {
  //stop Motor at 180 degrees
  Serial.print("Motor stopped because sensor not touched befor reaching 3 degrees angle");
  Serial.print("Press reset when you're ready to Start Motor again");
  delay(500);
}


void myStateChain() {

  printStateIfChanged(); 
  
  switch (currentStateVar)  {        // this is the start of the switch
    case START_TO10:                   // this starts the state of START and it is what we do if
      //current state == START

      Serial.print("START");
      startMotor();

      currentTouchState = digitalRead(TOUCH_SENSOR1_PIN);        //read new state of touch sensor

      if (currentTouchState == HIGH) {
        //The sensor is touched
        //set motor state depending on current state
        currentStateVar = MOVETO90;
      }
      break;                                    //this ends the state of "START"


    case MOVETO90:                                //start of the "MOVETO90" case
      //current state == MOVETO90

      Serial.print("Moving to 90 degrees angle");
      turnMotor90();

      if (pos = 90) {
        currentStateVar = MOVEBKTO0;       //set motor state depending on current state
      }
      break;                                    //this ends the state of "MOVETO90"


    case MOVEBKTO0:                               //start of the "MOVEBKTO3" case
      //current state == MOVEBKTO3
      Serial.print("Moving back to 3 degrees angle from 90 degrees angle");
      turnMotorBack0();
      break;                                    //this ends the state of "MOVEBKTO3"


    case MOVETO180:                               //start of the "MOVETO180" case
      //current state == MOVETO180

      Serial.print("Sensor not touched on time so rotation stopping");
      turnMotor180();
      if (pos = 180) {
        currentStateVar = STOP_AT180;
      }
      break;                                    //this ends the state of "MOVETO180"


    case STOP_AT180:                                    //start of the "STOP" case
      //motor moves to 180 degrees and stops until reset
      stopMotor();
      break;
  } // this is the end of the possible cases for switch(currentStateVar)machine

  //delay(100);
}


void printStateIfChanged() {
  static byte lastState;

  if (lastState != currentStateVar) {
    Serial.print("state changed from ");
    Serial.print ( stateNames[lastState] );
    Serial.print(" to ");
    Serial.print ( stateNames[currentStateVar] );
    Serial.println();
    lastState = currentStateVar;
  }
}


void loop() {
  myStateChain();                  // Run state machine
}