/*
 * Blink
 * Turns on an LED on for one second,
 * then off for one second, repeatedly.
 */

#include <Arduino.h>
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define REPRAP_DISCOUNT_SMART_CONTROLLER
#include "pins/pins_TRIGORILLA_14.h"
#elif defined(__AVR_ATmega328P__)
#define LED_PIN LED_BUILTIN
#define X_DIR_PIN PD2
#define X_STEP_PIN PD3
#define X_ENABLE_PIN A0
#define Y_DIR_PIN PD4
#define Y_STEP_PIN PD5
#define Y_ENABLE_PIN A1
#define Z_DIR_PIN PD6
#define Z_STEP_PIN PD7
#define Z_ENABLE_PIN A2
#define E0_DIR_PIN PB0
#define E0_STEP_PIN PB1
#define E0_ENABLE_PIN A3
#define E1_DIR_PIN PB2
#define E1_STEP_PIN PB3
#define E1_ENABLE_PIN A4
#endif

#define LCD_PINS_RS     16
#define LCD_PINS_ENABLE 17
#define LCD_PINS_D4     23
#define LCD_PINS_D5     25
#define LCD_PINS_D6     27
#define LCD_PINS_D7     29

#define BTN_EN1         31
#define BTN_EN2         33
#define BTN_ENC         35

#define BEEPER_PIN      37
#define KILL_PIN        41
#define SD_DETECT_PIN   49

#include "stepper.h"
#include <Wire.h>
#include <LiquidCrystal.h>

const int NUM_STEPPER = 5;

#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
TimerStepper stepper[NUM_STEPPER];
#elif defined(__AVR_ATmega328P__)
LoopStepper stepper[NUM_STEPPER];
#endif

LiquidCrystal lcd(LCD_PINS_RS, LCD_PINS_ENABLE, LCD_PINS_D4, LCD_PINS_D5, LCD_PINS_D6, LCD_PINS_D7);

unsigned long lastMillis = 0;
unsigned long lastCommand = 0;
const long interval = 1000;           // interval at which to blink (milliseconds)
int enable_pin[NUM_STEPPER] = {X_ENABLE_PIN, Y_ENABLE_PIN, Z_ENABLE_PIN, E0_ENABLE_PIN, E1_ENABLE_PIN};


void setup()
{
  Serial.begin(115200);
/*  delay(500);
  Serial.println("##########");
  Serial.println("MIDI");
  Serial.println("##########");
  Serial.flush();
*/
  // initialize LED digital pin as an output.
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);
  
  pinMode(TRIGORILLA_HEATER_0_PIN, OUTPUT);
  digitalWrite(TRIGORILLA_HEATER_0_PIN, HIGH);

  lcd.begin(20, 4);
  lcd.print("ROBERTO");

  stepper[0].begin( X_DIR_PIN,  X_STEP_PIN,  X_ENABLE_PIN);
  stepper[1].begin( Y_DIR_PIN,  Y_STEP_PIN,  Y_ENABLE_PIN);
  stepper[2].begin( Z_DIR_PIN,  Z_STEP_PIN,  Z_ENABLE_PIN);
  stepper[3].begin(E0_DIR_PIN, E0_STEP_PIN, E0_ENABLE_PIN);
  stepper[4].begin(E1_DIR_PIN, E1_STEP_PIN, E1_ENABLE_PIN);

  for (int i = 0; i < NUM_STEPPER; i++) {
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
    stepper[i].setMaxSpeed(8000);
#elif defined(__AVR_ATmega328P__)
    stepper[i].setMaxSpeed(1400);
#endif
    stepper[i].setAcceleration(4000);
  }

  // Timer0 is already used for millis() - we'll just interrupt somewhere
  // in the middle and call the "Compare A" function below
  OCR0A = 0xAF;
  TIMSK0 |= _BV(OCIE0A);
}

// Interrupt is called once a millisecond, 
ISR(TIMER0_COMPA_vect) {
  for (int i=0; i<NUM_STEPPER; i++) stepper[i].loop();
}

bool parseCmd(char* buf, int values[4]) {
  char cmd;
  int n = sscanf(buf, "%c %d,%d,%d,%d", &cmd, &values[0], &values[1], &values[2], &values[3]);
  if (n == 5) return true;
  else return false;
}

void loop()
{
  unsigned long currentMillis = millis();
  
  if(Serial.available()) {
    String command = Serial.readStringUntil('\n');
    lastCommand = millis();
    if (command.equals("ENABLE")) {
      stepper[0].enable();
      stepper[1].enable();
      stepper[2].enable();
      stepper[3].enable();
      //Serial.println("## ENABLE");
    } else if (command.equals("DISABLE")) {
      stepper[0].disable();
      stepper[1].disable();
      stepper[2].disable();
      stepper[3].disable();
      //Serial.println("## DISABLE");
    } else if (command.charAt(0) == 'A') {
      int values[4];
      char* buf = command.c_str();
      bool success = parseCmd(buf, values);
      if (success) {
        stepper[0].setAcceleration(values[0]);
        stepper[1].setAcceleration(values[1]);
        stepper[2].setAcceleration(values[2]);
        stepper[3].setAcceleration(values[3]);
      }
    } else if (command.charAt(0) == 'S') {
      int values[4];
      char* buf = command.c_str();
      bool success = parseCmd(buf, values);
      if (success) {
        stepper[0].setSpeedDirect(values[0]);
        stepper[1].setSpeedDirect(values[1]);
        stepper[2].setSpeedDirect(values[2]);
        stepper[3].setSpeedDirect(values[3]);
        Serial.print("Motor Speeds: ");
        Serial.print(values[0]); Serial.print(", ");
        Serial.print(values[1]); Serial.print(", ");
        Serial.print(values[2]); Serial.print(", ");
        Serial.print(values[3]); Serial.print("\n");
      }
    } else if (command.charAt(0) == 'T') {
      String message = command.substring(2);
      char* text = message.c_str();
      lcd.clear();
      lcd.setCursor(0,2);
      lcd.print(text);
    }
  }

  if (millis() - lastCommand > 5000) {
    stepper[0].disable();
    stepper[1].disable();
    stepper[2].disable();
    stepper[3].disable();
  }
/*
  stepper[0].setSpeed(-4000);
  stepper[1].setSpeed(-4000);
  stepper[2].setSpeed(4000);
  stepper[3].setSpeed(4000);
  delay(2000);
  stepper[0].setSpeed(0);
  stepper[1].setSpeed(0);
  stepper[2].setSpeed(0);
  stepper[3].setSpeed(0);
  while (stepper[0].isAccelerating() || stepper[1].isAccelerating() || stepper[2].isAccelerating() || stepper[3].isAccelerating()) {
    delay(20);
  }
  stepper[0].setSpeed(2400);
  stepper[1].setSpeed(2400);
  stepper[2].setSpeed(2400);
  stepper[3].setSpeed(2400);
  delay(4200);
  stepper[0].setSpeed(0);
  stepper[1].setSpeed(0);
  stepper[2].setSpeed(0);
  stepper[3].setSpeed(0);
  while (stepper[0].isAccelerating() || stepper[1].isAccelerating() || stepper[2].isAccelerating() || stepper[3].isAccelerating()) {
    delay(20);
  }
//  for (int i=0; i<NUM_STEPPER; i++) {
//    stepper[i].disable();
//  }
//  delay(2000);
*/
}