#include <STM32FreeRTOS.h>
#include <semphr.h>

// ---- System model: 2 tasks, 2 single-instance resources (mutexes) ----
#define NTASKS 2
#define NRES   2
#define TASK_A 0
#define TASK_B 1

SemaphoreHandle_t mutex[NRES];
const char *taskName[NTASKS] = { "A", "B" };

// ---- The two tables the detector reads (a real system keeps these updated
//      from the instrumented lock wrappers below) ----
volatile int holder[NRES]   = { -1, -1 };  // holder[r]  = task holding r, or -1
volatile int waitFor[NTASKS] = { -1, -1 }; // waitFor[t] = resource t waits on, or -1

// ================= instrumented lock / unlock =================
// The ONLY difference from a normal take/give: we record intent BEFORE we block
// (so the tables show the wait even while the task is stuck), and ownership AFTER.
void acquire(int me, int r) {
  waitFor[me] = r;                              // "I am about to wait for r"
  xSemaphoreTake(mutex[r], portMAX_DELAY);      // may block here (forever, if deadlocked)
  waitFor[me] = -1;                             // got it — no longer waiting
  holder[r]   = me;                             // I now hold r
}
void release(int me, int r) {
  holder[r] = -1;
  xSemaphoreGive(mutex[r]);
}

// ================= wait-for graph cycle detection =================
// Edge  t -> u  means "t waits for a resource that u holds."
static int waitsForTask(int t) {
  int r = waitFor[t];
  if (r == -1) return -1;                       // not blocked -> no out-edge
  return holder[r];                             // waits for whoever holds r
}

static int  color[NTASKS];                      // 0 white, 1 gray (on path), 2 black
static int  pathStack[NTASKS], sp;

static bool dfs(int t) {
  color[t] = 1; pathStack[sp++] = t;            // gray: on the current path
  int u = waitsForTask(t);
  if (u != -1) {
    if (color[u] == 1) {                        // back-edge to a gray node = CYCLE
      Serial.print("  DEADLOCK cycle: ");
      int i; for (i = 0; i < sp; i++) if (pathStack[i] == u) break;
      for (; i < sp; i++) { Serial.print("T"); Serial.print(taskName[pathStack[i]]); Serial.print(" -> "); }
      Serial.print("T"); Serial.println(taskName[u]);
      return true;
    }
    if (color[u] == 0 && dfs(u)) return true;
  }
  color[t] = 2; sp--;                           // black: no cycle through t
  return false;
}

bool detectDeadlock(void) {
  memset(color, 0, sizeof(color)); sp = 0;
  for (int t = 0; t < NTASKS; t++)
    if (color[t] == 0 && dfs(t)) return true;
  return false;
}

// ================= the deadlocking tasks =================
// A grabs 0 then 1.  B grabs 1 then 0.  Reverse order -> circular wait.
void taskA(void *pv) {
  vTaskDelay(pdMS_TO_TICKS(10));
  Serial.println("A: locking mutex 0");
  acquire(TASK_A, 0);
  Serial.println("A: holds 0, working...");
  vTaskDelay(pdMS_TO_TICKS(50));                // hold 0 while B grabs 1
  Serial.println("A: now locking mutex 1 (blocks - B has it)");
  acquire(TASK_A, 1);                           // <-- deadlocks here, forever
  Serial.println("A: got both (never printed)");
  release(TASK_A, 1); release(TASK_A, 0);
  vTaskDelete(NULL);
}
void taskB(void *pv) {
  vTaskDelay(pdMS_TO_TICKS(10));
  Serial.println("B: locking mutex 1");
  acquire(TASK_B, 1);
  Serial.println("B: holds 1, working...");
  vTaskDelay(pdMS_TO_TICKS(50));                // hold 1 while A grabs 0
  Serial.println("B: now locking mutex 0 (blocks - A has it)");
  acquire(TASK_B, 0);                           // <-- deadlocks here, forever
  Serial.println("B: got both (never printed)");
  release(TASK_B, 0); release(TASK_B, 1);
  vTaskDelete(NULL);
}

// ================= the periodic detector =================
#define DETECT_PERIOD_MS 1000                   // <-- the cost knob: often vs. rarely
void detectorTask(void *pv) {
  for (;;) {
    vTaskDelay(pdMS_TO_TICKS(DETECT_PERIOD_MS));
    Serial.println("[detector] scanning wait-for graph...");
    if (!detectDeadlock())
      Serial.println("  no deadlock");
    // (a real system would now RECOVER: abort a task or preempt a resource)
  }
}

// A heartbeat, to prove the RTOS keeps running while A and B are wedged.
void heartbeatTask(void *pv) {
  int beat = 0;
  for (;;) { vTaskDelay(pdMS_TO_TICKS(1000)); Serial.print("[heartbeat] "); Serial.println(++beat); }
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  Serial.println("--- start ---");
  mutex[0] = xSemaphoreCreateMutex();
  mutex[1] = xSemaphoreCreateMutex();
  xTaskCreate(taskA,        "A",   256, NULL, 1, NULL);
  xTaskCreate(taskB,        "B",   256, NULL, 1, NULL);
  xTaskCreate(detectorTask, "DET", 512, NULL, 2, NULL);
  xTaskCreate(heartbeatTask,"HB",  256, NULL, 1, NULL);
  vTaskStartScheduler();
}
void loop() {}