#include "config.h"
#include "turret.h"

Turret turret(
  PIN_X_DIRECTION, PIN_X_PULSE, PIN_X_ENABLE, PIN_X_HOME,
  PIN_Y_DIRECTION, PIN_Y_PULSE, PIN_Y_ENABLE, PIN_Y_HOME,
  PIN_LASER
);

void setup() {
  turret.init();

  Serial.begin(BAUDRATE);

  turret.laserOn().calibrate();
  turret.moveTo(0, 0, 0);
  delay(5000);

  // test pointage
  double panelWidth = 124.7;
  double panelHeight = 145.0;
  int panelHCount = 3;
  int panelVCount = 2;

  // double zeroOffsetV = -31.5;
  double zeroOffsetV = -20.8;
  // double zeroOffsetH = -184.5;
  double zeroOffsetH = -195.5;

  // Test constant step h,v
  Serial.println("Start demo_0");
  for (int i = 0; i < 20; i++) {
    Serial.print("> ");
    Serial.println(i);
    for (double step = -198.5; step < 176.0; step += 10) {
      Serial.print("[step]: ");
      Serial.println(step);
      turret.moveTo(step, 87, 0);
      // turret.moveBy(20.0, 0.0, 0.0);
      delay(500);
    }

    for (double step = 0; step < 200.0; step += 10) {
      Serial.print("[step]: ");
      Serial.println(step);
      turret.moveTo(0, step, 0);
      // turret.moveBy(20.0, 0.0, 0.0);
      delay(500);
    }

    delay(5000);
    turret.moveTo(0, 0, 0);
    delay(5000);
  }

  // Test rollX
  turret.moveTo(0 * panelWidth + zeroOffsetH, 0, 0);
  delay(5000);
  turret.moveTo((panelHCount * panelWidth) / 2 + zeroOffsetH, 0, 0);
  delay(5000);
  turret.moveTo(panelHCount * panelWidth + zeroOffsetH, 0, 0);
  delay(5000);
  turret.gotoZero();

  // Test rollY
  turret.moveTo(0, 0 * panelHeight + zeroOffsetV, 0);
  delay(5000);
  turret.moveTo(0, (panelVCount * panelHeight) / 2 + zeroOffsetV, 0);
  delay(5000);
  turret.moveTo(0, panelVCount * panelHeight + zeroOffsetV, 0);
  delay(5000);
  turret.gotoZero();
  
  // Test align to panels
  for (double panelH = 0; panelH < panelHCount + 1; panelH += 0.5) {
    for (double panelV = 0; panelV < panelVCount + 1; panelV += 0.5) {
      double x = panelH * panelWidth + zeroOffsetH;
      double y = panelV * panelHeight + zeroOffsetV;
      double z = 0;

      Serial.print("Goto (x, y, z) => (");
      Serial.print(x);
      Serial.print(", ");
      Serial.print(y);
      Serial.print(", ");
      Serial.print(z);
      Serial.println(")");

      turret.moveTo(x, y, z);

      delay(2000);
    }
  }

  turret.gotoZero();
}

void loop() {

}