/*  -------------------------------------------------------------
        Card Scan Thing - Magnús Pétursson - FabAcademy 2025
    -------------------------------------------------------------


    – On power-up: servo → SERVO_HOME → detach
    – Wait for 1st press: zero at BOTTOM endstop → prompt “Insert cards”
    – Wait for 2nd press: start feed loop:

         - HOME → TOP endstop (detect stack height), swipe
         - move down 4 mm, home → TOP, swipe
         - repeat until stack height no longer decreases
         - Once stack height no longer decreases 
           (i.e. no more cards or feed error) the machine stops.
-----------------------------------------------------------------*/

#include <Arduino.h>
#include <Servo.h>

/* --- Pin Definitions --- */

/* Motor Pins:*/
const uint8_t IN1A           = 27; // 1A 
const uint8_t IN1B           = 28; // 1B
const uint8_t IN2A           = 6;  // 2A
const uint8_t IN2B           = 7;  // 2B
const uint8_t SERVO_PIN      = 1;   // servo signal

/* Input Pins:*/
const uint8_t BTN_PIN        = 26;  // D0 / A0
const uint8_t ENDSTOP_TOP    = 4;   // D9 / MISO
const uint8_t ENDSTOP_BOTTOM = 2;   // bottom switch


/* --- Motion Constants --- */


const uint16_t STEP_DELAY_US      = 1000000UL / 200;    // 200 half-steps/sec
const uint16_t HALF_STEPS_PER_REV = 400;
const float    LEAD_MM            = 8.0;
const float    TRAVEL_MM          = 67.0;
const uint16_t MAX_STEPS          = uint16_t((TRAVEL_MM/LEAD_MM)*HALF_STEPS_PER_REV);
const uint16_t HOME_STEP_LIMIT    = MAX_STEPS + 100;    // safety cap

const uint16_t SWIPE_OFFSET_STEPS = 200;                // 4 mm = 200 half-steps
const uint8_t  SWIPE_ANGLE        = 45;                 // half-sweep
const uint8_t  SWIPE_REPEATS      = 1;
const uint16_t SWIPE_DELAY_MS     = 1000;               // ms between sweeps

const uint8_t  SERVO_HOME         =  90;                // startup home
const uint8_t  SERVO_REST         =  45;                // swipe rest

const uint16_t DEBOUNCE_MS        = 50;

/* --- Half-step sequence --- 

This sequence defines the order in which the motor coils are energized to achieve half-stepping.
Each sub-array corresponds to a half-step, with the four elements representing the state of the two coils (A and B) for each of the two motor phases.
*/
const bool HALF_SEQ[8][4] = {
  {1,0,0,0},{1,0,1,0},{0,0,1,0},{0,1,1,0},
  {0,1,0,0},{0,1,0,1},{0,0,0,1},{1,0,0,1}
};

uint8_t halfIdx = 0;
Servo swipeServo;

// apply one half-step

void applyHalf(uint8_t idx) {
  digitalWrite(IN1A, HALF_SEQ[idx][0]);
  digitalWrite(IN1B, HALF_SEQ[idx][1]);
  digitalWrite(IN2A, HALF_SEQ[idx][2]);
  digitalWrite(IN2B, HALF_SEQ[idx][3]);
}

// turn off all coils
void coilsOff() {
  digitalWrite(IN1A, LOW);
  digitalWrite(IN1B, LOW);
  digitalWrite(IN2A, LOW);
  digitalWrite(IN2B, LOW);
}

// returns steps taken until endstop (HIGH→LOW), cw=true for up
uint16_t homeToEndstop(uint8_t pin, bool cw) {
  uint16_t steps = 0;
  while (digitalRead(pin) == HIGH && steps < HOME_STEP_LIMIT) {
    applyHalf(halfIdx);
    halfIdx = cw ? (halfIdx + 1) & 7 : (halfIdx + 7) & 7;
    delayMicroseconds(STEP_DELAY_US);
    steps++;
  }
  coilsOff();
  delay(200);
  return steps;
}


// wait for button press, then 
void waitForButton(const char* msg) {
  Serial.println(msg);
  while (digitalRead(BTN_PIN) == HIGH) {}
  delay(DEBOUNCE_MS);
  while (digitalRead(BTN_PIN) == LOW) {}
  delay(DEBOUNCE_MS);
}


/*  perform one swipe: 

    - attach servo
    - move to SERVO_REST
    - move to SERVO_HOME + SWIPE_ANGLE
    - move to SERVO_HOME - SWIPE_ANGLE
    - repeats SWIPE_REPEATS times
    - move back to SERVO_REST
    - detach servo
*/

void swipe() {
  if (!swipeServo.attached()) swipeServo.attach(SERVO_PIN);
  swipeServo.write(SERVO_REST); delay(200);
  for (uint8_t i = 0; i < SWIPE_REPEATS; ++i) {
    swipeServo.write(SERVO_HOME + SWIPE_ANGLE);
    delay(SWIPE_DELAY_MS);
    swipeServo.write(SERVO_HOME - SWIPE_ANGLE);
    delay(SWIPE_DELAY_MS);
  }
  swipeServo.write(SERVO_REST); delay(200);
  swipeServo.detach();
}

// clear top switch by moving down SWIPE_OFFSET_STEPS

void clearTopSwitch() {
  for (uint16_t i = 0; i < SWIPE_OFFSET_STEPS; ++i) {
    applyHalf((halfIdx + 7) & 7);
    halfIdx = (halfIdx + 7) & 7;
    delayMicroseconds(STEP_DELAY_US);
  }
  coilsOff();
  delay(200);
}


// setup function: initialize pins, attach servo, and home to SERVO_HOME before detaching servo.

void setup() {
  Serial.begin(115200);
  while (!Serial) {}

  pinMode(IN1A, OUTPUT); pinMode(IN1B, OUTPUT);
  pinMode(IN2A, OUTPUT); pinMode(IN2B, OUTPUT);
  pinMode(BTN_PIN, INPUT_PULLUP);
  pinMode(ENDSTOP_TOP, INPUT_PULLUP);
  pinMode(ENDSTOP_BOTTOM, INPUT_PULLUP);

  // On startup: servo → HOME → detach

  swipeServo.attach(SERVO_PIN);
  swipeServo.write(SERVO_HOME);
  delay(1000);
  swipeServo.detach();

  Serial.println("Machine ready. Press button to zero at bottom.");
}


// Main loop:

void loop() {
  // Step 1: zero at bottom
  waitForButton("Press button to zero bottom");
  uint16_t bottomSteps = homeToEndstop(ENDSTOP_BOTTOM, false);
  Serial.println("Zeroed. Insert cards, then press button to start feeding.");

  // Step 2: start feed
  waitForButton("Press button to begin feed cycle");


  // Defines lasttopSteps to compare with next top steps. 
  uint16_t lastTopSteps = 0;


  // Step 3: feed loop

  while (true) {
    // home up
    uint16_t topSteps = homeToEndstop(ENDSTOP_TOP, true);
    Serial.print("Top steps: "); Serial.println(topSteps);
    // moves down to clear top switch
    clearTopSwitch();
    // swipe card
    swipe();

    // step down 4 mm
    Serial.println("Advancing 4 mm for next card");
    for (uint16_t i = 0; i < SWIPE_OFFSET_STEPS; ++i) {
      applyHalf((halfIdx + 7) & 7);
      halfIdx = (halfIdx + 7) & 7;
      delayMicroseconds(STEP_DELAY_US);
    }
    coilsOff();
    delay(200);

    // compares topSteps with lastTopSteps to determine if the feed cycle should continue
    if (topSteps <= lastTopSteps) {
      Serial.println("No more cards or feed error. Stopping.");
      break;
    }
    lastTopSteps = topSteps;
  }

  Serial.println("Feed cycle complete.");
  while (1);
}