/*
  Dialing Sequence program for 3D printed working Stargate
  Written by Cara McNab ( Carasibana )
  This sketch requires the following Libraries:
  Neopixel Library,  available via https://learn.adafruit.com/adafruit-neopixel-uberguide/arduino-library
  Adafruit Motor Shield Library, avaliable via https://learn.adafruit.com/adafruit-motor-shield-v2-for-arduino/install-software
*/
// Include the Motor shield and NeoPixel Libraries
#include <Wire.h>
//#include <Adafruit_MotorShield.h>
//#include "utility/Adafruit_PWMServoDriver.h"
#include <Adafruit_NeoPixel.h>
#include <AFMotor.h>
//#include <AccelStepper.h>
// Which pin on the Arduino is connected to the NeoPixels
#define PIN 6

// How many NeoPixels are attached to the Arduino
#define NUMPIXELS 9

// How many steps are required to advance the Symbol Ring by one symbol *needs tweaking
int STEPPERSYM = 31;

// Define the Pixel strand item "pixels"
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_RGB + NEO_KHZ800);

// Define the colours to use for the NeoPixels
uint32_t orange = pixels.Color(255, 40, 5);
uint32_t off = pixels.Color(0, 0, 0);

// Create the motor shield object with the default I2C address
// Adafruit_MotorShield AFMS = Adafruit_MotorShield();
//AF_Stepper myMotor(stepsPerRevolution, 2);
// Connect a stepper motor with 200 steps per revolution (1.8 degree)
// to motor port #2 (M3 and M4)
//Adafruit_StepperMotor *myMotor = AFMS.getStepper(200, 2);
const int stepsPerRevolution = 200;

AF_Stepper motorGate(200, 2);

//AccelStepper myMotor(1, 3, 2);

//AF_Stepper myMotor(stepsPerRevolution, 2);
// Define Stargate address to be dialed as array
// These numbers are the references to the Gate symbols, including the point of origin (1)
// reference can be found here http://rdanderson.com/stargate/glyphs/glyphs.htm

int SGAddress[] = {27, 7, 15, 32, 12, 30, 1};
byte ChevronPixels[] ={4,5,6,7,1,2,3,8};
// Define delay for Blinking
int BlinkDelay = 1500;

byte stateGate = 0;
unsigned long lastMilli = 0;
unsigned int  intervalWait = 1500;
int Chevron = 1;



// you can change these to DOUBLE or INTERLEAVE or MICROSTEP!
void forwardstep() {  
  motorGate.onestep(FORWARD, SINGLE);
}
void backwardstep() {  
  motorGate.onestep(BACKWARD, SINGLE);
}

//AccelStepper stepper(forwardstep, backwardstep); // use functions to step


void setup() {
  // AFMS.begin();  // create with the default frequency 1.6KHz
  Serial.begin(115200);
  Serial.println("Ready");

 motorGate.setSpeed(50);  // Set RPM of stepper mot
  //stepper.setMaxSpeed(800);  // Set RPM of stepper motor
  //stepper.setAcceleration(200); // Set acceleration value for the stepper
  //stepper.setCurrentPosition(0); // Set the current position to 0 steps
  

  pixels.begin(); // This initializes the NeoPixel library.
}

void loop() {

  //stepper.run();



  switch (stateGate) {
    case 0: {
        // make sure all neopixels are off
        for (int i = 0; i < NUMPIXELS; i++) {
          pixels.setPixelColor(i, off);
        }
        pixels.show(); // This sends the updated pixel color to the hardware.
        stateGate = 1;
      }
      break;
    case 1: {
        int numStep = (SGAddress[(Chevron - 1)] - 1) * STEPPERSYM;
        Serial.print("State1 Chevron: ");
        Serial.print(Chevron); Serial.print(" steps: ");
        Serial.println(numStep);
        //stepper.runToNewPosition(numStep);
        motorGate.step(numStep, FORWARD, INTERLEAVE);
        pixels.setPixelColor(4, orange);
        pixels.show();
        stateGate = 2;
      }
      break;
    case 2: if (millis() - lastMilli >= intervalWait) {
        lastMilli = millis();
        pixels.setPixelColor(4, off);
        pixels.setPixelColor(5, orange);
        pixels.show();
        stateGate = 3;
      }
      break;
    case 3: {
        Chevron++ ;
        int numStep = 0;
       if (Chevron < 8){
        if (SGAddress[(Chevron - 1)] > SGAddress[(Chevron - 2)]) {
          numStep = ((SGAddress[(Chevron - 2)] - 1) + (40 - SGAddress[(Chevron - 1)])) * STEPPERSYM;
        }
        else {
          numStep = (SGAddress[(Chevron - 1)] - SGAddress[(Chevron - 2)]) * STEPPERSYM;
        }
        Serial.print("State3 Chevron: ");
        Serial.print(Chevron); Serial.print(" steps: ");
        Serial.println(numStep);
        //stepper.runToNewPosition(numStep);
        motorGate.step(numStep, BACKWARD, INTERLEAVE);
        pixels.setPixelColor(4, orange);
        pixels.show();
        stateGate = 4;
       } else stateGate = 5;
      }
      break;
      case 4:if (millis() - lastMilli >= intervalWait) {
        lastMilli = millis();
        pixels.setPixelColor(4, off);
        pixels.setPixelColor(ChevronPixels[Chevron-1], orange);
        Serial.print("Setting Pixel:");Serial.println(ChevronPixels[Chevron-1]);
        pixels.show();
        stateGate = 3;
      }
      break;
      case 5: pixels.setPixelColor(0, orange);
              pixels.setPixelColor(8, orange);
              pixels.show();
              stateGate = 6;
              break;
  }

  /*

    // Chevron One (( Assumes starting on Earth (1) ))
    int Chevron = 1;

    int numStep = (SGAddress[(Chevron - 1)] - 1) * STEPPERSYM;
    myMotor.moveTo(numStep);
    //myMotor.step(numStep, FORWARD, INTERLEAVE);
    pixels.setPixelColor(4, orange);
    pixels.show();
    delay(BlinkDelay);
    pixels.setPixelColor(4, off);
    pixels.setPixelColor(5, orange);
    pixels.show();

    // Chevron Two

    Chevron = 2 ;

    if (SGAddress[(Chevron - 1)] > SGAddress[(Chevron - 2)]) {
      int numStep = ((SGAddress[(Chevron - 2)] - 1) + (40 - SGAddress[(Chevron - 1)])) * STEPPERSYM;
    }
    else {
      int numStep = (SGAddress[(Chevron - 1)] - SGAddress[(Chevron - 2)]) * STEPPERSYM;
    }
    //myMotor.step(numStep, BACKWARD, INTERLEAVE);
    myMotor.moveTo(numStep);
    pixels.setPixelColor(4, orange);
    pixels.show();
    delay(BlinkDelay);
    pixels.setPixelColor(4, off);
    pixels.setPixelColor(6, orange);
    pixels.show();

    // Chevron Three * Needs Tweaking

    Chevron = 3 ;

    if (SGAddress[(Chevron - 1)] > SGAddress[(Chevron - 2)]) {
      int numStep = ((SGAddress[(Chevron - 2)] - 1) + (40 - SGAddress[(Chevron - 1)])) * STEPPERSYM;
    }
    else {
      int numStep = (SGAddress[(Chevron - 1)] - SGAddress[(Chevron - 2)]) * STEPPERSYM;
    }
    //myMotor.step(numStep, FORWARD, INTERLEAVE);
    myMotor.moveTo(numStep);
    pixels.setPixelColor(4, orange);
    pixels.show();
    delay(BlinkDelay);
    pixels.setPixelColor(4, off);
    pixels.setPixelColor(7, orange);
    pixels.show();

    // Chevron Four

    Chevron = 4 ;

    if (SGAddress[(Chevron - 1)] > SGAddress[(Chevron - 2)]) {
      int numStep = ((SGAddress[(Chevron - 2)] - 1) + (40 - SGAddress[(Chevron - 1)])) * STEPPERSYM;
    }
    else {
      int numStep = (SGAddress[(Chevron - 1)] - SGAddress[(Chevron - 2)]) * STEPPERSYM;
    }
    //  myMotor.step(numStep, BACKWARD, INTERLEAVE);
    myMotor.moveTo(numStep);
    pixels.setPixelColor(4, orange);
    pixels.show();
    delay(BlinkDelay);
    pixels.setPixelColor(4, off);
    pixels.setPixelColor(1, orange);
    pixels.show();

    // Chevron Five * Needs Tweaking

    Chevron = 5 ;

    if (SGAddress[(Chevron - 1)] > SGAddress[(Chevron - 2)]) {
      int numStep = ((SGAddress[(Chevron - 2)] - 1) + (40 - SGAddress[(Chevron - 1)])) * STEPPERSYM;
    }
    else {
      int numStep = (SGAddress[(Chevron - 1)] - SGAddress[(Chevron - 2)]) * STEPPERSYM;
    }
    // myMotor.step(numStep, FORWARD, INTERLEAVE);
    myMotor.moveTo(numStep);
    pixels.setPixelColor(4, orange);
    pixels.show();
    delay(BlinkDelay);
    pixels.setPixelColor(4, off);
    pixels.setPixelColor(2, orange);
    pixels.show();

    // Chevron Six

    Chevron = 6 ;

    if (SGAddress[(Chevron - 1)] > SGAddress[(Chevron - 2)]) {
      int numStep = ((SGAddress[(Chevron - 2)] - 1) + (40 - SGAddress[(Chevron - 1)])) * STEPPERSYM;
    }
    else {
      int numStep = (SGAddress[(Chevron - 1)] - SGAddress[(Chevron - 2)]) * STEPPERSYM;
    }
    //myMotor.step(numStep, BACKWARD, INTERLEAVE);
    myMotor.moveTo(numStep);
    pixels.setPixelColor(4, orange);
    pixels.show();
    delay(BlinkDelay);
    pixels.setPixelColor(4, off);
    pixels.setPixelColor(3, orange);
    pixels.show();

    // Chevron Seven * Needs Tweaking

    Chevron = 7 ;

    if (SGAddress[(Chevron - 1)] > SGAddress[(Chevron - 2)]) {
      int numStep = ((SGAddress[(Chevron - 2)] - 1) + (40 - SGAddress[(Chevron - 1)])) * STEPPERSYM;
    }
    else {
      int numStep = (SGAddress[(Chevron - 1)] - SGAddress[(Chevron - 2)]) * STEPPERSYM;
    }
    //myMotor.step(numStep, FORWARD, INTERLEAVE);
    myMotor.moveTo(numStep);
    pixels.setPixelColor(4, orange);
    pixels.show();
    delay(BlinkDelay);
    pixels.setPixelColor(0, orange);
    pixels.setPixelColor(8, orange);
    pixels.show();

    // Restart Sequence
    delay(10000);
  */

}