#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "Pixelation7pt7b.h"
#include <Servo.h>

Servo myservo; // create servo object to control a servo

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3D ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define ENCODER_CW   19
#define ENCODER_CCW  17
#define ENCODER_SW   18
#define SWITCH_UP    2
#define SWITCH_DOWN  3
#define SERVO_BEND   7

uint8_t posX[4] = {2, 50, 52, 76};
uint8_t posY[5] = {2, 14, 26, 38, 51};
uint8_t widthX = 124;
uint8_t widthY = 11;

float ends = 4.0;
float middle = 5.0;
uint8_t units = 1;
float core = 0.35;

// display
uint8_t selected = 2; // 0 = ENDS, 1 = MIDDLE, 2 = CORE, 3 = UNITS, 4 = RUN
uint8_t updateGFX = 0;

// declare variables for the stepper motor pins
uint8_t motorPin1 = 8;     // Blue   - 28BYJ48 pin 1
uint8_t motorPin2 = 9;     // Pink   - 28BYJ48 pin 2
uint8_t motorPin3 = 10;    // Yellow - 28BYJ48 pin 3
uint8_t motorPin4 = 11;    // Orange - 28BYJ48 pin 4
                           // Red    - 28BYJ48 pin 5 (VCC)

uint8_t motorSpeed = 1000;  //variable to set stepper speed
uint8_t countsperrev = 512; // number of steps per full revolution
uint8_t stepperCount = 0; // 0 - 7
uint8_t lookup[8] = {B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001};

void setup()
{
  Serial.begin(115200);
  while (!Serial);
  {
    delay(1);
  }

  // set up the encoder and switch pins as inputs
  pinMode(ENCODER_CW, INPUT);
  pinMode(ENCODER_CCW, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  pinMode(SWITCH_UP, INPUT_PULLUP);
  pinMode(SWITCH_DOWN, INPUT_PULLUP);

  // set up the stepper motor pins as outputs
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(ENCODER_CW), readEncoder, FALLING);
  attachInterrupt(digitalPinToInterrupt(ENCODER_SW), encoderSwitch, FALLING);
  attachInterrupt(digitalPinToInterrupt(SWITCH_UP), switchUP, FALLING);
  attachInterrupt(digitalPinToInterrupt(SWITCH_DOWN), switchDOWN, FALLING);

  // initialise the stepper motor
  moveSteps(8);

  // initialise the servo motor
  myservo.attach(SERVO_BEND); // attaches the servo on pin 9 to the servo object
  myservo.write(90);

  // initialise the display
  initialiseDisplay();
}

// run a batch of wire cutting jobs
void runBatch()
{
  Serial.println("RUN");

  for (uint8_t t = 0; t < units; t++)
  {
    // move wire by "ends" amount (stepper)
    moveSteps(8);

    // trim end (servo)
    myservo.write(90);
    Serial.println("TRIM");

    // move wire by "middle" amount (stepper)
    moveSteps(8);

    // trim end (servo)
    myservo.write(90);
    Serial.println("TRIM");

    // move wire by "ends" amount (stepper)
    moveSteps(8);

    // cut wire (servo)
    myservo.write(90);
    Serial.println("CUT");
  }
}

// stepper motor steps movement
void moveSteps(uint8_t steps)
{
  for (uint8_t i = 0; i < steps; i++)
  {
    Serial.print("STEP ");
    Serial.println(stepperCount);

    setOutput(stepperCount);
    delayMicroseconds(motorSpeed);
    stepperCount++;

    if (stepperCount > 7)
    {
      stepperCount = 0;
    }
  }
}

// stepper motor output
void setOutput(uint8_t out)
{
  digitalWrite(motorPin1, bitRead(lookup[out], 0));
  digitalWrite(motorPin2, bitRead(lookup[out], 1));
  digitalWrite(motorPin3, bitRead(lookup[out], 2));
  digitalWrite(motorPin4, bitRead(lookup[out], 3));
}

void loop()
{
  // while (1)
  // {
    if (updateGFX)
    {
      updateGFX = 0;
      // Serial.println("updateGFX");
      mainGFX();
    }
  // }
}