#include <pci/hd.h>
#include <sys/video.h>
#include <sys/drives.h>
#include <sys/io.h>
#include <lib/kmalloc.h>

struct driveInfo *hdd0;
struct driveInfo *hdd1;
struct driveInfo *hdd2;
struct driveInfo *hdd3;
  
void initHardDisk() {
  hdd0 = (struct driveInfo *)kmalloc(sizeof(struct driveInfo),-2);
  hdd1 = (struct driveInfo *)kmalloc(sizeof(struct driveInfo),-2);
  hdd2 = (struct driveInfo *)kmalloc(sizeof(struct driveInfo),-2);
  hdd3 = (struct driveInfo *)kmalloc(sizeof(struct driveInfo),-2);
  hdd0->hdPort = 0x1F7;
  hdd0->hdDev  = 0x40;
  hdd1->hdPort = 0x1F7;
  hdd1->hdDev  = 0x50;
  hdd2->hdPort = 0x177;
  hdd2->hdDev  = 0x40;
  hdd3->hdPort = 0x177;
  hdd3->hdDev  = 0x50;
  if (!initDrive(hdd0)) {
    addDrive(1,1,hdd0,hdRead,hdWrite,0x0);
    }
  if (!initDrive(hdd1)) {
    addDrive(2,1,hdd1,hdRead,hdWrite,0x0);
    }
  if (!initDrive(hdd2)) {
    addDrive(3,1,hdd2,hdRead,hdWrite,0x0);
    }
  if (!initDrive(hdd3)) {
    addDrive(4,1,hdd3,hdRead,hdWrite,0x0);
    }            
  return;
  }
  
int initDrive(struct driveInfo *hdd) {
  char retVal  = 0x0;
  long counter = 0x0;
  short *tmp    = 0x0;
  for (counter = 1000000;counter >= 0;counter--) {
    retVal = inportByte(hdd->hdPort) & 0x80;
    if (!retVal) goto ready;
    }
  kprintf("Error Initializing Drive\n");
  return(1);
  ready:
  hdd->hdPort--;
  outportByte(hdd->hdPort,hdd->hdDev);
  hdd->hdPort++;
  outportByte(hdd->hdPort,0xEC);
  for (counter = 1000000;counter >= 0;counter--) {
    retVal = inportByte(hdd->hdPort);
    if ((retVal & 1) != 0x0) {
      kprintf("Error Drive Not Available\n");
      return(1);
      }
    if ((retVal & 8) != 0x0) {
      goto go;
      }
    }
  kprintf("Time Out Waiting On Drive\n");
  return(1);
  go:
  hdd->hdPort -= 7;
  tmp = (short *)hdd->hdSector;
  for (counter = 0;counter < (512/2);counter++) {
    tmp[counter] = inportWord(hdd->hdPort);
    }
  retVal = hdd->hdSector[0x5E] & 127;
  switch (retVal) {
    case 0:
      hdd->hdShift = 3;
      hdd->hdMulti = 8;
    case 2:
      hdd->hdShift = 1;
      hdd->hdMulti = retVal;
      break;
    case 4:
      hdd->hdShift = 2;
      hdd->hdMulti = retVal;
      break;
    case 8:
      hdd->hdShift = 3;
      hdd->hdMulti = retVal;
      break;
    case 16:
      hdd->hdShift = 4;
      hdd->hdMulti = retVal;
      break;
    case 32:
      hdd->hdShift = 5;
      hdd->hdMulti = retVal;
      break;
    case 64:
      hdd->hdShift = 6;
      hdd->hdMulti = retVal;
      break;
    default:
      kprintf("Error BLOCK Mode Unavailable: [%i]\n",retVal);
      return(1);
    }
  hdd->hdPort += 2;
  outportByte(hdd->hdPort,retVal);
  hdd->hdPort += 4;
  outportByte(hdd->hdPort,hdd->hdDev);
  hdd->hdPort++;
  outportByte(hdd->hdPort,0xC6);
  //retVal--; //I'm Not Sure Why He Was Doing This
  hdd->hdMask = retVal;
  hdd->hdSize = (hdd->hdSector[0x7B] * 256 * 256 * 256) + (hdd->hdSector[0x7A] * 256 * 256) + (hdd->hdSector[0x79] * 256) + hdd->hdSector[0x78];
  hdd->hdEnable = 1;
  kprintf("Drive: [0x%X/0x%X], Size: [%iSectors/%iKBytes]\n",hdd->hdPort,hdd->hdDev,hdd->hdSize,((hdd->hdSize*512)/1024));
  return(0);
  }

void hdWrite(struct driveInfo *hdd,long startSector,long sectorCount,void *baseAddr) {
  long counter           = 0x0;
  long retVal            = 0x0;
  short transactionCount = 0x0;
  short *tmp             = (short *)baseAddr;
  if (hdd->hdEnable == 0x0) {
    kprintf("Invalid Drive\n");
    return;
    }
  if ((sectorCount >> hdd->hdShift) == 0x0) {
    hdd->hdCalc = sectorCount; //hdd->hdMask;
    transactionCount = 1;
    }
  else {
    hdd->hdCalc = hdd->hdMulti;
    transactionCount = sectorCount >> hdd->hdShift;
    }
  for (;transactionCount > 0;transactionCount--) {
    for (counter = 1000000;counter >= 0;counter--) {
      retVal = inportByte(hdd->hdPort) & 0x80;
      if (!retVal) goto ready;
      }
    kprintf("Time Out Waiting On Drive\n");
    return;
    ready:
    hdd->hdPort -= 5;
    outportByte(hdd->hdPort,hdd->hdCalc);
    hdd->hdPort++;
    outportByte(hdd->hdPort,(startSector & 0xFF));
    hdd->hdPort++;
    retVal = startSector >> 8;
    outportByte(hdd->hdPort,(retVal & 0xFF));
    hdd->hdPort++;
    retVal >>= 8;
    outportByte(hdd->hdPort,(retVal & 0xFF));
    hdd->hdPort++;
    retVal >>= 8;
    retVal &= 0x0F;
    retVal |= hdd->hdDev;
    outportByte(hdd->hdPort,(retVal & 0xFF));
    hdd->hdPort++;
    outportByte(hdd->hdPort,0xC5);
    for (counter = 1000000;counter >= 0;counter--) {
      retVal = inportByte(hdd->hdPort);
      if ((retVal & 1) != 0x0) {
        kprintf("HD Write Error\n");
        return;
        }
      if ((retVal & 8) != 0x0) {
        goto go;
        }
      }
    kprintf("Time Out Waiting On Drive\n");
    return;
    go:
    hdd->hdPort -= 7;
    for (counter = 0;counter < (hdd->hdCalc << 8);counter++) {
      outportWord(hdd->hdPort,tmp[counter]);
      }
    hdd->hdPort += 7;
    startSector += hdd->hdCalc;
    }
  return;
  }

void hdRead(struct driveInfo *hdd,long startSector,long sectorCount,void *baseAddr) {
  long counter           = 0x0;
  long retVal            = 0x0;
  short transactionCount = 0x0;
  short *tmp             = (short *)baseAddr;
  kprintf("Moo\n");
  if (hdd->hdEnable == 0x0) {
    kprintf("Invalid Drive\n");
    return;
    }
  if ((sectorCount >> hdd->hdShift) == 0x0) {
    hdd->hdCalc = sectorCount;//(hdd->hdMask);
    transactionCount = 1;
    }
  else {
    hdd->hdCalc = hdd->hdMulti;
    transactionCount = sectorCount >> hdd->hdShift;
    }
  for (;transactionCount > 0;transactionCount--) {
    for (counter = 1000000;counter >= 0;counter--) {
      retVal = inportByte(hdd->hdPort) & 0x80;
      if (!retVal) goto ready;
      }
    kprintf("Time Out Waiting On Drive\n");
    return;
    ready:
    hdd->hdPort -= 5;
    outportByte(hdd->hdPort,hdd->hdCalc);
    hdd->hdPort++;
    outportByte(hdd->hdPort,(startSector & 0xFF));
    hdd->hdPort++;
    retVal = startSector >> 8;
    outportByte(hdd->hdPort,(retVal & 0xFF));
    hdd->hdPort++;
    retVal >>= 8;
    outportByte(hdd->hdPort,(retVal & 0xFF));
    hdd->hdPort++;
    retVal >>= 8;
    retVal &= 0x0F;
    retVal |= hdd->hdDev;
    outportByte(hdd->hdPort,(retVal & 0xFF));
    hdd->hdPort++;
    outportByte(hdd->hdPort,0xC4);
    for (counter = 1000000;counter >= 0;counter--) {
      retVal = inportByte(hdd->hdPort);
      if ((retVal & 1) != 0x0) {
        kprintf("HD Write Error\n");
        return;
        }
      if ((retVal & 8) != 0x0) {
        goto go;
        }
      }
    kprintf("Time Out Waiting On Drive\n");
    return;
    go:
    hdd->hdPort -= 7;
    for (counter = 0;counter < (hdd->hdCalc << 8);counter++) {
      tmp[counter] = inportWord(hdd->hdPort);
      }
    hdd->hdPort += 7;
    startSector += hdd->hdCalc;
    }
  return;
  }