//-------------------------------------------------
//          TestSuite: Realloc
//-------------------------------------------------

#include "lcd.h"
#include "util.h"
#include "os_core.h"
#include "os_scheduler.h"
#include "os_memory.h"

#include <avr/interrupt.h>
#include <stdlib.h>

#define DELAY 100

typedef struct {
	MemAddr head;
	size_t size;
	bool dummy;
} Chunk;

void writePat(Heap* heap, Chunk* c, MemValue pat) {
	size_t i;
	for (i = 0; i < c->size; i++) {
		heap->driver->write(c->head+i,pat);
	}
}

bool checkPat(Heap* heap, Chunk* c, MemValue pat) {
	size_t i;
	for (i = 0; i < c->size; i++) {
		if (heap->driver->read(c->head+i) != pat)
			return false;
	}
	return true;
}

void massAlloc(Heap* heap, Chunk* chunks, size_t count) {
	size_t i;
	for (i = 0; i < count; i++) {
		chunks[i].head = os_malloc(heap, chunks[i].size);
		if (!chunks[i].head)
			os_error("Malloc failure");
		writePat(heap, chunks+i, i);
	}
	for (i = 0; i < count; i++) {
		if (chunks[i].dummy)
			os_free(heap, chunks[i].head);
	}
}

void massFree(Heap* heap, Chunk* chunks, size_t count) {
	size_t i;
	for (i = 0; i < count; i++) {
		if (!chunks[i].dummy) {
		  os_free(heap, chunks[i].head);
		}
	}
}


bool makeTest(Heap* heap, uint8_t dummies, uint8_t reallocBits,
              uint8_t random, uint8_t destination) {
  os_setAllocationStrategy(heap, OS_MEM_FIRST);
  size_t i;

  size_t const sz = os_getHeapUseSize(heap);

  #define ISDUMMY(i) ((dummies >> (6-i)) & 1)
  Chunk chunks[] = {
    {.size = rand()%(sz/6) + sz/6, .dummy = ISDUMMY(0) },
	{.size = 1, .dummy = ISDUMMY(1) },
	{.size = rand()%(sz/6) + 1, .dummy = ISDUMMY(2) },
	{.size = rand()%(sz/6) + 1, .dummy = ISDUMMY(3) },
	{.size = rand()%(sz/6) + 1, .dummy = ISDUMMY(4) },
	{.size = 1, .dummy = ISDUMMY(5) },
    {.size = sz, .dummy = ISDUMMY(6) }
  };
  #undef ISDUMMY
  size_t const cSize = sizeof(chunks)/sizeof(*chunks);
  for (i = 0; i < cSize - 1; i++)
    chunks[cSize - 1].size -= chunks[i].size;

  massAlloc(heap, chunks, cSize);

  size_t newSize = 0;
  for (i = 0; i < cSize; i++) {
    newSize += chunks[i].size & ((uint16_t)-((reallocBits >> (6-i))&1));
  }
  if (random != 0xFF) {
    size_t add = rand()%(chunks[random].size);
    newSize += (add>1)?add:2 - 1;
  }

  Chunk const old = chunks[cSize/2];
  chunks[cSize/2].size = newSize;
  
  chunks[cSize/2].head = os_realloc(heap, chunks[cSize/2].head, newSize);

  for (i = 0; i < old.size; i++) {
    if (heap->driver->read(chunks[cSize/2].head+i) != cSize/2) {
	  os_error("realloc memcpy fail");
	}
  }

  writePat(heap, chunks+cSize/2, cSize/2);

  for (i = 0; i < cSize; i++) {
    if (!chunks[i].dummy && !checkPat(heap, chunks+i, i))
	  os_error("Realloc pattern broken");
  }

  bool found = false;
  for (i = 0; !found && i < cSize; i++)
    if ((1<<(6-i)) & destination) {
	  //if you can explain why this is disgustingly ugly, you get a cookie
	  if (i == cSize/2) {
	    found = chunks[cSize/2].head >= old.head && chunks[cSize/2].head < old.head + old.size;
	  } else {
	    found = chunks[cSize/2].head >= chunks[i].head && chunks[cSize/2].head < chunks[i].head + chunks[i].size;
	  }
	}

  massFree(heap, chunks, cSize);

  return found;
}

void test(char i, prog_char const* msg, Heap* heap) {

  static prog_char const spaces16[] = "                ";
  os_enterCriticalSection();
  if (heap == intHeap) {
	lcd_line1();
	lcd_writeProgString(spaces16);
    lcd_line1();
    lcd_writeChar(i);
	lcd_writeProgString(PSTR("i:"));
  }
  if (heap == extHeap) {
    lcd_line2();
	lcd_writeProgString(spaces16);
    lcd_line2();
    lcd_writeChar(i);
	lcd_writeProgString(PSTR("e:"));
  }
  lcd_writeProgString(msg);
  os_leaveCriticalSection();
  delayMs(5*DELAY);
}

void ok(Heap* heap) {
  os_enterCriticalSection();
  if (heap == intHeap) {
    lcd_goto(1,15);
  }
  if (heap == extHeap) {
    lcd_goto(2,15);
  }
  lcd_writeProgString(PSTR("OK"));
  os_leaveCriticalSection();
  delayMs(5*DELAY);
}

void runTest(Heap* heap) {
  test('1',PSTR("R.res."),heap);
  if (makeTest(heap, 0b1110101, 0b0001100, 0xFF, 0b0001000))
    ok(heap);
  else
    os_error("Right resizing fail");

  test('2',PSTR("L.res."),heap);
  if (makeTest(heap, 0b1010011, 0b0011000, 0xFF, 0b1010011))
    ok(heap);
  else
    os_error("Left resizing fail");
	
  test('3',PSTR("L.res.frc."),heap);
  if (makeTest(heap, 0b0010000, 0b0011000, 0xFF, 0b0010000))
    ok(heap);
  else
    os_error("Left resizing fail");

  test('4',PSTR("LR res."),heap);
  if (makeTest(heap, 0b1010101, 0b0001100, 2, 0b1010101))
    ok(heap);
  else
    os_error("LR resizing fail");

  test('5',PSTR("LR res.frc."),heap);
  if (makeTest(heap, 0b0010100, 0b0001100, 2, 0b0010000))
    ok(heap);
  else
    os_error("LR resizing fail");
	
	
  test('6',PSTR("Move"),heap);
  if (makeTest(heap, 0b1100001, 0b1000000, 0xFF, 0b1000001))
    ok(heap);
  else
    os_error("Move fail");

  test('7',PSTR("Shrink"),heap);
  if (makeTest(heap, 0b1110111, 0b0000000, 3, 0b0001000))
    ok(heap);
  else
    os_error("Shrink fail");

  test('8',PSTR("Keep"),heap);
  if (makeTest(heap, 0b1110111, 0b0001000, 0xFF, 0b0001000))
    ok(heap);
  else
    os_error("Keep fail");
}

bool volatile checkI = false;
bool volatile checkE = false;
	
PROGRAM(1, AUTOSTART) {
  runTest(intHeap);
  checkI = true;
}

PROGRAM(2, AUTOSTART) {
  runTest(extHeap);
  checkE = true;
}

PROGRAM(3, AUTOSTART) {
  while (!checkI || !checkE);
  lcd_clear();
  lcd_writeProgString(PSTR("ALL TESTS PASSED"));
  delayMs(3*DELAY);
  lcd_clear();
  delayMs(3*DELAY);
  lcd_writeProgString(PSTR("ALL TESTS PASSED"));
  delayMs(3*DELAY);
  lcd_clear();
  delayMs(3*DELAY);
  lcd_writeProgString(PSTR("ALL TESTS PASSED"));
  delayMs(3*DELAY);
}
