/*
  Peer-to-Peer Push-to-Call GSM Conference PoC
  Board : ESP8266 / NodeMCU
  Modem : SIM800L

  Every device uses the same MEMBER_NUMBERS list.
  Set DEVICE_MEMBER_INDEX differently on each device.
*/

#include <Arduino.h>

// ESP8266 hardware UART:
//   GPIO1 / TX -> SIM800L RXD through a voltage divider
//   GPIO3 / RX <- SIM800L TXD
// Disconnect both UART lines during firmware upload.
// Optional debug TX: Serial1 on GPIO2 / D4.

static const uint8_t CALL_BUTTON_PIN = D5;
static const uint8_t LED_STATUS_PIN = D1;

// Shared member list. Use the same order on every device.
const char* MEMBER_NUMBERS[] = {
  "+628111111111",  // index 0
  "+628222222222",  // index 1
  "+628333333333"   // index 2
};

// Change only this value for each physical device.
// Device 1 = 0, Device 2 = 1, Device 3 = 2, etc.
static const uint8_t DEVICE_MEMBER_INDEX = 0;

const uint8_t MEMBER_COUNT = sizeof(MEMBER_NUMBERS) / sizeof(MEMBER_NUMBERS[0]);

static const unsigned long BUTTON_DEBOUNCE_MS = 60;
static const unsigned long CALL_ANSWER_TIMEOUT_MS = 5000;
static const unsigned long CLCC_POLL_INTERVAL_MS = 500;
static const unsigned long FAST_BLINK_MS = 150;
static const unsigned long SLOW_BLINK_MS = 700;
static const unsigned long HOLD_SETTLE_MS = 900;
static const unsigned long MERGE_SETTLE_MS = 1800;

HardwareSerial& sim800 = Serial;
HardwareSerial& debugPort = Serial1;

enum ConferenceState {
  STATE_IDLE,
  STATE_DIAL_CURRENT,
  STATE_WAIT_CURRENT,
  STATE_HOLD_CONNECTED,
  STATE_NEXT_MEMBER,
  STATE_MERGE,
  STATE_READY_ALL,
  STATE_READY_PARTIAL,
  STATE_INCOMING_ACTIVE
};

ConferenceState conferenceState = STATE_IDLE;

String simLine;
bool buttonStableState = HIGH;
bool buttonLastRawState = HIGH;
bool ledState = false;
bool currentCallConnected = false;
bool setupComplete = false;
bool outgoingConferenceActive = false;
bool incomingCallActive = false;

uint8_t currentTargetIndex = 0;
uint8_t processedTargetCount = 0;
uint8_t answeredCount = 0;
uint8_t failedCount = 0;

unsigned long lastButtonChangeAt = 0;
unsigned long stateStartedAt = 0;
unsigned long lastClccPollAt = 0;
unsigned long lastLedToggleAt = 0;

uint8_t targetCount() {
  return MEMBER_COUNT > 0 ? MEMBER_COUNT - 1 : 0;
}

bool deviceIndexIsValid() {
  return DEVICE_MEMBER_INDEX < MEMBER_COUNT;
}

void setLed(bool on) {
  ledState = on;
  digitalWrite(LED_STATUS_PIN, on ? HIGH : LOW);
}

void updateBlink(unsigned long intervalMs) {
  if (millis() - lastLedToggleAt >= intervalMs) {
    lastLedToggleAt = millis();
    setLed(!ledState);
  }
}

void sendAT(const String& command, unsigned long waitMs) {
  debugPort.print(F(">> "));
  debugPort.println(command);
  sim800.println(command);
  if (waitMs > 0) delay(waitMs);
}

void sendAT(const String& command) {
  sendAT(command, 0);
}

void enterState(ConferenceState nextState) {
  conferenceState = nextState;
  stateStartedAt = millis();
  debugPort.print(F("State: "));
  debugPort.println((int)nextState);
}

String normalizePhoneNumber(String number) {
  number.trim();
  number.replace(" ", "");
  number.replace("-", "");
  return number;
}

String extractClipNumber(const String& line) {
  int firstQuote = line.indexOf('"');
  int secondQuote = line.indexOf('"', firstQuote + 1);
  if (firstQuote >= 0 && secondQuote > firstQuote) {
    return line.substring(firstQuote + 1, secondQuote);
  }
  return "";
}

int findMemberIndex(const String& caller) {
  String normalizedCaller = normalizePhoneNumber(caller);

  for (uint8_t i = 0; i < MEMBER_COUNT; i++) {
    String member = normalizePhoneNumber(String(MEMBER_NUMBERS[i]));
    if (normalizedCaller == member) return i;
  }

  return -1;
}

bool isAllowedMember(const String& caller) {
  return findMemberIndex(caller) >= 0;
}

bool selectFirstTarget() {
  for (uint8_t i = 0; i < MEMBER_COUNT; i++) {
    if (i != DEVICE_MEMBER_INDEX) {
      currentTargetIndex = i;
      return true;
    }
  }
  return false;
}

bool selectNextTarget() {
  for (uint8_t i = currentTargetIndex + 1; i < MEMBER_COUNT; i++) {
    if (i != DEVICE_MEMBER_INDEX) {
      currentTargetIndex = i;
      return true;
    }
  }
  return false;
}

void initSim800() {
  delay(3000);
  sendAT(F("AT"), 500);
  sendAT(F("ATE0"), 500);
  sendAT(F("AT+CMEE=2"), 500);
  sendAT(F("AT+CLIP=1"), 500);
  sendAT(F("AT+CPIN?"), 500);
  sendAT(F("AT+CREG?"), 500);
  sendAT(F("AT+CLVL=100"), 500);
  sendAT(F("AT+CMIC=0,10"), 500);
}

void resetOutgoingState() {
  currentTargetIndex = 0;
  processedTargetCount = 0;
  answeredCount = 0;
  failedCount = 0;
  currentCallConnected = false;
  setupComplete = false;
  outgoingConferenceActive = false;
}

void resetAllCallState() {
  resetOutgoingState();
  incomingCallActive = false;
}

void hangupAll(const __FlashStringHelper* reason) {
  debugPort.print(F("Hangup: "));
  debugPort.println(reason);
  sendAT(F("ATH"), 600);
  resetAllCallState();
  setLed(false);
  enterState(STATE_IDLE);
}

void dialCurrentMember() {
  if (currentTargetIndex >= MEMBER_COUNT || currentTargetIndex == DEVICE_MEMBER_INDEX) return;

  String command = F("ATD");
  command += MEMBER_NUMBERS[currentTargetIndex];
  command += ';';

  debugPort.print(F("Dialling member index "));
  debugPort.print(currentTargetIndex);
  debugPort.print(F(": "));
  debugPort.println(MEMBER_NUMBERS[currentTargetIndex]);

  currentCallConnected = false;
  sendAT(command);
}

void startConferenceCall() {
  if (!deviceIndexIsValid()) {
    debugPort.println(F("ERROR: DEVICE_MEMBER_INDEX is outside MEMBER_NUMBERS."));
    return;
  }

  if (targetCount() == 0 || !selectFirstTarget()) {
    debugPort.println(F("No other member numbers configured."));
    return;
  }

  if (incomingCallActive || conferenceState != STATE_IDLE) {
    debugPort.println(F("Device is already in a call."));
    return;
  }

  debugPort.print(F("Starting conference from member index "));
  debugPort.println(DEVICE_MEMBER_INDEX);

  resetOutgoingState();
  selectFirstTarget();
  setLed(false);
  enterState(STATE_DIAL_CURRENT);
}

void markCurrentFailed(const __FlashStringHelper* reason) {
  if (conferenceState != STATE_WAIT_CURRENT) return;

  debugPort.print(F("Member index "));
  debugPort.print(currentTargetIndex);
  debugPort.print(F(" failed: "));
  debugPort.println(reason);

  failedCount++;
  processedTargetCount++;
  currentCallConnected = false;

  // Release the current failed/outgoing call while preserving held calls.
  sendAT(F("AT+CHLD=1"), 250);
  enterState(STATE_NEXT_MEMBER);
}

void markCurrentAnswered() {
  if (conferenceState != STATE_WAIT_CURRENT || currentCallConnected) return;

  currentCallConnected = true;
  answeredCount++;
  processedTargetCount++;

  debugPort.print(F("Member index "));
  debugPort.print(currentTargetIndex);
  debugPort.println(F(" answered."));
}

void finishDialSequence() {
  if (answeredCount == 0) {
    hangupAll(F("No member answered"));
    return;
  }

  if (answeredCount > 1) {
    enterState(STATE_MERGE);
    return;
  }

  setupComplete = true;
  outgoingConferenceActive = true;
  enterState(failedCount == 0 ? STATE_READY_ALL : STATE_READY_PARTIAL);
}

void pollClcc() {
  if (millis() - lastClccPollAt >= CLCC_POLL_INTERVAL_MS) {
    lastClccPollAt = millis();
    sendAT(F("AT+CLCC"));
  }
}

void updateStatusLed() {
  switch (conferenceState) {
    case STATE_DIAL_CURRENT:
    case STATE_WAIT_CURRENT:
      updateBlink(answeredCount == 0 ? FAST_BLINK_MS : SLOW_BLINK_MS);
      break;

    case STATE_HOLD_CONNECTED:
    case STATE_NEXT_MEMBER:
    case STATE_MERGE:
    case STATE_READY_PARTIAL:
      updateBlink(SLOW_BLINK_MS);
      break;

    case STATE_READY_ALL:
    case STATE_INCOMING_ACTIVE:
      setLed(true);
      break;

    case STATE_IDLE:
    default:
      setLed(false);
      break;
  }
}

void updateConferenceFlow() {
  updateStatusLed();

  // The initiating device must keep the button pressed.
  if (!setupComplete &&
      conferenceState != STATE_IDLE &&
      conferenceState != STATE_INCOMING_ACTIVE &&
      buttonStableState == HIGH) {
    hangupAll(F("Push-to-call button released during setup"));
    return;
  }

  switch (conferenceState) {
    case STATE_IDLE:
    case STATE_INCOMING_ACTIVE:
      break;

    case STATE_DIAL_CURRENT:
      dialCurrentMember();
      enterState(STATE_WAIT_CURRENT);
      break;

    case STATE_WAIT_CURRENT:
      pollClcc();

      if (currentCallConnected) {
        if (processedTargetCount < targetCount()) {
          enterState(STATE_HOLD_CONNECTED);
        } else {
          finishDialSequence();
        }
      } else if (millis() - stateStartedAt >= CALL_ANSWER_TIMEOUT_MS) {
        markCurrentFailed(F("5-second answer timeout"));
      }
      break;

    case STATE_HOLD_CONNECTED:
      if (millis() - stateStartedAt < HOLD_SETTLE_MS) break;
      sendAT(F("AT+CHLD=2"), 350);
      enterState(STATE_NEXT_MEMBER);
      break;

    case STATE_NEXT_MEMBER:
      if (processedTargetCount >= targetCount() || !selectNextTarget()) {
        finishDialSequence();
      } else {
        enterState(STATE_DIAL_CURRENT);
      }
      break;

    case STATE_MERGE:
      if (millis() - stateStartedAt < MERGE_SETTLE_MS) break;

      debugPort.println(F("Merging answered member calls..."));
      sendAT(F("AT+CHLD=3"), 700);
      sendAT(F("AT+CLCC"), 200);

      setupComplete = true;
      outgoingConferenceActive = true;
      enterState(failedCount == 0 ? STATE_READY_ALL : STATE_READY_PARTIAL);
      break;

    case STATE_READY_ALL:
    case STATE_READY_PARTIAL:
      outgoingConferenceActive = true;
      break;
  }
}

void onButtonPressed() {
  if (conferenceState == STATE_IDLE && !incomingCallActive) {
    startConferenceCall();
  }
}

void onButtonReleased() {
  if (outgoingConferenceActive ||
      (conferenceState != STATE_IDLE && conferenceState != STATE_INCOMING_ACTIVE)) {
    hangupAll(F("Push-to-call button released"));
  }
}

void updateButton() {
  bool rawState = digitalRead(CALL_BUTTON_PIN);

  if (rawState != buttonLastRawState) {
    buttonLastRawState = rawState;
    lastButtonChangeAt = millis();
  }

  if (millis() - lastButtonChangeAt >= BUTTON_DEBOUNCE_MS &&
      rawState != buttonStableState) {
    buttonStableState = rawState;
    if (buttonStableState == LOW) onButtonPressed();
    else onButtonReleased();
  }
}

void answerMemberCall(int callerIndex) {
  debugPort.print(F("Auto-answering member index "));
  debugPort.println(callerIndex);
  sendAT(F("ATA"), 500);
  incomingCallActive = true;
  setupComplete = true;
  enterState(STATE_INCOMING_ACTIVE);
}

void rejectIncomingCall(const __FlashStringHelper* reason) {
  debugPort.print(F("Rejecting incoming call: "));
  debugPort.println(reason);
  sendAT(F("ATH"), 400);
}

int parseClccState(const String& line) {
  int comma1 = line.indexOf(',');
  int comma2 = line.indexOf(',', comma1 + 1);
  int comma3 = line.indexOf(',', comma2 + 1);
  if (comma1 < 0 || comma2 < 0 || comma3 < 0) return -1;
  return line.substring(comma2 + 1, comma3).toInt();
}

void handleSimLine(const String& rawLine) {
  String line = rawLine;
  line.trim();
  if (line.length() == 0) return;

  debugPort.print(F("SIM800: "));
  debugPort.println(line);

  if (line == F("RING")) {
    if (conferenceState == STATE_IDLE) updateBlink(FAST_BLINK_MS);
    return;
  }

  if (line.startsWith(F("+CLIP:"))) {
    String caller = extractClipNumber(line);
    int callerIndex = findMemberIndex(caller);

    debugPort.print(F("Caller: "));
    debugPort.print(caller);
    debugPort.print(F("; member index: "));
    debugPort.println(callerIndex);

    if (callerIndex < 0) {
      rejectIncomingCall(F("number is not in MEMBER_NUMBERS"));
    } else if (conferenceState != STATE_IDLE || incomingCallActive) {
      rejectIncomingCall(F("device is busy"));
    } else {
      answerMemberCall(callerIndex);
    }
    return;
  }

  if (line.startsWith(F("+CLCC:"))) {
    int callState = parseClccState(line);
    if (conferenceState == STATE_WAIT_CURRENT && callState == 0) {
      markCurrentAnswered();
    }
    return;
  }

  if (line.indexOf(F("CONNECT")) >= 0) {
    if (conferenceState == STATE_WAIT_CURRENT) markCurrentAnswered();
    return;
  }

  if (line.indexOf(F("BUSY")) >= 0) {
    markCurrentFailed(F("busy"));
    return;
  }

  if (line.indexOf(F("NO ANSWER")) >= 0) {
    markCurrentFailed(F("no answer"));
    return;
  }

  if (line.indexOf(F("NO CARRIER")) >= 0) {
    if (conferenceState == STATE_WAIT_CURRENT) {
      markCurrentFailed(F("no carrier"));
    } else {
      resetAllCallState();
      setLed(false);
      enterState(STATE_IDLE);
    }
    return;
  }
}

void readSim800() {
  while (sim800.available()) {
    char c = (char)sim800.read();

    if (c == '\n') {
      handleSimLine(simLine);
      simLine = "";
    } else if (c != '\r') {
      simLine += c;
    }
  }
}

void setup() {
  debugPort.begin(115200);
  delay(500);

  pinMode(CALL_BUTTON_PIN, INPUT_PULLUP);
  pinMode(LED_STATUS_PIN, OUTPUT);
  setLed(false);

  sim800.begin(9600);
  initSim800();

  debugPort.println();
  debugPort.println(F("Peer-to-peer SIM800L conference device ready."));
  debugPort.print(F("This device member index: "));
  debugPort.println(DEVICE_MEMBER_INDEX);

  if (!deviceIndexIsValid()) {
    debugPort.println(F("ERROR: Invalid DEVICE_MEMBER_INDEX."));
  }
}

void loop() {
  readSim800();
  updateButton();
  updateConferenceFlow();
}