#include "Paging.h"

Heap *CreateHeap(unsigned StartAddress, unsigned EndAddress, unsigned MaxAddress, bool SupervisorOnly, bool ReadOnly);
void *HeapAlloc(unsigned Size, bool PageAlign, Heap *_Heap);

/* * * * * * * * * * * * * * * * * * *
 * Bitset
 * * * * * * * * * * * * * * * * * * */
unsigned *Frames;
unsigned TmpFrames;

#define ID_FROM_BIT(x)		((x)/(8*4))	/* Retrieves an Index from a 'bitset'. */
#define OFFSET_FROM_BIT(x)	((x)%(8*4))	/* Retrieves an offset from a 'bitset'. */

/* Sets a bit in the Frames 'bitset'. */
void SetFrameBit(unsigned FrameAddr)
{
	unsigned Frame = (FrameAddr / 4096);
	unsigned Index = ID_FROM_BIT(Frame);
	unsigned Offset = OFFSET_FROM_BIT(Frame);

	Frames[Index] |= (0x1 << Offset);
}

/* Clears a bit from the Frames 'bitset'. */
void ClearFrameBit(unsigned FrameAddr)
{
	unsigned Frame = (FrameAddr / 4096);
	unsigned Index = ID_FROM_BIT(Frame);
	unsigned Offset = OFFSET_FROM_BIT(Frame);

	Frames[Index] &= ~(0x1 << Offset);
}

/* Tests if a bit in the Frames 'bitset' is set. */
unsigned TestFrameBit(unsigned FrameAddr)
{
	unsigned Frame = (FrameAddr / 4096);
	unsigned Index = ID_FROM_BIT(Frame);
	unsigned Offset = OFFSET_FROM_BIT(Frame);

	return (Frames[Index] & (0x1 << Offset));
}

/* Finds the first free frame in the Frames 'bitset'. */
unsigned FindFirstFreeFrame()
{
	unsigned i = 0;
	unsigned j = 0;
	unsigned CurBit = 0;

	for(i = 0; i < ID_FROM_BIT(TmpFrames); ++i)
	{
		/* Is there a bit free? */
		if(Frames[i] == 0xFFFFFFFF)
			continue;

		for(j = 0; j < 32; ++j)
		{
			CurBit = (0x1 << j);

			if(!(Frames[i] & CurBit))
				return ((32*i)+j);
		}
	}

	return 0;
}

/* * * * * * * * * * * * * * * * * * *
 * Paging
 * * * * * * * * * * * * * * * * * * */
PageDirectory *KernelDir = 0;			/* Kernel's PageDirectory. */
PageDirectory *CurrentDir = 0;			/* Current PageDirectory. */

extern Heap *TheHeap;					/* The actual heap. */
extern unsigned CurMemAddress;			/* Pointer to free memory that hasn't been allocated yet. */

extern "C" void CopyPhysicalPage(unsigned SourceAddr, unsigned DestAddr);

bool AllocFrame(Page *ThePage, bool Kernel, bool Writable)
{
	/* Has the page already been allocated? */
	if(ThePage->FrameAddress)
		return false;

	unsigned Index = FindFirstFreeFrame();

	/* Do some crazy unsigned Magic to see if there any free frames. */
	if(Index == (unsigned) -1)
		return false;

	SetFrameBit(Index * PAGE_SIZE);

	ThePage->Present = true;
	ThePage->Access = (Writable? PAGE_ACCESS_READWRITE : PAGE_ACCESS_READONLY);
	ThePage->UserMode = (Kernel? false : true);
	ThePage->FrameAddress = Index;
	ThePage->Size = PAGE_SIZE;

	return true;
}

/* Maps a page to a physical address. */
void ForceFrame(Page *_Page, bool Kernel, bool Writable, unsigned Address)
{
	_Page->Present = true;
	_Page->Access = (Writable? PAGE_ACCESS_READWRITE : PAGE_ACCESS_READONLY);
	_Page->UserMode = (Kernel? false : true);
	_Page->FrameAddress = Address >> 12;
	_Page->Size = PAGE_SIZE;

	SetFrameBit(Address);
}

/* Clones a PageTable. */
PageTable *CloneTable(PageTable *From, unsigned *Phys)
{
	/* Allocate and clear a new page table. */
	PageTable *NewTable = (PageTable *) Memory->AllocAlignPhys(sizeof(PageTable), Phys);
	Memory->Set(NewTable, 0, sizeof(PageTable));

	for(int i(0); i < 1024; ++i)
	{
		if(From->Table[i].FrameAddress)
		{
			/* Allocate a new frame and copy values. */
			AllocFrame(&NewTable->Table[i], false, false);

			NewTable->Table[i].Present =	From->Table[i].Present;
			NewTable->Table[i].Access =		From->Table[i].Access;
			NewTable->Table[i].UserMode =	From->Table[i].UserMode;
			NewTable->Table[i].Accessed =	From->Table[i].Accessed;
			NewTable->Table[i].Dirty =		From->Table[i].Dirty;

			CopyPhysicalPage(From->Table[i].FrameAddress * PAGE_SIZE, NewTable->Table[i].FrameAddress * PAGE_SIZE);
		}
	}

	return NewTable;
}

/* Clones a PageDirectory. */
PageDirectory *CloneDirectory(PageDirectory *From)
{
	unsigned Phys;

	/* Allocate and clear a new page directory. */
	PageDirectory *NewDir = (PageDirectory *) Memory->AllocAlignPhys(sizeof(PageDirectory), &Phys);
	Memory->Set(NewDir, 0, sizeof(PageDirectory));

	/* Offset of TablesPhysAddr from the start of NewDir. */
	NewDir->PhysAddr = (Phys + ((unsigned) NewDir->TablesPhysAddr) - ((unsigned) NewDir));

	for(int i(0); i < 1024; ++i)
	{
		if(From->Tables[i])
		{
			/* If the table is in the kernel, link it and use the same pointer. */
			if(KernelDir->Tables[i] == From->Tables[i])
			{
				NewDir->Tables[i] = From->Tables[i];
				NewDir->TablesPhysAddr[i] = From->TablesPhysAddr[i];
			}

			/* If it isn't, copy the table. */
			else
			{
				/* Re-use the Phys variable (save memory! ;)). */
				NewDir->Tables[i] = CloneTable(From->Tables[i], &Phys);
				NewDir->TablesPhysAddr[i] = Phys | 0x07;					//0x07 = Present, read-write and user-mode flags.
			}
		}
	}

	return NewDir;
}

void FreeFrame(Page *ThePage)
{
	if(!ThePage->FrameAddress)
		return;

	ClearFrameBit(ThePage->FrameAddress);
	ThePage->FrameAddress = 0x0;
}

void SwitchPageDirectory(PageDirectory *NewPageDir)
{
	unsigned CR0;
	CurrentDir = NewPageDir;

	ASM("mov %0, %%CR3" : : "r" (NewPageDir->TablesPhysAddr));		/* Write the physical addresses to CR3. */
	ASM("mov %%CR0, %0" : "=r" (CR0) :);							/* Read CR0 to the unsigned variable. */

	/* Enable paging by writing the new local CR0 to CR0. */
	CR0 |= 0x80000000;
	ASM("mov %0, %%CR0" : : "r" (CR0));
}

Page *GetPage(unsigned Address, bool Make, PageDirectory *Dir)
{
	Address /= PAGE_SIZE;					/* Convert Address to an ID. */

	unsigned TableID = Address / 1024;		/* Find the page table with this address. */

	/* Is this table already assigned? */
	if(Dir->Tables[TableID])
		return (&Dir->Tables[TableID]->Table[Address % 1024]);

	/* Should the page table be created? */
	else if(Make)
	{
		unsigned Tmp = 0;

		Dir->Tables[TableID] = reinterpret_cast<PageTable *>(Memory->AllocAlignPhys(sizeof(PageTable), &Tmp));

		Memory->Set(Dir->Tables[TableID], 0, PAGE_SIZE);

		/* Pass the physical address and enable the first 7 bits (1 = Present, 2 = PAGE_ACCESS_READWRITE, 4 = UserMode). */
		Dir->TablesPhysAddr[TableID] = Tmp | 0x7;

		return &Dir->Tables[TableID]->Table[Address % 1024];
	}

	return 0;
}

void InitPaging(/*unsigned ReserveVideoMem*/)
{
	/* Physical memory Size, filled in by GRUB. */
	unsigned LastPage = GRUB_Info->HigherMem * 1024;

	TmpFrames = (LastPage / PAGE_SIZE);

	Frames = reinterpret_cast<unsigned *>(Memory->Alloc(ID_FROM_BIT(TmpFrames)));
	Memory->Set(Frames, 0, ID_FROM_BIT(TmpFrames));

	KernelDir = reinterpret_cast<PageDirectory *>(Memory->AllocPgAlign(sizeof(PageDirectory)));
	Memory->Set(KernelDir, 0, sizeof(PageDirectory));

	///<Multitasking>
	KernelDir->PhysAddr = (unsigned) KernelDir->TablesPhysAddr;
	///</Multitasking>

	//CurrentDir = KernelDir;

	unsigned i;

	for(i = HEAP_STARTADDRESS; i < HEAP_STARTADDRESS + HEAP_START_SIZE; i += PAGE_SIZE)
		GetPage(i, true, KernelDir);

	/* Force mapping the specified pages to the specified physical addresses, currently not used. */
	//for(i = VBE_LFB_PHYSICAL_ADDRESS; i < VBE_LFB_PHYSICAL_ADDRESS + ReserveVideoMem; i += PAGE_SIZE)
	//{
	//	/* Create pages for VBE memory (otherwise it would cause a pagefault). */
	//	Page *TmpPage = GetPage(i, true, KernelDir);
	//	ForceFrame(TmpPage, false, true, i);
	//}

	i = 0;

	while(i < (CurMemAddress + PAGE_SIZE))
	{
		AllocFrame(GetPage(i, true, KernelDir), false, false);
		i += PAGE_SIZE;
	}

	for(i = HEAP_STARTADDRESS; i < HEAP_STARTADDRESS + HEAP_START_SIZE; i += PAGE_SIZE)
		AllocFrame(GetPage(i, true, KernelDir), false, false);

	/* Enable paging, the page fault handler is installed by the kernel itself. */
	SwitchPageDirectory(KernelDir);

	/* The heap. */
	TheHeap = CreateHeap(HEAP_STARTADDRESS, HEAP_STARTADDRESS + HEAP_START_SIZE, 0xCFFFF000, false, false);

	///<Multitasking>
	CurrentDir = CloneDirectory(KernelDir);
	//SwitchPageDirectory(CurrentDir); //<-- Third exception (14) with no resolution. This is what triple-faults.
	///</Multitasking>
}

void *AllocExtFromHeap(size_t Length, bool Align, unsigned *PhysAddr, Heap *TheHeap)
{
	void *TmpAddress = HeapAlloc(Length, Align, TheHeap);

	if(PhysAddr)
	{
		Page *TmpPage = GetPage((unsigned) TmpAddress, 0, KernelDir);
		(*PhysAddr) = TmpPage->FrameAddress * PAGE_SIZE + (unsigned) TmpAddress & 0xFFF;
	}

	return TmpAddress;
}
