#include <stdint.h>
#include "panic.h"
#include <stdbool.h>
#include "util.h"
#include "fat.h"

#define SPT FAT_INFO->sectors_per_track

enum {
  FP_STATUSA = 0x03f0,
  FP_STATUSB = 0x03f1,
  FP_OUT     = 0x03f2,
  FP_STATUS  = 0x03f4,
  FP_RATE    = 0x03f4,
  FP_DATA    = 0x03f5,
  FP_IN      = 0x03f7,
  FP_CONFIG  = 0x03f7
};

enum {
  FM_READY = 0x80,
  FM_IODIR = 0x40,//on for FDC -> CPU, off for CPU -> FDC
  FM_NDMA  = 0x20,
  FM_BUSY  = 0x10,
  FM_SEEK3 = 0x08,
  FM_SEEK2 = 0x04,
  FM_SEEK1 = 0x02,
  FM_SEEK0 = 0x01
};

enum {
  FOP_SPECIFY = 0x03,
  FOP_WRITE   = 0x05,
  FOP_READ    = 0x06,
  FOP_CALBRT  = 0x07,
  FOP_SENSE_I = 0x08,
  FOP_SEEK    = 0x0f,
  FOP_CONFIG  = 0x13,
  FOP_LOCK    = 0x14,

  FOP_MAG   = 0x40,
  FOP_MULTI = 0x80
};

enum {
  FC_SEEK    = 0x40,
  FC_NO_FIFO = 0x20,
  FC_NO_POLL = 0x10
  //(threshold - 1) mask = 0x0f
};

enum {
  FOR_SPIN3  = 0x80,
  FOR_SPIN2  = 0x40,
  FOR_SPIN1  = 0x20,
  FOR_SPIN0  = 0x10,
  FOR_ASYNC  = 0x08,
  FOR_RESET_ = 0x04
  //select mask = 0x03
};

uint8_t cyl;

#define FLOPPY_SPIN_LIMIT 65535

bool floppy_spin_in() {
  uint16_t spin = FLOPPY_SPIN_LIMIT;
  while (!(inb(FP_STATUS) & FM_READY))
    if (!--spin)
      panic("floppy_spin_in timed out");
      //return true;
  return !(inb(FP_STATUS) & FM_IODIR);
}

bool floppy_spin_out() {
  uint16_t spin = FLOPPY_SPIN_LIMIT;
  while (!(inb(FP_STATUS) & FM_READY))
    if (!--spin)
      panic("floppy_spin_out timed out");
      //return true;
  return inb(FP_STATUS) & FM_IODIR;
}

bool floppy_in(uint8_t *data) {
  if (floppy_spin_in())
    //return true;
    panic("floppy_in wrong io direction");
  *data = inb(FP_DATA);
  return false;
}

bool floppy_out(uint8_t data) {
  if (floppy_spin_out())
    //return true;
    panic("floppy_out wrong io direction");
  outb(FP_DATA, data);
  return false;
}

bool floppy_sense() {
  uint8_t st0;//TODO: check
  return floppy_out(FOP_SENSE_I) ||
         floppy_in(&st0) ||
         floppy_in(&cyl);
}

void floppy_spin_up() {
  outb(FP_OUT, FOR_SPIN0 | FOR_RESET_);
}

void floppy_spin_down() {
  outb(FP_OUT, FOR_RESET_);
}

void reset_floppy() {
  outb(FP_RATE, 0x80);
  if (floppy_sense() ||
      floppy_sense() ||
      floppy_sense() ||
      floppy_sense())
    panic("Floppy reset failed.");
  if (floppy_out(FOP_SPECIFY) ||
      floppy_out(0x80) ||//Step rate time * 16 + head unload time
      floppy_out(0x01))//Head load time * 2 + !dma
    panic("Floppy reset failed.");
  floppy_spin_down();
}

void init_floppy() {
  uint8_t res;
  if (floppy_out(FOP_CONFIG) ||
      floppy_out(0) ||
      floppy_out(FC_SEEK | FC_NO_FIFO) ||
      floppy_out(0) ||
      floppy_out(FOP_LOCK | FOP_MULTI) ||
      floppy_in(&res))
    panic("Floppy initialization failed.");
  reset_floppy();
  if (floppy_out(FOP_CALBRT) ||
      floppy_out(0) ||
      floppy_sense())
    panic("Floppy initialization failed.");
}

/*
bool seek_to(uint8_t to_cyl) {
  return floppy_out(FOP_SEEK) ||
         floppy_out(0) ||
         floppy_out(to_cyl) ||
         floppy_sense() ||
         (cyl != to_cyl);
}
*/

bool read_sector(uint16_t sector, void *buffer) {
  uint8_t c = sector / (2 * SPT),
          h = (sector % (2 * SPT)) / SPT,
          s = (sector % (2 * SPT)) % SPT + 1;
  if (floppy_out(FOP_READ | FOP_MAG | FOP_MULTI) ||
      floppy_out(h * 4) ||
      floppy_out(c) ||
      floppy_out(h) ||
      floppy_out(s) ||
      floppy_out(2) ||
      floppy_out(SPT) ||
      floppy_out(27) ||
      floppy_out(255) ||
      floppy_sense())
    return true;
  panic("Not implemented (read_sector)");
  //do read
  uint8_t st0, st1, st2, cp, hp, sp;
  if (floppy_in(&st0) ||
      floppy_in(&st1) ||
      floppy_in(&st2) ||
      floppy_in(&cp) ||
      floppy_in(&hp) ||
      floppy_in(&sp))
    return true;
  //check
  return false;
}

bool write_sector(uint16_t start, void *buffer) {
  panic("Not implemented (write_sector)");
}

uint8_t read_sectors(uint16_t start, uint8_t count, void *buffer) {
  floppy_spin_up();
  uint8_t i;
  for (i = 0; i < count; ++i) {
    if (read_sector(start++, buffer))
      break;
    buffer += 512;
  }
  floppy_spin_down();
  return i;
}

uint8_t write_sectors(uint16_t start, uint8_t count, void *buffer) {
  floppy_spin_up();
  uint8_t i;
  for (i = 0; i < count; ++i) {
    if (write_sector(start++, buffer))
      break;
    buffer += 512;
  }
  floppy_spin_down();
  return i;
}