#include "stdafx.h"
#include "GLUtils.h"
#include "MotionOptimizerInterfaces.h"

const float IDrawable::g_fMargin = 0.1f, IDrawable::g_fField = 1;

IDrawable::IDrawable ():
	m_penKnot (PS_SOLID, 1, RGB(0xFF, 0x80, 0x80)),
	m_penCage (PS_DASH, 1, RGB(0x80, 0xFF, 0x80)),
	m_penSpline (PS_SOLID, 1, RGB(0,0,0x40)),
	m_penGreen (PS_SOLID, 1, RGB(0,0xFF,0x40)),
	m_penGrid (PS_DOT, 1, RGB(0xC0, 0xC0, 0xC0)),
	m_penWeakSpline (PS_DASHDOT, 1, RGB(0x40,0x40,0xFF))
	{
		m_fntMain.CreateFont(15, 0, 0, 0, FW_NORMAL, FALSE, FALSE, 0, ANSI_CHARSET, OUT_DEFAULT_PRECIS, CLIP_DEFAULT_PRECIS, DEFAULT_QUALITY, DEFAULT_PITCH|FF_SWISS, "Arial");
		m_penRGB[0].CreatePen (PS_SOLID, 1, RGB(0xFF, 0, 0));
		m_penRGB[1].CreatePen (PS_SOLID, 1, RGB(0, 0xFF, 0));
		m_penRGB[2].CreatePen (PS_SOLID, 1, RGB(0, 0, 0xFF));
		m_penGridRGB[0].CreatePen (PS_DOT, 1, RGB(0xFF, 0x80, 0x80));
		m_penGridRGB[1].CreatePen (PS_DOT, 1, RGB(0x80, 0xFF, 0x80));
		m_penGridRGB[2].CreatePen (PS_DOT, 1, RGB(0x80, 0x80, 0xFF));
		m_penWeakSplineRGB[0].CreatePen (PS_DASH, 1, RGB(0xFF, 0xC0, 0xC0));
		m_penWeakSplineRGB[1].CreatePen (PS_DASH, 1, RGB(0xC0, 0xFF, 0xC0));
		m_penWeakSplineRGB[2].CreatePen (PS_DASH, 1, RGB(0xC0, 0xC0, 0xFF));
		m_penCPRGB[0].CreatePen (PS_SOLID, 2, RGB(0xFF,0,0));
		m_penCPRGB[1].CreatePen (PS_SOLID, 2, RGB(0,0xFF,0));
		m_penCPRGB[2].CreatePen (PS_SOLID, 2, RGB(0,0,0xFF));
	}

void IDrawable::drawSplineProj (CDC* pDC, BSplineVec3d* pSpline)
{
	CPoint ptKnot2D;
	int i, nNumKnots = pSpline->numKnots(), nNumCPs = pSpline->numCPs();

	// draw the knot points
	pDC->SelectObject (GetStockObject(LTGRAY_BRUSH));
	pDC->SelectObject (&m_penKnot);
	for (i = 0; i < nNumKnots; ++i)
	{
		float fKnotTime = pSpline->getKnotTime(i);
		ptKnot2D = VecToPoint((*pSpline)(fKnotTime));

		CPoint ptPoly[3] = {ptKnot2D,ptKnot2D,ptKnot2D};
		char szKnotTime[64];
		sprintf (szKnotTime, "%.3f", fKnotTime);
		pDC->ExtTextOut(ptKnot2D.x, ptKnot2D.y, 0, NULL, szKnotTime, NULL);

		if (i > 0 && i < nNumKnots-1)
		{
			ptPoly[0].y -= 5;
			ptPoly[1].y += 3;
			ptPoly[2].y += 3;
			ptPoly[1].x -= 4;
			ptPoly[2].x += 4;
		}
		else
		{
			ptPoly[0].y += 8;
			ptPoly[1].y -= 3;
			ptPoly[2].y -= 3;
			ptPoly[1].x += 4;
			ptPoly[2].x -= 4;
		}
		pDC->Polygon(ptPoly,SIZE_OF_ARRAY(ptPoly));
	}

	// draw control points
	for (i = 0; i < nNumCPs; ++i)
	{
		ptKnot2D = VecToPoint(pSpline->getCP(i));
		
		pDC->Ellipse (ptKnot2D.x - 2, ptKnot2D.y - 2, ptKnot2D.x + 3, ptKnot2D.y + 3);
	}

	// draw the cage
	pDC->SelectObject (&m_penCage);
	pDC->MoveTo (VecToPoint(pSpline->getCP(0)));
	for (i = 1; i < nNumCPs; ++i)
	{
		CPoint ptCP = VecToPoint(pSpline->getCP(i));
		pDC->LineTo(ptCP);
	}

	// draw the spline
	pDC->SelectObject(m_penSpline);

	float fDelta = 1e-4f;
	float fTime = 0;

	CPoint ptPrev = VecToPoint(pSpline->getValue(fTime)), ptNext;
	pDC->MoveTo(ptPrev);

	for (fTime += fDelta; fTime <= 1; fTime += fDelta)
	{
		ptNext = VecToPoint(pSpline->getValue((float)fTime));

		while (tabs(ptNext.x - ptPrev.x) < 1 && tabs(ptNext.y - ptPrev.y) < 1 && fDelta < 0.02)
		{
      fTime += fDelta;
			fDelta *= 2;
			ptNext = VecToPoint (pSpline->getValue((float)fTime));
		}

		while ((tabs(ptNext.x - ptPrev.x) > 2 || tabs(ptNext.y - ptPrev.y) > 2) && fDelta > 1e-7)
		{
      fTime -= (fDelta /= 2);
			ptNext = VecToPoint(pSpline->getValue((float)fTime));
		}

		pDC->LineTo(ptNext);
		ptPrev = ptNext;
	}	
}