#include <CmdParser.hpp>

const float motorAngle = 1.8;
const float stepSize = 1;

float Csteps = 5000;
float Msteps = 5000;
float Ysteps = 5000;
float Ksteps = 5000;
unsigned long rpm = 100;

bool confirm = false;

#define enPin 8
#define cDirPin 5
#define cStepPin 2
#define mDirPin 6
#define mStepPin 3
#define yDirPin 7
#define yStepPin 4
#define kDirPin 13
#define kStepPin 12

void setup() {
  pinMode(enPin, OUTPUT);
  pinMode(cDirPin, OUTPUT);
  pinMode(cStepPin, OUTPUT);
  pinMode(mDirPin, OUTPUT);
  pinMode(mStepPin, OUTPUT);
  pinMode(yDirPin, OUTPUT);
  pinMode(yStepPin, OUTPUT);
  pinMode(kDirPin, OUTPUT);
  pinMode(kStepPin, OUTPUT);

  digitalWrite(enPin, HIGH);
  Serial.begin(38400);
  Serial.println("Send \"START <Pump>\" to calibrate......");
}

void loop() {
  cmdRead();
}

void cmdRead() {
  CmdParser cmdParser;
  CmdBuffer<64> myBuffer;
  if (myBuffer.readFromSerial(&Serial)) {
    if (cmdParser.parseCmd(&myBuffer) != CMDPARSER_ERROR) {
      if (cmdParser.getParamCount() == 2) {
        if(cmdParser.equalCmdParam(0, "START")){
          if(cmdParser.equalCmdParam(1, "C")){
            loadPump('C');
            confirmation();
            Serial.println(" ");
            Serial.print("Calibrating Cyan Pump with ");Serial.print(Csteps);Serial.println(" Steps");
            cStepperRotate(Csteps, rpm);
            confirm = false;
            Serial.println(" ");
            Serial.println("1) Measure the volume of liquid pumped....");
            Serial.println("2) Use the formula to find the <Steps per Unit(ml)>");
            Serial.print("                              ");Serial.println(Csteps);
            Serial.println("   <Steps per Unit(ml)> = ---------------");
            Serial.println("                          Measured Volume");
            Serial.println("3) Modify the <Cspu> value with <Steps per Unit> in Main Program");
            Serial.println(" ");
            Serial.println("Send \"START <Pump>\" to calibrate......");
          }
          else if(cmdParser.equalCmdParam(1, "M")){
            loadPump('M');
            confirmation();
            Serial.println(" ");
            Serial.print("Calibrating Magenta Pump with ");Serial.print(Msteps);Serial.println(" Steps");
            mStepperRotate(Msteps, rpm);
            confirm = false;
            Serial.println(" ");
            Serial.println("1) Measure the volume of liquid pumped....");
            Serial.println("2) Use the formula to find the <Steps per Unit(ml)>");
            Serial.print("                              ");Serial.println(Msteps);
            Serial.println("   <Steps per Unit(ml)> = ---------------");
            Serial.println("                          Measured Volume");
            Serial.println("3) Modify the <Mspu> value with <Steps per Unit> in Main Program");
            Serial.println(" ");
            Serial.println("Send \"START <Pump>\" to calibrate......");
          }
          else if(cmdParser.equalCmdParam(1, "Y")){
            loadPump('Y');
            confirmation();
            Serial.println(" ");
            Serial.print("Calibrating Yellow Pump with ");Serial.print(Ysteps);Serial.println(" Steps");
            yStepperRotate(Ysteps, rpm);
            confirm = false;
            Serial.println(" ");
            Serial.println("1) Measure the volume of liquid pumped....");
            Serial.println("2) Use the formula to find the <Steps per Unit(ml)>");
            Serial.println("                              Ysteps");
            Serial.println("   <Steps per Unit(ml)> = ---------------");
            Serial.println("                          Measured Volume");
            Serial.println("3) Modify the <Yspu> value with <Steps per Unit> in Main Program");
            Serial.println(" ");
            Serial.println("Send \"START <Pump>\" to calibrate......");
          }
          else if(cmdParser.equalCmdParam(1, "K")){
            loadPump('K');
            confirmation();
            Serial.println(" ");
            Serial.print("Calibrating Key Pump with ");Serial.print(Ksteps);Serial.println(" Steps");
            kStepperRotate(Ksteps, rpm);
            confirm = false;
            Serial.println(" ");
            Serial.println("1) Measure the volume of liquid pumped....");
            Serial.println("2) Use the formula to find the <Steps per Unit(ml)>");
            Serial.print("                              ");Serial.println(Ksteps);
            Serial.println("   <Steps per Unit(ml)> = ---------------");
            Serial.println("                          Measured Volume");
            Serial.println("3) Modify the <Kspu> value with <Steps per Unit> in Main Program");
            Serial.println(" ");
            Serial.println("Send \"START <Pump>\" to calibrate......");
          }
          else {
            Serial.println(" ");
            Serial.println("Invalid <Pump>");
          }
        }
      }
      else {
        Serial.println(" ");
        Serial.println("Invalid Command");
      }
    }
  }
}

void cStepperRotate(float steps, float rpm) {
  float stepsPerRotation = (360.00 / motorAngle) / stepSize;
  unsigned long stepPeriodmicroSec = ((60.0000 / (rpm * stepsPerRotation)) * 1E6 / 2.0000) - 5;

  digitalWrite(cDirPin, HIGH);
  float totalSteps = steps;
  for (unsigned long i = 0; i < totalSteps; i++) {
    digitalWrite(cStepPin, HIGH);
    delayMicroseconds(stepPeriodmicroSec);
    digitalWrite(cStepPin, LOW);
    delayMicroseconds(stepPeriodmicroSec);
  }
}

void mStepperRotate(float steps, float rpm) {
  float stepsPerRotation = (360.00 / motorAngle) / stepSize;
  unsigned long stepPeriodmicroSec = ((60.0000 / (rpm * stepsPerRotation)) * 1E6 / 2.0000) - 5;

  digitalWrite(mDirPin, HIGH);
  float totalSteps = steps;
  for (unsigned long i = 0; i < totalSteps; i++) {
    digitalWrite(mStepPin, HIGH);
    delayMicroseconds(stepPeriodmicroSec);
    digitalWrite(mStepPin, LOW);
    delayMicroseconds(stepPeriodmicroSec);
  }
}

void yStepperRotate(float steps, float rpm) {
  float stepsPerRotation = (360.00 / motorAngle) / stepSize;
  unsigned long stepPeriodmicroSec = ((60.0000 / (rpm * stepsPerRotation)) * 1E6 / 2.0000) - 5;

  digitalWrite(yDirPin, HIGH);
  float totalSteps = steps;
  for (unsigned long i = 0; i < totalSteps; i++) {
    digitalWrite(yStepPin, HIGH);
    delayMicroseconds(stepPeriodmicroSec);
    digitalWrite(yStepPin, LOW);
    delayMicroseconds(stepPeriodmicroSec);
  }
}

void kStepperRotate(float steps, float rpm) {
  float stepsPerRotation = (360.00 / motorAngle) / stepSize;
  unsigned long stepPeriodmicroSec = ((60.0000 / (rpm * stepsPerRotation)) * 1E6 / 2.0000) - 5;

  digitalWrite(kDirPin, HIGH);
  float totalSteps = steps;
  for (unsigned long i = 0; i < totalSteps; i++) {
    digitalWrite(kStepPin, HIGH);
    delayMicroseconds(stepPeriodmicroSec);
    digitalWrite(kStepPin, LOW);
    delayMicroseconds(stepPeriodmicroSec);
  }
}

void loadPump(char motor) {
  digitalWrite(enPin, LOW);
  if (motor == 'C') {
    Serial.println(" ");
    Serial.print("Loading Fluid into Cyan Pump");
    cStepperRotate(2000, rpm);
    Serial.println("     .....Done");
  }
  else if (motor == 'M') {
    Serial.println(" ");
    Serial.print("Loading Fluid into Magenta Pump");
    mStepperRotate(2000, rpm);
    Serial.println("     .....Done");
  }
  else if (motor == 'Y') {
    Serial.println(" ");
    Serial.print("Loading Fluid into Yellow Pump");
    yStepperRotate(2000, rpm);
    Serial.println("     .....Done");
  }
  else if (motor == 'K') {
    Serial.println(" ");
    Serial.print("Loading Fluid into Key Pump");
    kStepperRotate(2000, rpm);
    Serial.println("     .....Done");
  }
}

void confirmation() {
  Serial.println(" ");
  Serial.println("Now empty the Flask to start calibrating.");
  Serial.println("Ready to start calibration...! (Send <Yes> to Start)");
  while(!confirm) {
    CmdParser cmdParser;
    CmdBuffer<64> myBuffer;
    if (myBuffer.readFromSerial(&Serial)) {
      if (cmdParser.parseCmd(&myBuffer) != CMDPARSER_ERROR) {
        if (cmdParser.getParamCount() == 1) {
          if(cmdParser.equalCmdParam(0, "Yes")) {
            confirm = true;
          }
        }
      }
    }
  }
}