

//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
//_/_/
//_/_/  This file is part of CMSDK version 2.0.0.170915_Beta4 (15 September 2017)
//_/_/  
//_/_/  developed by
//_/_/  
//_/_/  Communicative Machines Ltd. 
//_/_/    http://cmlabs.com/
//_/_/  Copyright (c) 2017
//_/_/  
//_/_/  in collaboration with 
//_/_/  
//_/_/  Icelandic Institute for Intelligent Machines ses
//_/_/    Menntavegur 1
//_/_/    101 Reykjavik
//_/_/    ICELAND
//_/_/    http://www.iiim.is/
//_/_/
//_/_/  and is released under the following
//_/_/  
//_/_/  EXTENDED BSD License
//_/_/
//_/_/  Redistribution and use in source and binary forms, with or without
//_/_/  modification, is permitted provided that the following conditions 
//_/_/  are met:
//_/_/
//_/_/  - Redistributions of source code must retain the above copyright 
//_/_/    and collaboration notice, this list of conditions and the following 
//_/_/    disclaimer.
//_/_/
//_/_/  - Redistributions in binary form must reproduce the above copyright 
//_/_/    notice, this list of conditions and the following disclaimer in the 
//_/_/    documentation and/or other materials provided with the distribution.
//_/_/
//_/_/  - Neither the name of its copyright holders nor the names of its 
//_/_/    contributors may be used to endorse or promote products derived from 
//_/_/    this software without specific prior written permission.
//_/_/
//_/_/  - CADIA Clause: The license granted in and to the software under this 
//_/_/    agreement is a limited-use license. The software may not be used in 
//_/_/    furtherance of: (i) intentionally causing bodily injury or severe 
//_/_/    emotional harm to any person; (ii) invading the personal privacy or 
//_/_/    violating the human rights of any person; or (iii) committing or 
//_/_/    preparing for any act of war. 
//_/_/
//_/_/  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
//_/_/  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
//_/_/  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 
//_/_/  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 
//_/_/  OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
//_/_/  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
//_/_/  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
//_/_/  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
//_/_/  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
//_/_/  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
//_/_/  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//_/_/
//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
#include "TemporalMemory.h"

namespace cmlabs {

TemporalMemory::TemporalMemory(MemoryController* master) {
	mutex = NULL;
	this->master = master;
	header = NULL;
	memorySize = 0;
	port = 0;
	node = 0;
	serial = 0;
}

TemporalMemory::~TemporalMemory() {
	if (mutex)
		mutex->enter(5000, __FUNCTION__);
	if (memorySize)
		utils::CloseSharedMemorySegment((char*) header, memorySize);
	header = NULL;
	if (mutex)
		mutex->leave();
	delete(mutex);
	mutex = NULL;
}

bool TemporalMemory::getMemoryUsage(uint64& alloc, uint64& usage) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKTEMPORALMEMORYSERIAL

	alloc = memorySize;
	usage = header->usage;

	mutex->leave();
	return true;
}

bool TemporalMemory::maintenance() {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKTEMPORALMEMORYSERIAL
	checkSlots(GetTimeNow());
	mutex->leave();
	return true;
}

bool TemporalMemory::create(uint32 slotCount, uint16 binCount, uint32 minBlockSize, uint32 maxBlockSize, uint64 initSize, uint64 maxSize, uint32 growSteps) {
	serial = master->incrementDynamicShmemSerial();
	port = master->getID();

	uint32 bitFieldSize = utils::Calc32BitFieldSize(slotCount);

	uint64 binDataSize = initSize - ( sizeof(TemporalMemoryStruct) + bitFieldSize + (slotCount * sizeof(SlotEntryStruct)) );

	// Calculate virgin bin sizes
	std::vector<BinHeaderStruct> binHeaders = calcBinHeaders(binCount, minBlockSize, maxBlockSize, binDataSize);
	//if (binHeaders.size() <= 2)
	//	return false;

	uint64 newInitSize = sizeof(TemporalMemoryStruct) + bitFieldSize + (slotCount * sizeof(SlotEntryStruct));
	std::vector<BinHeaderStruct>::iterator i = binHeaders.begin(), e = binHeaders.end();
	while (i != e) {
		newInitSize += i->size;
		i++;
	}

	mutex = new utils::Mutex(utils::StringFormat("PsycloneTemporalMemoryMutex_%u", port).c_str(), true);
	if (!mutex->enter(5000, __FUNCTION__))
		return false;
	
	memorySize = newInitSize;

	header = (TemporalMemoryStruct*) utils::CreateSharedMemorySegment(utils::StringFormat("PsycloneTemporalMemory_%u_%u", port, serial).c_str(), memorySize, true);
	if (!header) {
		mutex->leave();
		return false;
	}
	memset((char*)header, 0, (size_t)(sizeof(TemporalMemoryStruct) + header->bitFieldSize + (slotCount * sizeof(SlotEntryStruct))));
	header->size = memorySize;
	header->maxSize = maxSize;
	if (memorySize >= maxSize)
		header->growStep = 0;
	else if (maxSize - memorySize < 10000000) // 10MB
		header->growStep = maxSize - memorySize;
	else
		header->growStep = (maxSize - memorySize) / (growSteps ? growSteps : 8);
	header->cid = TEMPORALMEMORYID;
	header->createdTime = GetTimeNow();
	header->slotCount = slotCount;
	header->slotsInUse = 0;
	header->checkTimeInterval = 1000000; // 1 sec
	header->binCount = binCount;
	header->bitFieldSize = bitFieldSize;
	memset((char*)header + sizeof(TemporalMemoryStruct), 255, bitFieldSize);
	header->usage = sizeof(TemporalMemoryStruct) + header->bitFieldSize + (slotCount * sizeof(SlotEntryStruct)); // for now, update below
	this->port = port;
	master->setDynamicShmemSize(memorySize);

	//printf("%s", utils::PrintBitFieldString(((char*)header + sizeof(TemporalMemoryStruct)), header->slotCount, NULL).c_str()); fflush(stdout);
	//printf("%s", utils::PrintBitFieldString((const char*)header + sizeof(TemporalMemoryStruct), header->slotCount, NULL).c_str()); fflush(stdout);

	// init all slots
	slotEntries = (SlotEntryStruct*)(((char*)header) + sizeof(TemporalMemoryStruct) + header->bitFieldSize);
	memset((char*)slotEntries, 0, (size_t)(slotCount * sizeof(SlotEntryStruct)));

	uint64 binHeaderUsage = 0;
	// init all bins
	BinHeaderStruct* binHeader = (BinHeaderStruct*)(((char*)header) + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (slotCount * sizeof(SlotEntryStruct)));

	i = binHeaders.begin(), e = binHeaders.end();
	while (i != e) {
		*binHeader = *i;
		// reset bitfield
		memset((char*)binHeader + sizeof(BinHeaderStruct), 255, binHeader->bitFieldSize);
		// don't worry about the data, just leave it uninitialised
		binHeaderUsage += binHeader->usage; // should be sizeof(BinHeaderStruct) + binHeader->bitFieldSize
		i++;
		binHeader = (BinHeaderStruct*)(((char*)binHeader) + binHeader->size);
	}

	header->usage = sizeof(TemporalMemoryStruct) + header->bitFieldSize + (slotCount * sizeof(SlotEntryStruct)) + binHeaderUsage;

	mutex->leave();
	return true;
}

std::vector<BinHeaderStruct> TemporalMemory::calcBinHeaders(uint32 binCount, uint32 minBlockSize, uint32 maxBlockSize, uint64 binDataSize) {
	// we have binDataSize in total, split into binCount bins and set blockSizes from minBlockSize to maxBlockSize
	uint32 n;
	std::vector<BinHeaderStruct> binHeaders(binCount);

	// initial sanity checks
	if ((binCount < 1) || (minBlockSize > maxBlockSize) || (binDataSize < (binCount * minBlockSize)))
		return binHeaders;

	if (binCount == 1) {
		binHeaders[0].size = binDataSize;
		binHeaders[0].blockSize = minBlockSize;
		binHeaders[0].blocksInUse = 0;
		binHeaders[0].blockCount = (uint32)((binDataSize - sizeof(BinHeaderStruct)) / minBlockSize);
		binHeaders[0].bitFieldSize = utils::Calc32BitFieldSize(binHeaders[0].blockCount);
		return binHeaders;
	}
	else if (binCount == 2) {
		binHeaders[0].size = binDataSize / 2;
		binHeaders[0].blockSize = minBlockSize;
		binHeaders[0].blocksInUse = 0;
		binHeaders[0].blockCount = (uint32)((binHeaders[0].size - sizeof(BinHeaderStruct)) / minBlockSize);
		binHeaders[0].bitFieldSize = utils::Calc32BitFieldSize(binHeaders[0].blockCount);

		binHeaders[1].size = binDataSize / 2;
		binHeaders[1].blockSize = maxBlockSize;
		binHeaders[1].blocksInUse = 0;
		binHeaders[1].blockCount = (uint32)((binHeaders[1].size - sizeof(BinHeaderStruct)) / maxBlockSize);
		binHeaders[1].bitFieldSize = utils::Calc32BitFieldSize(binHeaders[1].blockCount);
		return binHeaders;
	}

	binHeaders[0].size = (uint64)(binDataSize * 0.25);
	uint64 memoryLeft = binDataSize - binHeaders[0].size;
	long double allocation = 0.5 / ((double)binCount - 2);
	for (n = 1; n < binCount-1; n++) {
		binHeaders[n].size = (uint64)(binDataSize * allocation); // flooring value
		memoryLeft -= binHeaders[n].size;
	}
	binHeaders[binCount - 1].size = memoryLeft;

	// now fill in the other parameters
	memoryLeft = binDataSize;
	uint32 blockSize = minBlockSize;
	uint32 blockSizeStep = (maxBlockSize - minBlockSize) / (binCount-1);
	for (n = 0; n < binCount; n++) {
		binHeaders[n].blockSize = blockSize;
		binHeaders[n].blocksInUse = 0;
		binHeaders[n].blockCount = (uint32)((binDataSize - sizeof(BinHeaderStruct)) / blockSize);
		binHeaders[n].bitFieldSize = utils::Calc32BitFieldSize(binHeaders[n].blockCount);
		// now reduce blockCount until it fits
		while (sizeof(BinHeaderStruct) + binHeaders[n].bitFieldSize + (binHeaders[n].blockCount * blockSize) > binHeaders[n].size) {
			binHeaders[n].blockCount -= 1;
			binHeaders[n].bitFieldSize = utils::Calc32BitFieldSize(binHeaders[n].blockCount);
		}
		binHeaders[n].usage = sizeof(BinHeaderStruct) + binHeaders[n].bitFieldSize;
		// recalc size to avoid padding
		binHeaders[n].size = binHeaders[n].usage + (binHeaders[n].blockCount * blockSize);
		memoryLeft -= binHeaders[n].size;
		blockSize += blockSizeStep;
	}

	// use any memory padding remaining to add blocks to bin 0
	if (memoryLeft > minBlockSize) {
		uint32 growBlocks = (uint32)(memoryLeft / minBlockSize);
		binHeaders[0].blockCount += growBlocks;
		// now reduce blockCount until it fits
		while (sizeof(BinHeaderStruct) + binHeaders[0].bitFieldSize + (binHeaders[0].blockCount * binHeaders[0].blockSize) > binHeaders[0].size) {
			binHeaders[0].blockCount -= 1;
			binHeaders[0].bitFieldSize = utils::Calc32BitFieldSize(binHeaders[0].blockCount);
		}
	}
	return binHeaders;
}



bool TemporalMemory::open() {
	serial = master->getDynamicShmemSerial();
	uint64 size = master->getDynamicShmemSize();
	this->port = master->getID();

	bool createdMutex = false;
	if (!mutex) {
		mutex = new utils::Mutex(utils::StringFormat("PsycloneTemporalMemoryMutex_%u", port).c_str());
		createdMutex = true;
		if (!mutex->enter(5000, __FUNCTION__))
			return false;
	}
	
	TemporalMemoryStruct* newHeader = (TemporalMemoryStruct*) utils::OpenSharedMemorySegment(utils::StringFormat("PsycloneTemporalMemory_%u_%u", port, serial).c_str(), size);
	if (!newHeader) {
		if (createdMutex)
			mutex->leave();
		return false;
	}
	if (newHeader->cid != TEMPORALMEMORYID) {
		utils::CloseSharedMemorySegment((char*)newHeader, size);
		if (createdMutex)
			mutex->leave();
		return false;
	}

	memorySize = size;
	if (header) 
		utils::CloseSharedMemorySegment((char*)header, header->size);
	header = newHeader;
	slotEntries = (SlotEntryStruct*) (((char*)header) + sizeof(TemporalMemoryStruct) + header->bitFieldSize);
	if (createdMutex)
		mutex->leave();
	// else leave mutex locked as we are calling from within the object
	return true;
}

bool TemporalMemory::setNodeID(uint16 id) {
	node = id;
	return true;
}

BinHeaderStruct* TemporalMemory::getBestBin(uint32 size, uint32& bin, uint32 &blocksNeeded) {

	BinHeaderStruct* binHeader = (BinHeaderStruct*)(((char*)header) + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (header->slotCount * sizeof(SlotEntryStruct)));
	for (bin = 0; bin < ((uint32)header->binCount-1); bin++) {
		// quick-check, if msg fits ok into bin, great
		blocksNeeded = (uint32)ceil((double)size / binHeader->blockSize);
		//spaceWaisted = (uint32)((double)size) % binHeader->blockSize;
		if (blocksNeeded < 5)
			return binHeader;
		binHeader = (BinHeaderStruct*)(((char*)binHeader) + binHeader->size);
	}
	blocksNeeded = (uint32)ceil((double)size / binHeader->blockSize);
	return binHeader;

	//BinSpaceFit* fit;
	//BinSpaceFit bestFit;
	//std::vector<BinSpaceFit> fits;
	//std::sort(fits.begin(), fits.end(), BinSpaceFitSort);
	//fit = &fits[header->binCount-1];
	//if (fit->blocksNeeded)

	//bestFit.spaceWaisted = MAXINT32;
	//for (n = 0; n < header->binCount; n++) {
	//	fit = &fits[n];
	//	if (fit->spaceWaisted < 1024)
	//		return fit->bin;
	//	if (fit->spaceWaisted < bestFit.spaceWaisted) {
	//		bestFit = *fit;
	//	}
	//	if (fit->blocksNeeded)
	//	fit->bin = n;
	//	fit->blocksNeeded = (uint32)ceil((double)size / binHeader->blockSize);
	//	fit->spaceWaisted = (uint32)((double)size) % binHeader->blockSize;
	//	binHeader = (BinHeaderStruct*)(((char*)binHeader) + binHeader->size);
	//}
}

// Insert new block of memory and return full id
bool TemporalMemory::insertMessage(DataMessage* msg, uint64& id) {
	uint64 now = GetTimeNow();
	uint64 eol = msg->getEOL();
	if (eol <= now) {
		LogPrint(0, LOG_MEMORY, 0, "Could not add message to TemporalMemory, expired already");
		return false;
	}

	if (!mutex || !mutex->enter(5000, __FUNCTION__) || !msg)
		return false;
	CHECKTEMPORALMEMORYSERIAL

	checkSlots(now);

	uint32 msgSize = msg->getSize();
	// Find the appropriate slot
	uint32 slot;
	if (!utils::GetFirstFreeBitLoc(((char*)header + sizeof(TemporalMemoryStruct)), header->bitFieldSize, slot) || (slot >= header->slotCount)) {
		// run out of slots, resize
		if (!resizeSlots()) {
			LogPrint(0, LOG_MEMORY, 0, "Could not add message to TemporalMemory, slot resize error");
			return false;
		}
		if (!utils::GetFirstFreeBitLoc(((char*)header + sizeof(TemporalMemoryStruct)), header->bitFieldSize, slot) || (slot >= header->slotCount)) {
			LogPrint(0, LOG_MEMORY, 0, "Could not add message to TemporalMemory, slot post resize error");
			return false;
		}
	}
	SlotEntryStruct* slotEntry = (SlotEntryStruct*)((char*)header + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (slot * sizeof(SlotEntryStruct)));
	uint32 bin;
	uint32 blocksNeeded;
	BinHeaderStruct* binHeader = getBestBin(msg->getSize(), bin, blocksNeeded);
	if (!binHeader) {
		LogPrint(0, LOG_MEMORY, 0, "Could not add message to TemporalMemory, bin search error");
		return false;
	}

	uint32 startBlock;
	if (!utils::GetFirstFreeBitLocN(((char*)binHeader + sizeof(BinHeaderStruct)), binHeader->bitFieldSize, blocksNeeded, startBlock) || (startBlock + blocksNeeded >= binHeader->blockCount)) {

		//printf("%s", utils::PrintBitFieldString(((char*)binHeader + sizeof(BinHeaderStruct)), binHeader->bitFieldSize, binHeader->blockCount, NULL).c_str()); fflush(stdout);

		// run out of continuous space in bin, resize
		if (!resizeBin(bin, binHeader, msg->getSize())) {
			LogPrint(0, LOG_MEMORY, 0, "Could not add message to TemporalMemory, bin resize error");
			return false;
		}
		slotEntry = (SlotEntryStruct*)((char*)header + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (slot * sizeof(SlotEntryStruct)));
		binHeader = getBestBin(msg->getSize(), bin, blocksNeeded);
		if (!binHeader) {
			LogPrint(0, LOG_MEMORY, 0, "Could not add message to TemporalMemory, bin post resize search error");
			return false;
		}
		if (!utils::GetFirstFreeBitLocN(((char*)binHeader + sizeof(BinHeaderStruct)), binHeader->bitFieldSize, blocksNeeded, startBlock) || (startBlock + blocksNeeded >= binHeader->blockCount)) {
			LogPrint(0, LOG_MEMORY, 0, "Could not add message to TemporalMemory, bin post resize error");
			return false;
		}
	}

	// Insert data
	uint64 localOffset = startBlock * binHeader->blockSize;
	memcpy(((char*)binHeader) + sizeof(BinHeaderStruct) + binHeader->bitFieldSize + localOffset, msg->data, msgSize);
	
	utils::SetBitN(startBlock, blocksNeeded, BITOCCUPIED, ((char*)binHeader + sizeof(BinHeaderStruct)), binHeader->bitFieldSize);
	slotEntry->bin = bin;
	slotEntry->eol = eol;
	slotEntry->offset = localOffset;
	slotEntry->serial++;
	slotEntry->size = msgSize;
	slotEntry->usage = blocksNeeded * binHeader->blockSize;
	slotEntry->startBlock = startBlock;
	slotEntry->blockUsage = blocksNeeded;

	binHeader->usage += slotEntry->usage;
	binHeader->blocksInUse += blocksNeeded;
	header->slotsInUse++;
	header->usage += slotEntry->usage;
	utils::SetBit(slot, BITOCCUPIED, ((char*)header + sizeof(TemporalMemoryStruct)), header->bitFieldSize);

	SETMSGID(id, node, slotEntry->serial, slot);

	if (header->usage > 1000000000)
		int n = 0;

	mutex->leave();
	//printf("x x x x x x\n"); fflush(stdout);
	return true;
}

// Get copy of block of memory
DataMessage* TemporalMemory::getCopyOfMessage(uint64 id) {
	if (node != GETMSGNODE(id))
		return NULL;

	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return NULL;
	CHECKTEMPORALMEMORYSERIAL

//	checkSlots();

	char* block = getMemoryBlock(id);
	if (!block) {
		mutex->leave();
		return NULL;
	}

	DataMessage* msg = new DataMessage(block, true);
	mutex->leave();
	return msg;
}

char* TemporalMemory::getMemoryBlock(uint64 id) {
	// Assume node id has already been checked
	// Assume mutex is locked
	// Find the slot
	uint32 slot = GETMSGSLOT(id);
	if (slot > header->slotCount)
		return NULL;

	SlotEntryStruct* slotEntry = (SlotEntryStruct*)((char*)header + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (slot * sizeof(SlotEntryStruct)));

	if ((slotEntry->serial != GETMSGSERIAL(id))
		|| (slotEntry->eol < GetTimeNow())
		|| !slotEntry->size	) {
		LogPrint(0, LOG_MEMORY, 0, "Message requested from TemporalMemory has expired");
		return NULL;
	}

	BinHeaderStruct* binHeader = (BinHeaderStruct*)(((char*)header) + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (header->slotCount * sizeof(SlotEntryStruct)));
	for (uint32 n = 0; n < slotEntry->bin; n++)
		binHeader = (BinHeaderStruct*)(((char*)binHeader) + binHeader->size);

	return (((char*)binHeader) + sizeof(BinHeaderStruct) + binHeader->bitFieldSize + slotEntry->offset);
}

bool TemporalMemory::checkSlots(uint64 now) {

	if ((header->lastCheckTime && (now > header->lastCheckTime + header->checkTimeInterval)) || !header->slotsInUse)
		return true;

	//printf("%s", utils::PrintBitFieldString(((char*)header + sizeof(TemporalMemoryStruct)), header->bitFieldSize, header->slotCount, NULL).c_str()); fflush(stdout);
	uint32 maxSlot;
	if (!utils::GetLastOccupiedBitLoc(((char*)header + sizeof(TemporalMemoryStruct)), header->bitFieldSize, maxSlot)) {
		// no slots appear to be occupied, we are all good
		LogPrint(0, LOG_MEMORY, 0, "TemporalMemory::checkSlots inconsistency, no slots found, but header claims %u slots in use...", header->slotsInUse);
		return true;
		// maxSlot = header->slotCount-1;
	}
	SlotEntryStruct* slotEntry = (SlotEntryStruct*)((char*)header + sizeof(TemporalMemoryStruct) + header->bitFieldSize);

	BinHeaderStruct* binHeader;
	for (uint32 n = 0; n <= maxSlot; n++) {
		if (slotEntry->size && (now >= slotEntry->eol)) {
			// reset storage
			binHeader = (BinHeaderStruct*)(((char*)header) + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (header->slotCount * sizeof(SlotEntryStruct)));
			for (uint32 i = 0; i < slotEntry->bin; i++)
				binHeader = (BinHeaderStruct*)(((char*)binHeader) + binHeader->size);
			utils::SetBitN(slotEntry->startBlock, slotEntry->blockUsage, BITFREE, ((char*)binHeader + sizeof(BinHeaderStruct)), binHeader->bitFieldSize);
			binHeader->usage -= slotEntry->usage;
			binHeader->blocksInUse -= slotEntry->blockUsage;
			header->usage -= slotEntry->usage;
			header->slotsInUse--;

			// reset slot
			slotEntry->size = 0;
			utils::SetBit(n, BITFREE, ((char*)header + sizeof(TemporalMemoryStruct)), header->bitFieldSize);
		}
		slotEntry += 1;
	}
	return true;
}


bool TemporalMemory::resizeSlots() {
	// For now just double slot count
	uint32 slotGrowth = header->slotCount * 4;
	std::vector<BinHeaderStruct> binHeaderSizes = calcBinHeadersGrowth();
	return resizeMemory(slotGrowth, binHeaderSizes);
}

bool TemporalMemory::resizeBin(uint32 bin, BinHeaderStruct* binHeader, uint32 msgSize) {
	uint32 slotGrowth = 0;
	if ((double)header->slotsInUse / header->slotCount > 0.8)
		slotGrowth = header->slotCount * 4;
	else if ((double)header->slotsInUse / header->slotCount > 0.5)
		slotGrowth = header->slotCount * 2;
	std::vector<BinHeaderStruct> binHeaderSizes = calcBinHeadersGrowth(bin, msgSize);
	return resizeMemory(slotGrowth, binHeaderSizes);
}

std::vector<BinHeaderStruct> TemporalMemory::calcBinHeadersGrowth(uint32 bin, uint32 msgSize) {
	// we have binDataSize in total, split into binCount bins and set blockSizes from minBlockSize to maxBlockSize
	std::vector<BinHeaderStruct> binHeaders(header->binCount);

	BinHeaderStruct* binHeader = (BinHeaderStruct*)(((char*)header) + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (header->slotCount * sizeof(SlotEntryStruct)));
	for (uint32 n = 0; n < header->binCount; n++) {
		binHeaders[n] = *binHeader;
		if ( ((double)binHeader->usage / binHeader->size > 0.8)
				|| (msgSize && (n == bin)) ) {
			binHeaders[n].blockCount = clmax(binHeaders[n].blockCount * 4, binHeaders[n].blockCount + (uint32)(20.0*((double)msgSize/binHeader->blockSize)));
			binHeaders[n].bitFieldSize = utils::Calc32BitFieldSize(binHeaders[n].blockCount);
			binHeaders[n].size = sizeof(BinHeaderStruct) + binHeaders[n].bitFieldSize + (binHeaders[n].blockCount * binHeaders[n].blockSize);
			// usage remains the same, of course
		}
		binHeader = (BinHeaderStruct*)(((char*)binHeader) + binHeader->size);
	}
	return binHeaders;
}

bool TemporalMemory::resizeMemory(uint32 slotGrowth, std::vector<BinHeaderStruct>& binHeaderSizes) {

	uint32 slotCount = header->slotCount + slotGrowth;
	uint32 bitFieldSize = utils::Calc32BitFieldSize(slotCount);
	uint64 newTotalSize = sizeof(TemporalMemoryStruct) + bitFieldSize + (slotCount * sizeof(SlotEntryStruct));

	for (uint32 n = 0; n < header->binCount; n++) {
		newTotalSize += binHeaderSizes[n].size;
	}

	if (newTotalSize > header->maxSize) {
		LogPrint(0, LOG_MEMORY, 0, "Temporal Memory hit maximum size %s when asked to grow by: %s to: %s total",
			utils::BytifySize((double)header->maxSize).c_str(),
			utils::BytifySize((double)newTotalSize - header->size).c_str(),
			utils::BytifySize((double)newTotalSize).c_str()
			);
		return false;
	}

	serial = master->incrementDynamicShmemSerial();

	TemporalMemoryStruct* newHeader = (TemporalMemoryStruct*) utils::CreateSharedMemorySegment(utils::StringFormat("PsycloneTemporalMemory_%u_%u", port, serial).c_str(), newTotalSize, true);
	if (!newHeader) {
		LogPrint(0, LOG_MEMORY, 0, "Temporal Memory couldn't allocate memory to grow by: %s to: %s total as requested",
			utils::BytifySize((double)newTotalSize - header->size).c_str(),
			utils::BytifySize((double)newTotalSize).c_str()
			);
		return false;
	}
	
	// First set the full new bitfield to unoccupied
	memset((char*)newHeader + sizeof(TemporalMemoryStruct), 255, bitFieldSize);
	// Copy old header and old bitFieldSize
	memcpy((char*)newHeader, header, sizeof(TemporalMemoryStruct) + header->bitFieldSize);
	newHeader->size = newTotalSize;
	// First set all new entries to 0
	memset((char*)newHeader + sizeof(TemporalMemoryStruct) + bitFieldSize, 0, (size_t)(slotCount * sizeof(SlotEntryStruct)));
	// Copy in old entries
	memcpy((char*)newHeader + sizeof(TemporalMemoryStruct) + bitFieldSize,
		(char*)header + sizeof(TemporalMemoryStruct) + header->bitFieldSize, (size_t)(header->slotCount * sizeof(SlotEntryStruct)));

	newHeader->slotCount = slotCount;
	newHeader->bitFieldSize = bitFieldSize;

	uint64 binGrowth = 0;
	BinHeaderStruct* binHeader = (BinHeaderStruct*)(((char*)header) + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (header->slotCount * sizeof(SlotEntryStruct)));
	BinHeaderStruct* newBinHeader = (BinHeaderStruct*)(((char*)newHeader) + sizeof(TemporalMemoryStruct) + newHeader->bitFieldSize + (newHeader->slotCount * sizeof(SlotEntryStruct)));
	for (uint32 n = 0; n < header->binCount; n++) {
		if (binHeaderSizes[n].size == binHeader->size) {
			// Just copy the full bin as is, header and all
			memcpy((char*)newBinHeader, binHeader, (size_t)binHeader->size);
		}
		else {
			binGrowth++;
			// First set the full new bitfield to unoccupied
			memset((char*)newBinHeader + sizeof(BinHeaderStruct), 255, binHeaderSizes[n].bitFieldSize);
			// Copy old header and old bitFieldSize
			memcpy((char*)newBinHeader, binHeader, sizeof(BinHeaderStruct) + binHeader->bitFieldSize);
			newBinHeader->bitFieldSize = binHeaderSizes[n].bitFieldSize;
			newBinHeader->blockCount = binHeaderSizes[n].blockCount;
			newBinHeader->size = binHeaderSizes[n].size;
			// Copy existing data
			memcpy((char*)newBinHeader + sizeof(BinHeaderStruct) + newBinHeader->bitFieldSize,
				(char*)binHeader + sizeof(BinHeaderStruct) + binHeader->bitFieldSize,
				(size_t)(binHeader->size - (sizeof(BinHeaderStruct) + binHeader->bitFieldSize)));
		}
		binHeader = (BinHeaderStruct*)(((char*)binHeader) + binHeader->size);
		newBinHeader = (BinHeaderStruct*)(((char*)newBinHeader) + newBinHeader->size);
	}

	if (slotGrowth && binGrowth)
		LogPrint(0, LOG_MEMORY, 0, "Temporal Memory slots grown by: %u and %u bins storage by %s to: %s total as requested",
			slotGrowth,
			binGrowth,
			utils::BytifySize((double)newTotalSize - header->size).c_str(),
			utils::BytifySize((double)newTotalSize).c_str()
			);
	else if (slotGrowth)
		LogPrint(0, LOG_MEMORY, 0, "Temporal Memory slots grown by: %u (%s to: %s total) as requested",
			slotGrowth,
			utils::BytifySize((double)newTotalSize - header->size).c_str(),
			utils::BytifySize((double)newTotalSize).c_str()
			);
	else
		LogPrint(0, LOG_MEMORY, 0, "Temporal Memory %u storage bins grown by: %s to: %s total as requested",
			binGrowth,
			utils::BytifySize((double)newTotalSize - header->size).c_str(),
			utils::BytifySize((double)newTotalSize).c_str()
			);

	// Get rid of the old memory allocation
	utils::CloseSharedMemorySegment((char*)header, memorySize);
	header = newHeader;
	memorySize = newTotalSize;
	master->setDynamicShmemSize(memorySize);
	
	return true;
}


//bool TemporalMemory::getSlotFromEOL(uint64 eol, uint16& slot) {
//	// Assume mutex is locked
//	uint64 now = GetTimeNow();
//	uint64 difTime = 0;
//	if (eol - now < 0)
//		return false;
//	difTime = eol - header->startTime;
//	uint16 difSlot = (uint16)(difTime / header->slotDuration);
//	// If too far in the future, choose end slot
//	if (difSlot >= header->slotCount)
//		difSlot = header->slotCount - 1;
//	// Find actual slot...
//	uint16 actualSlot = header->startSlot + difSlot;
//	if (actualSlot >= header->slotCount)
//		actualSlot -= header->slotCount;
//	slot = actualSlot;
//	return true;
//}
//
//bool TemporalMemory::growSlotSize(uint64 growAmount) {
//	// Assume mutex is locked
//
//	uint32 n;
//	//uint64 totalSize = sizeof(TemporalMemoryStruct) + (header->slotCount * sizeof(SlotHeaderStruct));
//	//for (n = 0; n<header->slotCount; n++)
//	//	totalSize += slotHeaders[n].size;
//	//if (totalSize > header->size) {
//	//	printf("Error!!!\n");
//	//}
//
//	uint64 totalGrowAmount = growAmount * header->slotCount;
//	if ((header->size >= header->maxSize) || !header->growStep) {
//		LogPrint(0, LOG_MEMORY, 0, "Temporal Memory has reached maximum size: %s total, %s per slot, cannot grow by %s as requested",
//			utils::BytifySize((double)header->size).c_str(),
//			utils::BytifySize((double)header->slotSize).c_str(),
//			utils::BytifySize((double)totalGrowAmount).c_str()
//			);
//		return false;
//	}
//
//	uint64 newTotalSize = header->size + header->growStep;
//	while (newTotalSize - header->size < totalGrowAmount)
//		newTotalSize += header->growStep;
//	
//	if (newTotalSize > header->maxSize) {
//		LogPrint(0, LOG_MEMORY, 0, "Temporal Memory cannot grow to %s as requested, current size: %s, maximum limit: %s",
//			utils::BytifySize((double)newTotalSize).c_str(),
//			utils::BytifySize((double)header->size).c_str(),
//			utils::BytifySize((double)header->maxSize).c_str()
//			);
//		return false;
//	}
//
//	serial = master->incrementDynamicShmemSerial();
//
//	TemporalMemoryStruct* newHeader = (TemporalMemoryStruct*) utils::CreateSharedMemorySegment(utils::StringFormat("PsycloneTemporalMemory_%u_%u", port, serial).c_str(), newTotalSize, true);
//	if (!newHeader) {
//		LogPrint(0, LOG_MEMORY, 0, "Temporal Memory couldn't allocate memory to grow by: %s to: %s total, %s per slot as requested",
//			utils::BytifySize((double)totalGrowAmount).c_str(),
//			utils::BytifySize((double)newTotalSize).c_str(),
//			utils::BytifySize((double)(newTotalSize/header->slotCount)).c_str()
//			);
//		return false;
//	}
//
//	// Copy header and all the slot headers
//	memcpy(newHeader, header, sizeof(TemporalMemoryStruct) + (header->slotCount * sizeof(SlotHeaderStruct)));
//	newHeader->size = newTotalSize;
//	uint64 newDataSize = newTotalSize - sizeof(TemporalMemoryStruct) - (header->slotCount * sizeof(SlotHeaderStruct));
//	newHeader->slotSize = (uint64)(newDataSize / newHeader->slotCount);
//	SlotHeaderStruct* newSlotHeaders = (SlotHeaderStruct*)(((char*)newHeader) + sizeof(TemporalMemoryStruct));
//
//	uint64 newOffset = sizeof(TemporalMemoryStruct) + (header->slotCount * sizeof(SlotHeaderStruct));
//
//	// Now update each slot and copy the slot content across
//	for (n=0; n<header->slotCount; n++) {
//		SlotHeaderStruct* oldSlotHeader = &slotHeaders[n];
//		SlotHeaderStruct* newSlotHeader = &newSlotHeaders[n];
//		newSlotHeader->size = newHeader->slotSize;
//		newSlotHeader->offset = newOffset;
//		memcpy((char*)newHeader + newSlotHeader->offset, (char*)header + oldSlotHeader->offset, (size_t)oldSlotHeader->usage);
//		newOffset += newHeader->slotSize;
//	}
//
//	// Get rid of the old memory allocation
//	utils::CloseSharedMemorySegment((char*)header, memorySize);
//	header = newHeader;
//	memorySize = newTotalSize;
//	slotHeaders = newSlotHeaders;
//	master->setDynamicShmemSize(memorySize);
//
//	//totalSize = sizeof(TemporalMemoryStruct) + (header->slotCount * sizeof(SlotHeaderStruct));
//	//for (n = 0; n<header->slotCount; n++)
//	//	totalSize += slotHeaders[n].size;
//	//if (totalSize > header->size) {
//	//	printf("Error: !!!\n");
//	//}
//
//	LogPrint(0, LOG_MEMORY, 0, "Temporal Memory size is now: %s total, %s per slot",
//		utils::BytifySize((double)header->size).c_str(),
//		utils::BytifySize((double)header->slotSize).c_str()
//		);
//	return true;
//}
//
//bool TemporalMemory::checkSlots() {
//	// Assume mutex is locked
//	uint64 now = GetTimeNow();
//	while ( now - header->startTime > header->slotDuration) {
//		// Move the current slot one forward
//		header->startSlot++;
//		if (header->startSlot > header->slotCount)
//			header->startSlot -= header->slotCount;
//		header->startTime += header->slotDuration;
//	//	printf("New startSlot: %u (%d)...\n", header->startSlot, GetTimeNow() - header->startTime);
//		// Find the slot
//		int16 delSlot = header->startSlot - 1;
//		if (delSlot < 0)
//			delSlot += header->slotCount;
//	//	LogPrint(0,LOG_MEMORY,0,"Resetting slot %u", delSlot);
//		resetSlot(delSlot, header->startTime + (header->slotCount*header->slotDuration));
//	}
//	return true;
//}
//
//bool TemporalMemory::resetSlot(uint16 slot, uint64 eol) {
////	LogPrint(0,LOG_MEMORY,0,"Resetting slot %u", slot);
//	SlotHeaderStruct* slotHeader = &slotHeaders[slot];
//	slotHeader->count = 0;
//	slotHeader->usage = 0;
//	slotHeader->eol = eol;
//	// we could resize the slot back to normal to save memory
//	return true;
//}
//
//uint64 TemporalMemory::calcResizeAmount(uint32 msgSize) {
//	if (msgSize < 10000)
//		return 256000;
//	else if (msgSize < 100000)
//		return 1024000;
//	else
//		return msgSize*10;
//}

bool TemporalMemory::logUsage() {
	std::string str = utils::StringFormat("TemporalMemory \tsize: %s \tusage: %s \tslots: %u \tinuse: %u\n",
		utils::BytifySize((double)header->size).c_str(),
		utils::BytifySize((double)header->usage).c_str(),
		header->slotCount,
		header->slotsInUse
		);

	BinHeaderStruct* binHeader = (BinHeaderStruct*)(((char*)header) + sizeof(TemporalMemoryStruct) + header->bitFieldSize + (header->slotCount * sizeof(SlotEntryStruct)));
	for (uint32 n = 0; n < header->binCount; n++) {
		str += utils::StringFormat("  Bin [%s]\tsize: %s \tusage: %s \tblocks: %u \tinuse: %u\n",
			utils::BytifySize((double)binHeader->blockSize).c_str(),
			utils::BytifySize((double)binHeader->size).c_str(),
			utils::BytifySize((double)binHeader->usage).c_str(),
			utils::BytifySize((double)binHeader->blockCount).c_str(),
			utils::BytifySize((double)binHeader->blocksInUse).c_str()
			);
		binHeader = (BinHeaderStruct*)(((char*)binHeader) + binHeader->size);
	}
	LogPrint(0, LOG_MEMORY, 0, str.c_str());
	return true;
}

bool TemporalMemory::UnitTest() {
	uint32 slotCount = 1000;
	uint16 binCount = 2;
	uint32 minBlockSize = 1024; // 1kb
	uint32 maxBlockSize = 64*1024; // 256kb
	uint64 initSize = 50000000; // 50MB
	uint64 maxSize = 500000000; // 500MB
	uint64 msgTTL = 5000000;
	uint32 dataSize;

	uint32 outerCircle = 100;
	uint32 innerCircle = 30;

	MasterMemory masterMemory;
	if (!masterMemory.create(12000)) {
		LogPrint(0, LOG_MEMORY, 0, "Cannot create MasterMemory...");
		return false;
	}

	TemporalMemory temporalMemory(&masterMemory);

	if (!temporalMemory.create(slotCount, binCount, minBlockSize, maxBlockSize, initSize, maxSize)) {
		LogPrint(0, LOG_MEMORY, 0, "Cannot create TemporalMemory...");
		return false;
	}

	uint64 n, m, bytes;
	DataMessage* msg;
	uint64 now = 0;
	uint64 id, sysTotal, sysUsage;

	if (temporalMemory.getMemoryUsage(sysTotal, sysUsage))
		printf("[---] Temporal Memory in use:\t%10s\t[%s allocated])\n", utils::BytifySize((double)sysUsage).c_str(), utils::BytifySize((double)sysTotal).c_str());


	char* data;

	std::map<uint64, uint64> sentMessages;

	for (n = 0; n<outerCircle; n++) {
		now = GetTimeNow();
		bytes = 0;
		for (m = 0; m<innerCircle; m++) {
			msg = new DataMessage();
			dataSize = (uint32)utils::RandomValue(minBlockSize / 3.0, maxBlockSize);
			data = new char[dataSize];
			msg->setData("Data", data, dataSize);
			delete[] data;
			msg->setCreatedTime(now);
			msg->setTTL(msgTTL);
			msg->setSerial((n*1000) + m);
			if (!temporalMemory.insertMessage(msg, id)) {
				fprintf(stderr, "[%llu][%llu] TemporalMemory insert memory failed...\n", n, m);
				delete(msg);
				return false;
			}
			bytes += msg->getSize();
			sentMessages[(n * 1000) + m] = id;
			delete(msg);
		}
		if (temporalMemory.getMemoryUsage(sysTotal, sysUsage))
			printf("[%llu] Temporal Memory in use:\t%10s\t[%s allocated] - %.3fms/msg - %.3fMB/sec\n",
				n, utils::BytifySize((double)sysUsage).c_str(), utils::BytifySize((double)sysTotal).c_str(),
				(double)(GetTimeAge(now)/innerCircle)/1000.0,
				((double)bytes / ((double)GetTimeAge(now)/1000000)) / (1024.0*1024.0)
				);
//		temporalMemory.logUsage();

		for (m = 0; m<innerCircle; m++) {
			msg = temporalMemory.getCopyOfMessage(sentMessages[(n * 1000) + m]);
			if (!msg) {
				fprintf(stderr, "[%llu][%llu] TemporalMemory retrieve failed...\n", n, m);
				delete(msg);
				return false;
			}
			else if (!msg->isValid()) {
				fprintf(stderr, "[%llu][%llu] TemporalMemory retrieve got invalid data...\n", n, m);
				delete(msg);
				return false;
			}
			if (msg->getSerial() != (n * 1000) + m) {
				fprintf(stderr, "[%llu][%llu] TemporalMemory retrieve got wrong message back (%llu != %llu)...\n", n, m, msg->getSerial(), (n * 1000) + m);
				//delete(msg);
				//return false;
			}
			delete(msg);
		}

		utils::Sleep(100);
	}

//	temporalMemory.logUsage();

	for (n = 0; n<outerCircle/2; n++) {
		temporalMemory.maintenance();
		if (temporalMemory.getMemoryUsage(sysTotal, sysUsage))
			printf("[%llu] Temporal Memory in use:\t%10s\t[%s allocated]\n", n, utils::BytifySize((double)sysUsage).c_str(), utils::BytifySize((double)sysTotal).c_str());
		utils::Sleep(200);
	}

//	temporalMemory.logUsage();
	fprintf(stdout, "*** Temporal Memory Management test ran successfully ***\n\n\n");

	return true;
}

} // namespace cmlabs













