#include <printf.h>
#include <paging.h>
#include <stdint.h>
#include <libc.h> 
#include <limits.h>
#include <kernel.h>

void debug_print_memory() {
  struct memory_table *memtable = (struct memory_table *)&_meminfo_loc; 
  printf(" __________________________________________________________________________\n");
  printf("| type\tstart\t\t\tend\t\t\tsize\t\t   |\n");
  printf("|--------------------------------------------------------------------------|\n");
  for(unsigned int i = 0; memtable[i].length > 0; i++) {
    printf("| %u %u\t0x%p\t0x%p\t0x%p |\n", memtable[i].type, memtable[i].ACPI, memtable[i].base, (memtable[i].base + memtable[i].length), memtable[i].length);
  }
  printf("----------------------------------------------------------------------------\n");
}

void debug_pmap() {
  struct phys_map* pmap;
  int pmap_i = 0, order;
  uint64_t buddy_size, blong_i, bbit_i, buddy_chunksize, omit_cnt;
  printf("Maximum buddy order: %u (up to %#x sized chunks)\n", MAX_BUDDY_ORDER, (0x1000 << MAX_BUDDY_ORDER - 1));
  for(pmap = (struct phys_map*)&_stage2_pagetable; pmap != 0; pmap = pmap->next) {
    printf("Table %u:\n"
        "\tPhysical Start:\t%#p\n"
        "\tTable location:\t%#p\n", pmap_i, pmap->zone_paddr, pmap);
    for(order = 0; order <= MAX_BUDDY_ORDER - 1; order++) {
      buddy_chunksize = (0x1000 << order); //TODO just put it in the for loop
      buddy_size = (((order == MAX_BUDDY_ORDER - 1) 
          ? (uint64_t *)pmap->next : pmap->buddy[order + 1]) - pmap->buddy[order]);
      printf("\tbuddy[%u]:\n"
          "\t\tAddress:\t%#x\n"
          "\t\tSize:\t\t%u\n"
          "\t\tBuddies:\t\t\n", order, pmap->buddy[order], buddy_size);

      omit_cnt = 0;

      for(blong_i = 0; blong_i < buddy_size; blong_i++) {
        for(bbit_i = 0; bbit_i < 64; bbit_i++) {
          if(*(pmap->buddy[order] + blong_i) & ((uint64_t)1 << bbit_i)) {
            if((omit_cnt < 20) || (blong_i == buddy_size - 1)) {
              printf("address %#x\tbit %u: %p\t is free\n",
                pmap->buddy[order] + blong_i, bbit_i, (uint64_t)pmap->zone_paddr + (((blong_i * 64) + bbit_i) * buddy_chunksize));
            }
            omit_cnt++;
            if(omit_cnt == 20) {
              printf("\t\t\t[more entries ommited]\n");
            }
          }
        }
      }
    }
    pmap_i++;
  }
}

void init_pmap() {
  struct memory_table *zones = (struct memory_table *)&_meminfo_loc;
  struct phys_map *pmap = (struct phys_map*)&_stage2_pagetable;

  unsigned int zone_i, pmap_i = 0;
  int budorder; 

  //we keep this info out of the struct because we won't need it after setting up
  uint64_t zone_len[MAX_ZONE_CNT], buddy_bitlen[MAX_ZONE_CNT][MAX_BUDDY_ORDER], *buddy_end;
  uint64_t pmap_size, pmap_bbitsize, pmap_blongsize, buddy_size, buddy_bit, pmap_bit;
  uint64_t threshold_bitsize, threshold_longsize = 0;

  
  void *paged_mem = (void *)&_stage2_pagetable + 0x200000;
  map_page(&_stage2_pagetable, &_stage2_pagetable, PAGE_SIZE_2M);

  for(zone_i = 0; zones[zone_i].length > 0; zone_i++) {

    if((zones[zone_i].type == MEM_AVAILABLE) && (zones[zone_i].ACPI & 1)) {

      //hopefully this should never happen...
      //I should dig into the docs to check before removing this.
      //We also could forget about MAX_ZONE_CNT if we did.
      
      if(zone_i >= MAX_ZONE_CNT) {
        printf("Only %u zones can be used! Modify MAX_ZONE_CNT in paging.h to use all memory.\n", MAX_ZONE_CNT);
        break;
      }


      if((zones[zone_i].base <= (void*)&_stage2_pagetable) && 
          (zones[zone_i].base + zones[zone_i].length >= (void *)&_stage2_pagetable)) { 
        pmap->zone_paddr = &_stage2_pagetable;
        zone_len[pmap_i] = zones[zone_i].length - (pmap->zone_paddr - zones[zone_i].base);
      }
      else {
        pmap->zone_paddr = zones[zone_i].base;
        zone_len[pmap_i] = zones[zone_i].length;
      }

      pmap->buddy[0] = (void *)pmap + sizeof(*pmap);

      for(budorder = 1; budorder < MAX_BUDDY_ORDER; budorder++) {
        buddy_bitlen[pmap_i][budorder - 1] = GET_BUDDY_BITLEN(zone_len[pmap_i], budorder - 1);
        pmap->buddy[budorder] = (uint64_t *)pmap->buddy[budorder - 1] + 
          LSIZE_FROM_BITLEN(buddy_bitlen[pmap_i][budorder - 1]);
      } 

      buddy_bitlen[pmap_i][MAX_BUDDY_ORDER - 1] = GET_BUDDY_BITLEN(zone_len[pmap_i], MAX_BUDDY_ORDER - 1);
      pmap->next = (void *)pmap->buddy[MAX_BUDDY_ORDER - 1] + 
        (LSIZE_FROM_BITLEN(buddy_bitlen[pmap_i][MAX_BUDDY_ORDER - 1]) * 8);

      pmap = pmap->next;
      pmap_i++;

      //allocates by an extra sizeof(struct phys_map),
      //but were about to discard anyway
      while((void *)pmap + sizeof(*pmap) >= paged_mem) {
        map_page(paged_mem, paged_mem, PAGE_SIZE_2M);
        paged_mem += 0x200000;
      }

    }
  }
  pmap_size = (void*)(pmap) - (void*)&_stage2_pagetable;
  if(pmap_size >= zone_len[0]) panic(); //TODO debugging

  pmap_i = 0;
  for(pmap = (struct phys_map*)&_stage2_pagetable; pmap->next != 0; pmap = pmap->next) {
    for(budorder = MAX_BUDDY_ORDER - 1; budorder >= 0; budorder--) {
      pmap_bbitsize = ceil((float)pmap_size / ((uint64_t)0x1000 << budorder));
      pmap_blongsize = pmap_bbitsize / 64;

      if(budorder == MAX_BUDDY_ORDER - 1) {
        buddy_size = (uint64_t *)pmap->next - pmap->buddy[budorder];
        buddy_end = (uint64_t *)pmap->next - 1;

        threshold_bitsize = ((pmap_blongsize * 64) + pmap_bbitsize) * 2;
      }
      else {
        buddy_size = pmap->buddy[budorder + 1] - pmap->buddy[budorder];
        buddy_end = pmap->buddy[budorder + 1] - 1;

        threshold_longsize = threshold_bitsize / 64;
      }
      pmap_bit = pmap_bbitsize & 63;
      buddy_bit = buddy_bitlen[pmap_i][budorder] & 63;


      if((pmap_bbitsize >= BITLEN_FROM_LSIZE(buddy_size)) && (pmap == (void *)&_stage2_pagetable)) {
        bzero(pmap->buddy[budorder], buddy_size * 8);
      }
      else {
        if(budorder == MAX_BUDDY_ORDER - 1) {
          if(pmap == (void*)&_stage2_pagetable) {
            if(pmap_blongsize) bzero(pmap->buddy[budorder], (pmap_blongsize - 1) * 8);
            if(pmap_bit) {
              *(pmap->buddy[budorder] + pmap_blongsize) = ~(((uint64_t)1 << pmap_bit) - 1);
            }
            else {
              *(pmap->buddy[budorder] + pmap_blongsize) = UINT64_MAX;
            }
            if(pmap_blongsize + 1 == buddy_size) {
              *buddy_end &= ((uint64_t)1 << buddy_bit) - 1;
            }
            else {
              memset(pmap->buddy[budorder] + pmap_blongsize + 1, UINT8_MAX, (buddy_size - 1) * 8);
              if(buddy_bit) {
                *buddy_end = ((uint64_t)1 << buddy_bit) - 1;
              }
              else {
                *buddy_end = UINT64_MAX;
              }
            }
            threshold_bitsize = ((pmap_blongsize * 64) + pmap_bbitsize) * 2;
          }
          else {
            memset(pmap->buddy[budorder], UINT8_MAX, (buddy_size - 1) * 8);
            if(buddy_bit) {
              *buddy_end = ((uint64_t)1 << buddy_bit) - 1;
            }
            else {
              *buddy_end = UINT64_MAX;
            }
          }
        }
        else if(pmap == (void *)&_stage2_pagetable) {
          if(threshold_longsize) bzero(pmap->buddy[budorder], (threshold_longsize - 1) * 8);

          if(threshold_bitsize > pmap_bbitsize)
            *(pmap->buddy[budorder] + threshold_longsize) = ((uint64_t)1 << ((threshold_bitsize - 1) & 63));
          
          if(buddy_size - threshold_longsize) 
            bzero(pmap->buddy[budorder] + threshold_longsize + 1, buddy_size - threshold_longsize);
          if(buddy_bit & 1) {
            *buddy_end = ((uint64_t)1 << (buddy_bit - 1));
          }
          threshold_bitsize = ((pmap_blongsize * 64) + pmap_bbitsize) * 2;
        }
        else {
          bzero(pmap->buddy[budorder], buddy_size);
          if(buddy_bit & 1) {
            *buddy_end = ((uint64_t)1 << ((buddy_bit) - 1));
          }
        }
      }
    }
    pmap_i++;
  }
}


/**
 * BIG TODO:
 * Paging turned out to be simpler then I thought. I've temporarily fixed the code, but needs to be rewritten/simplified.
 * Let's get rid of those nasty GOTOs if we can.
 * Also, once we get physical memory allocator up and running, impliment that in this function.
**/

bool map_page(void *virtual_addr, void *physical_addr, uint8_t size) {
  //printf("map page called\n");
  uintptr_t va_ptr = (uintptr_t)virtual_addr;
  uintptr_t pa_ptr = (uintptr_t)physical_addr;
  if((va_ptr % (1 << size)) || (pa_ptr % (1 << size))) {
    return 0; 
  }
  page_table *table = (page_table *)PAGEMAP_LOCATION;
  int pte_i = (va_ptr >> 12) & 0x1ff;
  int pde_i = (va_ptr >> 21) & 0x1ff;
  int pdpe_i = (va_ptr >> 30) & 0x1ff;
  int pml4e_i = (va_ptr >> 39) & 0x1ff;

  if(table->pml4e[pml4e_i].present) {
    if(table->pml4e[pml4e_i].base_ptr != (uintptr_t)&table->pdpe[pdpe_i] >> 12) goto error;
    if(table->pdpe[pdpe_i].present) {
      if(size == PAGE_SIZE_1G) {
        if(table->pdpe[pdpe_i].base_ptr == ((uintptr_t)pa_ptr >> 30 & 0x1ff))
          return true;
        goto error;
      }
      if(table->pdpe[pdpe_i].base_ptr != (uintptr_t)&table->pde[pde_i] >> 12) goto error;

      if(table->pde[pde_i].present) { 
        if(size == PAGE_SIZE_2M) {
          if(table->pde[pde_i].base_ptr == ((uintptr_t)pa_ptr >> 21 & 0x1ff))
            return true;
          goto error;
        }
        if(table->pde[pde_i].base_ptr != (uintptr_t)&table->pte[pte_i] >> 12) goto error;
        if(table->pte[pte_i].present) {
          if(table->pte[pte_i].base_ptr != ((pa_ptr >> 12) & 0x1ff)) goto error;
          return true;
        }
        else goto mod_page_pte;
      }
      else goto mod_page_pde;
    }
    else goto mod_page_pdpe;
  }
  else {
    table->pml4e[pml4e_i].base_ptr = (uintptr_t)&table->pdpe[pdpe_i] >> 12;
    table->pdpe[pml4e_i].read_write = 1;
    table->pml4e[pml4e_i].present = 1;
mod_page_pdpe:
    table->pdpe[pdpe_i].read_write = 1;
    //TODO you just found out things are a lot more simple then you thought!
    if(size == PAGE_SIZE_1G) {
      table->pdpe[pdpe_i].size = 1;
      table->pdpe[pdpe_i].base_ptr = pa_ptr >> 12;
      table->pdpe[pdpe_i].present = 1;
      return true;
    }
    table->pdpe[pdpe_i].base_ptr = (uintptr_t)&table->pde[pde_i] >> 12;
    table->pdpe[pdpe_i].present = 1;
mod_page_pde:
    table->pde[pde_i].read_write = 1;
    if(size == PAGE_SIZE_2M) {
      table->pde[pde_i].size = 1;
      table->pde[pde_i].base_ptr = pa_ptr >> 12;
      table->pde[pde_i].present = 1;
      return true;
    }
    table->pde[pde_i].base_ptr = (uintptr_t)&table->pte[pte_i] >> 12;
    table->pde[pde_i].present = 1;
mod_page_pte:
    table->pte[pte_i].base_ptr = pa_ptr >> 12;
    table->pte[pte_i].read_write = 1;
    table->pte[pte_i].present = 1;
    return true;
  }
error:
  printf("Page allocation error!\n");
  return false;
}