#include <stdio.h>
#include <math.h>

#include "vtor_key_frame.h"
#include "vtor_printf.h"

#ifdef __VTOR_KEY_FRAME__


// 增加
void VtorKeyFrame_Insert(VtorKeyFrame* keyFrame, double x, double y)
{
	keyFrame->pointCount++;
	keyFrame->points = realloc(keyFrame->points,
			keyFrame->pointCount * sizeof(KeyFramePoint));
	KeyFramePoint newPoint = {x, y};
	int i;
	for (i = keyFrame->pointCount - 2; i >= 0; i--)
	{
		if (keyFrame->points[i].x < newPoint.x)
		{
			break;
		}
		keyFrame->points[i + 1] = keyFrame->points[i];
	}
	keyFrame->points[i + 1] = newPoint;
}

// 修改
void VtorKeyFrame_Modify(VtorKeyFrame* keyFrame, double targetX, double newY)
{
	double minDiff = 10000.0;
	int minIndex = -1;
	for (int i = 0; i < keyFrame->pointCount; i++)
	{
		double diff = fabs(keyFrame->points[i].x - targetX);
		if (diff <= minDiff)
		{
			minDiff = diff;
			minIndex = i;
		}
	}
	if (minIndex!= -1)
	{
		keyFrame->points[minIndex].y = newY;
	}
}


// 查
double VtorKeyFrame_Calc(VtorKeyFrame* keyFrame, double x)
{
	double keyFrameValue = keyFrame->points[0].y;
	// 小于最小，或者大于最大，直接使用边界值
	if(x < keyFrame->points[0].x)
	{
		return keyFrameValue;
	}
	if(x > keyFrame->points[keyFrame->pointCount - 1].x)
	{
		keyFrameValue = keyFrame->points[keyFrame->pointCount - 1].y;
		return keyFrameValue;
	}
	for (int i = 0; i < keyFrame->pointCount - 1; i++)
	{
		if (x >= keyFrame->points[i].x && x <= keyFrame->points[i + 1].x)
		{
			double ratio = (x - keyFrame->points[i].x)
				/ (keyFrame->points[i + 1].x - keyFrame->points[i].x);
				
			keyFrameValue = keyFrame->points[i].y
				+ ratio * (keyFrame->points[i + 1].y - keyFrame->points[i].y);
			break;
		}
	}
	return keyFrameValue;
}

// 打印所有点
void VtorKeyFrame_Show(VtorKeyFrame* keyFrame)
{
	for (int i = 0; i < keyFrame->pointCount; i++)
	{
		VtorPrintf("\t[%d](%f, %f)\r\n",
			i, keyFrame->points[i].x, keyFrame->points[i].y);
	}
}

void VtorKeyFrame_Test(void)
{
	VtorKeyFrame demoKeyFrame = {NULL, 0};
    //VtorKeyFrame* keyFrame = malloc(sizeof(VtorKeyFrame));
    VtorKeyFrame* keyFrame = &demoKeyFrame;

    VtorKeyFrame_Insert(keyFrame, 0.0, 0.0);
    VtorKeyFrame_Insert(keyFrame, 1.0, 1.0);
	
    VtorKeyFrame_Insert(keyFrame, 1.0001, 2.0);
    VtorKeyFrame_Insert(keyFrame, 2.0, 4.0);
	
    VtorKeyFrame_Insert(keyFrame, 2.0001, 8.0);
    VtorKeyFrame_Insert(keyFrame, 3.0, 12.0);

    VtorKeyFrame_Show(keyFrame);

	for(double xValue = -0.5; xValue < 4; xValue += 0.1)
	{
		double yValue = VtorKeyFrame_Calc(keyFrame, xValue);
        VtorDebugChanPrintf("(%f, %f)\r\n", xValue, yValue);
	}
	free(keyFrame->points);
	
}

void VtorKeyFrame_Init()
{
}

#endif // __VTOR_KEY_FRAME__