using System;
using System.Collections.Generic;

namespace Cosmos.Hardware.Storage.ATA2 {
	public class ATA: BlockDevice {
        //private static readonly ushort[] mControllerAddresses1 = new ushort[] { 
        //    0x1F0,
        //    0x170
        //};
        //private static readonly ushort[] mControllerAddresses2 = new ushort[] { 
        //0x3F0,
        //    0x370
        //};
        private static ushort GetControllerAddress1(int aIndex) {
            if (aIndex == 0) {
                return 0x1F0;
            } else {
                return 0x170;
            }
        }

        private static ushort GetControllerAddress2(int aIndex) {
            if (aIndex == 0) {
                return 0x3F0;
            } else {
                return 0x370;
            }
        }

        private static int GetControllerAddressCount() {
            return 2;
        }
        private static string[] mControllerNumbers = new string[] { "First", "Second", "Third", "Fourth" };
        private static string[] mControllerChannels = new string[] { "Primary", "Secondary" };
        private static string[] mDriveNames = new string[] { "Master", "Slave" };
        private const byte ATA_DELAY_DEFAULT = 5;
        private const byte ATA_DATA = 0;
        private const byte ATA_ERROR = 1;
        private const byte ATA_SECTORCOUNT = 2;
        private const byte ATA_SECTORNUMBER = 3;
        private const byte ATA_CYLINDERLOW = 4;
        private const byte ATA_CYLINDERHIGH = 5;
        private const byte ATA_DRIVEHEAD = 6;
        private const byte ATA_STATUS = 7;
        private const byte ATA_COMMAND = 8;

        private const byte IDE_STATUSREG_ERR = 0x00000001;
        private const byte IDE_STATUSREG_IDX = 0x00000002;
        private const byte IDE_STATUSREG_CORR = 0x00000004;
        private const byte IDE_STATUSREG_DRQ = 0x00000008;
        private const byte IDE_STATUSREG_DSC = 0x00000010;
        private const byte IDE_STATUSREG_DWF = 0x00000020;
        private const byte IDE_STATUSREG_DRDY = 0x00000040;
        private const byte IDE_STATUSREG_BSY = 0x00000080;
        private const byte IDE_ERRORREG_ABRT = 0x00000004;

        private const uint Timeout = 60;

        private enum ATA_Commands : byte {
            Nop = 0x00,
            CfaRequestExtErrCode = 0x03,
            DeviceReset = 0x08,
            Recalibrate = 0x10,
            ReadSectors = 0x20,
            ReadSectorsExt = 0x24,
            ReadDmaExt = 0x25,
            ReadDmaQueuedExt = 0x26,
            ReadMultipleExt = 0x29,
            WriteSectors = 0x30,
            WriteSectorsExt = 0x34,
            WriteDmaExt = 0x35,
            WriteDmaQueuedExt = 0x36,
            CfaWriteSectorsWoErase = 0x38,
            WriteMultipleExt = 0x39,
            WriteVerify = 0x3C,
            ReadVerifySectors = 0x40,
            ReadVerifySectorsExt = 0x42,
            FormatTrack = 0x50,
            Seek = 0x70,
            CfaTranslateSector = 0x87,
            ExecuteDeviceDiagnostic = 0x90,
            InitializeDriveParameters = 0x91,
            InitializeDeviceParameters = 0x91,
            StandbyImmediate2 = 0x94,
            IdleImmediate2 = 0x95,
            Standby2 = 0x96,
            Idle2 = 0x97,
            CheckPowerMode2 = 0x98,
            Sleep2 = 0x99,
            Packet = 0xA0,
            IdentifyDevicePacket = 0xA1,
            IdentifyPacketDevice = 0xA1,
            Smart = 0xB0,
            CfaEraseSectors = 0xC0,
            ReadMultiple = 0xC4,
            WriteMultiple = 0xC5,
            SetMultipleMode = 0xC6,
            ReadDmaQueued = 0xC7,
            ReadDma = 0xC8,
            WriteDma = 0xCA,
            WriteDmaQueued = 0xCC,
            CfaWriteMultipleWoErase = 0xCD,
            StandbyImmediate1 = 0xE0,
            IdleImmediate1 = 0xE1,
            Standby1 = 0xE2,
            Idle1 = 0xE3,
            ReadBuffer = 0xE4,
            CheckPowerMode1 = 0xE5,
            Sleep1 = 0xE6,
            FlushCache = 0xE7,
            WriteBuffer = 0xE8,
            FlushCacheExt = 0xEA,
            IdentifyDevice = 0xEC,
            SetFeatures = 0xEF,
        }
        private enum DeviceControlBits : byte {
            /// <summary>
            /// High Order Byte (48-bit LBA)
            /// </summary>
            HighOrderByte = 0x80,
            /// <summary>
            /// Soft reset
            /// </summary>
            CB_DC_SRST = 0x04,// soft reset
            /// <summary>
            /// Disable interrupts
            /// </summary>
            CB_DC_NIEN = 0x02  // disable interrupts

        }
        
        private static Action<uint> mSleep;
		private readonly string mName;
		private readonly byte mControllerIndex;
		private readonly ushort mController;
		private readonly ushort mController2;
		private readonly ushort mController_Data;
		private readonly ushort mController_Error;
		private readonly ushort mController_FeatureReg;
		private readonly ushort mController_SectorCount;
		private readonly ushort mController_SectorNumber;
		private readonly ushort mController_CylinderLow;
		private readonly ushort mController_CylinderHigh;
		private readonly ushort mController_DeviceHead;
		private readonly ushort mController_PrimaryStatus;
		private readonly ushort mController_Command;
		private readonly ushort mController_AlternateStatus;
		private readonly ushort mController_DeviceControl;
		private readonly ushort mController_DeviceAddress;
		private readonly bool mIsPrimary;
		/* 
#define CB_DATA  0   // data reg         in/out pio_base_addr1+0
#define CB_ERR   1   // error            in     pio_base_addr1+1
#define CB_FR    1   // feature reg         out pio_base_addr1+1
#define CB_SC    2   // sector count     in/out pio_base_addr1+2
#define CB_SN    3   // sector number    in/out pio_base_addr1+3
#define CB_CL    4   // cylinder low     in/out pio_base_addr1+4
#define CB_CH    5   // cylinder high    in/out pio_base_addr1+5
#define CB_DH    6   // device head      in/out pio_base_addr1+6
#define CB_STAT  7   // primary status   in     pio_base_addr1+7
#define CB_CMD   7   // command             out pio_base_addr1+7
#define CB_ASTAT 8   // alternate status in     pio_base_addr2+6
#define CB_DC    8   // device control      out pio_base_addr2+6
#define CB_DA    9   // device address   in     pio_base_addr2+7*/
		private readonly byte mDrive;
		private ATA(string aName, byte aController, byte aDrive) {
			DebugUtil.SendMessage("ATA", aName);
			mName = aName;
			mControllerIndex = aController;
			mController = GetControllerAddress1(aController);
			mController2 = GetControllerAddress2(aController);
			mType = DeviceType.Storage;
			mDrive = aDrive;
			mController_Data = mController;
			mIsPrimary = aController == 0;
			mController_Error = (ushort)(mController + 1);
			mController_FeatureReg = (ushort)(mController + 1);
			mController_SectorCount = (ushort)(mController + 2);
			mController_SectorNumber = (ushort)(mController + 3);
			mController_CylinderLow = (ushort)(mController + 4);
			mController_CylinderHigh = (ushort)(mController + 5);
			mController_DeviceHead = (ushort)(mController + 6);
			mController_PrimaryStatus = (ushort)(mController + 7);
			mController_Command = (ushort)(mController + 7);
			mController_AlternateStatus = (ushort)(mController2 + 8);
			mController_DeviceControl = (ushort)(mController2 + 8);
			mController_DeviceAddress = (ushort)(mController2 + 9);
			IOWriteByte(mController_DeviceControl, 0);
			mBlockCount = GetBlockCount();
		}

		private ulong GetBlockCount() {
			IOWriteByte(mController_DeviceHead, (byte)((mDrive << 4) + (1 << 6)));
			uint xTimeout = Timeout;
			while ((IOReadByte(mController_Command) & IDE_STATUSREG_DRDY) == 0) {
				mSleep(1);
				xTimeout--;
			}
			if ((IOReadByte(mController_Command) & IDE_STATUSREG_DRDY) == 0) {
				throw new Exception("[ATA#1] GetBlockCount failed!");
			}
			IOWriteByte(mController_Command, 0xF8);
			xTimeout = Timeout;
			while ((IOReadByte(mController_Command) & IDE_STATUSREG_BSY) != 0) {
				mSleep(1);
				xTimeout--;
			}
			if ((IOReadByte(mController_Command) & IDE_STATUSREG_BSY) != 0) {
				throw new Exception("[ATA#2] GetBlockCount failed!");
			}
			uint xResult = IOReadByte(mController_SectorNumber);
			xResult += (uint)IOReadByte(mController_CylinderLow) << 8;
			xResult += (uint)IOReadByte(mController_CylinderHigh) << 16;
			xResult += (uint)(IOReadByte(mController_DeviceHead) & 0xF) << 8;
			return xResult;
		}

		public static void HandleInterruptSecondary() {
			mSecondaryInterruptCount++;
			IOReadByte((ushort)(GetControllerAddress1(0) + 7));
		}

		private static uint mSecondaryInterruptCount;

		public static void HandleInterruptPrimary() {
			mPrimaryInterruptCount++;
			IOReadByte((ushort)(GetControllerAddress1(1) + 7));
		}

		private static uint mPrimaryInterruptCount;

		public static void Initialize(Action<uint> aSleep) {
			if (aSleep == null) {
				throw new ArgumentNullException("aSleep");
			}
			mSleep = aSleep;
			DebugUtil.SendMessage("ATA", "Start Device Detection");
			DebugUtil.SendNumber("ATA", "Controllers", (uint)GetControllerAddressCount(), 32);
			for (byte xControllerBaseAIdx = 0; xControllerBaseAIdx < GetControllerAddressCount(); xControllerBaseAIdx++) {
				for (byte xDrive = 0; xDrive < 2; xDrive++) {
					IOWriteByte((ushort)(GetControllerAddress1(xControllerBaseAIdx) + ATA_DRIVEHEAD), (byte)((xControllerBaseAIdx << 4) | 0xA0 | (xDrive << 4)));

                    Console.WriteLine("ok1"); 
                    
                    mSleep(1);
                    Console.WriteLine("ok2");
					if (IOReadByte((ushort)(GetControllerAddress1(xControllerBaseAIdx) + ATA_STATUS)) == 0x50) {
						ATA xATA;
						Device.Add(xATA = new ATA(String.Concat(mControllerNumbers[xControllerBaseAIdx], " ", mDriveNames[xDrive]), xControllerBaseAIdx, xDrive));
						DebugUtil.SendNumber("ATA", "Device Size", (uint)xATA.BlockCount, 32);
                        Console.WriteLine("ok3");
					}
				}
			}
		}

		public override uint BlockSize {
			get {
				return 512;
			}
		}

		private readonly ulong mBlockCount;

		public override ulong BlockCount {
			get {
				return mBlockCount;
			}
		}
		/* 
#define CB_DATA  0   // data reg         in/out pio_base_addr1+0
#define CB_ERR   1   // error            in     pio_base_addr1+1
#define CB_FR    1   // feature reg         out pio_base_addr1+1
#define CB_SC    2   // sector count     in/out pio_base_addr1+2
#define CB_SN    3   // sector number    in/out pio_base_addr1+3
#define CB_CL    4   // cylinder low     in/out pio_base_addr1+4
#define CB_CH    5   // cylinder high    in/out pio_base_addr1+5
#define CB_DH    6   // device head      in/out pio_base_addr1+6
#define CB_STAT  7   // primary status   in     pio_base_addr1+7
#define CB_CMD   7   // command             out pio_base_addr1+7
#define CB_ASTAT 8   // alternate status in     pio_base_addr2+6
#define CB_DC    8   // device control      out pio_base_addr2+6
#define CB_DA    9   // device address   in     pio_base_addr2+7*/
		public override byte[] ReadBlock(ulong aBlock) {
			// 1) Read the status register of the primary or the secondary IDE controller. 
			// 2) The BSY and DRQ bits must be zero if the controller is ready. 
			DebugUtil.SendNumber("ATA", "ReadBlock", (ushort)aBlock, 32);
			uint xSleepCount = Timeout;
			while (((IOReadByte(mController_Command) & (IDE_STATUSREG_BSY | IDE_STATUSREG_DRQ)) != 0) && xSleepCount > 0) {
				mSleep(1);
				xSleepCount--;
			}
			if (((IOReadByte(mController_Command) & (IDE_STATUSREG_BSY | IDE_STATUSREG_DRQ)) != 0) && xSleepCount > 0) {
				throw new Exception("[ATA#2] Read failed");
			}
			//3) Set the DEV bit to 0 for Drive0 and to 1 for Drive1 on the selected IDE controller using 
			//   the Device/Head register and wait for approximately 400 nanoseconds using some NOP perhaps. 
			IOWriteByte(mController_DeviceHead, (byte)(mDrive << 4));
			//4) Read the status register again. 
			//5) The BSY and DRQ bits must be 0 again for you to know that the IDE controller and the selected IDE drive are ready. 
			xSleepCount = Timeout;
			while (((IOReadByte(mController_Command) & (IDE_STATUSREG_BSY | IDE_STATUSREG_DRQ)) != 0) && xSleepCount > 0) {
				mSleep(1);
				xSleepCount--;
			}
			if (((IOReadByte(mController_Command) & (IDE_STATUSREG_BSY | IDE_STATUSREG_DRQ)) != 0) && xSleepCount > 0) {
				throw new Exception("[ATA#5] Read failed");
			}
			// 6) Write the LBA28 address to the designated IDE registers. 
			IOWriteByte(mController_SectorNumber, (byte)aBlock);
			IOWriteByte(mController_CylinderLow, (byte)(aBlock >> 8));
			IOWriteByte(mController_CylinderHigh, (byte)(aBlock >> 16));
			IOWriteByte(mController_DeviceHead, (byte)(0xE0 | (mDrive << 4) | ((byte)((aBlock >> 24) & 0x0F))));
			// 7) Set the Sector count using the Sector Count register. 
			IOWriteByte(mController_SectorCount, 1);
			//8) Issue the Read Sector(s) command. 
			IOWriteByte(mController_Command, 0x20);
			// 9) Read the Error register. If the ABRT bit is set then the Read Sector(s) command 
			//    is not supported for that IDE drive. If the ABRT bit is not set, continue to the next step. 
			if ((IOReadByte(mController_Error) & IDE_ERRORREG_ABRT) == IDE_ERRORREG_ABRT) {
				throw new Exception("[ATA#9] Read failed");
			}
			// 10) If you want to receive interrupts after reading each sector, clear the nIEN bit in the 
			//     Device Control register. If you do not clear this bit then interrupts will not be generated 
			//     after the reading of each sector which might cause an infinite loop if you are waiting for them. 
			//     The Primary IDE Controller will generate IRQ14 and the secondary IDE controller generates IRQ 15. 
			IOWriteByte(mController_DeviceControl, 0); // receive interrupts...
			// 11) Read the Alternate Status Register (you may even ignore the value that is read) 
			IOReadByte(mController_AlternateStatus);
			// 12) Read the Status register for the selected IDE Controller. 
			IOReadByte(mController_Command);
			//13) Whenever a sector of data is ready to be read from the Data Register, the BSY bit 
			//    in the status register will be set to 0 and DRQ to 1 so you might want to wait until 
			//    those bits are set to the mentioned values before attempting to read from the drive. 
			xSleepCount = Timeout;
			while (((IOReadByte(mController_Command) & (IDE_STATUSREG_BSY | IDE_STATUSREG_DRQ)) != IDE_STATUSREG_DRQ) && xSleepCount != 0) {
				xSleepCount--;
				mSleep(1);
			}
			if ((IOReadByte(mController_Command) & (IDE_STATUSREG_BSY | IDE_STATUSREG_DRQ)) != IDE_STATUSREG_DRQ) {
				throw new Exception("[ATA#13] Read failed");
			}
			//14) Read one sector from the IDE Controller 16-bits at a time using the IN or the INSW instructions. 
			byte[] xResult = new byte[512];
			for (uint i = 0; i < 256; i++) {
				ushort xValue = IOReadWord(mController);
				xResult[i * 2] = (byte)xValue;
				xResult[(i * 2) + 1] = (byte)(xValue >> 8);
			}
			// 15) See if you have to read one more sector. If yes, repeat from step 11 again. 
			//16) If you don't need to read any more sectors, read the Alternate Status Register and ignore the byte that you read. 
			IOReadByte(mController_AlternateStatus);
			//17) Read the status register. When the status register is read, the IDE Controller will negate 
			//    the INTRQ and you will not have pending IRQs waiting to be detected. This is a MUST to read 
			//    the status register when you are done reading from IDE ports. 
			IOReadByte(mController_Command);
			DebugUtil.SendATA_BlockReceived(mControllerIndex, mDrive, (uint)aBlock, xResult);
			return xResult;
		}

		public override void WriteBlock(ulong aBlock, byte[] aContents) {
			uint xSleepCount = Timeout;
			while (((IOReadByte(mController_Command) & 0x80) == 0x80) && xSleepCount > 0) {
				mSleep(1);
				xSleepCount--;
			}
			if (xSleepCount == 0) {
				throw new Exception("[ATA|WriteBlockNew] Failed 1");
			}
			IOWriteByte(mController_FeatureReg, 0);
			IOWriteByte(mController_SectorCount, 1);
			IOWriteByte(mController_SectorNumber, (byte)aBlock);
			IOWriteByte(mController_CylinderLow, (byte)(aBlock >> 8));
			IOWriteByte(mController_CylinderHigh, (byte)(aBlock >> 16));
			IOWriteByte(mController_DeviceHead, (byte)(0xE0 | (mDrive << 4) | (byte)(aBlock >> 24)));
			IOWriteByte(mController_DeviceControl, 0); // receive interrupts...
			IOWriteByte(mController_Command, 0x30);
			xSleepCount = Timeout;
			while (((IOReadByte(mController_Command) & 0x80) == 0x80) && xSleepCount > 0) {
				mSleep(1);
				xSleepCount--;
			}
			if (xSleepCount == 0) {
				throw new Exception("[ATA|WriteBlockNew] Failed 2");
			}
			xSleepCount = Timeout;
			while (((IOReadByte(mController_AlternateStatus) & 0x8) == 0x8) && xSleepCount > 0) {
				mSleep(1);
				xSleepCount--;
			}
			if (xSleepCount == 0) {
				throw new Exception("[ATA|WriteBlockNew] Failed 3");
			}
			for (int i = 0; i < 256; i++) {
				IOWriteWord(mController_Data, (ushort)((aContents[i * 2]) | (aContents[i * 2 + 1] << 8)));
			}
		}

		public override string Name {
			get {
				return mName;
			}
		}
	}
}
