#include "../common.h"
#include "../all_drivers.h"

int _floppyIRQf = 0;
int _currentDrive = 0;
#define FLPY_SECTORS_PER_TRACK 80
#define DMA_BUFFER 0x1000

enum FLOPPY_IO {
 
	FLOPPY_DOR  = 0x3f2,
	FLOPPY_MSR  = 0x3f4,
	FLOPPY_FIFO = 0x3f5,	// data
	FLOPPY_CTRL = 0x3f7
};
enum FLOPPY_DOR_MASK {
 
	FLOPPY_DOR_MASK_DRIVE0	=	0,	//00000000
	FLOPPY_DOR_MASK_DRIVE1	=	1,	//00000001
	FLOPPY_DOR_MASK_DRIVE2	=	2,	//00000010
	FLOPPY_DOR_MASK_DRIVE3	=	3,	//00000011
	FLOPPY_DOR_MASK_RESET	=	4,	//00000100
	FLOPPY_DOR_MASK_DMA	=	8,	//00001000
	FLOPPY_DOR_MASK_DRIVE0_MOTOR=	16,	//00010000
	FLOPPY_DOR_MASK_DRIVE1_MOTOR=	32,	//00100000
	FLOPPY_DOR_MASK_DRIVE2_MOTOR=	64,	//01000000
	FLOPPY_DOR_MASK_DRIVE3_MOTOR=	128	//10000000
};
enum FLOPPY_MSR_MASK {
 
	FLOPPY_MSR_MASK_DRIVE1_POS_MODE =	1,	//00000001
	FLOPPY_MSR_MASK_DRIVE2_POS_MODE =	2,	//00000010
	FLOPPY_MSR_MASK_DRIVE3_POS_MODE =	4,	//00000100
	FLOPPY_MSR_MASK_DRIVE4_POS_MODE =	8,	//00001000
	FLOPPY_MSR_MASK_BUSY		=	16,	//00010000
	FLOPPY_MSR_MASK_DMA		=	32,	//00100000
	FLOPPY_MSR_MASK_DATAIO		=	64, 	//01000000
	FLOPPY_MSR_MASK_DATAREG		=	128	//10000000
};
enum FLOPPY_CMD {
	
	FDC_CMD_READ_TRACK	=	2,
	FDC_CMD_SPECIFY		=	3,
	FDC_CMD_CHECK_STAT	=	4,
	FDC_CMD_WRITE_SECT	=	5,
	FDC_CMD_READ_SECT	=	6,
	FDC_CMD_CALIBRATE	=	7,
	FDC_CMD_CHECK_INT	=	8,
	FDC_CMD_WRITE_DEL_S	=	9,
	FDC_CMD_READ_ID_S	=	0xa,
	FDC_CMD_READ_DEL_S	=	0xc,
	FDC_CMD_FORMAT_TRACK	=	0xd,
	FDC_CMD_SEEK		=	0xf
};
enum FLOPPY_CMD_EXT {
 
	FDC_CMD_EXT_SKIP	=	0x20,	//00100000
	FDC_CMD_EXT_DENSITY	=	0x40,	//01000000
	FDC_CMD_EXT_MULTITRACK	=	0x80	//10000000
};
enum FLOPPY_GAP3_LENGTH {
 
	FLOPPY_GAP3_LENGTH_STD = 42,
	FLOPPY_GAP3_LENGTH_5_14= 32,
	FLOPPY_GAP3_LENGTH_3_5 = 27
};
enum FLOPPY_SECTOR_DTL {
 
	FLOPPY_SECTOR_DTL_128	=	0,
	FLOPPY_SECTOR_DTL_256	=	1,
	FLOPPY_SECTOR_DTL_512	=	2,
	FLOPPY_SECTOR_DTL_1024	=	4
};


void floppyIRQ(struct regs *r){
	_floppyIRQf = 1;
}
void floppyInstall(){
	irq_install_handler(6, floppyIRQ);
	initDMA();
	floppyReset();
	floppyDriveData(13, 1, 0xf, true);
	
}
void waitForFloppyIRQ(){
	while(_floppyIRQf == 0) {itoa(1, 2);}
	_floppyIRQf = 0;
}

void initDMA(){
	outportb(0x0a,0x06);
	outportb(0xd8,0xff);
	outportb(0x04, 0);
	outportb(0x04, 0x10);
	outportb(0xd8, 0xff); 
	outportb(0x05, 0xff);
	outportb(0x05, 0x23);
	outportb(0x80, 0);
	outportb(0x0a, 0x02);
}
void prepDMARead(){
	outportb(0x0a, 0x06); 
	outportb(0x0b, 0x56);
	outportb(0x0a, 0x02);
}

void prepDMAWrite(){
	outportb(0x0a, 0x06);
	outportb(0x0b, 0x5a);
	outportb(0x0a, 0x02);
}

void floppyWriteDOR (uint8_t val){
	outportb(FLOPPY_DOR, val);
}
void floppyWriteCCR (uint8_t val) {
	outportb(FLOPPY_CTRL, val);
}
uint8_t floppyReadStatus() {
	return inportb(FLOPPY_MSR);
}
void floppySendCommand(uint8_t cmd) {
	for (int i = 0; i < 500; i++)
		if (floppyReadStatus() & FLOPPY_MSR_MASK_DATAREG)
			return outportb(FLOPPY_FIFO, cmd);
}
uint8_t floppyReadData() {
	for (int i = 0; i < 500; i++ )
		if (floppyReadStatus() & FLOPPY_MSR_MASK_DATAREG)
			return inportb(FLOPPY_FIFO);
}
void floppyCheckInterrupt(uint32_t* st0, uint32_t* cyl) {
	floppySendCommand(FDC_CMD_CHECK_INT);
 
	*st0 = floppyReadData();
	*cyl = floppyReadData();
}

void floppyReadSectorIMP(uint8_t head, uint8_t track, uint8_t sector) {
	uint32_t st0, cyl;
	prepDMARead();
	floppySendCommand(
		FDC_CMD_READ_SECT | FDC_CMD_EXT_MULTITRACK |
		FDC_CMD_EXT_SKIP | FDC_CMD_EXT_DENSITY);
	floppySendCommand(head << 2 | _currentDrive);
	floppySendCommand(track);
	floppySendCommand(head);
	floppySendCommand(sector);
	floppySendCommand(FLOPPY_SECTOR_DTL_512 );
	floppySendCommand(
		((sector + 1) >= FLPY_SECTORS_PER_TRACK)
			? FLPY_SECTORS_PER_TRACK : sector + 1);
	floppySendCommand(FLOPPY_GAP3_LENGTH_3_5 );
	floppySendCommand(0xff);
	waitForFloppyIRQ();
	for (int j=0; j<7; j++)
		floppyReadData();
	floppyCheckInterrupt(&st0,&cyl);
}
void floppyDriveData(uint32_t stepr, uint32_t loadt, uint32_t unloadt, bool dma) {
	uint32_t data = 0;
	floppySendCommand(FDC_CMD_SPECIFY);
	data = ((stepr & 0xf) << 4) | (unloadt & 0xf);
	floppySendCommand(data);
	data = (loadt) << 1 | (dma==true) ? 1 : 0;
	floppySendCommand(data);
}
void floppyMotor(bool mode){
	if(mode){
	    floppyWriteDOR(FLOPPY_DOR_MASK_DRIVE0_MOTOR | FLOPPY_DOR_MASK_RESET);
	} else {
	    floppyWriteDOR(FLOPPY_DOR_MASK_RESET);
	}
	sleep(150);
}
int floppyCalibrate(uint32_t drive) {
	uint32_t st0, cyl;
	if (drive >= 4)
		return -2;
	floppyMotor(true);
 
	for (int i = 0; i < 10; i++) {
		floppySendCommand(FDC_CMD_CALIBRATE);
		floppySendCommand(drive);
		waitForFloppyIRQ();
		floppyCheckInterrupt(&st0, &cyl);
		if (!cyl) {
			floppyMotor(false);
			return 0;
		}
	}
	floppyMotor(false);
	return -1;
}
int floppySeek(uint32_t cyl, uint32_t head) {
	uint32_t st0, cyl0;
	if (_currentDrive >= 4)
		return -1;
	for (int i = 0; i < 10; i++ ) {
		floppySendCommand(FDC_CMD_SEEK);
		floppySendCommand((head) << 2 | _currentDrive);
		floppySendCommand(cyl);
		waitForFloppyIRQ();
		floppyCheckInterrupt(&st0,&cyl0);
		if (cyl0 == cyl)
			return 0;
	}
	return -1;
}
void floppyDisableFDC() {
	floppyWriteDOR(0);
}
void floppyEnableFDC() {
	floppyWriteDOR(FLOPPY_DOR_MASK_RESET | FLOPPY_DOR_MASK_DMA);
}
void floppyReset() {
	uint32_t st0, cyl;
	floppyDisableFDC();
	floppyEnableFDC();
	waitForFloppyIRQ();
	for (int i=0; i<4; i++)
		floppyCheckInterrupt(&st0,&cyl);
	floppyWriteCCR(0);
	floppyDriveData(3,16,240,true);
	floppyCalibrate(_currentDrive);
}
void floppyLBAtoCHS(int lba,int *head,int *track,int *sector) {
	*head = ( lba % ( FLPY_SECTORS_PER_TRACK * 2 ) ) /
		( FLPY_SECTORS_PER_TRACK );
	*track = lba / ( FLPY_SECTORS_PER_TRACK * 2 );
	*sector = lba % FLPY_SECTORS_PER_TRACK + 1;
}
uint8_t* floppyReadSector(int sectorLBA) {
	if (_currentDrive >= 4)
		return 0;
	int head=0, track=0, sector=1;
	floppyLBAtoCHS(sectorLBA, &head, &track, &sector);
	floppyMotor(true);
	if (floppySeek(track, head) != 0)
		return 0;
	floppyReadSectorIMP(head, track, sector);
	floppyMotor(false);
	return (uint8_t*) DMA_BUFFER;
}