#include <Arduino.h>
#include "mode.h"
#include "camera_motion.h"

const int xdirPin = 2, ydirPin = 3, zdirPin = 4;
const int xstepPin = 5, ystepPin = 6, zstepPin = 7;
const int enablePin = 8;
const int xlimitPin = A0, ylimitPin = A1;

bool dingwei = false, chongfu = false, auto_mode = false;
bool xlimitJudge = false, ylimitJudge = false;

int Alist = 0, Plist = 0, frequency = 2;
long x_coordinate = 0, y_coordinate = 0, z_coordinate = 0;
long Px_axis[First_listSize], Py_axis[First_listSize];
long Ax_axis[Second_listSize], Ay_axis[Second_listSize];

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

  pinMode(enablePin, OUTPUT); pinMode(53, OUTPUT);
  pinMode(xdirPin, OUTPUT); pinMode(ydirPin, OUTPUT); pinMode(zdirPin, OUTPUT);
  pinMode(xstepPin, OUTPUT); pinMode(ystepPin, OUTPUT); pinMode(zstepPin, OUTPUT);
  pinMode(xlimitPin, INPUT_PULLUP); pinMode(ylimitPin, INPUT_PULLUP);

  digitalWrite(enablePin, LOW);
  digitalWrite(53, HIGH);

  Serial.println("啟用預先校正，請耐心等待!");
  //Correction();
  //Serial.println("Input char, 請輸入字元!");
}

void loop() {
  if (Serial.available() > 0) {
    char keys = Serial.read();
    switch (keys) {
      case 'L':
        // 預設點位加載
        Point_load();
        break;
      case 'Z':
        parseFCommand();
        break;
      case 'C':
        Correction();
        dingwei = false; chongfu = false; auto_mode = false; Plist = 0;
        break;
      case 'P':
        Point();
        break;
      case 'A':
        Again();
        break;
      case 'R':
        for (int i = 0; i < Plist; i++) {
          Px_axis[i] = 0; Py_axis[i] = 0;
        }
        Plist = 0; chongfu = false;
        Serial.println("點位已重設!");
        break;
      default:
        if (keys >= '1' && keys <= '9' && keys != '5') {
          dingwei = true;
          auto_mode = false;
          motor_control(keys);
        }
        break;
    }
  }
}