#include "Motor.h"

#define dirPin 46
#define stepPin 47
#define forwardLimitPIN 28
#define backLimitPIN 30

Motor motor(dirPin, stepPin, forwardLimitPIN, backLimitPIN, 0.15, 1500);
float steps;
//馬達轉一30mm

const int Pin_GreenLight   = 4;
const int Pin_YellowLight  = 6;
const int Pin_RedLight   = 8;
const int Pin_Buzzer   = 10;

String command;

void setup() {
  Serial.begin(9600);

  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(forwardLimitPIN, INPUT);
  pinMode(backLimitPIN, INPUT);

  pinMode(Pin_GreenLight, OUTPUT);
  pinMode(Pin_YellowLight, OUTPUT);
  pinMode(Pin_RedLight, OUTPUT);
  pinMode(Pin_Buzzer, OUTPUT);

  digitalWrite(dirPin, HIGH);
  digitalWrite(forwardLimitPIN, HIGH);
  digitalWrite(backLimitPIN, HIGH);
}

void loop() {
  serialEvent();
  delay(100);
}

void serialEvent() {
  while (Serial.available() > 0)
  {
    command += char(Serial.read());
    delay(1);
  }
  if (command.length() > 0)
  {
    readCommand(command);
    command = "";
  }
}

void readCommand(String cmd) {
  String _command;
  //  String _command, _strval;
    int substringval = 0;
  float distance = 0;
  float speed = 0;//mm/s
    substringval = cmd.indexOf('\n');
    if (substringval < 0) return;
    _command = cmd.substring(0, substringval);
  //  Serial.println(_command);
  //手動控制馬達移動距離 單位:mm
  if (_command.equals("mb") || _command.equals("MB")) {
    //    _strval = cmd.substring(substringval + 1, cmd.length());
    //    distance = _strval.toFloat();
    motor.move(-230);
    Serial.print("馬達移動後位置:"); Serial.println(motor.getCurrentPosition());
  }
  else if (_command.equals("mf") || _command.equals("MF")) {
    //    strval = cmd.substring(substringval + 1, cmd.length());
    //    distance = _strval.toFloat();
    motor.move(230);
    Serial.print("馬達移動後位置:"); Serial.println(motor.getCurrentPosition());
  }
  //馬達回到初始點
  else if (_command.equals("h") || _command.equals("H")) {
    //Serial.print("馬達回到初始點 start!!! 馬達移動前位置:"); Serial.println(motor.getCurrentPosition());
    motor.moveToHighLim();
    motor.move(1);
    motor.resetPosition();
    Serial.print("馬達移動後位置:"); Serial.println(motor.getCurrentPosition());
  }
  //變更速度
  //  else if (_command.equals("c") || _command.equals("C")) {
  //    _strval = cmd.substring(substringval + 1, cmd.length());
  //    speed = _strval.toFloat();
  //    motor.changeSpeed(speed);
  //  }
  else if (_command.equals("s") || _command.equals("S")) {
    motor.stop();
  }
  else if (_command == "Green") {
    Serial.println("Green Light On");
    OffLight();
    digitalWrite(Pin_GreenLight, HIGH);
  }
  else if (_command == "Yellow") {
    Serial.println("Yellow Light On");
    OffLight();
    digitalWrite(Pin_YellowLight, HIGH);
  }
  else if (_command == "Red") {
    OffLight();
    digitalWrite(Pin_RedLight, HIGH);
    Serial.println("Red Light On");

  }
  else if (_command == "Buzzer") {
    OffLight();
    digitalWrite(Pin_Buzzer, HIGH);
    Serial.println("Buzzer On");

  }
  else if (_command == "Off") {
    digitalWrite(Pin_GreenLight, LOW);
    digitalWrite(Pin_YellowLight, LOW);
    digitalWrite(Pin_RedLight, LOW);
    digitalWrite(Pin_Buzzer, LOW);
    Serial.println("All Off");

  }
  else {
    Serial.println("你想做什麼?");
  }
}
void OffLight()
{
  digitalWrite(Pin_GreenLight, LOW);
  digitalWrite(Pin_YellowLight, LOW);
  digitalWrite(Pin_RedLight, LOW);
  digitalWrite(Pin_Buzzer, LOW);
}