//#include "CytronMotorDriver.h"
// SingleBoard CytronMD actuator(PWM_DIR, 3, 2);  // PWM = Pin 3, DIR = Pin 2
//CytronMD actuator(PWM_DIR, 9, 8);  // PWM = Pin 9, DIR = Pin 8
//CytronMD pump(PWM_DIR, 11, 13);  // PWM = Pin 11, DIR = Pin 13
 
#define countDownLed LED_BUILTIN

long pauseTimeRandom;
long insertTimeRandom;
long hitCount = 0;

//--------------------------------------------------------
// Configuration
//--------------------------------------------------------
bool isTest = true;
bool usePump = true;

//long travelTime = 1200; // Speed 10cm/s --> Von 2.5cm bis 15.8cm zieht es aus
long travelTime = 1700; // Mit Messer (6.2cm), --> Von 2.5cm bis 21.5cm zieht es aus, dicke rote Markierung mit Pfeil, 1-2 Finger breit Abstand lassen

long travelTimeFull = 3000; // 3 seconds, speed 10cm/s
long initialTimeInSeconds = 285; // 4 minutes 45s

long stepPauseTimeMin = 1000;
long stepPauseTimeMax = 5000;

long insertTimeMin = 10000; // 10s 
long insertTimeMax = 20000; // 20s

long pauseTimeMin = 0; // 0s 
long pauseTimeMax = 20000; // 30s

long choppingFrequency = 7;
long choppingCountMin = 12;
long choppingCountMax = 12;
long choppingInsertTravelTime = 1200;
long choppingRetractTravelTime = 1280;
long choppingHitTime1 = 0;
long choppingHitTime2 = 3000;
bool isFirstChopping = true;

long pumpSpeed = 3;
long pumpTime = 3000;
//--------------------------------------------------------

void setup() {
  Serial.begin(9600);
  //randomSeed(analogRead(0)); // Use only if analog input pin 0 is unconnected 
  
  pinMode(countDownLed, OUTPUT);

  retractFully();

  if (isTest)
    initialTimeInSeconds = 2;

  for (int i = 0; i < initialTimeInSeconds / 2; i++)
  {
    digitalWrite(countDownLed, HIGH);
    delay(1000); // 1s

    digitalWrite(countDownLed, LOW);
    delay(1000); // 1s
  }

  if (isTest)
  {
    isFirstChopping = false;
    chop();
  }
}

void loop() {
  //if (hitCount == 0 || (hitCount % choppingFrequency == 0))
  insertStepwise();
  //else
  //  insert();

  delay(getInsertTime());
  hitCount++;
  
  retractStepwise();
  delay(getPauseTime());

  if (hitCount % choppingFrequency == 0)
  {
    chop();
    delay(getPauseTime());
  }
}

void insert() {
  extendActuator();
  delay(travelTime);
  stopActuator();
}

void insertStepwise() {
  extendActuator();
  delay(travelTime * 0.5);
  stopActuator();

  delay(getStepPauseTime());

  extendActuator();
  delay(travelTime * 0.5);
  stopActuator();
}

void retractStepwise() {
  startPump();

  retractActuator();
  delay(travelTime * 0.5);
  stopActuator();

  delay(pumpTime);
  stopPump();

  retractActuator();
  delay(travelTime * 0.6);
  stopActuator();
}

void retractFully() {
  stopPump();

  retractActuator();
  delay(travelTime * 1.1);
}

void chop() {
  Serial.println("Chop Start");
  
  long choppingCount = getChoppingCount();

  if (isFirstChopping) {
    insertStepwise();
  
    for (int i = 1; i <= choppingCount; i++) {
      delay(choppingHitTime1);

      startPump();
      retractActuator();
      delay(choppingRetractTravelTime);
      stopActuator();

      if (i % 3 == 0)
        stopPump();

      extendActuator();
      delay(choppingInsertTravelTime);
      stopActuator();
    }

    retractStepwise();
    isFirstChopping = false;
  }
  else {
    for (int i = 1; i <= choppingCount; i++) {
      unsigned long startMillis = millis();
    
      insertStepwise();
      delay(choppingHitTime2);
      retractStepwise();

      Serial.print("Single Chop End ");
      Serial.print((millis() - startMillis) / 1000);
      Serial.println("s");
    }
  }

  stopPump();
}

//------------------------------------------------------------
// Standards
//------------------------------------------------------------
void extendActuator() {
  delay(10); // 10ms
  //actuator.setSpeed(-255);  // Run forward at full speed.
}

void retractActuator() {
  delay(10); // 10ms
  //actuator.setSpeed(255);  // Run backward at full speed.
}

void stopActuator() {
  //actuator.setSpeed(0);
}

void startPump() {
  //if (usePump)
    //pump.setSpeed(pumpSpeed);
}

void stopPump() {
  //pump.setSpeed(0);
}

long getStepPauseTime() {
  long randomTime = random(stepPauseTimeMin, stepPauseTimeMax);
  
  Serial.print("Step Pause: ");
  Serial.print(randomTime );
  Serial.println("ms");
  
  return randomTime;
}

long getInsertTime() {
  long randomTime = random(insertTimeMin, insertTimeMax);
  
  Serial.print("Inserted: ");
  Serial.print(randomTime / 1000);
  Serial.println("s");
  
  return randomTime;
}

long getPauseTime() {
  long randomTime = random(pauseTimeMin, pauseTimeMax);

  Serial.print("Pause: ");
  Serial.print(randomTime / 1000);
  Serial.println("s");
  
  return randomTime;
}

long getChoppingCount() {
  long randomCount = random(choppingCountMin, choppingCountMax);

  Serial.print("Chopping Count: ");
  Serial.print(randomCount);
  Serial.println(" times");
  
  return randomCount;
}