// Include the motor libraries:
#include <Servo.h>
#include <AFMotor.h>

// Define parameters for system state
#define STATE_INIT    0
#define STATE_REST    1
#define STATE_REACH   2
#define STATE_TAP     3
#define STATE_RETRACT 4

// Define parameters for user control
#define PIN_ACTIVATION               2
#define PIN_SHOULDER_CONTACT        13
#define PIN_ENDSTOP                  6  // future extention
#define PIN_ATTENTION_LEVEL         A5
#define MAX_ATTENTION_LEVEL       1023  // max ADC value
#define NUMBING_FACTOR                (MAX_ATTENTION_LEVEL/3) // ADC resolution / number of levels

// Define parameters for the servo motor
#define PIN_SERVO_MOTOR     9
#define SERVO_MIN_DEGREE    0
#define SERVO_MAX_DEGREE  150

// Define parameters for the stepper motor
#define STEPPER_RESOLUTION      200 // steps per revolution
#define STEPPER_GEAR_RATIO    60/21 // motor gear vs elbow gear
#define STEPPER_MAX_DEGREES     100 // max number of degrees to move elbos
#define STEPPER_MIN_REACH         0 // min number of degrees to move elbow
#define STEPPER_CALC_RESOLUTION 571 // STEPPER_RESOLUTION * STEPPER_GEAR_RATIO
#define STEPPER_MAX_REACH       200 // (STEPPER_MAX_DEGREES/360)*(STEPPER_RESOLUTION*STEPPER_GEAR_RATIO))

// Create a new instance of the Servo class
Servo WristMotor; 

// Create a new instance of the Adafruit Stepper class
AF_Stepper ElbowMotor(STEPPER_RESOLUTION, 2);

int SystemState = STATE_INIT;
int AttentionLevel = 0;     // Measured attention level
int AttentionLevelOld = 0;  // Measured attention level
int ElbowPosition = 0;      // Exact step of the elbow
int i = 0;                  // temporary loop variable

void setup() {
  // Initialize the serial port for monitoring
  Serial.begin(9600);

  // Set the input pins
  pinMode(PIN_ACTIVATION, INPUT_PULLUP);
  pinMode(PIN_SHOULDER_CONTACT, INPUT_PULLUP);
  pinMode(PIN_ATTENTION_LEVEL, INPUT);

  // Set the servo pins 
  WristMotor.attach(PIN_SERVO_MOTOR);
  WristMotor.write(SERVO_MIN_DEGREE);

  ElbowMotor.setSpeed(30); // RPM, 15 is 2 seconds for full elbow extension

// Spiral: Speak instructions

  // Initialization done
  Serial.println("");
  Serial.println("*** Tap-o-matic 2000 is ready.");
  Serial.println("");
  SystemState = STATE_REST;
}

void loop() {
  switch (SystemState) {
     // Depending on the system state, do one of the following actions
    case STATE_REST:
      // System is at rest
      Serial.println("*** System is at rest.");
      ElbowMotor.release(); // Stop keeping torque on the motor

      // wait till activated
      AttentionLevel = 6;
      while (digitalRead(PIN_ACTIVATION)) {
        // Read requested attention level from potentiometer
        // and translate to number of taps
        AttentionLevel = (int)(analogRead(PIN_ATTENTION_LEVEL) / NUMBING_FACTOR);
        if (AttentionLevel != AttentionLevelOld) {
          // If there is a changed value, send it to the serial port
          Serial.print("Fucks given: "); Serial.println(AttentionLevel);
          AttentionLevelOld = AttentionLevel;
        }
      }

      // Set system state to next state
      SystemState = STATE_REACH;
      break;
    case STATE_REACH:
      // Arm is reaching
      Serial.println("*** System is reaching.");

      ElbowPosition = STEPPER_MIN_REACH;
      // While the shoulder is not reached and arm is not fully extended
      while (digitalRead(PIN_SHOULDER_CONTACT) && ElbowPosition++ <= STEPPER_MAX_REACH) {
        // reach further by increasing step
//      Serial.print("Moving "); Serial.print(i); Serial.print(" -> ");Serial.println(STEPPER_MAX_REACH);
        ElbowMotor.onestep(FORWARD, SINGLE);
      }
//    ElbowMotor.release();
      delay(1000);

      // Set system state to next state
      SystemState = STATE_TAP;
      break;
    case STATE_TAP:
      // System taps shoulder
      Serial.println("*** System is tapping.");

      // Tap the requested number of times
      for (i = 0; i < AttentionLevel; i++) {
        Serial.println("  Tap...");
        WristMotor.write(SERVO_MAX_DEGREE);
        delay(500);
        WristMotor.write(SERVO_MIN_DEGREE);
        delay(500);
//      Spiral: speak "there"
      }

      delay(1000);
      
      // Set system state to next state
      SystemState = STATE_RETRACT;
      break;
    case STATE_RETRACT:
      // System is retracting
      Serial.println("*** System is retracting.");

      ElbowMotor.step(ElbowPosition, BACKWARD, SINGLE);

      // Set system state to next state
      SystemState = STATE_REST;
      break;
    default:
      // System state is not one of the defined states
      Serial.println("*** Illegal state detected, switching to rest."); 

      SystemState = STATE_REST;
      break;
  }
}