#include <addr.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>
#include <panic.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;
extern char final_gdt_descriptor;

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));


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

struct gdt_descriptor {
  uint16_t size;
  void *offset;
} __attribute__((packed));

static struct gdt_descriptor gdtr;

void smp_boot() {
  uint8_t cores_active = 1;
  struct cores_info cores;
  struct icr_reg icr;
  get_coreinfo(&cores);
  if(cores.corecount == 1) {
    asm("sgdt [%0]"::"m"(gdtr));
    gdtr.offset = PHYS_TO_VIRT(gdtr.offset);
    asm("lgdt [%0]"::"m"(gdtr));
    return;
  }
  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 = (void *)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("Only %i cores online (expected %i)\n", cores_active, cores.corecount); //maybe add a panic
    PANIC(KERNEL_PANIC_SMP_FAILED);
  }
  printf("%i \n", cores_active);
  //asm("lgdt [%0]\n"::"r"(PHYS_TO_VIRT(&final_gdt_descriptor))); TODO NOW
}