/****************************************************************************/
/*
 * MODULE              JN-AN-1162 JenNet-IP Smart Home
 *
 * DESCRIPTION         Interpolation Implementation
 */
/****************************************************************************/
/*
 * This software is owned by NXP B.V. and/or its supplier and is protected
 * under applicable copyright laws. All rights are reserved. We grant You,
 * and any third parties, a license to use this software solely and
 * exclusively on NXP products [NXP Microcontrollers such as JN5168, JN5164].
 * You, and any third parties must reproduce the copyright and warranty notice
 * and any other legend of ownership on each copy or partial copy of the
 * software.
 *
 * 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 HOLDER 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.
 *
 * Copyright NXP B.V. 2014. All rights reserved
 */
/****************************************************************************/

/****************************************************************************/
/***        Include files                                                 ***/
/****************************************************************************/
/* Standard includes */
#include <string.h>
#include <stdlib.h>
#include <math.h>
/* SDK includes */
#include <jendefs.h>
/* Application includes */
#include "DeviceDefs.h"
#include "Interpolation.h"

/****************************************************************************/
/***        Macro Definitions                                             ***/
/****************************************************************************/

/****************************************************************************/
/***        Type Definitions                                              ***/
/****************************************************************************/

/****************************************************************************/
/***        Exported Functions                                            ***/
/****************************************************************************/

/****************************************************************************
 *
 * NAME: Interpolation_bStart
 *
 * DESCRIPTION:
 * Start new interpolation - pre-calculates required values
 *
 ****************************************************************************/
PUBLIC bool_t Interpolation_bStart(tsInterpolation *psInterpolation)
{
	bool_t    bReturn = psInterpolation->bForced;
	uint8    u8Dimension;

	/* Debug */
	DBG_vPrintf(DEBUG_INTERPOLATION, "\nInterpolation_bStart(%s", psInterpolation->acName);

	/* Limit number of dimensions to maximum */
	if (psInterpolation->u8Dimensions > DIMENSIONS_MAX) psInterpolation->u8Dimensions = DIMENSIONS_MAX;
	/* Calculate scaling bit shifts */
	psInterpolation->u8Scale = 32 - psInterpolation->u8Bits;
	/* Is the number of steps specified using a power of two ? */
	if (psInterpolation->u8StepsPowerOfTwo > 0)
	{
		/* Calculate the number of steps */
		psInterpolation->u32Steps = ((uint32) 1 << (uint32) psInterpolation->u8StepsPowerOfTwo);
	}
	/* Set remaining steps */
	psInterpolation->u32RemainingSteps = psInterpolation->u32Steps;

	/* Debug */
	DBG_vPrintf(DEBUG_INTERPOLATION, ", %d, %d", psInterpolation->u32Steps, psInterpolation->u8Dimensions);

	/* Loop through dimensions */
	for (u8Dimension = 0; u8Dimension < psInterpolation->u8Dimensions; u8Dimension++)
	{
		/* Debug */
		DBG_vPrintf(DEBUG_INTERPOLATION, ", [%d: %d -> %d", u8Dimension, psInterpolation->au32Source[u8Dimension], psInterpolation->au32Target[u8Dimension]);

		/* Transfer source value to source step and target step values */
		psInterpolation->au32SourceStep[u8Dimension] = psInterpolation->au32Source[u8Dimension];
		psInterpolation->au32TargetStep[u8Dimension] = psInterpolation->au32Source[u8Dimension];
		/* Zero cumulative scaled difference from source */
		psInterpolation->au32ScaledCumulativeDiff[u8Dimension] = 0;

		/* Valid number of steps ? */
		if (psInterpolation->u32Steps > 1)
		{
			/* Need to move up in this dimension ? */
			if (psInterpolation->au32Target[u8Dimension] > psInterpolation->au32Source[u8Dimension])
			{
				/* Debug */
				DBG_vPrintf(DEBUG_INTERPOLATION, ", UP");
				/* Calculate whole difference */
				psInterpolation->au32ScaledStepDiff[u8Dimension] = psInterpolation->au32Target[u8Dimension] - psInterpolation->au32Source[u8Dimension];
				/* Return success */
				bReturn = TRUE;
			}
			/* Need to move down in this dimension ? */
			else if (psInterpolation->au32Source[u8Dimension] > psInterpolation->au32Target[u8Dimension])
			{
				/* Debug */
				DBG_vPrintf(DEBUG_INTERPOLATION, ", DN");
				/* Calculate whole difference */
				psInterpolation->au32ScaledStepDiff[u8Dimension] = psInterpolation->au32Source[u8Dimension] - psInterpolation->au32Target[u8Dimension];
				/* Return success */
				bReturn = TRUE;
			}
			else
			{
				/* Debug */
				DBG_vPrintf(DEBUG_INTERPOLATION, ", NO");
				/* Zero whole difference */
				psInterpolation->au32ScaledStepDiff[u8Dimension] = 0;
			}

			/* Have a difference in this dimension to interpolate across ? */
			if (psInterpolation->au32ScaledStepDiff[u8Dimension] != 0)
			{
				/* Debug */
				DBG_vPrintf(DEBUG_INTERPOLATION, ", %d", psInterpolation->au32ScaledStepDiff[u8Dimension]);
				/* Scale whole difference up */
				psInterpolation->au32ScaledStepDiff[u8Dimension] <<= psInterpolation->u8Scale;
				/* Steps specified by power of two ? */
				if (psInterpolation->u8StepsPowerOfTwo > 0)
				{
					/* Calculate scaled difference for a single step (using bit-shift to divide - hurrah!) */
					psInterpolation->au32ScaledStepDiff[u8Dimension] >>= psInterpolation->u8StepsPowerOfTwo;
				}
				/* Discrete steps specified ? */
				else
				{
					/* Calculate scaled difference for a single step (using integer divide - boo!) */
					psInterpolation->au32ScaledStepDiff[u8Dimension] /= psInterpolation->u32Steps;
				}
				/* Debug */
				DBG_vPrintf(DEBUG_INTERPOLATION, ", %d]", (psInterpolation->au32ScaledStepDiff[u8Dimension] >> psInterpolation->u8Scale));
			}
		}
	}

	/* Debug */
	DBG_vPrintf(DEBUG_INTERPOLATION, ") = %d", bReturn);

	return bReturn;
}

/****************************************************************************
 *
 * NAME: Interpolation_bInterpolate
 *
 * DESCRIPTION:
 * Calculate next step of interpolation
 *
 ****************************************************************************/
PUBLIC bool_t Interpolation_bInterpolate(tsInterpolation *psInterpolation)
{
	bool_t    bReturn = FALSE;
	uint8    u8Dimension;

	/* Debug */
//	DBG_vPrintf(DEBUG_INTERPOLATION, "\nInterpolation_bInterpolate(%s", psInterpolation->acName);
//	DBG_vPrintf(DEBUG_INTERPOLATION, ", %d, %d", psInterpolation->u32RemainingSteps, psInterpolation->u8Dimensions);


	/* Got some steps left in interpolation ? */
	if (psInterpolation->u32RemainingSteps > 0)
	{
		/* Loop through dimensions */
		for (u8Dimension = 0; u8Dimension < psInterpolation->u8Dimensions; u8Dimension++)
		{
			/* Debug */
//			DBG_vPrintf(DEBUG_INTERPOLATION, ", [%d: %d -> %d", u8Dimension, psInterpolation->au32Source[u8Dimension], psInterpolation->au32Target[u8Dimension]);

			/* Copy previous step target into step source (before we change it) */
			psInterpolation->au32SourceStep[u8Dimension] = psInterpolation->au32TargetStep[u8Dimension];

			/* Not the last step ? */
			if (psInterpolation->u32RemainingSteps > 1)
			{
				/* Got a difference in this dimension ? */
				if (psInterpolation->au32ScaledStepDiff[u8Dimension] > 0)
				{
					/* Update cumulative scaled difference from source */
					psInterpolation->au32ScaledCumulativeDiff[u8Dimension] += psInterpolation->au32ScaledStepDiff[u8Dimension];

					/* Need to move up in this dimension ? */
					if (psInterpolation->au32Target[u8Dimension] > psInterpolation->au32Source[u8Dimension])
					{
						/* Debug */
//						DBG_vPrintf(DEBUG_INTERPOLATION, ", UP");
						/* Calculate target step value */
						psInterpolation->au32TargetStep[u8Dimension] = ((psInterpolation->au32Source[u8Dimension] << psInterpolation->u8Scale) + psInterpolation->au32ScaledCumulativeDiff[u8Dimension]) >> psInterpolation->u8Scale;
					}
					/* Need to move down in this dimension ? */
					else if (psInterpolation->au32Source[u8Dimension] > psInterpolation->au32Target[u8Dimension])
					{
						/* Debug */
//						DBG_vPrintf(DEBUG_INTERPOLATION, ", DN");
						/* Calculate new target step value */
						psInterpolation->au32TargetStep[u8Dimension] = ((psInterpolation->au32Source[u8Dimension] << psInterpolation->u8Scale) - psInterpolation->au32ScaledCumulativeDiff[u8Dimension]) >> psInterpolation->u8Scale;
					}
				}
				/* No difference in this dimension ? */
				else
				{
					/* Debug */
//					DBG_vPrintf(DEBUG_INTERPOLATION, ", NO");
				}

				/* Debug */
//				DBG_vPrintf(DEBUG_INTERPOLATION, ", %d, %d", (psInterpolation->au32ScaledStepDiff[u8Dimension] >> psInterpolation->u8Scale), (psInterpolation->au32ScaledCumulativeDiff[u8Dimension] >> psInterpolation->u8Scale));
			}
			/* Last step ? */
			else
			{
				/* Copy the target value to the target step value (to mop up any integer divide rounding errors) */
				psInterpolation->au32TargetStep[u8Dimension] = psInterpolation->au32Target[u8Dimension];
			}

			/* Debug */
//			DBG_vPrintf(DEBUG_INTERPOLATION, ", %d -> %d]", psInterpolation->au32SourceStep[u8Dimension], psInterpolation->au32TargetStep[u8Dimension]);
		}

		/* We performed the interpolation */
		bReturn = TRUE;
		/* Decrement remaining steps */
		psInterpolation->u32RemainingSteps--;
	}

	/* Debug */
//	DBG_vPrintf(DEBUG_INTERPOLATION, ") = %d", bReturn);

	return bReturn;
}

/****************************************************************************
 *
 * NAME: Interpolation_bEnd
 *
 * DESCRIPTION:
 * Ends interpolation
 *
 ****************************************************************************/
PUBLIC bool_t Interpolation_bEnd(tsInterpolation *psInterpolation)
{
	/* Debug */
	DBG_vPrintf(DEBUG_INTERPOLATION, "\nInterpolation_bEnd(%s, %d) = 1", psInterpolation->acName, psInterpolation->u32RemainingSteps);

	/* End any interpolation by setting the remaining steps to zero */
	psInterpolation->u32RemainingSteps          = 0;

	return TRUE;
}

/****************************************************************************/
/***        END OF FILE                                                   ***/
/****************************************************************************/
