#include "drive.h"

#include <stdlib.h>

const uint8_t SECTORS_PER_TRACK[40] = {
	21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21,
	19, 19, 19, 19, 19, 19, 19,
	18, 18, 18, 18, 18, 18,
	17, 17, 17, 17, 17, 17, 17, 17, 17, 17
};

const uint8_t BITRATE_PER_TRACK[40] = {
	76, 76, 76, 76, 76, 76, 76, 76, 76, 76, 76, 76, 76, 76, 76, 76, 76,
	68, 68, 68, 68, 68, 68, 68,
	65, 65, 65, 65, 65, 65,
	61 ,61, 61, 61, 61, 61, 61, 61, 61, 61
};

const uint8_t GCR_CONV[16] = {
    0x0a, 0x0b, 0x12, 0x13,
    0x0e, 0x0f, 0x16, 0x17,
    0x09, 0x19, 0x1a, 0x1b,
    0x0d, 0x1d, 0x1e, 0x15
};

void drive_init(drive_t *drive) {
	drive->spinning = false;
	drive->timer = 0;
	drive->image = NULL;

	drive->track = 17;
	drive->halftrack = 0;
	drive->image_ptr = 0x16500;
	drive->sector = 0;
	drive->byte = 0;

	drive->gcr_word = 0;
	drive->gcr_bits = 0;
	drive->bit_counter = 0;

	drive->gap_remain = 0;
	drive->sync_remain = 4;
	drive->header_remain = 8;
	drive->data_remain = 0;
	drive->data_checksum = 0;

	drive->current_idx = 0;
	drive->lookahead_idx = 0;

	drive_cycle(drive, 0, false);
}

void drive_step_up(drive_t *drive) {
	drive->halftrack = (drive->halftrack + 1) & 1;
	if (drive->track < 39 && drive->halftrack == 0) {
		drive->image_ptr += SECTORS_PER_TRACK[drive->track] << 8;
		drive->track += 1;
		drive->sector = drive->sector % SECTORS_PER_TRACK[drive->track];
	}
}

void drive_step_down(drive_t *drive) {
	drive->halftrack = (drive->halftrack + 1) & 1;
	if (drive->track > 0 && drive->halftrack == 0) {
		drive->image_ptr -= SECTORS_PER_TRACK[drive->track - 1] << 8;
		drive->track -= 1;
		drive->sector = drive->sector % SECTORS_PER_TRACK[drive->track];
	}
}

bool drive_cycle(drive_t *drive, uint8_t cycles, bool advance) {
	bool byte_ready = false;
	if (advance) {
		for (uint8_t i = 0; i < cycles; i++) {
			uint8_t j = (i + drive->current_idx) & (DRIVE_LOOKAHEAD - 1);
			byte_ready |= drive->byte_ready[j];
		}
		drive->current_idx = (drive->current_idx + cycles) & (DRIVE_LOOKAHEAD - 1);
	}

	while (drive->lookahead_idx != ((drive->current_idx - 1) & (DRIVE_LOOKAHEAD - 1))) {
		uint8_t j = (drive->lookahead_idx + 1) & (DRIVE_LOOKAHEAD - 1);
		uint8_t timer_next = drive->timer + BITRATE_PER_TRACK[drive->track];

		uint16_t prev = drive->unlatched[drive->lookahead_idx];
		if (timer_next < drive->timer) {
			if (drive->gcr_bits == 0) {
				/*
				if (drive->spinning && drive->image != NULL) {
					if (!drive->sync_flag && (drive->byte == 0 || drive->byte == 2)) {
						drive->gcr_word = 0x3ff;
						drive->sync_flag = true;
					} else {
						uint8_t b = drive->image[drive->image_ptr + (drive->sector << 8) + drive->byte];
						drive->gcr_word = (GCR_CONV[(b & 0xf0) >> 4] << 5) | GCR_CONV[b & 0x0f];
						drive->sync_flag = false;

						uint8_t byte_next = drive->byte + 1;
						uint8_t sector_next = drive->sector;
						if (byte_next == 0) {
							sector_next += 1;
						}
						if (sector_next >= SECTORS_PER_TRACK[drive->track]) {
							sector_next = 0;
						}
						drive->byte = byte_next;
						drive->sector = sector_next;
					}
				}

				drive->gcr_bits = 10;
				*/
				if (drive->image != NULL && drive->spinning) {
					if (drive->gap_remain > 0) {
						drive->gap_remain--;
						drive->gcr_word = 0x55;
						drive->gcr_bits = 8;
					} else if (drive->sync_remain > 0) {
						drive->sync_remain--;
						drive->gcr_word = 0x3ff;
						drive->gcr_bits = 10;
					} else if (drive->header_remain > 0) {
						drive->header_remain--;
						uint8_t track_plus_one = drive->track + 1;
						uint8_t checksum = drive->sector ^ track_plus_one;
						switch (drive->header_remain) {
						case 7:
							drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0x8];
							break;
						case 6:
							drive->gcr_word = (GCR_CONV[(checksum & 0xf0) >> 4] << 5) | GCR_CONV[checksum & 0x0f];
							break;
						case 5:
							drive->gcr_word = (GCR_CONV[(drive->sector & 0xf0) >> 4] << 5) | GCR_CONV[drive->sector & 0x0f];
							break;
						case 4:
							drive->gcr_word = (GCR_CONV[(track_plus_one & 0xf0) >> 4] << 5) | GCR_CONV[track_plus_one & 0x0f];
							break;
						case 3:
						case 2:
							drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0x0];
							break;
						case 1:
						case 0:
							drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0xf];
							break;
						}
						if (drive->header_remain == 0) {
							drive->gap_remain = 9;
							drive->sync_remain = 4;
							drive->data_remain = 260;
							drive->data_checksum = 0;
						}
						drive->gcr_bits = 10;
					} else if (drive->data_remain > 0) {
						drive->data_remain--;
						switch (drive->data_remain) {
						case 259:
							drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0x7];
							break;
						case 2:
							drive->gcr_word = (GCR_CONV[(drive->data_checksum & 0xf0) >> 4] << 5) | GCR_CONV[drive->data_checksum & 0x0f];
							break;
						case 1:
						case 0:
							drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0xf];
							break;
						default:
							{
								uint8_t b = drive->image[drive->image_ptr + (drive->sector << 8) + drive->byte];
								drive->gcr_word = (GCR_CONV[(b & 0xf0) >> 4] << 5) | GCR_CONV[b & 0x0f];
								drive->data_checksum ^= b;	

								uint8_t byte_next = drive->byte + 1;
								uint8_t sector_next = drive->sector;
								if (byte_next == 0) {
									sector_next += 1;
								}
								if (sector_next >= SECTORS_PER_TRACK[drive->track]) {
									sector_next = 0;
								}
								drive->byte = byte_next;
								drive->sector = sector_next;
							}
							break;
						}
						if (drive->data_remain == 0) {
							drive->gap_remain = 10;
							drive->sync_remain = 4;
							drive->header_remain = 8;
						}
						drive->gcr_bits = 10;
					}
				}
			}

			drive->unlatched[j] = ((prev << 1) | ((drive->gcr_word & 0x200) >> 9)) & 0x3ff;
			drive->gcr_word <<= 1;
			drive->gcr_bits--;
			drive->bit_counter += 1;
		} else {
			drive->unlatched[j] = prev;
		}

		drive->byte_ready[j] = false;
		drive->latched[j] = drive->latched[drive->lookahead_idx];
		if ((drive->unlatched[j] & 0x3ff) == 0x3ff) {
			drive->bit_counter = 0;
			drive->sync[j] = true;
		} else {
			if (drive->bit_counter == 8) {
				drive->bit_counter = 0;
				drive->byte_ready[j] = true;
				drive->latched[j] = drive->unlatched[j] & 0xff;
			}
			drive->sync[j] = false;
		}

		drive->lookahead_idx = j;
		drive->timer = timer_next;
	}

	return byte_ready;
}