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

#define Class CFDC325572
extern Class MSVC4Bug;


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

static void GCRToData(byte* pbData, const byte* pbGCR, int iLength) {

  static const byte NONE = 0;
  static const byte abData[32] = {
    NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE,
    NONE, 0x08, 0x00, 0x01, NONE, 0x0C, 0x04, 0x05,
    NONE, NONE, 0x02, 0x03, NONE, 0x0F, 0x06, 0x07,
    NONE, 0x09, 0x0A, 0x0B, NONE, 0x0D, 0x0E, NONE
  };

  while (iLength > 0) {

    int i0 = pbGCR[0] >> 3;
    int i1 = ((pbGCR[0] << 2) | (pbGCR[1] >> 6)) & 31;
    int i2 = (pbGCR[1] >> 1) & 31;
    int i3 = ((pbGCR[1] << 4) | (pbGCR[2] >> 4)) & 31;
    int i4 = ((pbGCR[2] << 1) | (pbGCR[3] >> 7)) & 31;
    int i5 = (pbGCR[3] >> 2) & 31;
    int i6 = ((pbGCR[3] << 3) | (pbGCR[4] >> 5)) & 31;
    int i7 = pbGCR[4] & 31;

    *pbData++ = (byte)((abData[i0] << 4) | abData[i1]);
    *pbData++ = (byte)((abData[i2] << 4) | abData[i3]);
    *pbData++ = (byte)((abData[i4] << 4) | abData[i5]);
    *pbData++ = (byte)((abData[i6] << 4) | abData[i7]);

    pbGCR += 5;
    iLength -= 5;
  }

  assert(iLength == 0);
}


////////////////////////////////////////////////////////////////////////////////
// searches for the end of the next SYNC (2 * FF)
byte* CFDC325572::FindEndOfNextSync(byte* pbGCR) {
  if (pbGCR >= pbGCREnd) {
    pbGCR = pbGCRStart;
  }
  flag fHasWrapped = false;
  for (;;) {
    pbGCR = (byte*)memchr(pbGCR, 0xFF, pbGCREnd - pbGCR);
    if (pbGCR == NULL) {
      if (fHasWrapped) {
        return NULL;
      }
      pbGCR = pbGCRStart;
      fHasWrapped = true;
    } else {
      pbGCR++;
      if (pbGCR >= pbGCREnd) {
        pbGCR = pbGCRStart;
      }
      int iMax = pbGCREnd - pbGCRStart;
      if (*pbGCR == 0xFF) {
        do {
          pbGCR++;
          if (pbGCR >= pbGCREnd) {
            pbGCR = pbGCRStart;
          }
          if (iMax-- == 0) {
            return NULL; // Killertrack
          }
        } while (*pbGCR == 0xFF);
        return pbGCR;
      }
    }
  }
}


////////////////////////////////////////////////////////////////////////////////
// converts track from GCR to D64 data and writes it

void CFDC325572::WriteTrack() {

  fDirty = false;
  if (hFile == NULL) {
    return;
  }

  // get number of sectors and offset into D64 file
  int iOffset;
  int iSectors;
  if (iTrack < 18) {
    iOffset = ((iTrack - 1) * 21) * 256;
    iSectors = 21;
  } else if (iTrack < 25) {
    iOffset = ((18 - 1) * 21 + (iTrack - 18) * 19) * 256;
    iSectors = 19;
  } else if (iTrack < 31) {
    iOffset = ((18 - 1) * 21 + (25 - 18) * 19 + (iTrack - 25) * 18) * 256;
    iSectors = 18;
  } else {
    iOffset = ((18 - 1) * 21 + (25 - 18) * 19 + (31 - 25) * 18 + (iTrack - 31) * 17) * 256;
    iSectors = 17;
  }

  // allocate memory for D64 track
  byte* pbBuffer = MemAlloc(iSectors * 256);
  try {

    // initialize D64 track with 4B 01 01...
    memset(pbBuffer, 0x01, iSectors * 256);
    for (int i = 0; i < iSectors; i++) {
      pbBuffer[i * 256] = 0x4B;
    }

    byte* pbGCR = pbGCRStart;
    byte* pbFirstSync = NULL;
    for (;;) {

      // search SYNC
      pbGCR = FindEndOfNextSync(pbGCR);

      // Killertrack
      if (pbGCR == NULL) {
        break;
      }

      // only one round
      if (pbGCR == pbFirstSync) {
        break;
      }
      if (pbFirstSync == NULL) {
        pbFirstSync = pbGCR;
      }

      // check for header
      if (*pbGCR == 0x52) { // GCR for 08

        // read header
        verify(pbGCREnd - pbGCR >= 10);
        byte abHeader[8];
        GCRToData(abHeader, pbGCR, 10);
        pbGCR += 10;
        if (abHeader[2] < iSectors) {

          // read data
          pbGCR = FindEndOfNextSync(pbGCR);
          if (pbGCR == NULL) {
            break;
          }
          if (*pbGCR == 0x55) { // GCR for 07
            verify(pbGCREnd - pbGCR >= 325);
            byte abData[260];
            GCRToData(abData, pbGCR, 325);
            pbGCR += 325;
            memcpy(pbBuffer + abHeader[2] * 256, abData + 1, 256);
          }

        }
      }
    }

    // write track into file
    try {
      SetFilePointer(hFile, iOffset, NULL, FILE_BEGIN);
      WriteFile(hFile, pbBuffer, iSectors * 256);
    } catch (...) {
    }

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


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

static byte* DataToGCR(byte* pbGCR, const byte* pbData, int iLength) {

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

  while (iLength > 0) {
    
    int i0 = abGCR[pbData[0] >> 4];
    int i1 = abGCR[pbData[0] & 15];
    int i2 = abGCR[pbData[1] >> 4];
    int i3 = abGCR[pbData[1] & 15];
    int i4 = abGCR[pbData[2] >> 4];
    int i5 = abGCR[pbData[2] & 15];
    int i6 = abGCR[pbData[3] >> 4];
    int i7 = abGCR[pbData[3] & 15];

    *pbGCR++ = (byte)((i0 << 3) | (i1 >> 2));
    *pbGCR++ = (byte)((i1 << 6) | (i2 << 1) | (i3 >> 4));
    *pbGCR++ = (byte)((i3 << 4) | (i4 >> 1));
    *pbGCR++ = (byte)((i4 << 7) | (i5 << 2) | (i6 >> 3));
    *pbGCR++ = (byte)((i6 << 5) | i7);

    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] = 0x08;
  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 (i = 0; i < 9; i++) {
    *pbGCR++ = 0x55;
  }

  // 5 SYNCs
  for (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 (i = 0; i < 256; i++) {
    bChecksum ^= pbData[i];
  }
  abData[257] = bChecksum;
  abData[258] = 0;
  abData[259] = 0;
  pbGCR = DataToGCR(pbGCR, abData, sizeof abData);

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

  return pbGCR;
}


////////////////////////////////////////////////////////////////////////////////
// reads track and converts it to GCR

void CFDC325572::ReadTrack() {

  if (hFile == NULL) {
    return;
  }

  if (pbGCRStart != NULL) {
    MemFree(pbGCRStart);
    pbGCRStart = NULL;
    pbGCREnd = NULL;
    pbData = NULL;
  }

  int iOffset;
  int iSectors;
  if (iTrack < 18) {
    iOffset = ((iTrack - 1) * 21) * 256;
    iSectors = 21;
  } else if (iTrack < 25) {
    iOffset = ((18 - 1) * 21 + (iTrack - 18) * 19) * 256;
    iSectors = 19;
  } else if (iTrack < 31) {
    iOffset = ((18 - 1) * 21 + (25 - 18) * 19 + (iTrack - 25) * 18) * 256;
    iSectors = 18;
  } else {
    iOffset = ((18 - 1) * 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;
  }
}


////////////////////////////////////////////////////////////////////////////////
// head positioning

void CFDC325572::OnStep(byte bState, byte bChanges) {
  __asm {
  push EBX
  push ThisReg
  mov ThisReg,this
  mov AL,bState
  mov AH,bChanges

  test AH,00000011b
  je NoStep
  push EAX

  // 2 or 3 -> 0 = 2up, 0 -> 3 = 2down
  mov AH,mvar(bStep)
  and EAX,0x0303
  mov mvar(bStep),AL
  mov EBX,mvar(iTrack)
  inc EBX
  cmp EAX,0x0200
  je Set
  cmp EAX,0x0300
  je Set
  sub EBX,4
  cmp EAX,0x0003
  je Set
  add EBX,2
Set:

  // get bit 1 from current state
  shr AL,1
  and BL,11111110b
  or BL,AL

  // range between 1 and 35
  inc EBX
  mov EAX,1
  cmp EBX,EAX
  jle Limit
  mov EAX,35
  cmp EBX,EAX
  jg Limit
  mov EAX,EBX
Limit:

  // set new track
  cmp EAX,mvar(iTrack)
  je SameTrack
  mov mvar(iTrack),EAX
  }
  static CFDC325572* p;
  __asm mov p,ThisReg
  p->ReadTrack();
  __asm {
SameTrack:

  pop EAX
NoStep:

  pop ThisReg
  pop EBX
  }
}


////////////////////////////////////////////////////////////////////////////////
// set speed

proc(OnClocks)

  mov mvar(iClocks),EAX

  EndMsg(mvar(Clocks))
endp


////////////////////////////////////////////////////////////////////////////////
// read next GCR byte

void CFDC325572::OnReadNextByte() {
  __asm {
  push EBX
  push ThisReg
  mov ThisReg,ECX

  // read byte
  mov EBX,mvar(pbData)
  and EBX,EBX
  je Return
  mov AL,[EBX]

  // set pointer to next byte
  inc EBX
  cmp EBX,mvar(pbGCREnd)
  jb NotAtEnd
  mov EBX,mvar(pbGCRStart)
NotAtEnd:
  mov mvar(pbData),EBX

  // set SYNC if two bytes $FF (a single $FF can be part of data)
  mov AH,mvar(bLastByte)
  mov mvar(bLastByte),AL
  cmp AX,0xFFFF
  jne SetData

  SetLow(mvar(Sync), 1)
  jmp Return

SetData:

  // output byte if not SYNC
  SetPort(mvar(Data), 1)
  SetHigh(mvar(Sync), 1)

  // signal byte ready
  SetLow(mvar(ByteReady), 2)
  SetHigh(mvar(ByteReady), 2)

Return:

  pop ThisReg
  pop EBX
  }

  // call me again in iClocks cycles
  NextByte.StartCounter(iClocks);

}


////////////////////////////////////////////////////////////////////////////////
// write next GCR byte

void CFDC325572::OnWriteNextByte() {
  __asm {
  push EBX
  push ThisReg
  mov ThisReg,ECX

  // write byte
  mov EBX,mvar(pbData)
  and EBX,EBX
  je Return
  GetPort(mvar(Data))
  mov [EBX],AL

  // set pointer to next byte
  inc EBX
  cmp EBX,mvar(pbGCREnd)
  jb NotAtEnd
  mov EBX,mvar(pbGCRStart)
NotAtEnd:
  mov mvar(pbData),EBX

  // signal byte ready
  SetLow(mvar(ByteReady), 2)
  SetHigh(mvar(ByteReady), 2)

Return:

  pop ThisReg
  pop EBX
  }

  // call me again in iClocks cycles
  NextByte.StartCounter(iClocks);

}


////////////////////////////////////////////////////////////////////////////////
// switch between read (high) and write (low)

proc(OnOEHigh)
  } 
  static CFDC325572* p;
  __asm mov p,ECX
  p->WriteTrack();
  p->NextByte.SetOnFire((pfn)p->OnReadNextByte);
  __asm {
  ret
endp


proc(OnOELow)
  push EBX
  push ThisReg
  mov ThisReg,ECX

  // set Data port to input
  mov AL,0xFF
  SetPort(mvar(Data), 1)

  } 
  static CFDC325572* p;
  __asm mov p,ThisReg
  p->NextByte.SetOnFire((pfn)p->OnWriteNextByte);
  __asm {

  mov mvar(fDirty),1

  pop ThisReg
  pop EBX
  ret
endp


////////////////////////////////////////////////////////////////////////////////
// switch motor on/off

static void AddEvent() {
  static Timer* p;
  static int i;
  __asm mov p,EAX
  __asm mov i,EBX
  p->StartCounter(i);
  __asm xor EAX,EAX
}
static void RemoveEvent() {
  Timer* p;
  __asm mov p,EAX
  p->StopCounter();
}


proc(OnMTRHigh)
  push ThisReg
  mov ThisReg,ECX
  push EBX

  lea EAX,mvar(NextByte)
  mov EBX,mvar(iClocks)
  call AddEvent

  pop EBX
  pop ThisReg
  ret
endp


proc(OnMTRLow)
  push EBX
  push ThisReg
  mov ThisReg,ECX

  lea EAX,mvar(NextByte)
  call RemoveEvent

  pop ThisReg
  pop EBX
  ret
endp


////////////////////////////////////////////////////////////////////////////////
// change disk

CString CFDC325572::GetDisk() {
  return Disk;
}

void CFDC325572::SetDisk(const CString NewDisk) {
  
  if (hFile > 0) {
    CloseHandle(hFile);
    hFile = NULL;
  }

  if (pbGCRStart != NULL) {
    MemFree(pbGCRStart);
    pbGCRStart = NULL;
    pbGCREnd = NULL;
    pbData = NULL;
  }

  Disk = NewDisk;
  fReadOnly = FALSE;
  
  if (!NewDisk.IsEmpty()) {

    hFile = OpenFile(NewDisk);

    // check if file is read only
    if (_access(NewDisk, 02) != 0) {
      fReadOnly = TRUE;
    }

    // read disk ID
    byte abBuffer[256];
    SetFilePointer(hFile, (18 - 1) * 21 * 256, NULL, FILE_BEGIN);
    ReadFile(hFile, abBuffer, 256);
    bID1 = abBuffer[162];
    bID2 = abBuffer[163];

    ReadTrack();
  }
}


////////////////////////////////////////////////////////////////////////////////
// constructor

/*
  CLine MTR;            // high = motor on
  CPort Step;           // binary increment/decrement to move head
  int iTrack;           // current track from 1 to 35+
  byte bStep;           // bits 0 and 1 from stepper motor
  byte abStepFill[3];

  CString Disk;         // current disk image or ""
  HANDLE hFile;
  flag fReadOnly;

  byte* pbGCRStart;     // buffer with GCR data, encoded on the fly
  byte* pbData;         // read/write pointer into GCR
  byte* pbGCREnd;       // for pbData wrap around
  Timer NextByte;       // function to increment pbData for next byte
  CMsg Clocks;          // system clocks between bytes 26/28/30/32
  int iClocks;
  flag fDirty;          // new data has been written to current track

  CLine OE;             // output enable, high = read, low = write
  CLine SOE;            // default = high, what the hell is this?
  CLine ByteReady;      // low for one cycle = byte has been read/written
  CLine Sync;           // low = GCR data $FF occured

  CPort Data;           // 
  byte bID1;            // ID from D64 images for GCR header
  byte bID2;
  byte abIDFill[2];
*/

CFDC325572::CFDC325572() {

  MTR.Init("MTR", this);
  MTR.SetOnHigh(make_pfn(fn(OnMTRHigh)));
  MTR.SetOnLow(make_pfn(fn(OnMTRLow)));
  Step.Init("Step", this);
  Step.SetOnChange((pfnbb)OnStep);
  iTrack = 18;
  bStep = 0;

  hFile = NULL;
  fReadOnly = FALSE;

  pbGCRStart = NULL;
  pbData = NULL;
  pbGCREnd = NULL;
  // motor initially on
  Clocks.pObject = this;
  Clocks.pfnOnMsg = fn(OnClocks);
  iClocks = 26;
  fDirty = FALSE;

  NextByte.Init("NextByte", this);
  NextByte.SetOnFire((pfn)OnReadNextByte);
  NextByte.StartCounter(iClocks);

  OE.Init("OE", this);
  OE.SetOnHigh(make_pfn(fn(OnOEHigh)));
  OE.SetOnLow(make_pfn(fn(OnOELow)));
  SOE.Init("SOE", this);
  ByteReady.Init("ByteReady", this);
  Sync.Init("Sync", this);

  Data.Init("Data", this);
  bID1 = 0;
  bID2 = 0;
  bLastByte = 0;

}


////////////////////////////////////////////////////////////////////////////////
// destructor

CFDC325572::~CFDC325572() {

  SetDisk("");

}
