

//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
//_/_/
//_/_/  This file is part of CMSDK version 2.1.1.260726 (26 July 2026)
//_/_/  
//_/_/  developed by
//_/_/  
//_/_/  Communicative Machines Ltd. 
//_/_/    http://cmlabs.com/
//_/_/  Copyright (c) 2026
//_/_/  
//_/_/  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 "VantagePoints.h"
#include "PsyTime.h"
#include "UnitTestFramework.h"

namespace cmlabs {

VantagePoints::VantagePoints() {
	count = 0;
	inUseMap = NULL;
	state = IDLE;
	intrinsicTimescaleMS = 60000;
	timeoutMS = 60000;
	motionStartedTime = 0;
}

VantagePoints::~VantagePoints() {
}

bool VantagePoints::init(const char* xml) {
	if (!xml)
		return false;

	//<vantagepoints>
	//	<point id="1" label="A" data="2,2,45,6" overlap="0.1,0.3,0.5" />
	//	<point id="2" label="B" data="2,2,45,6" overlap="0.1,0.3,0.5" />
	//	<point id="3" label="C" data="2,2,45,6" overlap="0.1,0.3,0.5" />
	//</vantagepoints>

	XMLResults xmlResults;
	XMLNode node = XMLNode::parseString(xml, "vantagepoints", &xmlResults);

	// display error message (if any)
	if (xmlResults.error != eXMLErrorNone)
		return false;

	return init(node);
}
bool VantagePoints::init(XMLNode node) {
	XMLNode subNode;
	const char* str = node.getAttribute("timescale"); // in secs
	if (str)
		intrinsicTimescaleMS = utils::Ascii2Uint32(str) * 1000;

	str = node.getAttribute("timeout"); // in secs
	if (str)
		timeoutMS = utils::Ascii2Uint32(str) * 1000;

	count = (uint32)node.nChildNode("point");
	if (!count) {
		LogPrint(0, 0, 1, "No points found");
		return false;
	}

	inUseMap = new uint32[count+1]; // don't use index 0
	memset(inUseMap, 0, (count+1) * sizeof(uint32));

	std::vector<std::string> overlapsStr;
	std::vector<std::string>::iterator i, e;
	PerchPoint point;
	const char* id, *label, *data, *overlap;
	int n = 0;
	while (!(subNode = node.getChildNode("point", n++)).isEmpty()) {
		if ((id = subNode.getAttribute("id")) &&
			(label = subNode.getAttribute("label")) &&
			(data = subNode.getAttribute("data")) &&
			(overlap = subNode.getAttribute("overlap"))) {
			point.id = utils::Ascii2Uint32(id);
			if (point.id > count) {
				LogPrint(0, 0, 1, "Point %s ID is too high: %u > %u", id, point.id, count);
				return false;
			}
			if (points.find(point.id) != points.end())
				return false;
			point.entityID = 0;
			point.state = FREE;
			point.lastVisit = GetTimeNow();
			point.label = label;
			point.data = data;
			overlapsStr = utils::TextListSplit(overlap, ",");
			if (overlapsStr.size() == 0) {
				// put equal overlap values in
				for (uint32 n = 0; n <= count; n++) // index 1 based, so add and ignore index 0
					point.overlaps.push_back(1);
			}
			else {
				if (overlapsStr.size() != count) {
					LogPrint(0, 0, 1, "Not enough overlaps for point %s, need %u, only found %u", id, count, overlapsStr.size());
					return false;
				}
				point.overlaps.clear();
				point.overlaps.push_back(0); // index 1 based, so add and ignore index 0
				i = overlapsStr.begin();
				e = overlapsStr.end();
				while (i != e) {
					point.overlaps.push_back(utils::Ascii2Float64((*i).c_str()));
					i++;
				}
			}
			// ensure no benefit to move to itself :-)
			point.overlaps[point.id] = 1; // 100% overlap
			points[point.id] = point;
		}
		else {
			if (!id || !label)
				LogPrint(0, 0, 1, "Point id or name not specified");
			else if (!overlap)
				LogPrint(0, 0, 1, "Point %s ID has no overlap specified", point.id);
			else if (!data)
				LogPrint(0, 0, 1, "Point %s ID has no data specified", point.id);
			return false;
		}
	}
	return true;
}


const char* VantagePoints::getPointData(uint32 id) {
	if (!count || !id || (id > count))
		return NULL;
	PerchPoint* perch = &(points[id]);
	return perch->data.c_str();
}

const char* VantagePoints::getPointLabel(uint32 id) {
	if (!count || !id || (id > count))
		return NULL;
	PerchPoint* perch = &(points[id]);
	return perch->label.c_str();
}


uint32 VantagePoints::reserveNextPoint(uint32 entityID) {
	if (!count || (motionStartedTime && (GetTimeAgeMS(motionStartedTime) < timeoutMS)))
		return 0;

	uint32 n;
	double bestVal, val;
	PerchPoint* perch = NULL;

	uint32 ownPerch = 0, bestPerch = 0, occPerch = 0, eID;
	if (state == IDLE)
		bestPerch = 1;
	else {
		// find own current perch and any occupied perch
		for (n = 1; n <= count; n++) {
			if (!(eID = inUseMap[n]))
				continue;
			perch = &(points[n]);
			if ((perch->state == RESERVED) && (GetTimeAgeMS(perch->lastVisit) > timeoutMS)) {
				perch->state = FREE;
				perch->entityID = 0;
				inUseMap[perch->id] = 0;
				continue;
			}
			occPerch = n;
			if (!ownPerch && (eID == entityID)) {
				ownPerch = n;
				continue;
			}
		}
		if (!occPerch)
			bestPerch = 1;
		else {
			// go find best unoccupied perch from there
			if (ownPerch)
				perch = &(points[ownPerch]);
			else
				perch = &(points[occPerch]);
			bestVal = 0;
			for (n = 1; n <= count; n++) {
				if (!inUseMap[n]) {
					val = 1 - perch->overlaps[n];
					if (intrinsicTimescaleMS)
						val *= ((double)GetTimeAgeMS((&(points[n]))->lastVisit) / intrinsicTimescaleMS);
					if (val > bestVal) {
						bestVal = val;
						bestPerch = n;
					}
				}
			}
		}
	}
	
	if (!bestPerch)
		return 0;

	uint64 now = GetTimeNow();

	if (ownPerch) {
		perch = &(points[ownPerch]);
		perch->state = FREE;
		perch->entityID = 0;
		perch->lastVisit = now;
		inUseMap[perch->id] = 0;
	}

	perch = &(points[bestPerch]);
	perch->state = RESERVED;
	perch->entityID = entityID;
	perch->lastVisit = now;
	inUseMap[perch->id] = entityID;
	state = INMOTION;
	motionStartedTime = now;
	return perch->id;
}

bool VantagePoints::occupyPoint(uint32 id, uint32 entityID) {
	if (!count || !id || (id > count) || !entityID)
		return false;

	PerchPoint* perch = &(points[id]);
	if (perch->state != RESERVED)
		return false;
	if (perch->entityID != entityID)
		return false;

	perch->state = OCCUPIED;
	state = SETTLED;
	motionStartedTime = 0;
	return true;
}

bool VantagePoints::freePoint(uint32 id, uint32 entityID) {
	if (!count || !id || (id > count) || !entityID)
		return false;

	PerchPoint* perch = &(points[id]);
	if (perch->state != RESERVED)
		return false;
	if (perch->entityID != entityID)
		return false;

	inUseMap[id] = 0;
	perch->state = FREE;
	state = SETTLED;
	motionStartedTime = 0;
	return true;
}



bool VantagePoints::UnitTest() {
	unittest::progress(0, "init from XML");

	// 1. Initialisation from XML. Three points, overlap row length == count (3).
	const char* xml =
		"<vantagepoints timescale=\"60\" timeout=\"60\">\n"
		"	<point id=\"1\" label=\"A\" data=\"2,2,45,6\" overlap=\"1.0,0.3,0.5\" />\n"
		"	<point id=\"2\" label=\"B\" data=\"3,3,90,7\" overlap=\"0.3,1.0,0.2\" />\n"
		"	<point id=\"3\" label=\"C\" data=\"4,4,30,8\" overlap=\"0.5,0.2,1.0\" />\n"
		"</vantagepoints>\n";

	{
		VantagePoints vp;
		if (!vp.init(xml)) { unittest::fail("VantagePoints test: init from XML failed"); return false; }
		if (vp.count != 3) { unittest::fail("VantagePoints test: count != 3 (got %u)", vp.count); return false; }
		if (vp.state != IDLE) { unittest::fail("VantagePoints test: initial state != IDLE"); return false; }

		// 2. Point accessors
		unittest::progress(20, "point accessors");
		const char* lbl = vp.getPointLabel(1);
		if (!lbl || strcmp(lbl, "A") != 0) { unittest::fail("VantagePoints test: getPointLabel(1) != A"); return false; }
		const char* dat = vp.getPointData(2);
		if (!dat || strcmp(dat, "3,3,90,7") != 0) { unittest::fail("VantagePoints test: getPointData(2) mismatch"); return false; }
		// out-of-range / zero id must return NULL
		if (vp.getPointLabel(0) != NULL) { unittest::fail("VantagePoints test: getPointLabel(0) not NULL"); return false; }
		if (vp.getPointData(99) != NULL) { unittest::fail("VantagePoints test: getPointData(99) not NULL"); return false; }

		// 3. Reserve -> occupy lifecycle.
		unittest::progress(40, "reserve and occupy");
		uint32 entity = 1001;
		uint32 reserved = vp.reserveNextPoint(entity);
		// From IDLE the implementation always reserves point 1.
		if (reserved != 1) { unittest::fail("VantagePoints test: first reserve expected id 1 (got %u)", reserved); return false; }
		if (vp.state != INMOTION) { unittest::fail("VantagePoints test: state != INMOTION after reserve"); return false; }

		// While in motion (motionStartedTime set, within timeout) a new reserve must back off.
		if (vp.reserveNextPoint(2002) != 0) { unittest::fail("VantagePoints test: reserve during motion should return 0"); return false; }

		// Occupy with wrong entity must fail.
		if (vp.occupyPoint(reserved, 9999)) { unittest::fail("VantagePoints test: occupy with wrong entity succeeded"); return false; }
		// Occupy with correct entity must succeed and settle.
		if (!vp.occupyPoint(reserved, entity)) { unittest::fail("VantagePoints test: occupy with correct entity failed"); return false; }
		if (vp.state != SETTLED) { unittest::fail("VantagePoints test: state != SETTLED after occupy"); return false; }
		// Occupying an already-occupied (non-RESERVED) point must fail.
		if (vp.occupyPoint(reserved, entity)) { unittest::fail("VantagePoints test: re-occupy of OCCUPIED point succeeded"); return false; }
	}

	// 4. Reserve -> free lifecycle (free only works on a RESERVED point).
	unittest::progress(60, "reserve and free");
	{
		VantagePoints vp;
		if (!vp.init(xml)) { unittest::fail("VantagePoints test: re-init failed"); return false; }
		uint32 entity = 7;
		uint32 reserved = vp.reserveNextPoint(entity);
		if (!reserved) { unittest::fail("VantagePoints test: reserve returned 0"); return false; }
		// free with wrong entity must fail.
		if (vp.freePoint(reserved, 1234)) { unittest::fail("VantagePoints test: free with wrong entity succeeded"); return false; }
		// free with correct entity on a RESERVED point must succeed.
		if (!vp.freePoint(reserved, entity)) { unittest::fail("VantagePoints test: free with correct entity failed"); return false; }
		// freeing a now-FREE point must fail (not RESERVED).
		if (vp.freePoint(reserved, entity)) { unittest::fail("VantagePoints test: free of FREE point succeeded"); return false; }
	}

	// 5. Edge / robustness: uninitialised instance and bad XML.
	unittest::progress(80, "edge cases");
	{
		VantagePoints vp;
		if (vp.count != 0) { unittest::fail("VantagePoints test: fresh count != 0"); return false; }
		if (vp.reserveNextPoint(1) != 0) { unittest::fail("VantagePoints test: reserve on empty != 0"); return false; }
		if (vp.occupyPoint(1, 1)) { unittest::fail("VantagePoints test: occupy on empty succeeded"); return false; }
		if (vp.freePoint(1, 1)) { unittest::fail("VantagePoints test: free on empty succeeded"); return false; }
		if (vp.getPointData(1) != NULL) { unittest::fail("VantagePoints test: getPointData on empty not NULL"); return false; }
		if (vp.init((const char*)NULL)) { unittest::fail("VantagePoints test: init(NULL) succeeded"); return false; }
	}

	// 6. Throughput metric: repeated reserve/occupy/free cycles.
	unittest::progress(90, "throughput");
	{
		const uint32 iters = 2000;
		uint32 ok = 0;
		uint64 t0 = GetTimeNow();
		for (uint32 i = 0; i < iters; i++) {
			VantagePoints vp;
			if (!vp.init(xml))
				continue;
			uint32 id = vp.reserveNextPoint(i + 1);
			if (id && vp.occupyPoint(id, i + 1))
				ok++;
		}
		double us = (double)(GetTimeNow() - t0);
		if (ok != iters) { unittest::fail("VantagePoints test: throughput loop incomplete (%u/%u)", ok, iters); return false; }
		if (us > 0)
			unittest::metric("reserve_occupy_throughput", (double)iters / us * 1e6, "ops/s", true);
		unittest::detail("completed %u reserve/occupy cycles in %.0f us", iters, us);
	}

	unittest::progress(100, "done");
	return true;
}

void Register_VantagePoints_Tests() {
	UnitTestRunner::instance().registerTest("vantagepoints", VantagePoints::UnitTest,
		"Vantage point reserve/occupy/free lifecycle and XML init", "data");
}

} // namespace cmlabs
