#include "fastlz.h"
#include <cstddef>
#include <cstdlib>
#include <malloc.h>
#include <mm_types.h>
#include <nds.h>
#include <filesystem.h>
#include <nds/arm9/background.h>
#include <nds/arm9/cache.h>
#include <nds/arm9/decompress.h>
#include <nds/arm9/video.h>
#include <nds/bios.h>
#include <nds/debug.h>
#include <nds/dma.h>
#include <nds/system.h>
#include <nds/timers.h>
#include <stdio.h>
#include <maxmod9.h>
#include <fcntl.h>
#include <sys/_intsup.h>
#include <sys/_stdint.h>

#define TIMER_SPEED (BUS_CLOCK/1024)

void wait_forever(const char* msg);
size_t LoadFile(const char* file, unsigned char **buffer);
void LoadNextChunk(void *output);
void onVBL();

class CircularQueue {
public:
  int front, rear, size, current;
  CircularQueue(int s) {
    size = s;
    front = rear = -1;
		current = 0;
  }

  bool isFull() {
    return (front == 0 && rear == size - 1) || (rear == (front - 1) % (size - 1));
  }

  bool isEmpty() {
    return front == -1;
  }

  void push() {
    if (isFull()) {
      fprintf(stderr, "Queue is full. Cannot push\n");
      return;
    }

    if (isEmpty()) {
      front = rear = 0;
    } else {
      rear = (rear + 1) % size;
    }

		current++;
  }

  void pop() {
    if (isEmpty()) {
      fprintf(stderr, "Queue is empty. Cannot pop\n");
      return;
    }

    if (front == rear) {
      front = rear = -1;
    } else {
      front = (front + 1) % size;
    }

		current--;
  }
};

int ptr_background;
int ptr_subbackground;
volatile int file_image = -1;
volatile int file_music = -1;

volatile bool noQueue = false;
constexpr int queue_size = 6;

constexpr int chunk_size = 10;
volatile int chunk_counter = 0;
volatile int chunk_current = 0;

constexpr int frame_size = 256*192;
volatile int frame_counter = 0;
uint8_t frame_decompress[frame_size*chunk_size];
uint8_t frame_buffer[frame_size*chunk_size*queue_size];
CircularQueue frame_queue(queue_size);

constexpr int music_buffer_size = 4000*16;

mm_word on_stream_request( mm_word length, mm_addr dest, mm_stream_formats format ) {
	if(file_music){
		size_t samplesize = 1;
		switch(format){
			case MM_STREAM_8BIT_MONO: samplesize = 1; break;
			case MM_STREAM_8BIT_STEREO: samplesize = 2; break;
			case MM_STREAM_16BIT_MONO: samplesize = 2; break;
			case MM_STREAM_16BIT_STEREO: samplesize = 4; break;
		}

		int res = read(file_music, dest, length*samplesize);

		if(res){
			length = res/samplesize;
		} else {
			mmStreamClose();
			close(file_music);
			length = 0;
		}

	}

	return length;
}

int main(void) {

	videoSetMode(MODE_5_3D);
	videoSetModeSub(MODE_0_2D);

	vramSetBankA(VRAM_A_MAIN_BG);
	vramSetBankB(VRAM_B_TEXTURE);
	vramSetBankF(VRAM_F_TEX_PALETTE);

	consoleDemoInit();
	consoleDebugInit(DebugDevice_NOCASH);

	nitroFSInit(NULL);

	file_image = open("nitro:/image.bin", O_RDONLY);
	if (file_image == -1) wait_forever("cannot load image.bin");

	file_music = open("nitro:/music.raw", O_RDONLY);
	if (file_music == -1) wait_forever("cannot load music.raw");

	unsigned char* pal = NULL;
	size_t pal_len = LoadFile("nitro:/palette.bin", &pal);

	ptr_background = bgInit(3, BgType_Bmp8, BgSize_B8_256x256, 0, 0);

	dmaCopy(pal, BG_PALETTE, pal_len);

	// DC_FlushAll();

	// fprintf(stderr, "start debug\n");
	// int cframe = 0;
	// int hframe = 0;
	// int ticks = 0;


	// while(!feof(in)) 
	// {
	// 	timerStart(0, ClockDivider_1024, 0, NULL);
	// 	readNextBatch(in , chunk);
	// 	ticks = timerStop(0);
	// 	int milis = ((ticks%TIMER_SPEED)*1000) /TIMER_SPEED;
		
	// 	if (milis > 50) fprintf(stderr, "%i\n", cframe);
	// 	cframe++;
	// }

	// fprintf(stderr, "%i\n", hframe);


	// fseek(in, 0, SEEK_SET);

	fprintf(stderr, "preload start\n");
	while(!frame_queue.isFull()) {
		frame_queue.push();
		LoadNextChunk(&frame_buffer[frame_size*chunk_size*frame_queue.rear]);
	}
	fprintf(stderr, "preload done %i\n", frame_queue.current);

	DC_FlushAll();

	mm_ds_system sys;
	sys.mod_count 			= 0;
	sys.samp_count			= 0;
	sys.mem_bank			  = 0;
	sys.fifo_channel		= FIFO_MAXMOD;
	mmInit( &sys );

	mm_stream mystream;
	mystream.sampling_rate	= 22050;
	mystream.buffer_length	= music_buffer_size;
	mystream.callback		= on_stream_request;
	mystream.format			= MM_STREAM_16BIT_MONO;
	mystream.timer			= MM_TIMER2;
	mystream.manual			= true;

	DC_FlushAll();

	mmStreamOpen( &mystream );

	// free(pal);
	// free(sagiri_bg);

	while(1) {

		irqSet(IRQ_VBLANK, onVBL);

		if (!frame_queue.isFull()) {
			frame_queue.push();
			LoadNextChunk(&frame_buffer[frame_size*chunk_size*frame_queue.rear]);
			// fprintf(stderr, "read %i\n", queue.rear);
		}

		if (!(frame_counter % 4)) mmStreamUpdate();
	}

	return 0;
}

void onVBL()
{
	// DC_FlushRange(&chunk[256*192*4 * queue.front + 256*192*chunk_counter], 256*192*4);
	dmaCopyAsynch(&frame_buffer[frame_size*chunk_size*frame_queue.front + frame_size*chunk_counter], bgGetGfxPtr(ptr_background), frame_size);
	// if (frame_queue.current < frame_queue.size*0.4) {
		// fprintf(stderr, "%i\n ", chunk_counter);
		// fprintf(stderr, "count: %i, %i\n ", chunk_counter, frame_queue.current);
	// }

	chunk_counter++;
	frame_counter++;

	if (chunk_counter >= chunk_size) {
		if (!frame_queue.isEmpty()) {
			frame_queue.pop();
			chunk_counter = 0;
		} else {
			chunk_counter = chunk_size;
			noQueue = true;
			fprintf(stderr, "NO QUEUE!!!!!\n");
		}
	}
}

void LoadNextChunk(void *output)
{
	uint32_t compress_size;
	read(file_image, &compress_size, sizeof(uint32_t));
	
	uint32_t flag = compress_size & 0x3;
	compress_size = compress_size >> 2;


	if (flag == 0x01) // RLE
	{
		read(file_image, frame_decompress, compress_size);
		decompress(frame_decompress, output, RLE);
		// fprintf(stderr, "RLE\n");
	}
	else if (flag == 0x10) // RAW
	{
		read(file_image, output, compress_size);
		// fprintf(stderr, "RAW\n");
	}
	else if (flag == 0x00) // fastlz
	{
		read(file_image, frame_decompress, compress_size);
		fastlz_decompress(frame_decompress, compress_size, output, frame_size*chunk_size);
		// fprintf(stderr, "fastlz\n");
	}
	chunk_current++;
	noQueue = false;
	// DC_FlushRange(output, 256*192*4);
}

size_t LoadFile(const char* file, unsigned char **buffer)
{
	FILE *in = fopen(file, "rb");
	if (in == NULL) wait_forever("cannot load file!");

	fseek(in, 0, SEEK_END);
	long file_size = ftell(in);
	rewind(in);

	*buffer = (unsigned char*)malloc(file_size);
	if (*buffer == NULL) wait_forever("Failed to malloc!");

	size_t bytes_read = fread(*buffer, 1, file_size, in);
	if (bytes_read != file_size)
	{
		fclose(in);
		free(*buffer);
		wait_forever("Failed to read");
	}

	return bytes_read;
}

void wait_forever(const char* msg)
{
	fprintf(stderr, "%s\n", msg);
	fprintf(stderr, "%s\n", msg);
  while (1) swiWaitForVBlank();
}