#include <EDK.h>
#include "General.h"
#include "CFDC325572.h"

#define Class CFDC325572


////////////////////////////////////////////////////////////////////////////////
// converts data to GCR, iLength must be dividable by 4

static byte* DataToGCR(byte* pbCGR, byte* pbData, int iLength) {

  static byte abGCR[16] = {
    0x0A, 0x0B, 0x12, 0x13, 0x0E, 0x0F, 0x16, 0x17,
    0x09, 0x19, 0x1A, 0x1B, 0x0D, 0x1D, 0x1E, 0x15
  };

  while (iLength > 0) {
    
    byte b0 = abGCR[pbData[0] >> 4];
    byte b1 = abGCR[pbData[0] & 15];
    byte b2 = abGCR[pbData[1] >> 4];
    byte b3 = abGCR[pbData[1] & 15];
    byte b4 = abGCR[pbData[2] >> 4];
    byte b5 = abGCR[pbData[2] & 15];
    byte b6 = abGCR[pbData[3] >> 4];
    byte b7 = abGCR[pbData[3] & 15];

    *pbGCR++ = (byte)((b0 << 3) | (b1 >> 2));
    *pbGCR++ = (byte)((b1 << 6) | (b2 << 1) | (b3 >> 4));
    *pbGCR++ = (byte)((b3 << 4) | (b4 >> 1));
    *pbGCR++ = (byte)((b4 << 7) | (b5 << 2) | (b6 >> 3));
    *pbGCR++ = (byte)((b6 << 5) | b7);

    pbData += 4;
    iLength -= 4;
  }

  ASSERT(iLength == 0);

  return pbGCR;
}


////////////////////////////////////////////////////////////////////////////////
// converts sector to GCR

byte* CFDC325572::SectorToGCR(byte* pbGCR, byte* pbData, int iSector) {

  // 5 SYNCs
  for (int i = 0; i < 5; i++) {
    *pbGCR++ = 0xFF;
  }

  // header = $08, checksum, sector, track, ID1, ID2, $0F, $0F
  byte abHeader[8];
  abHeader[0] = $08;
  abHeader[2] = (byte)iSector;
  abHeader[3] = (byte)iTrack;
  abHeader[4] = bID1;
  abHeader[5] = bID2;
  abHeader[6] = 0x0F;
  abHeader[7] = 0x0F;
  abHeader[1] = (byte)(abHeader[2] ^ abHeader[3] ^ abHeader[4] ^ abHeader[5]);
  pbGCR = DataToGCR(pbGCR, abHeader, sizeof abHeader);

  // GAP1
  for (int i = 0; i < 9; i++) {
    *pbGCR++ = 0x55;
  }

  // 5 SYNCs
  for (int i = 0; i < 5; i++) {
    *pbGCR++ = 0xFF;
  }

  // data = $07, 256 data, checksum, 0, 0
  byte abData[260];
  abData[0] = 0x07;
  memcpy(abData + 1, pbData, 256);
  byte bChecksum = 0;
  for (int i = 0; i < 256; i++) {
    bChecksum ^= pbData[i];
  }
  abData[257] = bChecksum;
  abData[258] = 0;
  abData[259] = 0;
  pbGCR = DataToGCR(pbGCR, abData, sizeof abHeader);

  // GAP2 makes 360 GCR bytes total, depends on measurement in reality
  for (int i = 0; i < 6; i++) {
    *pbGCR++ = 0x55;
  }

  return pbGCR;
}


////////////////////////////////////////////////////////////////////////////////
// converts track to GCR

byte* CFDC325572::TrackToGCR() {

  int iOffset;
  int iSectors;
  if (iTrack < 18) {
    iOffset = ((iTrack - 1) * 21) * 256;
    iSectors = 21;
  } else if (iTrack < 25) {
    iOffset = (18 * 21 + (iTrack - 18) * 19) * 256;
    iSectors = 19;
  } else if (iTrack < 31) {
    iOffset = (18 * 21 + (25 - 18) * 19 + (iTrack - 25) * 18) * 256;
    iSectors = 18;
  } else {
    iOffset = (18 * 21 + (25 - 18) * 19 + (31 - 25) * 18 + (iTrack - 31) * 17) * 256;
    iSectors = 17;
  }

  byte* pbBuffer = MemAlloc(iSectors * 256);
  try {
    SetFilePointer(hFile, iOffset, NULL, FILE_BEGIN);
    ReadFile(hFile, pbBuffer, iSectors * 256);

    byte* pbGCR = MemAlloc(iSectors * 360);

    byte* pb = pbGCR;
    int iSector = 0;
    for (int i = 0; i < iSectors; i++) {
      pb = SectorToGCR(pb, pbBuffer + iSector * 256, iSector);
      iSector += 10;
      if (iSector >= iSectors) {
        iSector -= iSectors;
        ASSERT(iSectors == 17 | iSectors == 18 | iSectors == 19 | iSectors == 21);
        if (iSector == 0) {
          iSector = 1;
        }
      }
    }
    ASSERT(pb == pbGCR + iSectors * 360);

    if (pbGCRStart != NULL) {
      pbData = pbData - pbGCRStart + pbGCR;
      if (pbData >= pb) {
         pbData = pbGCR;
      }
      MemFree(pbGCRStart);
    } else {
      pbData = pbGCR;
    }
    pbGCRStart = pbGCR;
    pbGCREnd = pb;

    MemFree(pbBuffer);

  } catch (...) {
    MemFree(pbBuffer);
    throw;
  }

}
