// Auto_Seat
// Timed automatic seat controller with relay protection.

#define seatRelayD8 8
#define seatRelayD9 9
#define parkingPin A0
#define ignitionPin 7

const uint16_t SEAT_HARD_LIMIT_UP_MS = 6000;
const int PARKING_ON_THRESHOLD = 950;
const int PARKING_OFF_THRESHOLD = 940;
const int DOWN_SPEED_NUM = 11;
const int DOWN_SPEED_DEN = 10;
const unsigned long IGNITION_STABILIZE_MS = 1000UL;
const unsigned long DRIVE_CONFIRM_MS = 80UL;
const unsigned long PARK_CONFIRM_MS = 250UL;
const unsigned long RELAY_DEADTIME_MS = 120UL;
const unsigned long DOWN_EDGE_MARGIN_MS = 300UL;

boolean ignitionState = LOW;
boolean parkingState = LOW;
boolean prevIgnitionState = LOW;
boolean confirmedDriveMode = LOW;
boolean pendingDriveMode = LOW;
bool isStabilizing = false;
bool downEdgeMarginActive = false;
int parkingRawValue = 0;
long currentPosMs = 0;
long targetPosMs = 0;
long downSpeedRemainder = 0;
unsigned long lastTime = 0;
unsigned long stabilizeStartTime = 0;
unsigned long pendingDriveModeStartTime = 0;
unsigned long relayDeadtimeStartTime = 0;
unsigned long downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
int currentSeatAction = 0;
int requestedSeatAction = 0;
int lastStoppedSeatAction = 0;

void setup() {
  pinMode(parkingPin, INPUT);
  pinMode(ignitionPin, INPUT);
  pinMode(seatRelayD8, OUTPUT);
  pinMode(seatRelayD9, OUTPUT);
  applySeatAction(0);
  seat(0);
  measurement();
  confirmedDriveMode = getRawDriveMode();
  pendingDriveMode = confirmedDriveMode;
  targetPosMs = getTargetForConfirmedMode();
  lastTime = millis();
}

void loop() {
  measurement();
  unsigned long currentTime = millis();
  unsigned long deltaTime = currentTime - lastTime;
  lastTime = currentTime;

  if (prevIgnitionState == LOW && ignitionState == HIGH) {
    isStabilizing = true;
    stabilizeStartTime = currentTime;
  }
  prevIgnitionState = ignitionState;

  if (isStabilizing && currentTime - stabilizeStartTime < IGNITION_STABILIZE_MS) {
    seat(0);
    updateRelayOutput(currentTime);
    return;
  }
  isStabilizing = false;

  updateConfirmedDriveMode(currentTime);
  targetPosMs = getTargetForConfirmedMode();
  updateMovement(deltaTime);
  updateRelayOutput(currentTime);
}

void measurement() {
  ignitionState = digitalRead(ignitionPin);
  parkingRawValue = analogRead(parkingPin);
  if (parkingState == LOW && parkingRawValue >= PARKING_ON_THRESHOLD) parkingState = HIGH;
  else if (parkingState == HIGH && parkingRawValue <= PARKING_OFF_THRESHOLD) parkingState = LOW;
}

boolean getRawDriveMode() { return ignitionState == HIGH && parkingState == LOW; }
long getTargetForConfirmedMode() { return confirmedDriveMode == HIGH ? SEAT_HARD_LIMIT_UP_MS : 0; }

void updateConfirmedDriveMode(unsigned long currentTime) {
  boolean rawDriveMode = getRawDriveMode();
  if (rawDriveMode != pendingDriveMode) {
    pendingDriveMode = rawDriveMode;
    pendingDriveModeStartTime = currentTime;
  }
  unsigned long confirmMs = pendingDriveMode == HIGH ? DRIVE_CONFIRM_MS : PARK_CONFIRM_MS;
  if (pendingDriveMode != confirmedDriveMode && currentTime - pendingDriveModeStartTime >= confirmMs) {
    confirmedDriveMode = pendingDriveMode;
  }
}

void updateMovement(unsigned long deltaTime) {
  if (downEdgeMarginActive) {
    seat(-1);
    downEdgeMarginTime += deltaTime;
    if (downEdgeMarginTime >= DOWN_EDGE_MARGIN_MS) {
      downEdgeMarginActive = false;
      downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
      seat(0);
    }
    return;
  }

  if (currentPosMs < targetPosMs) {
    seat(1);
    currentPosMs += deltaTime;
    if (currentPosMs >= targetPosMs) {
      currentPosMs = targetPosMs;
      seat(0);
    }
  } else if (currentPosMs > targetPosMs) {
    seat(-1);
    long scaled = deltaTime * DOWN_SPEED_NUM + downSpeedRemainder;
    currentPosMs -= scaled / DOWN_SPEED_DEN;
    downSpeedRemainder = scaled % DOWN_SPEED_DEN;
    if (currentPosMs <= targetPosMs) {
      currentPosMs = targetPosMs;
      downSpeedRemainder = 0;
      if (targetPosMs == 0) {
        downEdgeMarginActive = true;
        downEdgeMarginTime = 0;
      } else {
        seat(0);
      }
    }
  } else {
    seat(0);
  }
}

void setRelay(int relayPin, boolean state) { digitalWrite(relayPin, state ? LOW : HIGH); }
void seat(int action) { requestedSeatAction = action; }

void updateRelayOutput(unsigned long currentTime) {
  if (requestedSeatAction == currentSeatAction) {
    applySeatAction(currentSeatAction);
    return;
  }
  if (currentSeatAction != 0) {
    lastStoppedSeatAction = currentSeatAction;
    currentSeatAction = 0;
    relayDeadtimeStartTime = currentTime;
    applySeatAction(0);
    return;
  }
  bool sameDirectionResume = requestedSeatAction != 0 && requestedSeatAction == lastStoppedSeatAction;
  bool deadtimeDone = currentTime - relayDeadtimeStartTime >= RELAY_DEADTIME_MS;
  if (sameDirectionResume || deadtimeDone) currentSeatAction = requestedSeatAction;
  applySeatAction(currentSeatAction);
}

void applySeatAction(int action) {
  if (action == 1) { setRelay(seatRelayD8, 1); setRelay(seatRelayD9, 0); }
  else if (action == -1) { setRelay(seatRelayD8, 0); setRelay(seatRelayD9, 1); }
  else { setRelay(seatRelayD8, 0); setRelay(seatRelayD9, 0); }
}