/*
  Wokwi | questions
  Boeing 737 weather radar control for museum demonstration
  Bwana — 1/19/25 at 4:35 AM

  after activation of PIR sensor red LED active green LED
  not active, turn radar stepper motor 1 left and right with
  2 optical sensors to change direction,   2nd stepper motor
  is up and down but is only active after the 1st stepper motor
  has gone left and right 5 times, the 2nd stepper may only go
  from top to middle second time of 5 left and right of 1 stepper
  motor from center to bottom and this checked by optical sensor
  then next 5 times left right back to center etc.
  (the up and down stepper has only 3 positions Up, Middle and Down)
  both stepper motors stop when PIR sensor is not active.

  https://wokwi.com/projects/420498213590653953
  https://fasinfo.be/

  THIS IS FOR TEST ONLY!
  Tilt behaves poorly, and the steppers run "blocking" code
*/

//#include <Arduino.h>
#include <AccelStepper.h>

#define INTERFACE_TYPE 1 // 1 = DRIVER (Step/Dir)

const int NUM_SCANS = 5;
// pin constants
const int PIR_PIN =      6;   // PIR sensor output pin
const int X_DIR_PIN =   10;   // X DIR pin voor DRV8825
const int X_STEP_PIN =  11;   // X STEP pin voor DRV8825
const int Y_DIR_PIN =    4;   // Y DIR pin voor DRV8825
const int Y_STEP_PIN =   5;   // Y STEP pin voor DRV8825
const int LF_END_PIN =  12;   // Optische sensor links
const int RT_END_PIN =   9;   // Optische sensor rechts
const int DN_END_PIN =   2;   // Optische sensor links
const int UP_END_PIN =   3;   // Optische sensor rechts
const int GRN_LED_PIN =  8;   // Groene LED pin
const int RED_LED_PIN =  7;   // Rode LED pin

long tiltPos[3];  // used to store UP / MIDDLE / DOWN positions

AccelStepper stepperX =
  AccelStepper(INTERFACE_TYPE, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY =
  AccelStepper(INTERFACE_TYPE, Y_STEP_PIN, Y_DIR_PIN);

bool checkMotion()  {
  static bool oldMotionState = false;

  bool isMotion = false;
  int motionState = digitalRead(PIR_PIN);
  if (motionState != oldMotionState)  {
    oldMotionState = motionState;
    if (motionState == HIGH) {
      isMotion = true;
      Serial.println(F("Motion detected"));
    } else  {
      //Serial.println(F("Motion stopped"));
    }
  }
  return isMotion;
}

void doScan() {
  for (int y = 0; y < 3; y++) {
    moveY(tiltPos[y]);
    for (int x = 0; x < NUM_SCANS; x++) { // 5
      scanX(RT_END_PIN, 2000, 20);
      scanX(LF_END_PIN, -2000, -20);
    }
  }
}

void initialize() {
  Serial.println(F("System initialization"));
  // set right limit
  Serial.println(F("Press 'RT stop' to set right limit"));
  scanX(RT_END_PIN, 2000, 50);
  Serial.print(F("Right limit position: "));
  Serial.println(stepperX.currentPosition());
  // set left limit
  Serial.println(F("Press 'LF stop' to set left limit"));
  scanX(LF_END_PIN, -2000, -50);
  Serial.print(F("Left limit position: "));
  Serial.println(stepperX.currentPosition());
  // set down limit
  Serial.println(F("Press 'DN stop' to set lower limit"));
  scanY(DN_END_PIN, 2000, 50);
  Serial.print(F("Down limit position: "));
  Serial.println(stepperY.currentPosition());
  tiltPos[2] = stepperY.currentPosition();
  // set up limit
  Serial.println(F("Press 'UP stop' to set upper limit"));
  scanY(UP_END_PIN, -2000, -50);
  Serial.print(F("Up limit position: "));
  Serial.println(stepperY.currentPosition());
  tiltPos[0] = stepperY.currentPosition();
  tiltPos[1] = (tiltPos[2] - tiltPos[0]) / 2;
  Serial.print(F("Tilt pos 0: "));
  Serial.println(tiltPos[0]);
  Serial.print(F("Tilt pos 1: "));
  Serial.println(tiltPos[1]);
  Serial.print(F("Tilt pos 2: "));
  Serial.println(tiltPos[2]);
  Serial.println(F("System initialized\n"));
}

// blocking
void scanX(int endPin, long pos, int speed)  {
  if (digitalRead(endPin) == HIGH) {
    stepperX.moveTo(pos);
    while (digitalRead(endPin) == HIGH) {
      stepperX.run();
      stepperX.setSpeed(speed);
    }
    stepperX.stop();
  }
}

// blocking
void scanY(int endPin, long pos, int speed)  {
  if (digitalRead(endPin) == HIGH) {
    stepperY.moveTo(pos);
    while (digitalRead(endPin) == HIGH) {
      stepperY.run();
      stepperY.setSpeed(speed);
    }
    stepperY.stop();
  }
}

// blocking, and takes the long way home
void moveY(long pos)  {
  stepperY.runToNewPosition(pos);
}

void setup() {
  Serial.begin(9600);
  // Pin modes
  pinMode(PIR_PIN, INPUT);
  pinMode(LF_END_PIN, INPUT_PULLUP);
  pinMode(RT_END_PIN, INPUT_PULLUP);
  pinMode(UP_END_PIN, INPUT_PULLUP);
  pinMode(DN_END_PIN, INPUT_PULLUP);
  pinMode(X_DIR_PIN, OUTPUT);
  pinMode(Y_DIR_PIN, OUTPUT);
  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(Y_STEP_PIN, OUTPUT);
  pinMode(GRN_LED_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  // set steppers
  stepperX.setMaxSpeed(200.0);  // set maximum speed
  stepperX.setAcceleration(50.0); // set acceleration
  stepperX.setSpeed(25.0);      // set initial speed
  stepperY.setMaxSpeed(200.0);
  stepperY.setAcceleration(50.0);
  stepperY.setSpeed(25.0);
  // initialize
  digitalWrite(GRN_LED_PIN, HIGH);
  digitalWrite(RED_LED_PIN, LOW);
  initialize();
}

void loop() {
  bool isActivated = checkMotion();
  if (isActivated)  {
    Serial.println(F("Scan running"));
    digitalWrite(GRN_LED_PIN, LOW);
    digitalWrite(RED_LED_PIN, HIGH);
    doScan(); // blocking
    Serial.println(F("Scan stopped"));
    digitalWrite(GRN_LED_PIN, HIGH);
    digitalWrite(RED_LED_PIN, LOW);
  }
}