#include <Arduino.h>
#include <avr/interrupt.h>

/*
 * =================================================
 * VARIÁVEIS E CONFIGURAÇÕES DO KERNEL
 * =================================================
 */
#define Slice 50000UL         // Tempo de cada tique (Quantum) em microssegundos (50000us = 50ms)
#define MaxNumberTask 6
#define NUM_TASKS 6
#define SizeTaskStack 256     // Tamanho da pilha da tarefa
#define MAX_NKREAD_QUEUE 6    // Numero máximo de threads esperando por leitura
#define MAX_NKPRINT_QUEUE 50  // Número máximo de mensagens esperando por impressão
#define MAX_NAME_LENGTH 30

unsigned int NumberTaskAdd = -1;
volatile int TaskRunning = 0;
char myName[MAX_NAME_LENGTH];
int SchedulerAlgorithm;

/*
 * =================================================
 * ESTRUTURA DO DESCRITOR DE TAREFAS (TCB)
 * =================================================
 */
typedef struct {
    uint8_t *P;                    // Ponteiro de pilha atual (DEVE ser o primeiro para o ASM)
    int16_t Tid;
    const char *name;
    unsigned short Prio;
    unsigned int Time;
    unsigned short Join;
    unsigned short State;
    uint8_t Stack[SizeTaskStack];  // Vetor de pilha alocado por task
} TaskDescriptor;

// Instanciação dos descritores
TaskDescriptor Descriptors[MaxNumberTask];

volatile uint8_t current_task = 0;
volatile uint8_t flag_led = 0;

enum Scheduler { RR, RM, EDF };
enum Taskstates { INITIAL, READY, RUNNING, DEAD, BLOCKED };

typedef struct {
    int queue[MaxNumberTask];
    int tail;
    int head;
} ReadyList;
ReadyList ready_queue;

typedef struct {
    short count;
    int sem_queue[MaxNumberTask], tail, header;
} sem_t;

typedef struct {
    int tid;             // ID da thread esperando pela leitura
    const char *format;  // Formato da entrada esperado (similar ao scanf)
    void *var;           // Argumentos onde os dados serão armazenados
} NkReadQueueEntry;

typedef struct {
    const char *format;  // Formato da entrada esperado (similar ao printf)
    char type;           // 'd', 'f', 'c', 's', '%'
    union {
        int i;
        float f;
        char c;
        const char *s;
    } var;               // Variável que será impressa
} NkPrintQueueEntry;

NkReadQueueEntry nkreadQueue[MAX_NKREAD_QUEUE];
int nkreadQueueHead = 0;
int nkreadQueueTail = 0;

NkPrintQueueEntry nkprintQueue[MAX_NKPRINT_QUEUE];
int nkprintQueueHead = 0;
int nkprintQueueTail = 0;
volatile bool printTailMutex;
volatile bool printHeadMutex;

char serialInputBuffer[128];  // Buffer para armazenar a entrada da serial
int serialInputIndex = 0;

typedef struct {
    int CallNumber;
    unsigned char *p0;
    unsigned char *p1;
    unsigned char *p2;
    unsigned char *p3;
} Parameters;

volatile Parameters kernelargs;

enum sys_temCall {
    TASKCREATE, SEM_WAIT, SEM_POST, SEM_INIT,
    WRITELCDN, WRITELCDS, EXITTASK, SLEEP,
    MSLEEP, USLEEP, LIGALED, DESLIGALED, START,
    TASKJOIN, SETMYNAME, GETMYNAME, NKPRINT,
    GETMYNUMBER, NKREAD
};

/*
 * =================================================
 * MACROS DE SALVAMENTO E RESTAURAÇÃO DE CONTEXTO
 * =================================================
 */
#define SAVE_CONTEXT()          \
asm volatile (                  \
    "push r0               \n\t" \
    "in r0, __SREG__       \n\t" \
    "cli                   \n\t" \
    "push r0               \n\t" \
    "lds r0, 0x003B        \n\t" \
    "push r0               \n\t" \
    "lds r0, 0x003C        \n\t" \
    "push r0               \n\t" \
    "push r1               \n\t" \
    "clr r1                \n\t" \
    "push r2               \n\t" \
    "push r3               \n\t" \
    "push r4               \n\t" \
    "push r5               \n\t" \
    "push r6               \n\t" \
    "push r7               \n\t" \
    "push r8               \n\t" \
    "push r9               \n\t" \
    "push r10              \n\t" \
    "push r11              \n\t" \
    "push r12              \n\t" \
    "push r13              \n\t" \
    "push r14              \n\t" \
    "push r15              \n\t" \
    "push r16              \n\t" \
    "push r17              \n\t" \
    "push r18              \n\t" \
    "push r19              \n\t" \
    "push r20              \n\t" \
    "push r21              \n\t" \
    "push r22              \n\t" \
    "push r23              \n\t" \
    "push r24              \n\t" \
    "push r25              \n\t" \
    "push r26              \n\t" \
    "push r27              \n\t" \
    "push r28              \n\t" \
    "push r29              \n\t" \
    "push r30              \n\t" \
    "push r31              \n\t" \
)

#define RESTORE_CONTEXT()       \
asm volatile (                  \
    "pop r31               \n\t" \
    "pop r30               \n\t" \
    "pop r29               \n\t" \
    "pop r28               \n\t" \
    "pop r27               \n\t" \
    "pop r26               \n\t" \
    "pop r25               \n\t" \
    "pop r24               \n\t" \
    "pop r23               \n\t" \
    "pop r22               \n\t" \
    "pop r21               \n\t" \
    "pop r20               \n\t" \
    "pop r19               \n\t" \
    "pop r18               \n\t" \
    "pop r17               \n\t" \
    "pop r16               \n\t" \
    "pop r15               \n\t" \
    "pop r14               \n\t" \
    "pop r13               \n\t" \
    "pop r12               \n\t" \
    "pop r11               \n\t" \
    "pop r10               \n\t" \
    "pop r9                \n\t" \
    "pop r8                \n\t" \
    "pop r7                \n\t" \
    "pop r6                \n\t" \
    "pop r5                \n\t" \
    "pop r4                \n\t" \
    "pop r3                \n\t" \
    "pop r2                \n\t" \
    "pop r1                \n\t" \
    "pop r0                \n\t" \
    "sts 0x003C, r0        \n\t" \
    "pop r0                \n\t" \
    "sts 0x003B, r0        \n\t" \
    "pop r0                \n\t" \
    "out __SREG__, r0      \n\t" \
    "pop r0                \n\t" \
)

/*
 * =================================================
 * ROTINAS DO KERNEL & SYSTEM CALLS
 * =================================================
 */
void wakeUP() {
    for (int i = 0; i < MaxNumberTask; i++) {
        if (Descriptors[i].State == BLOCKED && Descriptors[i].Time > 0) {
            Descriptors[i].Time--;
            if (Descriptors[i].Time == 0) {
                Descriptors[i].State = READY;
            }
        }
    }
}

/*
 * FUNÇÃO GENÉRICA CORRIGIDA
 */
void sys_sleep(unsigned int segundos) {
    // Calcula dinamicamente quantos tiques existem em 1 segundo baseado na macro Slice
    // Ex: 1000000 / 50000 = 20 tiques por segundo.
    unsigned long tiques_por_segundo = 1000000UL / Slice;
    
    Descriptors[current_task].Time = segundos * tiques_por_segundo; 
    Descriptors[current_task].State = BLOCKED;

    // Força o disparo da interrupção do Timer 1 de forma genérica usando os registradores do hardware
    TIFR1 |= (1 << OCF1A); 
    sei(); 

    // Aguarda com segurança até a ISR assumir e salvar o contexto atual
    while(Descriptors[current_task].State == BLOCKED) {
        asm volatile("nop");
    }
}

void kernel() {
    switch (kernelargs.CallNumber) {
        case SLEEP:
            sys_sleep((uintptr_t)kernelargs.p0);
            break;
        default:
            break;
    }
}

void callsvc(Parameters *args) {
    noInterrupts();
    kernelargs = *args;
    kernel();
    interrupts();
}

void sleep(int time) {
    Parameters arg;
    arg.CallNumber = SLEEP;
    arg.p0 = (unsigned char *)(uintptr_t)time;
    callsvc(&arg);
}

/*
 * =================================================
 * INTERRUPÇÃO DO TIMER & ESCALONADOR CENTRAL
 * =================================================
 */
ISR(TIMER1_COMPA_vect, ISR_NAKED) {
    SAVE_CONTEXT();

    wakeUP();

    Descriptors[current_task].P = (uint8_t*)SP;

    uint8_t next = current_task;
    do {
        next++;
        if (next >= MaxNumberTask) {
            next = 0;
        }
    } while (Descriptors[next].State == BLOCKED && next != current_task);

    current_task = next;

    SP = (uint16_t)Descriptors[current_task].P;

    RESTORE_CONTEXT();
    asm volatile("reti");
}

/*
 * =================================================
 * CRIAÇÃO DE TAREFAS E INICIALIZAÇÃO DO TIMER
 * =================================================
 */
void taskcreate(uint8_t id, void (*task)(), const char *task_name, unsigned short priority) {
    uint8_t *sp = &Descriptors[id].Stack[SizeTaskStack - 1];
    uint32_t addr = (uint32_t)(uintptr_t)task;

    *sp-- = (uint8_t)(addr & 0xFF);         
    *sp-- = (uint8_t)((addr >> 8) & 0xFF);  
    *sp-- = (uint8_t)((addr >> 16) & 0xFF); 

    *sp-- = 0x00; // r0
    *sp-- = 0x80; // SREG com interrupções globais ativas
    
    *sp-- = 0x00; // RAMPZ (0x3B)
    *sp-- = 0x00; // EIND (0x3C)
    
    *sp-- = 0x00; // r1

    for (uint8_t i = 2; i <= 31; i++) {
        *sp-- = 0x00;
    }

    Descriptors[id].P = sp;
    Descriptors[id].Tid = id;
    Descriptors[id].name = task_name;
    Descriptors[id].Prio = priority;
    Descriptors[id].State = READY;
    Descriptors[id].Time = 0;
}

/*
 * Inicializa o Timer baseado na configuração da macro Slice
 */
void initTimer() {
    TCCR1A = 0;
    TCCR1B = 0;
    TCNT1  = 0;

    TCCR1B |= (1 << WGM12);               // Modo CTC
    TCCR1B |= (1 << CS11) | (1 << CS10);   // Prescaler de 64

    // Formula genérica para o comparador do hardware baseado no Slice em microssegundos:
    // OCR1A = (Clock * Tempo_em_segundos / Prescaler) - 1
    // Como Slice está em microssegundos, dividimos por 1.000.000
    OCR1A = (uint16_t)(((F_CPU / 64.0) * (Slice / 1000000.0)) - 1); 
    
    TIMSK1 |= (1 << OCIE1A);              
}

void runTask0() {
    current_task = 0;
    uint16_t initial_sp = (uint16_t)Descriptors[0].P;
    asm volatile (
        "out __SP_L__, %A0 \n\t"
        "out __SP_H__, %B0 \n\t"
        :
        : "r" (initial_sp)
    );

    RESTORE_CONTEXT();
    asm volatile("ret"); 
}

/*
 * =================================================
 * THREADS DE USUÁRIO (PASSANDO TEMPO EM SEGUNDOS)
 * =================================================
 */
void task1() __attribute__((noinline));
void task1() {
    pinMode(13, OUTPUT);
    while (1) {
        Serial.print("T1");
        sleep(5); // Dorme por 1 Segundo
    }
}

void task2() __attribute__((noinline));
void task2() {
    while (1) {
        Serial.print("T2");
        sleep(10); // Dorme por 2 Segundos
        flag_led = !flag_led; 
        digitalWrite(13, flag_led);
    }
}

void task3() __attribute__((noinline));
void task3() {
    while (1) {
        Serial.print("T3");
        sleep(15); // Dorme por 3 Segundos
    }
}

void task4() __attribute__((noinline));
void task4() {
    while (1) {
        Serial.print("T4");
        sleep(20); 
    }
}

void task5() __attribute__((noinline));
void task5() {
    while (1) {
        Serial.print("T5");
        sleep(25); 
    }
}

void task6() __attribute__((noinline));
void task6() {
    while (1) {
        Serial.print("T6");
        sleep(30); 
    }
}

/*
 * =================================================
 * CONFIGURAÇÃO ARDUINO (SETUP)
 * =================================================
 */
void setup() {
    Serial.begin(9600);
    Serial.println("RTOS Iniciou com cálculo de Slice genérico...");
    cli();

    taskcreate(0, task1, "Task_1", 1);
    taskcreate(1, task2, "Task_2", 1);
    taskcreate(2, task3, "Task_3", 1);
    taskcreate(3, task4, "Task_4", 1);
    taskcreate(4, task5, "Task_5", 1);
    taskcreate(5, task6, "Task_6", 1);
    
    initTimer();
    runTask0();
}

void loop() {
    // Vazio
}