#include <Wire.h>

const byte CALIBRATE = 1;
const byte GO_TO_LETTER = 2;
const byte SPIN = 3;
const byte TEST = 4;

const int numberOfFollowers = 3;
const int followerAddresses[] = {9, 10, 11};

enum LeaderState {
  Initializing,
  CalibrateFollowers,
  SendingReceiving,
  DoNothing,
};
LeaderState leaderState = Initializing;
enum FollowerState {
  Waiting,
  Calibrating,
  SearchingForLetter,
  Spinning,
};
FollowerState followerStates[numberOfFollowers];

bool calibrated = false;
const int answerSize = 1;
int wordCntr = 0;

String poem [] = {"The     ", "Robots  ", "Are     ", "Coming  "};

void setup() {

  // setting up FollowerState enum
  for (int i = 0; i < sizeof(followerStates) / sizeof(followerStates[0]); i++) {
    followerStates[i] = Waiting;
  }
  leaderState = CalibrateFollowers; // setting leaderState initial state
  Wire.begin(); // begins communication
  Serial.begin(9600);
}

// Will repeat on loop while program is running.
void loop() {

  // Switch statement tells us whether to calibrate or work on moving follower to specific letter
  switch (leaderState) {
    case (CalibrateFollowers):
      sendCalibrate();
      delay(5000);
      leaderState = SendingReceiving;
      break;
    case (SendingReceiving):
      sendCharacter();
      leaderState = DoNothing;
      break;
  }
}

// Tells followers to spin to characters
void sendCharacter() {
  
  Serial.println("sending letter");

  // loops through followers and switches follower state and sends target character
  for (int i = 0; i < numberOfFollowers; i++) {
    Wire.beginTransmission(followerAddresses[i]);
    followerStates[i] = SearchingForLetter;
    Wire.write(GO_TO_LETTER);
    Wire.write('Q'); // sending target character
    Wire.endTransmission();
  }
  
  int response = 0;

  // checks that each follower has arrived at the specified letter and updates state accordingly
  while (followerStateContains(SearchingForLetter)) {
    for (int i = 0; i < numberOfFollowers; i++) {
      Wire.requestFrom(followerAddresses[i], answerSize);
      while (Wire.available()) {
        response = Wire.read();
      }
      if (response == 1) {
        followerStates[i] = Waiting;
      }
    }
  }
  
  Serial.println("done sending letter");
}

// Tells followers to calibrate to zero position in split flap
void sendCalibrate() {
  
  Serial.println("sending calibrate to followers");

  // sends calibrate command to each follower
  for (int i = 0; i < numberOfFollowers; i++) {
    followerStates[i] = Calibrating;
    Wire.beginTransmission(followerAddresses[i]);
    Wire.write(CALIBRATE);
    Wire.write(CALIBRATE);
    Wire.endTransmission();
  }
  
  Serial.println("sent");
  int response = 0;

  while (followerStateContains(Calibrating)) { // will hang here until calibration is done for each follower
    for (int i = 0; i < numberOfFollowers; i++) {
      Wire.requestFrom(followerAddresses[i], answerSize);
      while (Wire.available()) {
        response = Wire.read();
      }
      if (response == 1) {
        followerStates[i] = Waiting;
      }
    }
  }
  
  Serial.println("followers done calibrating");
}

// Function when called sends a SPIN command to followers to constantly spin. 
void sendSpin() {
  Serial.println("sending toggle spin to followers");
  for (int i = 0; i < numberOfFollowers; i++) {
    followerStates[i] = Spinning;
    Wire.beginTransmission(followerAddresses[i]);
    Wire.write(SPIN);
    Wire.write(SPIN);
    Wire.endTransmission();
  }
}

bool followerStateContains(FollowerState state) {
  for (int i = 0; i < numberOfFollowers; i++) {
    if (followerStates[i] == state) {
      return true;
    }
  }
  return false;
}