#include <avr/io.h>
#include <avr/interrupt.h>
#define context_save \
    __asm__ volatile ("PUSH R0"); \
    __asm__ volatile ("PUSH R1"); \
    __asm__ volatile ("PUSH R2"); \
    __asm__ volatile ("PUSH R3"); \
    __asm__ volatile ("PUSH R4"); \
    __asm__ volatile ("PUSH R5"); \
    __asm__ volatile ("PUSH R6"); \
    __asm__ volatile ("PUSH R7"); \
    __asm__ volatile ("PUSH R8"); \
    __asm__ volatile ("PUSH R9"); \
    __asm__ volatile ("PUSH R10"); \
    __asm__ volatile ("PUSH R11"); \
    __asm__ volatile ("PUSH R12"); \
    __asm__ volatile ("PUSH R13"); \
    __asm__ volatile ("PUSH R14"); \
    __asm__ volatile ("PUSH R15"); \
    __asm__ volatile ("PUSH R16"); \
    __asm__ volatile ("PUSH R17"); \
    __asm__ volatile ("PUSH R18"); \
    __asm__ volatile ("PUSH R19"); \
    __asm__ volatile ("PUSH R20"); \
    __asm__ volatile ("PUSH R21"); \
    __asm__ volatile ("PUSH R22"); \
    __asm__ volatile ("PUSH R23"); \
    __asm__ volatile ("PUSH R24"); \
    __asm__ volatile ("PUSH R25"); \
    __asm__ volatile ("PUSH R26"); \
    __asm__ volatile ("PUSH R27"); \
    __asm__ volatile ("PUSH R28"); \
    __asm__ volatile ("PUSH R29"); \
    __asm__ volatile ("PUSH R30"); \
    __asm__ volatile ("PUSH R31"); \
//    __asm__ volatile ("IN R25, __SREG__");  \
//    __asm__ volatile ("PUSH R25");

#define context_restore \
    __asm__ volatile ("POP R31"); \
    __asm__ volatile ("POP R30"); \
    __asm__ volatile ("POP R29"); \
    __asm__ volatile ("POP R28"); \
    __asm__ volatile ("POP R27"); \
    __asm__ volatile ("POP R26"); \
    __asm__ volatile ("POP R25"); \
    __asm__ volatile ("POP R24"); \
    __asm__ volatile ("POP R23"); \
    __asm__ volatile ("POP R22"); \
    __asm__ volatile ("POP R21"); \
    __asm__ volatile ("POP R20"); \
    __asm__ volatile ("POP R19"); \
    __asm__ volatile ("POP R18"); \
    __asm__ volatile ("POP R17"); \
    __asm__ volatile ("POP R16"); \
    __asm__ volatile ("POP R15"); \
    __asm__ volatile ("POP R14"); \
    __asm__ volatile ("POP R13"); \
    __asm__ volatile ("POP R12"); \
    __asm__ volatile ("POP R11"); \
    __asm__ volatile ("POP R10"); \
    __asm__ volatile ("POP R9"); \
    __asm__ volatile ("POP R8"); \
    __asm__ volatile ("POP R7"); \
    __asm__ volatile ("POP R6"); \
    __asm__ volatile ("POP R5"); \
    __asm__ volatile ("POP R4"); \
    __asm__ volatile ("POP R3"); \
    __asm__ volatile ("POP R2"); \
    __asm__ volatile ("POP R1"); \
    __asm__ volatile ("POP R0"); \


#define Check_Seg1 if ((SP)<&queue.queue[queue.current_task_running].stack[0] || (SP)>&queue.queue[queue.current_task_running].stack[maximum_processor_stack_size]){  \
              Bound_check \
              SP=current_sp; \
              segfault(); \
      }
#define maximum_processor_stack_size 60 

#define PSP Henry_Print("SP:" );int_strp(SP); 
#define PBP    volatile uint16_t Y; \
    __asm__("MOVW %0,R28" : "=r" (Y)); \
    Henry_Print("BP: "); \
    int_strp(Y);
#define Bound_check Henry_Print("LSP: ");int_strp(&queue.queue[queue.current_task_running].stack[0]); \
              PSP \
              Henry_Print("HSP: ");int_strp(&queue.queue[queue.current_task_running].stack[maximum_processor_stack_size]); \


#define Bound_check_in_task Henry_Print("LSP: ");int_strp(&self->stack[0]); \
              PSP \
              PBP\
              Henry_Print("HSP: ");int_strp(&self->stack[maximum_processor_stack_size]); \


#define _Henry_Delay(timeMS) // will possibly not be implmented
#define USART_SPINLOCK for(volatile int d_spinlock_usart=0; d_spinlock_usart< 1000; d_spinlock_usart++); // an arbotrary ammount of time wasted by cycles to use USART 
//because I'm too lazy to implement a better delay function



struct Context {
  volatile uint8_t r0;
  volatile uint8_t r1;
  volatile uint8_t r2;
  volatile uint8_t r3;
  volatile uint8_t r4;
  volatile uint8_t r5;
  volatile uint8_t r6;
  volatile uint8_t r7;
  volatile uint8_t r8;
  volatile uint8_t r9;
  volatile uint8_t r10;
  volatile uint8_t r11;
  volatile uint8_t r12;
  volatile uint8_t r13;
  volatile uint8_t r14;
  volatile uint8_t r15;
  volatile uint8_t r16;
  volatile uint8_t r17;
  volatile uint8_t r18;
  volatile uint8_t r19;
  volatile uint8_t r20;
  volatile uint8_t r21;
  volatile uint8_t r22;
  volatile uint8_t r23;
  volatile uint8_t r24;
  volatile uint8_t r25;
  volatile uint8_t r26;
  volatile uint8_t r27;
  volatile uint8_t r28;
  volatile uint8_t r29;
  volatile uint8_t r30;
  volatile uint8_t r31;
  volatile uint8_t SREGc;
};



 struct Task{
  uint8_t running:2; // 0: running 1: over 2: (waiting for a task switch)
  //uint8_t state:2;   
  int8_t priority;
  struct Context * context; // gonna allocate a context when the time comes to save space
  volatile uint8_t stack[maximum_processor_stack_size]; // this is the part of the stack that the return address is on
  void (*task)(Task * self);
};





struct Queue{
  struct Task queue[5];
  volatile uint8_t tasks;
  unsigned long int current_sp;
  uint16_t interrupt_nest = 0;
  uint8_t current_task_running; // is a relative pointer to whatever is in the queuee PID 

};


volatile struct Queue queue; // will be the main queue (Should make a macro to initialize this)

int queue_task(void (*task)(), int priority){
    struct Task buffer = {
      .running = 0,
      .priority = priority,
      .context=0,
      .stack={},
      .task = task
    };
    queue.queue[queue.tasks] = buffer;
    queue.tasks++;
}




void task1(Task * self){
  PORTD ^= 4;

}


void task2(Task * self){
  //volatile char k[50] = {};
  //Bound_check_in_task
  //int d[20]={21,3};
  PORTD ^= 8;
  for(long long int i = 0; ; i++){
    Henry_Print("Holt: ");
    int_strp(i);
  }
}

void segfault(){
  cli();
  Henry_Print("Segfault");
  while(1);
}



int main(){

#define BAUDRATE (((F_CPU / (9600 * 16UL))) - 1)
UBRR0H = (BAUDRATE >> 8);
UBRR0L = BAUDRATE;
UCSR0B |= (1 << TXEN0) | (1 << RXEN0);


    cli();
      DDRD= 0b00011100;
      TCCR1A = 0;           // Init Timer1A
      TCCR1B = 0;           // Init Timer1B
      TCCR1B |= B00000100;  // Prescaler = 256
      OCR1A =30000;        // Timer Compare1A Register
      TIMSK1 |= B00000010;  // Enable Timer COMPA Interrupt
    queue_task(&task1, 1);
    queue_task(&task2, 1);

          sei();

    while(1);
}
 
void int_strp(int f){
  int buf =f;
  char k[11];
  for(int i =0; i< 11; i++){
     k[10-i] =(buf%10)^48;
    buf = buf/10;
  }
  for(int i =0; i< 11; i++){
        UDR0= k[i];
        for(volatile int d=0; d< 2000; d++);
  }
  UDR0 = '\n';
  for(volatile int d=0; d< 2000; d++);
}


void Henry_Print(const char * d){
  for(int i=0; d[i] !=0; i++){
    UDR0 = d[i];
    USART_SPINLOCK;
  }
  USART_SPINLOCK;
}
uint16_t current_sp;


struct Task adam_t = { 
  // adam task bringer of all tasks. 
//task that controls all other tasks
  .running = 0,
  .priority = 0,
  .context = 0,
  .stack = {},
  .task = &adam
}; // adam task 


void adam(struct Task * task){
  SP-=2; // will not be returning to interrupt
  cli();
  Task * cur_task = &queue.queue[queue.current_task_running];
    for(int i =0; i< 5; i++){
      struct Task * cur_task = &queue.queue[queue.current_task_running];
      cur_task->running = 1;
    }
  sei();
  while(1); // halt forever because everything has been done...
}




//only checks memory for the ended process

 ISR(TIMER1_COMPA_vect){
    cli();
    while(1);
}