#include <cstdint>
#include <cstring>
#include <array>

#include "cortexa8.hh"
#include "omap35x_intc.hh"
#include "omap35x_prcm.hh"
#include "globals.hh"
#include "util.hh"
#include "mmio.hh"
#include "scheduler.hh"
#include "uart.hh"

class UART_impl {
public:
  UART_impl(uintptr_t base, int irq) : base_{base}, irq_(irq), semNewData_{0} {
    global::intc->register_handler(irq_, std::bind(&UART_impl::recv_handler, this), 1);
    global::intc->enable_int(irq_);

    r_lcr() = 0xBF; // Configuration mode B
    uint8_t dllsave = r_dll(), dlhsave = r_dlh();
    r_dll() = 0;
    r_dlh() = 0;
    r_efr() |= (1<<4);
    r_lcr() = 0x80; // Configuration mode A
    r_fcr() = 0x21;
    r_sysc() = 0x15;
    r_wer() |= (1<<5);
    r_dll() = dllsave;
    r_dlh() = dlhsave;
    r_lcr() = 0x03; // Operational mode
    r_ier() = 0x11;
  }

  ~UART_impl() {
    global::intc->disable_int(irq_);
  }

  void write(char const* data, int const& len) {
    for(int i = 0;i < len;++i)
      sendb(*data++);
  }

  int read(char *buf, int const& len) {
    int pos = 0;

    while(true) {
      cortexa8::ScopedSetIF disint{};

      // Data available in recvbuffer?
      if(((recvbuffer_rdptr_+1)%RECVBUFFERSIZE) != recvbuffer_wrptr_) {
	int avail = recvbuffer_wrptr_ - recvbuffer_rdptr_;
	if(avail < 0)
	  avail = RECVBUFFERSIZE+avail;
	avail -= 1;

	// Advance to first byte to read
	recvbuffer_rdptr_ = (recvbuffer_rdptr_+1)%RECVBUFFERSIZE;

	int toread = (avail>len)?len:avail;
	if(toread+recvbuffer_rdptr_ > RECVBUFFERSIZE) {
	  memcpy(buf+pos, &recvbuffer_[recvbuffer_rdptr_], RECVBUFFERSIZE-(recvbuffer_rdptr_));
	  toread -= RECVBUFFERSIZE-recvbuffer_rdptr_;
	  pos += RECVBUFFERSIZE-recvbuffer_rdptr_;
	  recvbuffer_rdptr_ = 0;
	}
	memcpy(buf+pos, &recvbuffer_[recvbuffer_rdptr_], toread);
	pos += toread;
	recvbuffer_rdptr_ = (recvbuffer_rdptr_+toread-1)%RECVBUFFERSIZE;
      }

      newdata_ = false;

      // Try to read directly from UART if more data req'd than was in buffer
      while((pos < len) && (r_lsr() & 0x1)) {
	char rd = r_data();
	if(rd == '\r')
	  rd = '\n';
	if(echo_)
	  sendb(rd);
	buf[pos++] = rd;
      }

      //disint.restore();

      if(pos > 0)
	return pos;

      semNewData_.acquire();
    }
  }
 
private:
  void recv_handler() {
    while(r_lsr() & 0x1) { // while there are bytes
      char rd = r_data();
      if(rd == '\r')
	rd = '\n';
      if(echo_)
	sendb(rd);
      if(recvbuffer_wrptr_ == recvbuffer_rdptr_) { // Overflow
      } else {
	recvbuffer_[recvbuffer_wrptr_] = rd;
	recvbuffer_wrptr_ = (recvbuffer_wrptr_+1)%RECVBUFFERSIZE;
      }
    }
    
    if(semNewData_.hasWaiters())
      semNewData_.release();
    global::prcm->clear_wake_per(11);
  }

  MMIO_alloc base_;
  int irq_;
  Scheduler::Semaphore semNewData_;

  uint8_t volatile& r_data() {
    return _reg8(base_.get_virt(), 0);
  }

  uint8_t volatile& r_dll() {
    return _reg8(base_.get_virt(), 0);
  }

  uint8_t volatile& r_dlh() {
    return _reg8(base_.get_virt(), 4);
  }

  uint8_t volatile& r_ier() {
    return _reg8(base_.get_virt(), 4);
  }

  uint8_t volatile& r_fcr() {
    return _reg8(base_.get_virt(), 8);
  }

  uint8_t volatile& r_efr() {
    return _reg8(base_.get_virt(), 8);
  }

  uint8_t volatile& r_lcr() {
    return _reg8(base_.get_virt(), 0xc);
  }

  uint8_t volatile& r_lsr() {
    return _reg8(base_.get_virt(), 0x14);
  }

  uint8_t volatile& r_ssr() {
    return _reg8(base_.get_virt(), 0x44);
  }

  uint8_t volatile& r_sysc() {
    return _reg8(base_.get_virt(), 0x54);
  }

  uint8_t volatile& r_wer() {
    return _reg8(base_.get_virt(), 0x5c);
  }

  static const size_t RECVBUFFERSIZE = 128;
  std::array<char, RECVBUFFERSIZE> recvbuffer_;
  volatile size_t recvbuffer_rdptr_ = (RECVBUFFERSIZE-1), recvbuffer_wrptr_ = 0;
  volatile bool newdata_ = false;

  bool echo_ = true;

  void _wait_txnotfull() {
    while(r_ssr() & 0x1) {}
  }

  void _wait_rxnotempty() {
    while(!(r_lsr() & 0x1)) {}
  }

  void sendb(char b) {
    _wait_txnotfull();
    r_data() = b;
  }

  char recvb() {
    _wait_rxnotempty();
    return r_data();
  }
};

UART::UART(uintptr_t base, int irq) : impl_(new UART_impl(base, irq)) {
}

UART::~UART() {
}

void UART::write(const char *data, int const& len) {
  impl_->write(data, len);
}

int UART::read(char *buf, int const& len) {
  return impl_->read(buf, len);
}


void EarlyUART::write(char const* data, int const& len) {
  for(int i = 0;i < len;++i)
    sendb(*data++);
}

int EarlyUART::read(char *buf, int const& len) {
  char rd = r_data();
  if(rd == '\r')
    rd = '\n';
  sendb(rd);
  buf[0] = rd;
  return 1;
}

uint8_t volatile& EarlyUART::r_data() {
  return _reg8(base_, 0);
}

uint8_t volatile& EarlyUART::r_lsr() {
  return _reg8(base_, 0x14);
}

uint8_t volatile& EarlyUART::r_ssr() {
  return _reg8(base_, 0x44);
}

void EarlyUART::_wait_txnotfull() {
  while(r_ssr() & 0x1) {}
}

void EarlyUART::_wait_rxnotempty() {
  while(!(r_lsr() & 0x1)) {}
}

void EarlyUART::sendb(char b) {
  _wait_txnotfull();
  r_data() = b;
}

char EarlyUART::recvb() {
  _wait_rxnotempty();
  return r_data();
}