/*
  @author-queen
  06-27-23
  assume
  full extend is equal to pos 1024
  full retract is equal to pos 0
  going up or home pos is retract
  going down is extend

  this code can be use in DC or AC actuator with
  just minor configuration change in the hardware setup
*/

#include <EEPROM.h>
//set the pins need in the code
#define RETRACT 2
#define EXTEND 3
#define SET 4
#define POS_1 5
#define POS_2 6
#define POS_3 7
#define FOR_W 8
#define BAC_W 9
#define position A0
//declare all variables need later
int pos, up, down, set, pos_1, pos_2, pos_3,
    pos_1_value, pos_2_value, pos_3_value ;
bool going_retract = false;
bool going_extend = false;

void setup() {
  Serial.begin(115200);
  // put your setup code here, to run once:
  //set pin if input or output
  pinMode(EXTEND, INPUT_PULLUP);
  pinMode(RETRACT, INPUT_PULLUP);
  pinMode(SET, INPUT_PULLUP);
  pinMode(POS_1, INPUT_PULLUP);
  pinMode(POS_2, INPUT_PULLUP);
  pinMode(POS_3, INPUT_PULLUP);
  pinMode(FOR_W, OUTPUT);
  pinMode(BAC_W, OUTPUT);

  //set motor drive to low/off in all direction on start up
  digitalWrite(FOR_W, LOW);
  digitalWrite(BAC_W, LOW);

  //get data saved into memory during boot up
  EEPROM.get(1, pos_1_value);
  EEPROM.get(20, pos_2_value);
  EEPROM.get(40, pos_3_value);
  Serial.println(pos_1_value);
  Serial.println(pos_2_value);
  Serial.println(pos_3_value);
  delay(200);
}


void loop() {
  // put your main code here, to run repeatedly:
  pos = analogRead(position);
  up = digitalRead(RETRACT);
  down = digitalRead(EXTEND);
  set = digitalRead(SET);
  pos_1 = digitalRead(POS_1);
  pos_2 = digitalRead(POS_2);
  pos_3 = digitalRead(POS_3);

  //going the actuator retract
  if (!down && pos < 1020) {
    Serial.println("extend");
    digitalWrite(FOR_W, HIGH);
    digitalWrite(BAC_W, LOW);
  } else {
    digitalWrite(FOR_W, LOW);
    digitalWrite(BAC_W, LOW);
  }

  //going the actuator extend
  if (!up && pos > 5) {
    Serial.println("Retract");
    digitalWrite(FOR_W, LOW);
    digitalWrite(BAC_W, HIGH);
  } else {
    digitalWrite(FOR_W, LOW);
    digitalWrite(BAC_W, LOW);
  }


  //saving current position in memory
  if (!pos_1 && !set) {
    EEPROM.put(1, pos);
    pos_1_value = pos;
    Serial.print("POS 1 saved ");
    Serial.println(pos_1_value);
    delay(200);
  }

  if (!pos_2 && !set) {
    EEPROM.put(20, pos);
    pos_2_value = pos;
    Serial.print("POS 2 saved ");
    Serial.println(pos_2_value);
    delay(200);
  }

  if (!pos_3 && !set) {
    EEPROM.put(40, pos);
    pos_3_value = pos;
    Serial.print("POS 3 saved ");
    Serial.println(pos_3_value);
    delay(200);
  }



  //go to set position
  //lets pretend that max expand is 1024 and max retract is 0

  //going to position 1
  if (!pos_1 && set && pos_1_value >= 0) {
    Serial.println("Moving to pos 1 ongoing");
    delay(200);
    while (1) {
      pos = analogRead(position);

      //set direction 1 time only
      if (pos > pos_1_value && !going_extend && !going_retract) {
        going_extend = true;
        Serial.println("going extend");
      }
      if (pos < pos_1_value && !going_extend && !going_retract) {
        going_retract = true;
        Serial.println("going retract");
      }
      if (going_retract) {
        digitalWrite(FOR_W, LOW);
        digitalWrite(BAC_W, HIGH);
        Serial.print("going up ");
        Serial.print(pos_1_value);
        Serial.print("  ");
        Serial.println(pos);
        if (pos >= pos_1_value) {
          Serial.println("exit normal");
          digitalWrite(FOR_W, LOW);
          digitalWrite(BAC_W, LOW);
          going_retract = false;
          going_extend = false;
          break;
        }
      }
      if (going_extend) {
        digitalWrite(FOR_W, HIGH);
        digitalWrite(BAC_W, LOW);
        Serial.print("going down ");
        Serial.print(pos_1_value);
        Serial.print("  ");
        Serial.println(pos);
        if (pos <= pos_1_value) {
          Serial.println("exit normal");
          digitalWrite(FOR_W, LOW);
          digitalWrite(BAC_W, LOW);
          going_retract = false;
          going_extend = false;
          break;
        }
      }
      //exit automatically if in current position
      if (pos == pos_1_value) {
        Serial.println("now in position exiting");
        digitalWrite(FOR_W, LOW);
        digitalWrite(BAC_W, LOW);
        break;
      }
    }

  }

  //going to position 2
  if (!pos_2 && set && pos_2_value >= 0) {
    Serial.println("Moving to pos 2 ongoing");
    delay(200);
    while (1) {
      pos = analogRead(position);

      //set direction 1 time only
      if (pos > pos_2_value && !going_extend && !going_retract) {
        going_extend = true;
        Serial.println("going extend");
      }
      if (pos < pos_2_value && !going_extend && !going_retract) {
        going_retract = true;
        Serial.println("going retract");
      }
      if (going_retract) {
        digitalWrite(FOR_W, LOW);
        digitalWrite(BAC_W, HIGH);
        Serial.print("going up ");
        Serial.print(pos_2_value);
        Serial.print("  ");
        Serial.println(pos);
        if (pos >= pos_2_value) {
          Serial.println("exit normal");
          digitalWrite(FOR_W, LOW);
          digitalWrite(BAC_W, LOW);
          going_retract = false;
          going_extend = false;
          break;
        }
      }
      if (going_extend) {
        digitalWrite(FOR_W, HIGH);
        digitalWrite(BAC_W, LOW);
        Serial.print("going down ");
        Serial.print(pos_2_value);
        Serial.print("  ");
        Serial.println(pos);
        if (pos <= pos_2_value) {
          Serial.println("exit normal");
          digitalWrite(FOR_W, LOW);
          digitalWrite(BAC_W, LOW);
          going_retract = false;
          going_extend = false;
          break;
        }
      }
      //exit automatically if in current position
      if (pos == pos_2_value) {
        Serial.println("now in position exiting");
        digitalWrite(FOR_W, LOW);
        digitalWrite(BAC_W, LOW);
        break;
      }
    }
  }

  //going to position 3
  if (!pos_3 && set && pos_3_value >= 0) {
    Serial.println("Moving to pos 3 ongoing");
    delay(200);
    while (1) {
      pos = analogRead(position);

      //set direction 1 time only
      if (pos > pos_3_value && !going_extend && !going_retract) {
        going_extend = true;
        Serial.println("going extend");
      }
      if (pos < pos_3_value && !going_extend && !going_retract) {
        going_retract = true;
        Serial.println("going retract");
      }
      if (going_retract) {
        digitalWrite(FOR_W, LOW);
        digitalWrite(BAC_W, HIGH);
        Serial.print("going up ");
        Serial.print(pos_3_value);
        Serial.print("  ");
        Serial.println(pos);
        if (pos >= pos_3_value) {
          Serial.println("exit normal");
          digitalWrite(FOR_W, LOW);
          digitalWrite(BAC_W, LOW);
          going_retract = false;
          going_extend = false;
          break;
        }
      }
      if (going_extend) {
        digitalWrite(FOR_W, HIGH);
        digitalWrite(BAC_W, LOW);
        Serial.print("going down ");
        Serial.print(pos_3_value);
        Serial.print("  ");
        Serial.println(pos);
        if (pos <= pos_3_value) {
          Serial.println("exit normal");
          digitalWrite(FOR_W, LOW);
          digitalWrite(BAC_W, LOW);
          going_retract = false;
          going_extend = false;
          break;
        }
      }
      //exit automatically if in current position
      if (pos == pos_3_value) {
        Serial.println("now in position exiting");
        digitalWrite(FOR_W, LOW);
        digitalWrite(BAC_W, LOW);
        break;
      }
    }
  }
}