/* dmalloc.c - dynamic storage allocator.
 *  Copyright (C) 2022 by Jed Marti
 *   This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see https://www.gnu.org/licenses/.
 */

// Note: All values are stored LSB byte first.

#include <C8051F380.h>
#include <stdint.h>

#include "lfxmit.h"
#include "config.h"
#include "serial.h"
#include "storage.h"


// First free entry.
__xdata uint16_t firstFree;

// The active list.
__xdata uint16_t actives;


// The data space.
__xdata uint8_t D[DSIZE];

// The execution/parsing stack.
extern __xdata uint16_t ST[];
extern uint8_t          SPTR;

// The symbol table.
extern __xdata SymtabEntry S[];
extern __xdata uint8_t     SINDEX;

// Constants.
extern __xdata uint16_t  iZERO;
extern __xdata uint16_t  iONE;

// FOR loop table.
extern __xdata forValues FRT[];
extern __xdata uint8_t   FINDEX;

// Are we GC'ing.
__bit areWeGCing;


// Mimics an address register. All routines requiring D space use this.
uint16_t DREG;

// Accumulator simulators. 
__xdata floatOrInt  IACC;
__xdata floatOrInt  IACCB;
__xdata floatOrInt  FACC;
__xdata floatOrInt  FACCB;
__xdata uint16_t DREGB;

// Note: tried to make IACC into low RAM makes arithmetic into a pig needing 16 extra RAM
// Note: bytes and causing everything to fail.


////////////////////////////////////////////////////////////////////////////////////////////////////
// Moving around dynamically allocated storage.


uint16_t next(void)
// For a cell address in DREG, return the address of the next element.
{
  return ((uint16_t) D[DREG + 2]) + (((uint16_t) (D[DREG + 1] & 0x03)) << 8);
}



uint16_t size(void)
// For a cell address in DREG, return the address of the size of this block.
{
  return (((uint16_t) (D[DREG] & 0x0F)) << 6) + ((uint16_t) (D[DREG + 1] >> 2));
}



uint8_t tag(void)
// For a cell address in DREG, return the 4 bit tag as low 4 bits.
{
  return D[DREG] >> 4;
}


void setNext(uint16_t n)
// For the cell address in DREG, set the next address.
{
  uint8_t __xdata *ptr;
  ptr = &D[DREG + 1];
  *ptr++ = (*ptr & 0xFC) + ((n >> 8) & 0x03);
  *ptr = n;
}



static void setNextX(uint16_t n)
// Same as above, but offset is n and value to store is DREG. Used only by the compacting
// garbage collector.
{
  uint8_t __xdata *ptr;
  ptr = &D[n + 1];
  *ptr++ = (*ptr & 0xFC) + ((DREG >> 8) & 0x03);
  *ptr = DREG;
}




void setSize(uint16_t s)
// For the cell address in DREG, set the size. Must be 0 to 1023.
{
  uint8_t __xdata *ptr;
  ptr = &D[DREG];
  *ptr++ = (*ptr & 0xF0) + (s >> 6);
  *ptr = (*ptr & 0x03) + (s << 2);
}


void setTag(uint8_t t)
// For a cell address in DREG, set the tag bits. These are shifted from the low order 4 bits
// to the upper bits.
{
  D[DREG] = (D[DREG] & 0x0F) + (t << 4);
}




void setByte(uint8_t v)
// Set some random location in D.
{
  if (DREG >= DSIZE)
  {
    putstring("Guard ");
    putuint16(DREG);
    terpri();
    for(;;);
  }
  D[DREG] = v;
}



void setIACC(void)
// Save the accumulator into the boxed area pointed to by DREG.
{
  // putstring("ACC = "); putuint32(IACC.i); putstring("  DREG "); puthex16(DREG); terpri();
  *((int32_t*) &D[DREG + 3]) = IACC.i;
}



void getIACC(void)
// DREG points to a boxed integer. Get the integer value there into the accumulator.
{
  IACC.i = *((int32_t*) &D[DREG + 3]);
}



void getIACCB(void)
// DREG points to a boxed integer. Get the integer value there into the second accumulator.
{
  IACCB.i = *((int32_t*) &D[DREG + 3]);
}



void setFACC(void)
//    Copy the floating accumulator into a boxed float area in dynmaic storage.
{
  uint32_t __xdata *f;
  f = (uint32_t __xdata *) &FACC;
  *((uint32_t*) &D[DREG + 3]) = *f;
}



void getFACC(void)
// Copy the float from the dynamic area (a boxed float) into the floating accumulator.
{
  FACC.f = *((float *) &D[DREG + 3]);
}



void getFACCB(void)
// Copy the float from the dynamic area (a boxed float) into the floating accumulator B.
{
  FACCB.f = *((float *) &D[DREG + 3]);
}



void setFloat(uint32_t __xdata *f)
{

  //  putstring("setFloat "); puthex16((uint16_t) f); putch(' ');
  //  puthexbyte(*((uint8_t __xdata *) f));
  //  puthexbyte(*(((uint8_t __xdata *) f) + 1));
  //    puthexbyte(*(((uint8_t __xdata *) f) + 2));
  //    puthexbyte(*(((uint8_t __xdata *) f) + 3));terpri();
  *((uint32_t __xdata *) &D[DREG + 3]) = *f;
}



uint8_t getByte(void)
// Get a byte, bounds check.
{
  if (DREG >= DSIZE)
  {
    putstring("Guard ");
    putuint16(DREG);
    terpri();
    for(;;);
  }
  return D[DREG];
}



void initDynamic(void)
//     Initialize the dynamic storage space. This clears out everything and creates the big
// free block. ZERO and ONE are added by the ints.c code.
{
  // The list of active values.
  actives = 0x03FF;

  // First free block.
  DREG = firstFree = 0;
  setNext(0x3FF);
  setSize(DSIZE);
}



////////////////////////////////////////////////////////////////////////////////////////////////////



static void mark(uint16_t i)
{
  D[i] |= GC_TAG;
}



static void unmark(uint16_t i)
{
  D[i] &= ~GC_TAG;
}


static void compact(void)
{
  uint16_t f;
  uint16_t n;
  uint16_t sz;
  for (f = firstFree;;)
  {
    DREG = f;
    n = next();
    if (n == 0x3FF) return;

    if (f + size() == n)
    {
      sz = size();
      DREG = n;
      sz += size();
      n = next();
      DREG = f;
      setSize(sz);
      setNext(n);
    }
    else
    {
      f = DREG = n;
      n = next();
    }
  }
}


static void sortFree(void)
// i is a D space offset that is to be freed. Add it to the free list in order by offset.
// The compaction will come later.
{
  uint16_t prev;

  // Use our stack for DREG as the caller needs it.
  prev = 0xFFFF;
  DREGB = DREG;
  for (DREG = firstFree; DREG != 0x03FF; DREG = next())
  {
    if (DREG > DREGB)
    {
      // Insert in list.
      if (prev != 0xFFFF)
      {
        setNextX(DREGB);
        DREG = prev;
        setNext(DREGB);
      }

      // Otherwise, set the firstFree to it.
      else
      {
        DREG = DREGB;
        setNext(firstFree);
        firstFree = DREGB;
      }
      return;
    }
    prev = DREG;
  }

  // Totally empty?
  if (prev == 0xFFFF)
  {
    firstFree = DREGB;
    DREG = DREGB;
    setNext(0x3FF);
  }

  // Otherwise, add to tail.
  else
  {
    DREG = prev;
    setNext(DREGB);
    DREG = DREGB;
    //putstring("Adding "); puthex16(DREG); putstring(" as tail\n");
    setNext(0x3FF);
    //    dumpRAM();
    //    for(;;);
  }
}



void gc(void)
//     Compacting garbage collector.
{
  uint8_t i;
  uint16_t prev;

  LED2 = 1;

  //   putstring("GC\n");
  // To keep out of infinite loop.
  areWeGCing = 1;

  // Mark constants.
  //  putstring("Starting mark\n");
  mark(iONE);
  mark(iZERO);

  // Mark stuff in the symbol table, the evaluation stack and the FOR loop stack.
  for (i = 0; i < SINDEX; ++i)
  {
    mark(S[i].addr);
    mark(S[i].pname);
  }

  // Symbol table.
  for (i = 0; i < SPTR; ++i) mark(ST[i]);

  // Loops.
  for (i = 0; i < FINDEX; ++i)
  {
    mark(FRT[i].limit);
    mark(FRT[i].increment);
  }
  //  putstring("After mark\n");
  //  dumpRAM();

  // Now sweep.
  //  putstring("  Sweep\n");
  prev = 0xFFFF;
  for (DREG = actives; DREG != 0x3FF; )
  {
    //    putstring("  sweep at "); puthex16(DREG); terpri();
    DREGB = DREG;
    if ((D[DREG] & GC_TAG) == 0)
    {
      //      putstring("sweep next is "); puthex16(next()); terpri();
      pushST(next());

      // First remove it from the actives list.
      if (prev == 0xFFFF) actives = next();
      else
      {
        pushST(DREG);
        pushST(DREGB);
        DREGB = next();
        DREG = prev;
        setNext(DREGB);
        DREGB = popST();
        DREG = popST();
      }
      //putstring("freeing "); puthex16(DREG); terpri();
      sortFree();
      DREG = popST();
    }
    else
    {
      prev = DREG;
      DREG = next();
    }

    // Can be removed when it works.
    if (DREG == DREGB)
    {
      putstring("Infinite sweep\n");
      dumpRAM();
      for(;;);
    }
  }

  // Compact the free list.
  compact();
  
  // Unmark stuff.
  //  putstring("  Unmark\n");
  unmark(iONE);
  unmark(iZERO);
  for (i = 0; i < SINDEX; ++i)
  {
    unmark(S[i].addr);
    unmark(S[i].pname);
  }
  for (i = 0; i < SPTR; ++i) unmark(ST[i]);
  for (i = 0; i < FINDEX; ++i)
  {
    unmark(FRT[i].limit);
    unmark(FRT[i].increment);
  }

  //  putstring("  done\n");
  //  dumpRAM();

  LED2 = 0;
}



uint16_t dmalloc(uint16_t sze)
// Search the free table for the size nearest to this. Allocate a block and return its offset.
// The size in each free entry includes the header size.
// Lots of locals moved to xdata to limit low RAM usage at the cost of code space.
{
  __xdata uint16_t bestdiff;
  __xdata uint16_t bestloc;
  uint16_t prev;
  __xdata uint16_t bestprev;
  uint16_t s;
  uint16_t i;

  areWeGCing = 0;
  //  putstring("malloc = "); putuint16(sze); terpri();

  // We need 3 bytes for the header.
  sze += 3;

 loop:
  bestdiff = 65535;
  prev = bestloc = bestprev = 0xFFFF;

  for (DREG = firstFree; DREG != 0x03FF; DREG = next())
  {
    //putstring("  DREG="); puthex16(DREG); putstring(", size="); putbyte(size()); terpri();

    // Get size.
    s = size();
    if (s >= sze && s - sze < bestdiff)
    {
      bestdiff = s - sze;
      bestloc = DREG;
      bestprev = prev;
    }
    prev = DREG;
  }

  //putstring("bestdiff "); putuint16(bestdiff);
  //putstring("  bestloc "); puthex16(bestloc);
  //putstring("  bestprev "); puthex16(bestprev);
  //terpri();

  // Quit if we're out of storage.
  if (bestdiff == 65535)
  {
    if (areWeGCing)
    {
      dumpRAM();
      putstring("Out of Space\n");
      for (;;);
    }
    //dumpRAM();
    gc();
    areWeGCing = 1;
    goto loop;
  }

  // If there's less than 4 bytes left, scarf them up. Otherwise we might have space for just
  // a 3 byte header. 4 bytes lets us store a single byte entry there (symbol say).
  if (bestdiff < 4)
  {
    //    putstring("scarfing\n");

    // First skip around the new block at bestloc resetting pointers.
    DREG = bestloc;
    if (bestprev == 0xFFFF) firstFree = next();
    else
    {
      prev = next();
      DREG = bestprev;
      setNext(prev);
    }

    // Now set the size and clear the tag.
    DREG = bestloc;
    setTag(0);
    setNext(actives);
    setSize(sze + bestdiff);
    actives = bestloc;
    return bestloc;
  }

  // Otherwise, resize the space beyond - it's bigger than we need.
  else
  {
    DREG = bestloc;
    bestdiff = size() - sze;
    i = bestloc + sze,        // Next empty place.
      s = next();             // Next free cell.
    //   putstring("  bestloc 0x"); puthex16(bestloc);
    //   putstring("  i "); puthex16(i);
    //   putstring("  bestdiff "); putuint16(bestdiff);
    //   putstring("  s "); puthex16(s); terpri();

    // Attach new empty space to previous element.
    if (bestprev == 0xFFFF) firstFree = i;
    else
    {
      DREG = bestprev;
      setNext(i);
    }

    // Create the new empty space chain.
    DREG = i;
    setTag(0);
    setNext(s);
    setSize(bestdiff);
    //   putstring("  new chain at s "); putuint16(DREG); putstring("->"); puthexbyte(D[DREG]);
    //    putch(' '); puthexbyte(D[DREG + 1]);  putch(' '); puthexbyte(D[DREG + 2]); terpri();
      
    // Our new cell onto the actives chain.
    DREG = bestloc;
    setNext(actives);
    setSize(sze);
    actives = bestloc;
    //putstring("  new cell at "); putuint16(DREG); putstring("->"); puthexbyte(D[DREG]);
    //putch(' '); puthexbyte(D[DREG + 1]);  putch(' '); puthexbyte(D[DREG + 2]); terpri();
      
    return bestloc; 
  }
}



void dumpRAM(void)
{
  uint16_t i;
  putstring("firstFree "); puthex16(firstFree); terpri();
  putstring("actives   "); puthex16(actives); terpri();

  for (i = 0; i < DSIZE; ++i)
  {
    if ((i & 0x0F) == 0)
    {
      puthex16(i);
      putstring(": ");
    }
    puthexbyte(D[i]);
    if ((i & 0x0F) == 0x0F) terpri();
    else putch(' ');
  }
  terpri();
}



// EOF
