#include <kernel.h>
#include <printf.h>
#include <madt.h>
#include <timer.h>
#include <cpuid.h>
#include <int.h>
#include <libc.h>
#include <paging.h>
#include <heap.h>

#define LAPIC_ICR_LOW   192
#define LAPIC_ICR_HIGH  196

extern lapic_t lapic;
extern char __load_start_smp_bootloader, __load_stop_smp_bootloader;
extern uint8_t *smp_bootstrap_corecount;
extern uint8_t smp_bootstrap_bsp;
extern uint64_t *smp_bootstrap_stackarray;

struct icr_reg {
  uint8_t vector;
  unsigned int deliv_mode:3;
  unsigned int dest_mode:1;
  unsigned int deliv_status:1;
  unsigned int reserved:1;
  unsigned int level:1; //0 = de-assert
  unsigned int trig_mode:1;
  unsigned int reserved_1:2;
  unsigned int dest_shorthand:2;
  unsigned int reserved_2:12;
}__attribute__((packed));

//1: get bsp number             2
//2: get list of lapics         3
//3: copy code                  1
//4: 


static inline void write_icr(uint8_t dest, uint32_t message) {
  lapic[LAPIC_ICR_HIGH] = (uint32_t)dest << 24;
  lapic[LAPIC_ICR_LOW] = message;
}

void smp_boot() {
  uint8_t cores_active = 1; //TODO change in asm
  struct cores_info cores;
  struct icr_reg icr;
  get_coreinfo(&cores);
  bzero(&icr, sizeof(icr));

  void **core_stacks = malloc(sizeof(void *) * (cores.corecount - 1));
  for(unsigned int s = 0; s < (cores.corecount - 1); s++) {
    core_stacks[s] = palloc(0x1000);
  }

  memcpy(PHYS_TO_VIRT((void *)0x8000), PHYS_TO_VIRT(&__load_start_smp_bootloader), 
      &__load_stop_smp_bootloader - &__load_start_smp_bootloader);
  smp_bootstrap_corecount = &cores_active;
  smp_bootstrap_bsp = cores.bsp;
  smp_bootstrap_stackarray = (uint64_t)core_stacks;



  icr.deliv_mode = 0b101;
  icr.dest_shorthand = 0b11;
  icr.level = 1;
  write_icr(0, *(uint32_t *)&icr);
  usleep(10000);

  icr.deliv_mode = 0b110;
  icr.vector = 8;
  write_icr(0, *(uint32_t *)&icr);
  usleep(200);
  if(cores_active != cores.corecount) write_icr(0, *(uint32_t *)&icr);
  usleep(200);
  if(cores_active != cores.corecount) {
    printf("NOT ALL CORES ONLINE\n");
    asm("cli\nhlt");
  }
  else {
    printf("%i!!!!!!\n", cores_active);
  }
}