#include <base.hpp>
#include <blockdev.hpp>
#include <io.h>
#include <textDISP.hpp>
#include <string.h>
#define ATAPIO_ERR 0x01
#define ATAPIO_DRQ 0x08
#define ATAPIO_SRV 0x10
#define ATAPIO_DF  0x20
#define ATAPIO_RDY 0x40
#define ATAPIO_BSY 0x80
#define DATA 0
#define ATAPI_RESV 1
#define SECTOR_CNT 2
#define LBAlo 3
#define LBAmid 4
#define LBAhi 5
#define DRV 6
#define CMD 7
#define ALTCMD 0x206
namespace MTGosHAL {
IDE::IDE(uint16_t baseport, uint8_t num): baseport(baseport), num(num), existent(0), numDevices(0) {
    if(inb(baseport+CMD)!=0xFF) { //Does the ATA controller return floating bus?
        // Checking existence of the master
        outb(baseport+DRV, 0xE0);
        outb(baseport+LBAlo,0x55);
        if(inb(baseport+LBAlo)==0x55) {
            existent|=1;
            numDevices++;
        }
        // Checking existence of the slave
        outb(baseport+DRV, 0xF0);
        outb(baseport+LBAlo,0x55);
        if(inb(baseport+LBAlo)==0x55) {
            existent|=2;
            numDevices++;
        }
    } else {
      //err << "ATA device could not be found!\n";
    }
    outb(baseport+ALTCMD,0x40);
    inb(baseport+CMD);
    inb(baseport+CMD);
    inb(baseport+CMD);
    outb(baseport+ALTCMD,0x00);
}
auto IDE::getDriveCnt() -> int32_t {return numDevices;}
auto IDE::getDriveNumByName(const char* name) -> int32_t {
    if(strlen(name)!=5)
        return -1; //Format is ATA[0-3][sm] (regex)
    if((name[0]!=name[2])||(name[2]!='A'))
        return -1;
    if(name[1]!='T')
        return -1;
    int32_t drivenum=name[3]-0x30;
    if(drivenum!=num)
        return -1;
    if((name[4]!='s')&&(name[4]!='m'))
        return -1;
    drivenum<<=1;
    drivenum+=(name[4]=='s')?1:0;
    if(!(existent&(1<<drivenum)))
        return -1;
    return drivenum;
}
auto IDE::readSector(int32_t drive, uint64_t sectorNum, uint8_t *buf) -> void {
    if(drive<(num*2)||drive>(num*2+1))
        return;
    if(!(existent&(1<<(drive-num*2))))
        return;
    while(inb(baseport+ALTCMD)&ATAPIO_BSY);
    outb(baseport+DRV, 0x40 | (drive&1)<<4);
    outb(baseport+SECTOR_CNT, 0);
    outb(baseport+LBAlo, (uint8_t)(sectorNum>>24));
    outb(baseport+LBAmid, (uint8_t)(sectorNum>>32));
    outb(baseport+LBAhi, (uint8_t)(sectorNum>>40));
    outb(baseport+SECTOR_CNT, 1);
    outb(baseport+LBAlo, (uint8_t)(sectorNum));
    outb(baseport+LBAmid, (uint8_t)(sectorNum>>8));
    outb(baseport+LBAhi, (uint8_t)(sectorNum>>16));
    outb(baseport+CMD, 0x24);
    inb(baseport+CMD);
    inb(baseport+CMD);
    inb(baseport+CMD);
    while(inb(baseport+CMD)&0x80);
    uint16_t *bufw=(uint16_t *)buf;
    asm volatile ("rep insw" : : "D"((int)bufw),"d"(baseport),"c"(256));
}
auto IDE::readSector(int32_t drive, uint64_t sectorNum, uint32_t num, uint8_t *buf) -> void {
    if(drive<(this->num*2)||drive>(this->num*2+1))
        return;
    if(!(existent&(1<<(drive-this->num*2))))
        return;
    if(num==0)
        return;
    if(num>65536)
        return;
    if(num==65536)
        num=0;
    while(inb(baseport+ALTCMD)&ATAPIO_BSY);
    outb(baseport+DRV, 0x40 | (drive&1)<<4);
    outb(baseport+SECTOR_CNT, (uint8_t)(num >> 8));
    outb(baseport+LBAlo, (uint8_t)(sectorNum>>24));
    outb(baseport+LBAmid, (uint8_t)(sectorNum>>32));
    outb(baseport+LBAhi, (uint8_t)(sectorNum>>40));
    outb(baseport+SECTOR_CNT, (uint8_t)num);
    outb(baseport+LBAlo, (uint8_t)(sectorNum));
    outb(baseport+LBAmid, (uint8_t)(sectorNum>>8));
    outb(baseport+LBAhi, (uint8_t)(sectorNum>>16));
    outb(baseport+CMD, 0x24);
    inb(baseport+CMD);
    inb(baseport+CMD);
    inb(baseport+CMD);
    while(inb(baseport+CMD)&0x80);
    uint16_t *bufw=(uint16_t *)buf;
    asm volatile ("rep insw" : : "D"((int)bufw),"d"(baseport),"c"(256*num));
}
auto IDE::writeSector(int32_t drive, uint64_t sectorNum, uint8_t *buf) -> void {
    if(drive<(this->num*2)||drive>(this->num*2+1))
        return;
    if(!(existent&(1<<(drive-this->num*2))))
        return;
    while(inb(baseport+ALTCMD)&ATAPIO_BSY);
    outb(baseport+SECTOR_CNT, (uint8_t)(0));
    outb(baseport+LBAlo, (uint8_t)(sectorNum>>24));
    outb(baseport+LBAmid, (uint8_t)(sectorNum>>32));
    outb(baseport+LBAhi, (uint8_t)(sectorNum>>40));
    outb(baseport+SECTOR_CNT, (uint8_t)1);
    outb(baseport+LBAlo, (uint8_t)(sectorNum));
    outb(baseport+LBAmid, (uint8_t)(sectorNum>>8));
    outb(baseport+LBAhi, (uint8_t)(sectorNum>>16));
    outb(baseport+CMD, 0x34);
    inb(baseport+CMD);
    inb(baseport+CMD);
    inb(baseport+CMD);
    while(inb(baseport+CMD)&ATAPIO_BSY);
    for(int i=0;i<256;i++) {
        asm volatile("outsw; aad" : : "S"((int)buf),"d"(baseport),"a"(8787)); //The AAD is for creating a short delay between writes, as the compiler might unroll this loop. 
    }
  }
auto IDE::writeSector(int32_t drive, uint64_t sectorNum, uint32_t num, uint8_t *buf) -> void {
    if(drive<(this->num*2)||drive>(this->num*2+1))
        return;
    if(!(existent&(1<<(drive-this->num*2))))
        return;
    if(num==0)
        return;
    if(num>65536)
        return;
    if(num==65536)
        num=0;
    while(inb(baseport+ALTCMD)&ATAPIO_BSY);
    outb(baseport+SECTOR_CNT, (uint8_t)(num >> 8));
    outb(baseport+LBAlo, (uint8_t)(sectorNum>>24));
    outb(baseport+LBAmid, (uint8_t)(sectorNum>>32));
    outb(baseport+LBAhi, (uint8_t)(sectorNum>>40));
    outb(baseport+SECTOR_CNT, (uint8_t)num);
    outb(baseport+LBAlo, (uint8_t)(sectorNum));
    outb(baseport+LBAmid, (uint8_t)(sectorNum>>8));
    outb(baseport+LBAhi, (uint8_t)(sectorNum>>16));
    outb(baseport+CMD, 0x34);
    inb(baseport+CMD);
    inb(baseport+CMD);
    inb(baseport+CMD);
    while(inb(baseport+CMD)&ATAPIO_BSY);
    for(uint32_t i=0;i<(num<<8);i++) {
        asm volatile("outsw; aad" : : "S"((int)buf),"d"(baseport),"a"(8787)); //The AAD is for creating a short delay between writes, as the compiler might partially unroll this loop. 
    }
  }
}