#include "MemUtils.h"
#include "string.h"
#include "DosStructs.h"
#include "IC_ImageTypes.h"
#include "Nibblizer.h"

Nib_Logue		gNibAddrPrologue	= kNibAddrPrologue;
Nib_Logue		gNibDataPrologue	= kNibDataPrologue;
Nib_Logue		gNibEpliogue		= kNibEpliogue;

static Byte Disk2Byte[] = {
	0x00, 0x04, 0xFF, 0xFF, 0x08, 0x0C, 0xFF, 0x10, 0x14, 0x18, 0xFF, 0xFF, 
	0xFF, 0xFF, 0xFF, 0xFF, 0x1C, 0x20, 0xFF, 0xFF, 0xFF, 0x24, 0x28, 0x2C,
	0x30, 0x34, 0xFF, 0xFF, 0x38, 0x3C, 0x40, 0x44, 0x48, 0x4C, 0xFF, 0x50, 
	0x54, 0x58, 0x5C, 0x60, 0x64, 0x68, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x6C, 0xFF, 0x70, 0x74, 0x78, 0xFF, 0xFF, 
	0xFF, 0x7C, 0xFF, 0xFF, 0x80, 0x84, 0xFF, 0x88, 0x8C, 0x90, 0x94, 0x98, 
	0x9C, 0xA0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xA4, 0xA8, 0xAC, 0xFF, 0xB0, 
	0xB4, 0xB8, 0xBC, 0xC0, 0xC4, 0xC8, 0xFF, 0xFF, 0xCC, 0xD0, 0xD4, 0xD8,
	0xDC, 0xE0, 0xFF, 0xE4, 0xE8, 0xEC, 0xF0, 0xF4, 0xF8, 0xFC 
};

static Byte Byte2Disk[] = {
	0x96, 0x97, 0x9A, 0x9B, 0x9D, 0x9E, 0x9F, 0xA6, 0xA7, 0xAB, 0xAC, 0xAD, 
	0xAE, 0xAF, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB9, 0xBA, 0xBB, 0xBC, 
	0xBD, 0xBE, 0xBF, 0xCB, 0xCD, 0xCE, 0xCF, 0xD3, 0xD6, 0xD7, 0xD9, 0xDA, 
	0xDB, 0xDC, 0xDD, 0xDE, 0xDF, 0xE5, 0xE6, 0xE7, 0xE9, 0xEA, 0xEB, 0xEC, 
	0xED, 0xEE, 0xEF, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF9, 0xFA, 0xFB, 
	0xFC, 0xFD, 0xFE, 0xFF
};

static Byte FlipBit1[4] = { 0, 2,   1,  3   };
static Byte FlipBit2[4] = { 0, 8,   4,  12  };
static Byte FlipBit3[4] = { 0, 32,  16, 48  };
static Byte FlipBit4[4] = { 0, 128, 64, 192 };

// low(0) = Even bits, hi(1) = Odd bits
// But swapped when written to disk (little endian)
static	Nib_4n4Short	Set4n4(Byte data)
{
	Nib_4n4Short		result;
	
	result.low	= (data >> 1) | kNib_4n4Mask;
	result.high	= data | kNib_4n4Mask;
	
	return result;
}

// low(0) = Even bits, hi(1) = Odd bits
// But swapped when written to disk (little endian)
static	inline	Byte	Get4n4(Nib_4n4Short data)
{
	return ((data.low <<= 1) | 0x01) & data.high;
}

static inline	Nib_4n4Short	Nib_ComputeAddrFieldChecksum(
	Nib_AddressField	*addrP)
{
	ushort		result = *(ushort *)&(addrP->volume) 
		^ *(ushort *)&(addrP->track) 
		^ *(ushort *)&(addrP->sector);
	
	return  *(Nib_4n4Short *)&result;
}

static	Boolean		Nib_IsAddressField(Nib_AddressField *addrFieldP)
{
	Boolean		looksGoodB =
		   addrFieldP->prologue.data[0] == gNibAddrPrologue.data[0]
		&& addrFieldP->prologue.data[1] == gNibAddrPrologue.data[1]
		&& addrFieldP->epilogue.data[0] == gNibEpliogue.data[0]
		&& addrFieldP->epilogue.data[1] == gNibEpliogue.data[1];
	
	if (looksGoodB) {
		Nib_4n4Short		checkSum = Nib_ComputeAddrFieldChecksum(addrFieldP);
		
		looksGoodB = *(ushort *)&checkSum == *(ushort *)&addrFieldP->checksum;
		ASSERT(looksGoodB);
	}
	
	return looksGoodB;
}

static	void		Nib_SetAddrField(
	Nib_AddressField	*addrP, 
	short				curVolume, 
	short				curTrack, 
	short				curSector)
{
	addrP->prologue		= gNibAddrPrologue;
	addrP->volume		= Set4n4(curVolume);
	addrP->track		= Set4n4(curTrack);
	addrP->sector		= Set4n4(curSector);
	addrP->checksum		= Nib_ComputeAddrFieldChecksum(addrP);
	addrP->epilogue		= gNibEpliogue;
}

static	Boolean		Nib_IsDataField(Nib_DataField *dataFieldP)
{
	Nib_AddressField	*addrFieldP = (Nib_AddressField *)dataFieldP;
	
	return 
		   addrFieldP->epilogue.data[1] != gNibEpliogue.data[1]
		&& dataFieldP->prologue.data[0] == gNibDataPrologue.data[0]
		&& dataFieldP->prologue.data[1] == gNibDataPrologue.data[1]
		&& dataFieldP->epilogue.data[0] == gNibEpliogue.data[0]
		&& dataFieldP->epilogue.data[1] == gNibEpliogue.data[1];
}

/**************************************************************/
static	void			Decode6n2(Nib_6n2Byte *dataP)
{
	ushort	L1, L2;

	// convert from disk bytes
	for (L1 = 0; L1 <= Nib_kChecksumIndex; L1++)
		dataP[L1] = Disk2Byte[dataP[L1] - 0x96];

	// un-checksum data, byte zero already always the same
	for (L1 = 1; L1 <= Nib_kChecksumIndex; L1++)
		dataP[L1] = dataP[L1] ^ dataP[L1 - 1];

	// stuff "2" aux data buffer back with "6" part
	// need two extra zero bytes at end for looping. 
	// Checksum (should be) always zero, need one more.
	if (dataP[Nib_kChecksumIndex] == 0x00) {
		Byte	oldEpilogByte = dataP[Nib_kEpilogStartIndex];
	
		dataP[Nib_kEpilogStartIndex] = 0x00;
		
		for (L1 = 86, L2 = 0; L1 < 172; L1++, L2++) {
			dataP[L1]			|= FlipBit1[((dataP[L2] >> 2) & 0x3F) & 3];
			dataP[L1 + 86]		|= FlipBit1[((dataP[L2] >> 4) & 0x0F) & 3];
			dataP[L1 + 172]		|= FlipBit1[((dataP[L2] >> 6) & 0x03) & 3];
		}
		
		dataP[Nib_kEpilogStartIndex]	= oldEpilogByte;
		dataP[Nib_kChecksumIndex]		= 0;
	}
}

// Checksum (0 + 342) & one more byte (0 + 343) 
// needed to make aux buffer loop okay (need two extra 0s)
// Last two bytes only use 4 bits each, not 6 and 2
//  0 ...  83   84  85 The three bytes in each column...
// 86 ... 169  170 171
//172 ... 255 (256 257 = extra bytes, set to zero)
//--- ... ---  --- ---
//  0 ...  83   84  85 Make this Aux buffer byte
static	void	Encode6n2(Nib_6n2Byte *dataP)
{
	ushort	L1, L2;
	Byte	oldEpilogByte = dataP[Nib_kEpilogStartIndex];

	dataP[Nib_kChecksumIndex]		= 0x00;
	dataP[Nib_kEpilogStartIndex]	= 0x00;
	
	// create "2" aux buffer
	for (L1 = 86, L2 = 0; L1 < 172; L1++, L2++) {
		dataP[L2] = 
			FlipBit2[dataP[L1] & 3] | 
			FlipBit3[dataP[L1 + 86] & 3] | 
			FlipBit4[dataP[L1 + 172] & 3];
	}

	// create checksummed data and the checksum byte itself
	// byte zero is already always okay
	for (L1 = Nib_kChecksumIndex; L1 > 0; L1--) {
		dataP[L1] = dataP[L1] ^ dataP[L1 - 1];
	}

	// convert to disk bytes
	for (L1 = 0; L1 < Nib_kEpilogStartIndex; L1++) {
		dataP[L1] = Byte2Disk[(dataP[L1] & 0xFC) / 4];
	}

	dataP[Nib_kEpilogStartIndex] = oldEpilogByte;
}

/**************************************************************/
static	Boolean		Nib_Decode(Nib_Disk *nibDiskP, DiskImageRec *imageRec)
{
	short			curTrack, curSector, newTrack, newSector;

	for (
		curTrack = 0;
		curTrack < Gen_kTracksPerDisk;
		curTrack++
	) {
		for (
			curSector = 0;
			curSector < Gen_kSectorsPerTrack;
			curSector++
		) {
			Nib_Sector	*curSectorP = 
				&nibDiskP->track[curTrack].sector[curSector];
			
			Decode6n2(curSectorP->data.data);
			
			if (curSectorP->data.checksum != 0) {
				ReportErrorStr(-1, "Corrupted Nibblized Disk. (Maybe DOS 3.2?)");
				return FALSE;
			}
			
			newTrack	= Get4n4(curSectorP->address.track);
			newSector	= Get4n4(curSectorP->address.sector);
			
			if (
				newTrack >= Gen_kTracksPerDisk
				|| newSector >= Gen_kSectorsPerTrack
			) {
				ReportErrorStr(-1, "Illegal track/sector on Nibblized Disk.");
				return FALSE;
			}
			
			memcpy(
				&imageRec->image.gen->track[newTrack].sector[newSector], 
				&curSectorP->data.data[86], 
				sizeof(Gen_Sector));

//			imageRec->image.gen->track[newTrack].sector[newSector] = 
//				*(Gen_Sector *)&curSectorP->data.data[86];
		}
	}
	
	ImageRec_OrigOrder(imageRec)	= FSType_GEN;	
	imageRec->curOrder	= FSType_GEN;

	return TRUE;
}

/**************************************************************/
Boolean		Nib_Normalize(Boolean smallB, Nib_Disk *destDiskP)
{
	short				curTrack		= 0, 
						curSector		= 0,
						curByte			= 0;
	Boolean				success			= TRUE,
						doneB			= FALSE;
	Nib_ByteTrack		*srcTrackP		= NULL,
						*destTrackP		= NULL;
	Nib_AddressField	*addrFieldP		= NULL;
	Nib_DataField		*dataFieldP		= NULL;
	Nib_Disk			*sourceDiskP	= (Nib_Disk *)TrackNewPtrClear("nib disk (normalizing)", sizeof(Nib_Disk));
	
	if (sourceDiskP == NULL) {
		ReportError(IC_Err_OUT_OF_MEMORY);
		return FALSE;
	}
	
	memcpy(sourceDiskP, destDiskP, 0x00038E00);
//	*sourceDiskP = *destDiskP;
		
	for (
		curTrack = 0, 
			srcTrackP	= (Nib_ByteTrack *)sourceDiskP, 
			destTrackP	= (Nib_ByteTrack *)destDiskP;
		curTrack < Gen_kTracksPerDisk;
		curTrack++, 
			srcTrackP++,
			destTrackP++
	) {		
		curByte		= 0;
		doneB		= FALSE;
		
		do {
			addrFieldP = (Nib_AddressField *)&srcTrackP->byte[curByte];
			if (Nib_IsAddressField(addrFieldP)) {
				doneB = TRUE;
			} else do {
				curByte++;
				ASSERT(curByte < sizeof(Nib_Track));
				if (curByte >= sizeof(Nib_Track)) goto error;
			} while (srcTrackP->byte[curByte] != kNibFirstPrologueByte);
		} while (!doneB);

		//	copy 2nd half of track to beginning
		memcpy(&destTrackP->byte[0], &srcTrackP->byte[curByte], sizeof(Nib_Track) - curByte);
		
		//	copy 1st half of track to end
		memcpy(&destTrackP->byte[sizeof(Nib_Track) - curByte], &srcTrackP->byte[0], curByte);
		
		//	put it back into source
		sourceDiskP->track[curTrack] = destDiskP->track[curTrack];
		
		//	and fill dest with sync bytes
		memfill(destTrackP, 0xFF, sizeof(Nib_Track));

		curByte		= 0;
		
		for (
			curSector = 0;
			curByte < sizeof(Nib_Track) && curSector < Gen_kSectorsPerTrack;
			curSector++
		) {
			doneB		= FALSE;
	
			do {
				addrFieldP = (Nib_AddressField *)&srcTrackP->byte[curByte];
				if (Nib_IsAddressField(addrFieldP)) {
					doneB = TRUE;
				} else do {
					curByte++;
					ASSERT(curByte < sizeof(Nib_Track));
					if (curByte >= sizeof(Nib_Track)) goto error;
				} while (srcTrackP->byte[curByte] != kNibFirstPrologueByte);
			} while (!doneB);
						
			memcpy(
				&destDiskP->track[curTrack].sector[curSector].address, 
				addrFieldP, sizeof(Nib_AddressField));

//			destDiskP->track[curTrack].sector[curSector].address = *addrFieldP;

			curByte += sizeof(Nib_AddressField);
			ASSERT(curByte < sizeof(Nib_Track));
			if (curByte >= sizeof(Nib_Track)) goto error;
			
			doneB		= FALSE;

			do {
				dataFieldP = (Nib_DataField *)&srcTrackP->byte[curByte];
				if (Nib_IsDataField(dataFieldP)) {
					doneB = TRUE;
				} else do {
					curByte++;
					ASSERT(curByte < sizeof(Nib_Track));
					if (curByte >= sizeof(Nib_Track)) goto error;
				} while (srcTrackP->byte[curByte] != kNibFirstPrologueByte);
			} while (!doneB);

			memcpy(
				&destDiskP->track[curTrack].sector[curSector].data, 
				dataFieldP, sizeof(Nib_DataField));
				
//			destDiskP->track[curTrack].sector[curSector].data 
//				= *dataFieldP;

			curByte += sizeof(Nib_DataField);
			ASSERT(curByte < sizeof(Nib_Track));
			if (curByte >= sizeof(Nib_Track)) goto error;
		}
	}
	
	goto done;

	error:
	success = FALSE;
	
	done:
	TrackDisposePtr((Ptr)sourceDiskP);
	
	return success;
}

/**************************************************************/
Boolean		Nib_DeNibblize(Nib_Disk *nibDiskP, DiskImageRec *imageRec)
{
	Boolean		success = FALSE;
	
	ASSERT(sizeof(Nib_ByteTrack) == sizeof(Nib_Track));
	ASSERT(sizeof(Nib_Disk) == kNib_DiskSize);

	success = Nib_Normalize(FALSE, nibDiskP);
	
	if (success) {
		success = Nib_Decode(nibDiskP, imageRec);
	}
	
	return success;
}

void		Nib_Nibblize(DiskImageRec *imageRec, Nib_Disk *nibDiskP)
{
	short		curTrack, curSector;
	Byte		volNum;
	
	if (imageRec->osType == FSType_DOS) {
		volNum = Dos_GetVolNum(imageRec);
	} else {
		volNum = 254;
	}

	memfill(nibDiskP, 0xFF, sizeof(Nib_Disk));
	
	for (
		curTrack = 0;
		curTrack < Gen_kTracksPerDisk;
		curTrack++
	) {
		for (
			curSector = 0;
			curSector < Gen_kSectorsPerTrack;
			curSector++
		) {
			Nib_Sector		*curSectorP = 
				&nibDiskP->track[curTrack].sector[curSector];
			
			Nib_SetAddrField(&curSectorP->address, volNum, curTrack, curSector);
			
			//	copy it into the far end of the sector, and
			//	encoding it expands the data down to fill the sector
			memcpy(
				&curSectorP->data.data[Nib_kBytesPerSector - Gen_kBytesPerSector],
				&imageRec->image.gen->track[curTrack].sector[curSector], 
				sizeof(Gen_Sector));
			
//			*(Gen_Sector *)&curSectorP->data.data[Nib_kBytesPerSector - Gen_kBytesPerSector] 
//				= imageRec->image.gen->track[curTrack].sector[curSector];
			
			Encode6n2(curSectorP->data.data);

			curSectorP->data.prologue	= gNibDataPrologue;
			curSectorP->data.epilogue	= gNibEpliogue;
		}
	}
}

