#include <Adafruit_GFX.h>
#include <Adafruit_PWMServoDriver.h>
#include <Adafruit_SSD1306.h>
#include <Conceptinetics.h>
#include <Encoder.h>
#include <SPI.h>
#include <TMCStepper.h>
#include <AccelStepper.h>
#include <Wire.h>
#include <TimerOne.h>

/* Relay start */
const byte RELAY_PIN = 22;
/* Relay end */

/* DMX shield start */
const int CHANNELS_TOTAL = 5;
const int MAX_START_CHANNEL = 512 - CHANNELS_TOTAL + 1;
DMX_Slave dmxSlave(CHANNELS_TOTAL);

int startChannel = 1;
int lastDisplayedStartChannel = 0;

int lastChannel1Value = -1;
int lastChannel2Value = -1;
int lastChannel3Value = -1;
int lastChannel4Value = -1;
int lastChannel5Value = -1;
/* DMX shield end */

/* Display start */
const int DISPLAY_WIDTH = 128;
const int DISPLAY_HEIGHT = 32;
const byte OLED_RESET = 4; // reset pin (-1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(DISPLAY_WIDTH, DISPLAY_HEIGHT, &Wire, OLED_RESET);
const int REFRESH_EVERY = 40; // in millis
long lastDisplayRefresh = 0;
/* Display end */

/* Encoder start */
const byte ENCODER_A_PIN = 18;
const byte ENCODER_B_PIN = 19;
const byte ENCODER_SW_PIN = 24;
Encoder encoder(ENCODER_A_PIN, ENCODER_B_PIN);
long lastEncoderPosition = 0;
int lastSwPinValue = LOW;
/* Encoder end */

/* Stepper start */
const byte EN_PIN = 50; // Enable
const byte SW_MOSI = 48; // Software Master Out Slave In (MOSI)
const byte SW_SCK = 46; // Software Slave Clock (SCK)
const byte CS_PIN = 44; // Chip select
const byte SW_MISO = 42; // Software Master In Slave Out (MISO)
const byte STEP_PIN = 40; // Step
const byte DIR_PIN = 38; // Direction

const float R_SENSE = 0.11; // Match to your driver (SilentStepStick series use 0.11)

TMC2130Stepper driver = TMC2130Stepper(CS_PIN, R_SENSE, SW_MOSI, SW_MISO, SW_SCK); // Software SPI
AccelStepper stepper = AccelStepper(stepper.DRIVER, STEP_PIN, DIR_PIN);

const int BASE_STEP_COUNT = 200;
const int MICROSTEPS = 16;
const int STEPS_PER_REV = BASE_STEP_COUNT * MICROSTEPS;
/* Stepper end */

/* LEDs start */
Adafruit_PWMServoDriver pwmDriver = Adafruit_PWMServoDriver();
const byte PWM_LED1_PIN = 0;
const byte PWM_LED2_PIN = 1;
const byte PWM_LED3_PIN = 2;
const byte PWM_LED4_PIN = 3;

int led1CurrentValue = 4095;
int led2CurrentValue = 4095;
int led3CurrentValue = 4095;
int led4CurrentValue = 4095;

int led1GoalValue = 4095;
int led2GoalValue = 4095;
int led3GoalValue = 4095;
int led4GoalValue = 4095;
/* LEDs end */

/* Mapped values start */
int brightnessMapping[256];
float speedMapping[256];
/* Mapped values end */

void setup() {
  initializePins();
  initializePwmDriver();
  initializeDisplay();
  initializeStepper();
  showChannelSelector();
  precalcValues();
  enableDmxShield();

  Timer1.initialize(100);
  Timer1.attachInterrupt(pollStepper);
}

void pollStepper() {
  stepper.run();
}

void loop() {
  /* LED #1 */
  if (led1CurrentValue != led1GoalValue) {
    led1CurrentValue += (led1GoalValue > led1CurrentValue) ? 1 : -1;
    pwmDriver.setPWM(PWM_LED1_PIN, 0, led1CurrentValue);
  }

  /* LED #2 */
  if (led2CurrentValue != led2GoalValue) {
    led2CurrentValue += (led2GoalValue > led2CurrentValue) ? 1 : -1;
    pwmDriver.setPWM(PWM_LED2_PIN, 0, led2CurrentValue);
  }

  /* LED #3 */
  if (led3CurrentValue != led3GoalValue) {
    led3CurrentValue += (led3GoalValue > led3CurrentValue) ? 1 : -1;
    pwmDriver.setPWM(PWM_LED3_PIN, 0, led3CurrentValue);
  }

  /* LED #4 */
  if (led4CurrentValue != led4GoalValue) {
    led4CurrentValue += (led4GoalValue > led4CurrentValue) ? 1 : -1;
    pwmDriver.setPWM(PWM_LED4_PIN, 0, led4CurrentValue);
  }
}

void initializePins() {
  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW);

  pinMode(ENCODER_SW_PIN, INPUT);

  pinMode(CS_PIN, OUTPUT);
  digitalWrite(CS_PIN, HIGH);
}

void initializePwmDriver() {
  pwmDriver.begin();
  pwmDriver.setPWMFreq(1600);
  pwmDriver.setPWM(PWM_LED1_PIN, 0, led1CurrentValue);
  pwmDriver.setPWM(PWM_LED2_PIN, 0, led2CurrentValue);
  pwmDriver.setPWM(PWM_LED3_PIN, 0, led3CurrentValue);
  pwmDriver.setPWM(PWM_LED4_PIN, 0, led4CurrentValue);
}

void initializeDisplay() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.cp437(true); // use full 256 char 'Code Page 437' font
  display.display();
}

void initializeStepper() {
  driver.begin();
  driver.rms_current(600);
  driver.en_pwm_mode(1); // Enable extremely quiet stepping
  driver.pwm_autoscale(1);
  driver.microsteps(MICROSTEPS);

  stepper.setMaxSpeed(0);
  stepper.setAcceleration(STEPS_PER_REV * 25);
  stepper.setEnablePin(EN_PIN);
  stepper.setPinsInverted(false, false, true);
  stepper.enableOutputs();
}

void showChannelSelector() {
  lastEncoderPosition = encoder.read();
  while (true) {
    int currentSwPinValue = digitalRead(ENCODER_SW_PIN);
    if (lastSwPinValue == HIGH && currentSwPinValue == LOW) {
      displaySelectedChannels();
      break;
    } else {
      lastSwPinValue = currentSwPinValue;
    }

    long currentEncoderPosition = encoder.read();
    long diff = currentEncoderPosition - lastEncoderPosition;
    lastEncoderPosition = currentEncoderPosition - (diff % 4);

    startChannel += diff / 4;
    startChannel = (startChannel < 1) ? 1 : startChannel;
    startChannel = (startChannel > MAX_START_CHANNEL) ? MAX_START_CHANNEL : startChannel;

    bool displayRedrawn = displayStartChannel();
    if (!displayRedrawn) {
      delay(2);
    }
  }
}

bool displayStartChannel() {
  if (startChannel == lastDisplayedStartChannel) {
    return false;
  }

  long currentTime = millis();
  if (lastDisplayRefresh == 0 || currentTime - lastDisplayRefresh >= REFRESH_EVERY) {
    display.clearDisplay();
    display.setCursor(1, 1);
    display.println("Start ch.:");
    display.println(startChannel);
    display.display();

    lastDisplayRefresh = currentTime;
    lastDisplayedStartChannel = startChannel;

    return true;
  }
  return false;
}

void displaySelectedChannels() {
  display.clearDisplay();
  display.setCursor(4, 8);
  display.print("ch.");
  display.print(startChannel);
  display.print("-");
  display.println(startChannel + CHANNELS_TOTAL - 1);
  display.display();
}

void precalcValues() {
  for (int i = 0; i < 256; i++) {
    brightnessMapping[i] = mapBrightness(i);
    speedMapping[i] = mapSpeed(i);
  }
}

int mapBrightness(int channelValue) {
  if (channelValue == 0) {
    return 4095;
  }

  int mappedValue;
  if (channelValue <= 20) {
    mappedValue = map(channelValue, 0, 20, 4075, 4055);
  } else if (channelValue <= 50) {
    mappedValue = map(channelValue, 20, 50, 4055, 4000);
  } else if (channelValue <= 200) {
    mappedValue = map(channelValue, 50, 200, 4000, 1500);
  } else {
    mappedValue = map(channelValue, 200, 255, 1500, 0);
  }
  return mappedValue;
}

float mapSpeed(int channelValue) {
  float rpm = ((float)channelValue / 255 * 69) + 1;
  return STEPS_PER_REV * rpm / 60;
}

void enableDmxShield() {
  digitalWrite(RELAY_PIN, HIGH);
  dmxSlave.onReceiveComplete(onFrameReceiveComplete);
  dmxSlave.enable();
  dmxSlave.setStartAddress(startChannel);
}

void onFrameReceiveComplete(void) {
  int channel1Value = dmxSlave.getChannelValue(1);
  int channel2Value = dmxSlave.getChannelValue(2);
  int channel3Value = dmxSlave.getChannelValue(3);
  int channel4Value = dmxSlave.getChannelValue(4);
  int channel5Value = dmxSlave.getChannelValue(5);

  /* Channel #1, stepper */
  if (channel1Value != lastChannel1Value) {
    lastChannel1Value = channel1Value;
    stepper.setMaxSpeed(speedMapping[channel1Value]);

    // flawed solution, need a better periodic workaround
    if (abs(stepper.distanceToGo()) < STEPS_PER_REV) {
      stepper.move(1000000);
    }
  }

  /* Channel #2, LED #1 */
  if (channel2Value != lastChannel2Value) {
    lastChannel2Value = channel2Value;
    led1GoalValue = brightnessMapping[channel2Value];
  }

  /* Channel #3, LED #2 */
  if (channel3Value != lastChannel3Value) {
    lastChannel3Value = channel3Value;
    led2GoalValue = brightnessMapping[channel3Value];
  }

  /* Channel #4, LED #3 */
  if (channel4Value != lastChannel4Value) {
    lastChannel4Value = channel4Value;
    led3GoalValue = brightnessMapping[channel4Value];
  }

  /* Channel #5, LED #4 */
  if (channel5Value != lastChannel5Value) {
    lastChannel5Value = channel5Value;
    led4GoalValue = brightnessMapping[channel5Value];
  }
}