#include <crtdbg.h>
#include "TCG.h"
#include "NDTSRS2AErrors.h"
#include "rq.h"
#include "NDTSRS2AInternal.h"

using namespace NDTS::RS2A;

TCGInternal::TCGInternal()
{
	this->boardIndex = rs2aGlobalState.cardIndex;
	isInterfaceTriggered = false;
	simpleGain = 80;
	isSimpleGain = true;
	isNormalisationOn = false;
	samplesPerAScan = 0;
	aScansPerBScan = 0;
	samplesDropPerAScan = 0;
	this->isInPreCommit = false;

	for (int i=0; i < NDTS_MAXIMUM_NUMBER_OF_SAMPLES ; i++)
	{
		pShadowCurve[i] = 0;
	}
	for (int i=0; i < NDTS_MAXIMUM_NUMBER_OF_ELEMENTS; i++)
	{
		pNormalisationValues[i] = 0;
	}
	pointsInShadowCurve = NDTS_MAXIMUM_NUMBER_OF_ELEMENTS;
	isInitialised = false;

}

TCGInternal::~TCGInternal()
{

}

void TCGInternal::initialise()
{
	isInitialised = true;
	sendCurve();
}

bool TCGInternal::setNormalisation(int element, int value)
{
	if (element >= NDTS_MAXIMUM_NUMBER_OF_ELEMENTS)
	{
		RS2A_HandleRS2AError(RS2A_TCG_ValueOutOfRange);	
		return false;
	}
	if (value < -255 || value > 255)
	{
		RS2A_HandleRS2AError(RS2A_TCG_ValueOutOfRange);	
		return false;
	}
	if (value == pNormalisationValues[element])
		return true;

	pNormalisationValues[element] = value;

	return sendCurve();
}

bool TCGInternal::setCurve(int *pTCGCurve, int pointsInCurve)
{
	if (pointsInCurve >= NDTS_MAXIMUM_NUMBER_OF_SAMPLES ||
		pointsInCurve < 1)
	{
		RS2A_HandleRS2AError(RS2A_TCG_ValueOutOfRange);	
		return false;
	}

	// Check for curve being a duplicate and range check the values in the curve
	bool identical = true;
	for (int i=0; i < pointsInCurve; i++)
	{
	//	if (i%100 == 0)
	//		NDTS_LogLine(NDTS_LLErr, "%d, %d", i, pTCGCurve[i]);
		if (pTCGCurve[i] > 255 || pTCGCurve[i] < 0)
		{
			RS2A_HandleRS2AError(RS2A_TCG_ValueOutOfRange);	
			return false;
		}

		if (pShadowCurve[i] != pTCGCurve[i])
		{
			identical = false;
			break;
		}
	}

	if (identical)
		return true;

	// Copy the values and send the curve
	pointsInShadowCurve = pointsInCurve;
	for (int i=0; i < pointsInCurve; i++)
	{
		pShadowCurve[i] = pTCGCurve[i];
	}

	return sendCurve();
}
void TCGInternal::setIsInterfaceTriggered(bool isInterfaceTriggered)
{
	if (this->isInterfaceTriggered == isInterfaceTriggered)
		return;

	this->isInterfaceTriggered = isInterfaceTriggered;
	sendCurve();
}


void TCGInternal::setIsSimpleGain(bool isSimpleGain)
{
	this->isSimpleGain = isSimpleGain;
	sendCurve();
}

void TCGInternal::setSimpleGain(int  simpleGain)
{
	this->simpleGain = simpleGain;
	sendCurve();
}

void TCGInternal::setIsNormalisationOn(bool isNormalisationOn)
{
	if (this->isNormalisationOn == isNormalisationOn)
		return;

	this->isNormalisationOn = isNormalisationOn;
	sendCurve();
}

void TCGInternal::setSamplesPerAScan(int samplesPerAScan)
{
	_ASSERTE(samplesPerAScan > 0 && samplesPerAScan < NDTS_MAXIMUM_NUMBER_OF_SAMPLES);
	if (this->samplesPerAScan == samplesPerAScan)
		return;

	this->samplesPerAScan = samplesPerAScan;
//	Todo: only send curve if multiplier has changed
	sendCurve();
}

void TCGInternal::setSamplesDropPerAScan(int numberOfSamplesDrop)
{
	if (this->samplesDropPerAScan == numberOfSamplesDrop)
		return;

	this->samplesDropPerAScan = numberOfSamplesDrop;
//	Todo: only send curve if multiplier has changed
	sendCurve();
}

void TCGInternal::setAScansPerBScan(int aScansPerBScan)
{
	_ASSERTE(aScansPerBScan > 0 && aScansPerBScan < NDTS_MAXIMUM_NUMBER_OF_ELEMENTS);
	if (this->aScansPerBScan == aScansPerBScan)
		return;

	this->aScansPerBScan = aScansPerBScan;
	sendCurve();
}
	
bool TCGInternal::sendCurve()
{
	_ASSERTE(isInitialised);

	// Don't keep attempting to send the curve in a pre commit where the range and drop samples are adjusted
	if (isInPreCommit)
		return true;

	if (!RQ_SetTCGTriggerEvent(boardIndex, (isInterfaceTriggered == true) ? RQ_TCGTEInterfaceGate : RQ_TCGTEAbsolute))
		return false;

	if (!RQ_SetTCGEnable(boardIndex, NDTS_BFalse))
		return false;

	int tcgModeMask = 0; 
	// Todo: doesn't work with a range > 32k non normalised
	// Todo: doesn't work with normalisation over ranges greater than samplesPerElement.  It all also needs further testing

	int samplesPerElement = RQ_MAXIMUM_NUMBER_OF_TCG_VALUES;
	if (isNormalisationOn)
	{
		if (aScansPerBScan < 33)
		{
			tcgModeMask = 1; // 32 curves max 8K points
			samplesPerElement = 8192;
		}
		else if (aScansPerBScan < 65)
		{
			tcgModeMask = 2; // 64 curves max 4K points
			samplesPerElement = 4096;
		}
		else
		{
			tcgModeMask = 3; // 128 curves max 2K points
			samplesPerElement = 2048;
		}
	}

	int samplesDropDivider = samplesDropPerAScan + 1;

	int tcgPointRatio = (samplesPerAScan * samplesDropDivider / samplesPerElement) + 1; 

	if (!RQ_SetTCGNormalisedStart(boardIndex, tcgModeMask, tcgPointRatio))
		return false;

	const int tcgBufferSize = 32 * 8096;
	// Send the full buffer if normalised, otherwise just the number of samples which will be much quicker
	int numberOfTCGValues = (isNormalisationOn) ? tcgBufferSize : samplesPerAScan * samplesDropDivider;
	int validShadowSamples = pointsInShadowCurve / tcgPointRatio;

	// pReSampledShadowCurve contains the non normalised shadow curve under sampled to the point ratio 
	// pReSampledShadowCurve also expands the shadow curve to to take into account drop samples, where
	// e.g. The A Scan range is 100, drop samples are 3, range is therefore 400 clock ticks.
	// The shadow curve contains 100 samples but the TCG is played out at the full rate and needs 400 samples
	int pReSampledShadowCurve[NDTS_MAXIMUM_NUMBER_OF_SAMPLES];

	if (isSimpleGain)
	{
		for (int point = 0; point < samplesPerElement; point++)
		{
			pReSampledShadowCurve[point] = simpleGain;
		}
	}
	else
	{
		for (int point = 0; point < samplesPerElement; point++)
		{
			// Set 0 for samples after the valid range because firmware should now take care of setting the first point of the next curve
			_ASSERTE(point < NDTS_MAXIMUM_NUMBER_OF_SAMPLES);
			pReSampledShadowCurve[point] = (point < validShadowSamples * samplesDropDivider) ? pShadowCurve[(point / samplesDropDivider) * tcgPointRatio] : 0;
		}
	}

	// pTCGValues contains the normalised curve
	unsigned short pTCGValues[tcgBufferSize]; // Maximum number of TCG values

	for (int element = 0; element < ((isNormalisationOn) ? aScansPerBScan : 1); element++)
	{
		int offset = element * samplesPerElement;
		for (int point = 0; point < samplesPerElement; point++)
		{
			_ASSERTE(offset + point < tcgBufferSize);
			// If normalisation sends the TCG out of range, just clamp it, don't return an error
			int value = pReSampledShadowCurve[point] + pNormalisationValues[element];
			if (value < 0)
				value = 0;
			if (value > 255)
				value = 255;
			pTCGValues[offset + point] = value; 
		}
	}
		
	static short fixValue = 255;
	static short  fixStart = 100;
	static short  fixSize = 0;
	for (int i=fixStart; i < fixSize; i++)
	{
		pTCGValues[i] = fixValue;
	}

	if (!RQ_SetTCGValuesNormalised(boardIndex, pTCGValues, numberOfTCGValues))
		return false;

	if (!RQ_SetTCGNormalisedFinish(boardIndex))
		return false;

    if (!RQ_SetTCGEnable(boardIndex, NDTS_BTrue))
		return false;

	return true;
}

void TCGInternal::preCommitStart()
{
	_ASSERTE(!isInPreCommit);
	isInPreCommit = true;
}
void TCGInternal::preCommitComplete()
{
	_ASSERTE(isInPreCommit);
	isInPreCommit = false;
	sendCurve();
}
