#include "system.h"
#include "debug.h"

struct DEBUG_S {
  uint32_t          UNUSED0;
  volatile uint32_t DATA0;
  volatile uint32_t DATA1;
};
static DEBUG_S & DBGM = * reinterpret_cast<DEBUG_S * const> (0xe00000f0);
static constexpr unsigned FUNCONF_DEBUGPRINTF_TIMEOUT = 160000u;
// Převzato i když jsem to nekontroloval. Ono je to dost divoké.
extern "C" {
  void handle_debug_input( int numbytes, uint8_t * data ) __attribute__((used,weak));
  void handle_debug_input( int numbytes, uint8_t * data ) { }
  void internal_handle_input( volatile uint32_t * dmdata0 ) {
      uint32_t dmd0 = * dmdata0;
      int bytes = (dmd0 & 0x3f) - 4;
      if( bytes > 0 )	{
          handle_debug_input( bytes, ((uint8_t*)dmdata0) + 1 );
      }
  }
};
// where [status word] is:
//   b7 = is a "printf" waiting?
//   b0..b3 = # of bytes in printf (+4).  (5 or higher indicates a print of some kind)
//     note: if b7 is 0 in reply, but b0..b3 have >=4 then we received data from host.
static int _write (const char * buf, const int size) {
  char buffer[4] = { 0 };
  int place = 0;
  uint32_t lastdmd;
  uint32_t timeout = FUNCONF_DEBUGPRINTF_TIMEOUT; // Give up after ~40ms

  if( size == 0 )	{
      lastdmd = DBGM.DATA0;
      if( lastdmd && !(lastdmd & 0x80) ) internal_handle_input( (uint32_t*) & DBGM.DATA0 );
  }
  while( place < size ) {
      int tosend = size - place;
      if( tosend > 7 ) tosend = 7;

      while( ( lastdmd = DBGM.DATA0 ) & 0x80 )
          if( timeout-- == 0 ) return place;

      if( lastdmd ) internal_handle_input( (uint32_t*) & DBGM.DATA0 );

      timeout = FUNCONF_DEBUGPRINTF_TIMEOUT;

      int t = 3;
      while( t < tosend ) {
          buffer[t-3] = buf[t+place];
          t++;
      }
      DBGM.DATA1 = *(uint32_t*)&(buffer[0]);
      t = 0;
      while( t < tosend && t < 3 ) {
          buffer[t+1] = buf[t+place];
          t++;
      }
      buffer[0] = 0x80 | (tosend + 4);
      DBGM.DATA0 = *(uint32_t*)&(buffer[0]);

      //buf += tosend;
      place += tosend;
  }
  return size;
}

Debug::Debug() noexcept : BaseLayer() {
  // Clear out the sending flag.
  DBGM.DATA1 = 0u;
  DBGM.DATA0 = 0x80u;
  // Tohle je asi zbytečné.
  while ( DBGM.DATA0 & 0x80 );
}
uint32_t Debug::Down(const char * buf, const uint32_t size) {
  /* Z nějakého záhadného důvodu pokud namastím kód přímo sem,
   * optimalizace ho vyhodí. Nějak se to nesnáší s virtuálními
   * metodami.
   * */
  return _write (buf, size);
}