#define MAX_TASKS 3
#define TASK_STACK_SIZE 256

#include "stm32g431xx.h"
#include "string.h"

typedef unsigned int uint;

volatile uint32_t sec = 0;
int task_counter = 0;
bool stop = false;

struct Contexter
{
  uint R0;
  uint R1;
  uint R2;
  uint R3;
  uint R12;
  uint LR;   // адрес возврата из ПП
  uint PC;   // счётки команд, на какой остановилась работа ПП
  uint xPSR; // регистр состояния
};

struct Task
{
  uint sizeStack;
  uint *stack;
  int (*task)();
  bool isActive;
  bool isFirstRun;
  uint returnError;
};

Task task_list[MAX_TASKS];

Task myZeroTask;
uint zeroStack[sizeof(Contexter)];

void PendSV_mainHandler();

extern "C" void SysTick_Handler()
{
  sec++;
  SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
}

extern "C" __task __stackless void PendSV_Handler(void)
{
  if (stop)
  {
    asm volatile("MRS %0, PSP" : "=r"(myZeroTask.stack));
    PendSV_mainHandler();
  }

  asm volatile(
      "CMP     R1, #1 \n"
      "IT      EQ \n"
      "MSREQ   PSP, R0 \n" // если первый раз запускается задача, загружаем указатель на стек из памяти

      "MRS R0, PSP \n"
      "STMDB R0!, {R4-R11} \n" // Закидываем в стек r4-r11
      "MSR PSP, R0 \n" ::

      "R0"(task_list[task_counter].stack),
      "R1"(task_list[task_counter].isFirstRun));

  asm volatile("mov %0, R0" : "=r"(task_list[task_counter].stack));

  task_list[task_counter].isFirstRun = false;

  PendSV_mainHandler();
}

void __stackless PendSV_mainHandler()
{
  // Ищём следующую активную задачу для переключения на неё.
  Task *myTask;

  int lastIndex = task_counter;

  do
  {
    task_counter++;

    if (task_counter > MAX_TASKS - 1)
      task_counter = 0;

    myTask = &task_list[task_counter];

    if (myTask->isActive)
    {
      asm volatile("MSR PSP, %[psp]" ::[psp] "r"(myTask->stack));

      if (!myTask->isFirstRun)
        asm volatile(
            "MRS r0, PSP        \n" // Получаем PSP в r0
            "LDMIA r0!, {r4-r11}\n" // Восстанавливаем r4-r11 из стека
            "MSR PSP, r0        \n" // Обновляем PSP
        );
      else
        myTask->isFirstRun = false;

      asm volatile("mov LR, #0xFFFFFFFD"); // LR - 0xFFFFFFFD (Возвращаемся в Thread режим, используем стек процесса PSP)

      stop = false;
      
      return;
    }

  } while (task_counter != lastIndex);

  // Если не нашли активную задачу переходим в заглущку
  asm volatile("" ::"R0"(myZeroTask.stack));

  asm volatile(
      "MSR PSP, R0        \n" // Обновляем PSP
  );

  asm volatile("mov LR, #0xFFFFFFFD");
  stop = true;
}

int func1();
int func2();

uint func1Stack[TASK_STACK_SIZE];
uint func2Stack[TASK_STACK_SIZE];
uint func3Stack[TASK_STACK_SIZE];

int a, b, c;

int func1()
{
  int arr[10] = {0xab};
  while (1)
  {
    a++;
  }
}

int func2()
{
  while (1)
  {
    b++;
  }
}

int func3()
{
  a = 778;
}

int mute()
{
  while (1)
  {
  }
}

int newTask(int (*new_task)(), uint *stack, uint size)
{
  static int task_index = 0;

  Task &myTask = task_list[task_index];

  myTask.stack = stack + size - sizeof(Contexter) / sizeof(uint);

  myTask.sizeStack = size;
  myTask.task = new_task;

  myTask.isActive = true;
  myTask.isFirstRun = true;

  Contexter *cx = (Contexter *)myTask.stack;

  cx->PC = (uint)new_task;
  cx->xPSR = 0x01000000;
  cx->LR = (uint)mute;

  task_index++;

  return task_index;
}

void zeroTask(int (*new_task)(), uint *stack, uint size)
{
  Task &myTask = myZeroTask;

  myTask.stack = stack + (size - sizeof(Contexter)) / sizeof(uint);

  myTask.sizeStack = size;
  myTask.task = new_task;

  myTask.isActive = false;

  Contexter *cx = (Contexter *)myTask.stack;

  cx->PC = (uint)new_task;
  cx->xPSR = 0x01000000;
  cx->LR = (uint)mute;
}

int main()
{
  int aaa[100];

  aaa[99] = 0;

  SysTick_Config(SystemCoreClock);

  Task *muteVoid;
  zeroTask(mute, zeroStack, sizeof(Contexter));

  newTask(func1, func1Stack, TASK_STACK_SIZE);
  newTask(func2, func2Stack, TASK_STACK_SIZE);
  newTask(func3, func3Stack, TASK_STACK_SIZE);

  while (true)
  {
  }
}