// Include the motor libraries:
#include <Servo.h>
#include <AccelStepper.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         4
#define PIN_ENDSTOP                  6
#define PIN_ATTENTION_LEVEL         A5
#define MAX_ATTENTION_LEVEL        664
#define NUMBING_FACTOR                (MAX_ATTENTION_LEVEL/3) // ADC resolution / number of levels

// Define parameters for the servo motor
#define PIN_SERVO_MOTOR     5
#define SERVO_MIN_DEGREE    0
#define SERVO_MAX_DEGREE  135

// 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   180 // max number of degrees to move elbos
#define STEPPER_MIX_REACH       0 // min number of degrees to move elbow
#define STEPPER_MAX_REACH     286 // (STEPPER_MAX_DEGREES/360)*(STEPPER_RESOLUTION*STEPPER_GEAR_RATIO))

// Define the Arduino Motor Shield rev3 pins from the datasheet
#define PIN_DIR_A   12
#define PIN_PWM_A    3
#define PIN_BRAKE_A  9
#define PIN_SENSE_A  0
#define PIN_DIR_B   13
#define PIN_PWM_B   11
#define PIN_BRAKE_B  8
#define PIN_SENSE_B  1

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

// Create a new instance of the AccelStepper class
AccelStepper ElbowMotor = AccelStepper(2, PIN_DIR_A, PIN_DIR_B);

int SystemState = STATE_INIT;
int AttentionLevel = 0; // Measured attention level
int AttentionLevelOld = 0; // Measured attention level
int i = 0; // temporaty loop variable

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

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

  // Set the PWM and brake pins so that the direction pins can be used to control the motor:
  pinMode(PIN_PWM_A, OUTPUT);
  pinMode(PIN_BRAKE_A, OUTPUT);
  pinMode(PIN_SENSE_A, INPUT);
  pinMode(PIN_PWM_B, OUTPUT);
  pinMode(PIN_BRAKE_B, OUTPUT);
  pinMode(PIN_SENSE_B, INPUT);

  digitalWrite(PIN_PWM_A, HIGH);
  digitalWrite(PIN_BRAKE_A, LOW);
  digitalWrite(PIN_PWM_B, HIGH);
  digitalWrite(PIN_BRAKE_B, LOW);
  
  // Set the motor speed (Steps per second):
  ElbowMotor.setSpeed(50);
  ElbowMotor.setMaxSpeed(100);
  ElbowMotor.setAcceleration(50);

  pinMode(PIN_ACTIVATION, INPUT_PULLUP);
  pinMode(PIN_SHOULDER_CONTACT, INPUT_PULLUP);
  pinMode(PIN_ATTENTION_LEVEL, INPUT);

// 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.");

      // 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("Attention level: "); 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.");

      ElbowMotor.moveTo(STEPPER_MAX_REACH);
      ElbowMotor.setSpeed(100);
      
      // While the shoulder is not reached and arm is not fully extended
      while (digitalRead(PIN_SHOULDER_CONTACT) && 
             ElbowMotor.currentPosition() != ElbowMotor.targetPosition()) {
        // reach further by increasing step
        Serial.print("Moving "); Serial.print(ElbowMotor.currentPosition()); Serial.print(" -> ");Serial.println(ElbowMotor.targetPosition());
        ElbowMotor.runSpeedToPosition();
      }
      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.moveTo(STEPPER_MIN_REACH);
      ElbowMotor.setSpeed(100);
      while (ElbowMotor.currentPosition() != ElbowMotor.targetPosition()) {
        // while not fully retracted
        Serial.print("Moving "); Serial.print(ElbowMotor.currentPosition()); Serial.print(" -> ");Serial.println(ElbowMotor.targetPosition());
        ElbowMotor.runSpeedToPosition();
      }
      // 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;
  }
}