#include "scene.h"
#include "v3.h"
#include "m33.h"
#include "ppc.h"

Scene *scene;

using namespace std;

#include <fstream>

#include <iostream>

Scene::Scene() {

	lfb = 0;
	rfb = 0;
	lppc = rppc = 0;
	canfb = 0;
	canppc = 0;

	ppcPTM = 0;
	fbPTM = 0;

	cgi = 0; soi = 0;

	morphFraction = 0.0f;

	gui = new GUI();
	gui->show();

	L = V3(0.0f, 0.0f, 0.0f);
	specc = 180.0f;

	int u0 = 20;
	int v0 = 20;
	int sci = 2;
	int w = 378;
	int h = 504;

	fb = new FrameBuffer(u0, v0, w, h, 0);
	fb->label("SW First person");
	fb->show();

	hwfb = new FrameBuffer(u0 /*+ w + 20*/, v0, w, h, 0);
	hwfb->isHW = 1;
	hwfb->label("Fixed Pipeline First person");
	//	hwfb->show();

	gpufb = new FrameBuffer(u0 + w + 20, v0, w, h, 0);
	gpufb->isHW = 2;
	gpufb->label("GPU First person");
	//	gpufb->show();

	fb3 = new FrameBuffer(u0, v0, w, h, 1);
	fb3->label("Third person");
	fb3->position(u0 + fb->w + 20, v0);
	//fb3->show();

	gui->uiw->position(u0, v0 + fb->h + 60);

	float hfov = 65.5f;
	ppc = new PPC(fb->w, fb->h, hfov);
	ppc3 = new PPC(fb3->w, fb3->h, 90.0f);

	tmsN = 11;
	tms = new TM[tmsN];
	tms[0].id = 0;
	tms[1].id = 1;
	tms[0].SetToBox(V3(0.0f, 0.0f, -100.0f), V3(30.0f, 30.0f, 30.0f),
		V3(1.0f, 0.5f, 0.0f));
	tms[0].enabled = 0;
	tms[1].LoadBin("geometry/teapot1K.bin");
	tms[1].enabled = 0;
	AABB aabb = tms[1].ComputeAABB();
	cerr << "INFO: teapot aabb: " << aabb.corners[0] << "; " << aabb.corners[1] << endl;
	// ppc->C = tms[1].GetCenterOfMass() + V3(0.0f, 0.0f, 110.0f);
	// L = ppc->C;

	ppc3->PositionAndOrient(ppc->C + V3(50.0f, 100.0f, 50.0f), tms[1].GetCenterOfMass(), V3(0.0f, 1.0f, 0.0f));
	tms[2].SetToRectangle(V3(0.0f, 120.0f, -1000.0f), V3(1000.0f, 230.0f, 0.0f), V3(0.0f, 0.0f, 0.0f));
	tms[2].cols[0] = tms[2].cols[1] = V3(1.0f, 1.0f, 1.0f);
	tms[2].id = 2;
	tms[3].SetToRectangle(V3(0.0f, -120.0f, -1000.0f), V3(1000.0f, 230.0f, 0.0f), V3(0.0f, 0.0f, 0.0f));
	tms[3].cols[0] = tms[3].cols[1] = V3(1.0f, 1.0f, 1.0f);
	tms[3].id = 3;
	tms[2].enabled = tms[3].enabled = 0;

	texts = new FrameBuffer(0, 0, 128, 128, 2);
	texts->SetBWCheckerboard(16);
	texts->label("texture");
	//	texts->show();
	tms[4].SetToRectangle(V3(0.0f, 0.0f, -100.0f), V3(100.0f, 100.0f, 0.0f), V3(1.0f, 1.0f, 1.0f));
	tms[4].tex = texts;
	tms[4].enabled = 0;
	tms[4].id = 4;

	aabb = tms[1].ComputeAABB();
	V3 bC = tms[1].GetCenterOfMass();
	bC[1] = aabb.corners[0][1];
	tms[5].SetToBox(bC, V3(200.0f, 1.0f, 200.0f), V3(1.0f, 0.0f, 0.0f));
	tms[5].id = 5;
	L = L + V3(30.0f, 80.0f, -20.0f);
	tms[5].enabled = 0;

	tms[6].SetToRectangle(bC, V3(200, 200.0f, 0.0f), V3(0.0f, 0.0f, 0.3f));
	tms[6].RotateAboutAxis(bC, V3(1.0f, 0.0f, 0.0f), 90.0f);
	tms[6].enabled = 0;

	V3 TO(0.0f, 73.0f, 0.0f);
	tms[7].SetToRectangle(TO,
		V3(152.0f, 305.0f, 0.0f), V3(1.0f, 0.0f, 0.0f));
	tms[7].RotateAboutAxis(TO, V3(1.0f, 0.0f, 0.0f), 90);
	tms[7].enabled = 0;

	// 217; 101
	V3 boxCenter = V3(0.0f, 273.0f / 2.0f, 0.0f) +
		V3(152.0f / 2.0f + 217.0f - 576.0f / 2.0f + 65.0f,
			0.0f,
			591.0f / 2.0f - 305.0f / 2.0f - 101.0f - 65.0f);
	V3 boxDims = V3(576.0f - 130.0f, 273.0f, 591.0f - 130.0f);
	tms[8].SetToBox(boxCenter, boxDims, V3(0.0f, 0.0f, 0.0f));
	tms[8].cols[7] =
		tms[8].cols[6] =
		tms[8].cols[5] =
		tms[8].cols[4] = V3(1.0f, 1.0f, 0.0f);
	tms[8].enabled = 0;
	tms[8].id = 8;

	tms[9].SetToBox(V3(0.0f, 0.0f, -100.0f), V3(30.0f, 30.0f, 30.0f), V3(1.0f, 0.0f, 0.0f));
	tms[9].enabled = 1;
	tms[9].id = 9;

	tms[10].SetToBox(V3(0.0f, 40.0f, -100.0f), V3(30.0f, 30.0f, 30.0f), V3(1.0f, 0.0f, 0.0f));
	tms[10].enabled = 1;
	tms[10].id = 10;

	smppc = 0;
	smfb = 0;
	gfb = 0;

	//	ppc->LoadFromTextFile("view.txt");
	RenderAll();

}

void Scene::Render(FrameBuffer *currfb, PPC *currppc) {

	unsigned int bgr = 0xFFFF0000;
	currfb->Clear(bgr, 0.0f);

	for (int tmi = 0; tmi < tmsN; tmi++) {
		if (currfb->id == 1) {
			continue;
		}
		if (!tms[tmi].enabled)
			continue;
		if (ppcPTM && fbPTM) {
			tms[tmi].RenderPT(ppcPTM, fbPTM, currppc, fb);
			continue;
		}
		//		tms[tmi].RenderPoints(ppc, fb);
//		tms[tmi].RenderWireframe(currppc, currfb);
//		tms[tmi].RenderFilled(currppc, currfb);
		tms[tmi].RenderPoints(currppc, currfb);

	}

	if (ppcPTM && fbPTM) {
		ppcPTM->Visualize(100.0f, currppc, currfb);
	}

#if 0
	if (currfb->id == 1) {
		float visz = 40.0f;
		ppc->Visualize(visz, currppc, currfb);
		fb->VisualizeImagePoints(visz, ppc, ppc3, fb3);
		fb->Visualize3DPoints(ppc, ppc3, fb3);
	}

	currfb->Draw3DSegment(L, L + V3(0.0f, 5.0f, 0.0f), V3(1.0f, 1.0f, 0.0f), V3(1.0f, 1.0f, 0.0f), currppc);
	currfb->Draw3DSegment(L, L + V3(5.0f, 0.0f, 0.0f), V3(1.0f, 1.0f, 0.0f), V3(1.0f, 1.0f, 0.0f), currppc);
	currfb->Draw3DSegment(L, L + V3(0.0f, 0.0f, 5.0f), V3(1.0f, 1.0f, 0.0f), V3(1.0f, 1.0f, 0.0f), currppc);
#endif

	currfb->redraw();
}

void Scene::ShadowMapSetup() {


	int smw = 128;
	smppc = new PPC(smw, smw, 55.0f);
	smppc->PositionAndOrient(L, tms[1].GetCenterOfMass(), V3(0.0f, 1.0f, 0.0f));
	FrameBuffer *tmpsmfb = new FrameBuffer(100, 100, smw, smw, 2);
	Render(tmpsmfb, smppc);
	tmpsmfb->label("Shadow Map");
	tmpsmfb->show();

	smfb = tmpsmfb;

}


void Scene::PTMDemo() {


	V3 TC = tms[7].GetCenterOfMass();
	ppc->PositionAndOrient(TC + V3(-200.0f, 100.0f, 200.0f), TC, V3(0.0f, 1.0f, 0.0f));

	ppc->LoadFromTextFile("mydbg/solPPC.txt");

	FrameBuffer *im0;
	im0 = new FrameBuffer(100, 100, 10, 10, 0);
	im0->LoadTiff("images/LWSN3151A_s.tif");
	cerr << im0->w << " " << im0->h << endl;
	im0->position(20 + 20 + fb->w, 20);
	im0->show();

	// load the five tuples
	ifstream ifs("images/LWSN3151AFTS.txt");
	int ftpsN;
	ifs >> ftpsN;
	V3 *uv = new V3[ftpsN];
	V3 *xyz = new V3[ftpsN];
	for (int i = 0; i < ftpsN; i++) {
		uv[i][2] = 0.0f;
		ifs >> uv[i][0] >> uv[i][1] >> xyz[i];
	}
	ifs.close();

	RenderAll();
	for (int uv = 0; uv < fb->w*fb->h; uv++)
		im0->zb[uv] = fb->zb[uv];
	ppcPTM = new PPC(*ppc);
	fbPTM = im0;

	cerr << "INFO: error: " << ErrorPT(ftpsN, uv, xyz, ppc) << endl;
	RenderAll();
	BlendErrorViz(fb, im0);
	VizErrorPT(ftpsN, uv, xyz, ppc, fb);
	fb->redraw();
	Fl::check();
	return;

	PPC ppc0(*ppc); // initial guess
	float fovamp = 10.0f;
	int fstepsN = 17;
	V3 tamps(50.0f, 50.0f, 50.0f);
	V3 ramps(10.0f, 10.0f, 4.0f);
	int tstepsN[3] = { 17, 17, 17 };
	int rstepsN[3] = { 17, 17, 7 };

	float minError = ErrorPT(ftpsN, uv, xyz, ppc);
	PPC solPPC(ppc0);

	int si = 0;
	int sN = fstepsN * tstepsN[0] * tstepsN[1];

	for (int ifv = 0; ifv < fstepsN; ifv++) {

		float fov0 = ppc0.GetHFOV();
		float fracf = (float)ifv / ((float)fstepsN - 1);
		float currfov = fov0 - fovamp / 2.0f + fovamp * fracf;
		PPC ppcf(ppc0.w, ppc0.h, currfov);
		ppcf.PositionAndOrient(ppc0.C, ppc0.C + ppc0.GetVD()*100.0f, ppc0.b*-1.0f);


		for (int itx = 0; itx < tstepsN[0]; itx++) {
			PPC c0ppc(ppcf);
			float fracf = (float)itx / ((float)tstepsN[0] - 1);
			V3 tv = (c0ppc.a.UnitVector())*(-tamps[0] / 2.0f + tamps[0] * fracf);
			c0ppc.Translate(tv);

			for (int ity = 0; ity < tstepsN[1]; ity++) {
				PPC c1ppc(c0ppc);
				float fracf = (float)ity / ((float)tstepsN[1] - 1);
				V3 tv = (c1ppc.b.UnitVector())*-1.0f*(-tamps[1] / 2.0f + tamps[1] * fracf);
				c1ppc.Translate(tv);

				float prind = (float)si / (float)sN * 100.0f;
				prind = (int)(prind * 100);
				prind /= 100.0f;
				if ((si % 10) == 0) {
					cerr << "INFO: best match: " << minError << "; " << prind << "%        \r";
				}
				si++;

				for (int itz = 0; itz < tstepsN[2]; itz++) {
					PPC c2ppc(c1ppc);
					float fracf = (float)itz / ((float)tstepsN[2] - 1);
					V3 tv = (c2ppc.GetVD().UnitVector())*-1.0f*(-tamps[2] / 2.0f + tamps[2] * fracf);
					c2ppc.Translate(tv);

					for (int irx = 0; irx < rstepsN[0]; irx++) {
						PPC c3ppc(c2ppc);
						float fracf = (float)irx / ((float)rstepsN[0] - 1);
						float panAngle = -ramps[0] / 2.0f + ramps[0] * fracf;
						c3ppc.Pan(panAngle);


						for (int iry = 0; iry < rstepsN[1]; iry++) {
							PPC c4ppc(c3ppc);
							float fracf = (float)iry / ((float)rstepsN[1] - 1);
							float tiltAngle = -ramps[1] / 2.0f + ramps[1] * fracf;
							c4ppc.Tilt(tiltAngle);

							for (int irz = 0; irz < rstepsN[2]; irz++) {
								PPC c5ppc(c4ppc);
								float fracf = (float)irz / ((float)rstepsN[2] - 1);
								float rollAngle = -ramps[2] / 2.0f + ramps[2] * fracf;
								c5ppc.Roll(rollAngle);


								(*ppc) = c5ppc;
								float currError = ErrorPT(ftpsN, uv, xyz, ppc);

								if (currError < minError) {
									minError = currError;
									cerr << "INFO: found better match: " << minError << "; " << prind << "%        \r";
									solPPC = (*ppc);
									RenderAll();
									BlendErrorViz(fb, im0);
									VizErrorPT(ftpsN, uv, xyz, ppc, fb);
									fb->redraw();
									Fl::check();
								}
							}
						}
					}
				}
			}
		}
	}

	cerr << endl;

	*ppc = solPPC;
	ppc->SaveToTextFile("mydbg/solPPC.txt");

	Render(fb, &solPPC);
	BlendErrorViz(fb, im0);
	VizErrorPT(ftpsN, uv, xyz, &solPPC, fb);

	cerr << "INFO: reprojection error: " << ErrorPT(ftpsN, uv, xyz, ppc);

}

void Scene::BlendErrorViz(FrameBuffer *fb0, FrameBuffer *fb1) {

	for (int v = 0; v < fb0->h; v++) {
		for (int u = 0; u < fb0->w; u++) {
			unsigned int gcui = fb0->Get(u, v);
			if (gcui == 0xFFFF0000) {
				fb0->Set(u, v, fb1->Get(u, v));
			}
			else {
				V3 gc;
				gc.SetColor(fb0->Get(u, v));
				V3 phc;
				phc.SetColor(fb1->Get(u, v));
				V3 bc = (gc + phc) / 2.0f;
				fb0->Set(u, v, bc.GetColor());
			}
		}
	}
	fb0->redraw();
}


void Scene::VizErrorPT(int ftpsN, V3 *uv, V3 *xyz, PPC *ppc, FrameBuffer *vizFB) {

	for (int i = 0; i < ftpsN; i++) {
		// true position of corners
		int u0, v0;
		u0 = (int)(uv[i][0]);
		v0 = (int)(uv[i][1]);
		V3 pp; ppc->Project(xyz[i], pp);
		pp[2] = uv[i][2] = FLT_MAX;
		vizFB->Draw2DSegment(uv[i], pp, V3(0.0f, 1.0f, 0.0f), V3(1.0f, 0.0f, 0.0f));
		vizFB->Draw2DRectangle(u0, v0 - 13, u0, v0 + 13, 0xFF00FF00);
		vizFB->Draw2DRectangle(u0 - 13, v0, u0 + 13, v0, 0xFF00FF00);
		// positions of corners based on guess
		vizFB->Draw3DPoint(xyz[i], V3(1.0f, 0.0f, 0.0f), 7.0f, ppc);
		uv[i][2] = 0.0f;
	}

	vizFB->redraw();

}

float Scene::ErrorPT(int ftpsN, V3 *uv, V3 *xyz, PPC *ppc) {

	float ret = 0;
	for (int i = 0; i < ftpsN; i++) {
		V3 P;
		int projOK = ppc->Project(xyz[i], P);
		P[2] = 0.0f;
		ret += (P - uv[i])*(P - uv[i]);
	}
	ret /= ftpsN;
	ret = sqrtf(ret);

	return ret;

}

void Scene::PanoDemo() {

	if (lfb)
		return;

	lfb = new FrameBuffer(20, 20, 10, 10, 100);
	lfb->LoadTiff("images/Pano1S.tif");
	lfb->label("Left");
	//		lfb->SaveAsTiff("images/Pano1S.tif");
	lfb->show();
	rfb = new FrameBuffer(20 + lfb->w + 20, 20, 10, 10, 101);
	rfb->LoadTiff("images/Pano2S.tif");
	rfb->label("Right");
	//		rfb->SaveAsTiff("images/Pano2S.tif");
	rfb->show();

	float hfov = 50.0f;
	lppc = new PPC(lfb->w, lfb->h, hfov);
	rppc = new PPC(rfb->w, rfb->h, hfov);
	rppc->Pan(20.0f);

	fb->hide();

	canfb = new FrameBuffer(20 + lfb->w + 20 + rfb->w + 20, 20, 600, 500, 102);
	canfb->label("Registration canvas");
	canfb->show();
	canppc = new PPC(canfb->w, canfb->h, 100.0f);

	V3 rtp0(0.0f, 0.0f, 20.0f); // roll tilt pan
	V3 rtpAmp(5.0f, 10.0f, 20.0f);
	int rtpStepsN[] = { 5, 5, 5 };
	V3 rtpSol;
	float minError = FLT_MAX;
	int i = 0;
	int itsN = 10;
	for (int iti = 0; iti < itsN; iti++) {
		for (int ri = 0; ri < rtpStepsN[0]; ri++) {
			V3 frac;
			frac[0] = (float)ri / (float)(rtpStepsN[0] - 1);
			for (int ti = 0; ti < rtpStepsN[1]; ti++) {
				frac[1] = (float)ti / (float)(rtpStepsN[1] - 1);
				for (int pi = 0; pi < rtpStepsN[2]; pi++) {
					frac[2] = (float)pi / (float)(rtpStepsN[2] - 1);
					V3 crtp = rtp0 - rtpAmp / 2.0f + V3(rtpAmp[0] * frac[0],
						rtpAmp[1] * frac[1], rtpAmp[2] * frac[2]);
					*rppc = *lppc;
					rppc->Pan(crtp[2]);
					rppc->Tilt(crtp[1]);
					rppc->Roll(crtp[0]);
					float currError = StitchingError(lfb, lppc, rfb, rppc);
					if (currError < minError) {
						minError = currError;
						rtpSol = crtp;
						canfb->Clear(0xFF888888, 0.0f);
						VisualizeInCanvas(lfb, lppc, canfb, canppc);
						VisualizeInCanvas(rfb, rppc, canfb, canppc);
						canfb->redraw();
						Fl::check();
					}
					float pind = (float)i /
						(float)(itsN * rtpStepsN[0] * rtpStepsN[1] * rtpStepsN[2]) * 100.0f;
					pind = ((float)((int)(pind * 100.0f))) / 100.0f;
					if ((i % 50) == 0)
						cerr << "MIN ERROR: " << minError << "; pind: "
						<< pind << "%         \r";
					i++;
				}
			}
		}
		rtpAmp = rtpAmp / 2.0f;
		rtp0 = rtpSol;
	}

	cerr << endl << "DONE" << endl;
	cerr << "MIN ERROR: " << minError << endl;

}

float Scene::StitchingError(FrameBuffer *fb0, PPC *ppc0,
	FrameBuffer *fb1, PPC *ppc1) {

	M33 rotMapping;
	ppc1->SetupRotationMapping(rotMapping, ppc0);
	int pixN = 0;
	float error = 0.0f;
	for (int v1 = 0; v1 < fb1->h; v1++) {
		for (int u1 = 0; u1 < fb1->w; u1++) {
			V3 u1v11(.5f + (float)u1, .5f + (float)v1, 1.0f);
			V3 u0v01 = rotMapping * u1v11;
			u0v01 = u0v01 / u0v01[2];
			unsigned int c0 = fb0->LookUpBilinearUV(u0v01[0], u0v01[1]);
			if (c0 == 0x00000000)
				continue;
			V3 c0v; c0v.SetColor(c0);
			V3 c1v; c1v.SetColor(fb1->Get(u1, v1));
			V3 c01v = c0v - c1v;
			error += c01v * c01v;
			pixN++;
		}
	}

	if (pixN < 1000)
		return FLT_MAX;

	error /= ((float)pixN * 3.0f);
	return sqrtf(error)*255.0f;

}

void Scene::VisualizeInCanvas(FrameBuffer *tovfb, PPC *tovppc,
	FrameBuffer *cfb, PPC *cppc) {


	M33 rotMapping;
	cppc->SetupRotationMapping(rotMapping, tovppc);

	for (int v = 0; v < cfb->h; v++) {
		for (int u = 0; u < cfb->w; u++) {
			V3 u1v11(.5f + (float)u, .5f + (float)v, 1.0f);
			V3 u0v0 = rotMapping * u1v11;
			u0v0 = u0v0 / u0v0[2];
			unsigned int color = tovfb->GetGuarded((int)u0v0[0], (int)u0v0[1]);
			if (color == 0x00000000)
				continue;
			unsigned int oldColor = cfb->Get(u, v);
			if (oldColor == 0xFF888888)
				canfb->Set(u, v, color);
			else {
				V3 oldColorv; oldColorv.SetColor(oldColor);
				V3 colorv; colorv.SetColor(color);
				colorv = (oldColorv + colorv)*0.5f;
				color = colorv.GetColor();
				canfb->Set(u, v, color);
			}
		}
	}
}


void Scene::EpiDemo() {


	unsigned int bgrColor = 0xFF888888;
	fb->SetBGR(bgrColor);
	lfb = new FrameBuffer(20, 20, fb->w, fb->h, 300);
	lfb->LoadTiff("images/stereoL.tif");
//	lfb->SaveAsTiff("images/stereoL.tif");
	lfb->label("stereo left");
//	lfb->SetBGR(bgrColor);
	lfb->show();
	lppc = new PPC(*ppc);


	rfb = new FrameBuffer(20+20+fb->w, 20, fb->w, fb->h, 300);
	rfb->LoadTiff("images/stereoR.tif");
//	rfb->SaveAsTiff("images/stereoR.tif");
	rfb->label("stereo right");
//	rfb->SetBGR(bgrColor);
	rfb->show();

	rppc = new PPC(*ppc);
	rppc->Translate(V3(2.54f*1.5f*3.0f, 0.0f, 0.0f));
//	rppc->Pan(-20.0f);

	while (1) {

//		rfb->Clear(bgrColor, 0.0f);
//		lfb->Clear(bgrColor, 0.0f);

		cerr << "INPUT: left point: ";
		int ul, vl;
		cin >> ul >> vl;
		float ulf = (float)ul + .5f;
		float vlf = (float)vl + .5f;
#if 0
		int ptsN = 500;
		float z0 = 30.0f;
		float zn = 30000.0f;
		for (int i = 0; i < ptsN; i++) {
			V3 p = lppc->GetRayTip(ulf, vlf, z0 + (zn-z0)/(ptsN-1)*i);
			rfb->Draw3DPoint(p, V3(0.8f, 0.8f, 1.0f), 3, rppc);
		}
		rfb->redraw();
		Fl::check();
		continue;
#endif

		// search for correspondence automatically
		float urf, vrf;
		FindCorrespondence(ulf, vlf, lfb, lppc, rfb, rppc, urf, vrf);
		rfb->DrawCircle((int)urf, (int)vrf, 3.0f, 0xFF00FFFF);

		V3 p0 = lppc->GetRayTip(ulf, vlf, 30.0f);
		V3 p1 = lppc->GetRayTip(ulf, vlf, 500.0f);
		V3 c0(1.0f, 0.0f, 0.0f), c1(0.5f, 0.0f, 0.0f);
		rfb->Draw3DPoint(p0, c0, 3.0f, rppc);
		rfb->Draw3DPoint(p1, c1, 3.0f, rppc);
		rfb->Draw3DSegment(p0, p1, c0, c1, rppc);
		rfb->redraw();
		lfb->DrawCircle(ul, vl, 7.0f, 0xFF00FF00);
		lfb->redraw();
		Fl::check();
	}

	return;

	fb->hide();

	fb->SetBGR(bgrColor);
	V3 tv = V3(200.0f, 0.0f, 0.0f);
	int stepsN = 1000;
	for (int i = 0; i < stepsN; i++) {
		float frac = (float)i / (float)(stepsN - 1);
		ppc->Translate(tv*frac);
		tms[9].RenderPoints(ppc, fb);
		tms[10].RenderPoints(ppc, fb);
		fb->redraw();
		Fl::check();
	}


}

float Scene::PatchColorDifference(FrameBuffer *lfb, float ulf, float vlf, 
	FrameBuffer *rfb, float urf, float vrf, int pSize) {

	float err = 0.0f;
	for (int dv = -pSize / 2; dv <= pSize / 2; dv++) {
		for (int du = -pSize / 2; du <= pSize / 2; du++) {
			unsigned int lcol = lfb->LookUpBilinearUV(ulf + (float)du, vlf + (float)dv);
			V3 lcolv; lcolv.SetColor(lcol);
			unsigned int rcol = rfb->LookUpBilinearUV(urf + (float)du, vrf + (float)dv);
			V3 rcolv; rcolv.SetColor(rcol);
			err += (rcolv - lcolv).Length();
		}
	}

	return err * 255.0f / pSize / pSize;

}


// non-standard stereo configuration
void Scene::FindCorrespondence(float ulf, float vlf, FrameBuffer *lfb,
	PPC *lppc, FrameBuffer *rfb, PPC *rppc, float &urf, float &vrf) {

	float zn = 30.0f, zf = 500.0f; // range of depths we are searching for
	V3 Pn = lppc->GetPoint(ulf, vlf, zn); // 3D point at nearest possible location
	V3 Pf = lppc->GetPoint(ulf, vlf, zf); // .. farthest possible location
	V3 pPn; rppc->Project(Pn, pPn); // projection on right image for nearest possible point
									// ... aka the near point of the epipolar segment
	V3 pPf; rppc->Project(Pf, pPf); // ... aka the far end of the epipolar segment
	pPf[2] = pPn[2] = 0.0f;

	int patchSize = 7;

	int stepsN = ceilf((pPf - pPn).Length());
	float minError = FLT_MAX;
	V3 lcol; lcol.SetColor(lfb->Get((int)ulf, (int)vlf));
	ofstream ofstr("mydbg/epiError.csv");
	for (int si = 0; si < stepsN; si++) {
		float scf = (float)si / (float)(stepsN - 1);
		V3 cP = pPn + (pPf - pPn)*scf;
#if 0
		unsigned int rcoli = rfb->LookUpBilinearUV(cP[0], cP[1]);
		V3 rcol; rcol.SetColor(rcoli);
		float currError = (rcol - lcol).Length();
#else
		float currError = PatchColorDifference(lfb, ulf, vlf, rfb, cP[0], cP[1], patchSize);
#endif
		ofstr << cP[0] << ", " << currError << endl;
		if (currError < minError) {
			minError = currError;
			urf = cP[0];
			vrf = cP[1];
		}
	}
	ofstr.close();

}


void Scene::DBG() {

	{
		EpiDemo();
		return;
	}

	{

		PanoDemo();
		return;

	}


	{
		if (fbPTM == 0) {
			PTMDemo();
			return;
		}
		else {
			PPC currppc(*ppcPTM);
			V3 transv = ppcPTM->a.UnitVector()*200.0f;
			for (int stepi = 0; stepi < 100; stepi++) {
				V3 currtv = transv / -2.0f + transv * (float)stepi / 99.0f;
				currppc = *ppcPTM;
				currppc.Translate(currppc.GetVD()*-100.0f);
				currppc.Translate(currtv);
				Render(fb, &currppc);
				fb->redraw();
				Fl::check();
			}
			return;
		}
	}

	{

		ppc->LoadFromTextFile("view.txt");
		RenderAll();
		return;
	}

	{

		for (int fi = 0; fi < 1000; fi++) {
			morphFraction = (float)fi / 1299.0f;
			RenderAll();
			Fl::check();
		}
		return;


	}

	{
		V3 L0 = tms[1].GetCenterOfMass() + V3(0.0f, 0.0f, 100.0f);
		V3 L1 = tms[1].GetCenterOfMass() + V3(0.0f, 100.0f, 0.0f);
		for (int fi = 0; fi < 100; fi++) {
			L = L0 + (L1 - L0)*(float)fi / 99.0f;
			cerr << L << "        \r";
			RenderAll();
			Fl::check();
		}
		return;

	}

	{
		PPC ppc1(*ppc);
		ppc1.LoadFromTextFile("view.txt");
		PPC ppc0(*ppc);
		for (int fi = 0; fi < 100; fi++) {
			ppc->SetInterpolated(&ppc0, &ppc1, (float)fi / 99.0f);
			RenderAll();
			Fl::check();
		}
		return;
	}

	{
		ppc->LoadFromTextFile("view.txt");
		Fl::check();
		ShadowMapSetup();
		Fl::check();
		RenderAll();
		Fl::check();
		int gfbw = smfb->w, gfbh = smfb->h;
		gfb = new GFB(gfbw, gfbh);
		// add sampling locations from output image
		gfb->AddSLs(ppc, fb, smppc);
		gfb->SLStats();
		// render scene from light viewpoint on generalized framebuffer
		for (int tmi = 0; tmi < tmsN; tmi++) {
			if (!tms[tmi].enabled)
				continue;
			tms[tmi].RenderGFB(smppc, gfb);
		}
		// collecting shadow information
		gfb->ApplyShadow(fb);
		fb->redraw();
		Fl::check();
		return;

	}

	{

		V3 L0 = L;
		V3 L1 = L + V3(50.0f, -20.0f, -30.0f);

		ppc->LoadFromTextFile("view.txt");
		int fsN = 30;
		for (int fi = 0; fi < fsN; fi++) {
			L = L0 + (L1 - L0)*(float)fi / (float)fsN;
			RenderAll();
			ShadowMapSetup();
			RenderAll();
			Fl::check();
		}
		L = L0;
		return;

	}

	{
		float s0 = 10000.0f;
		float s1 = 10.0f;
		for (int fi = 0; fi < 100; fi++) {
			specc = s0 + (s1 - s0)*(float)fi / 99.0f;
			RenderAll();
			Fl::check();
		}
		return;

	}

	{
		V3 L0 = tms[1].GetCenterOfMass() + V3(0.0f, 0.0f, 100.0f);
		V3 L1 = tms[1].GetCenterOfMass() + V3(0.0f, 100.0f, 0.0f);
		for (int fi = 0; fi < 100; fi++) {
			L = L0 + (L1 - L0)*(float)fi / 99.0f;
			RenderAll();
			Fl::check();
		}
		return;

	}

	{

		float len = 10.0f;
		tms[1].VisualizeNormals(ppc, fb, len);
		fb->redraw();
		return;

	}

	{

		fb->Clear(0xFFFFFFFF, 0.0f);
		fb->Draw3DPoint(ppc->C + V3(0.0f, 0.0f, -100.0f), V3(0.0f, 0.0f, 1.0f),
			11.2f, ppc);
		fb->redraw();
		return;

	}

	{

		V3 a(1.0f, 2.0f, 0.25f);
		a = a.UnitVector();
		V3 C = tms[1].GetCenterOfMass();
		for (int i = 0; i < 360; i++) {
			tms[1].RotateAboutAxis(C, a, 1.0f);
			Render(fb, ppc);
			Fl::check();
		}
		return;

	}


	{

		PPC ppc0 = *ppc;
		V3 O = tms[0].GetCenterOfMass();
		V3 newC = ppc->C + V3(50.0f, 40.0f, 30.0f);
		ppc->PositionAndOrient(newC, O, V3(0.0f, 1.0f, 0.0f));
		PPC ppc1 = *ppc;
		int framesN = 300;
		for (int fi = 0; fi < framesN; fi++) {
			float fracf = (float)fi / (float)(framesN - 1);
			ppc->SetInterpolated(&ppc0, &ppc1, fracf);
			Render(fb, ppc);
			Fl::check();
		}
		*ppc = ppc0;
		return;

	}

	{

		float hfov = 55.0f;
		PPC ppc(fb->w, fb->h, hfov);

		V3 P(0.0f, 0.0f, -100.0f);
		V3 projP;
		if (ppc.Project(P, projP)) {
			cerr << projP << endl;
			fb->DrawCircle((int)projP[0], (int)projP[1], 5.0f, 0xFF00FF00);
			fb->redraw();
		}
		else {
			cerr << "point behind head" << endl;
		}


		return;

	}

	{

		M33 m;
		m[0] = V3(1.0f, -3.0f, 7.0f);
		m[1] = V3(2.0f, 10.0f, 4.3f);
		m[2] = V3(-8.0f, -50.0f, 1.3f);

		M33 m1 = m.Inverted();
		cerr << m1 * m << endl << m * m1 << endl;
		return;


	}


	{
		V3 p0(23.5f, 100.1f, 0.0f);
		V3 p1 = p0 + V3(300.0f, 0.0f, 0.0f);
		V3 p2(23.5f, 300.1f, 0.0f);
		V3 p3 = p2 + V3(300.0f, 0.0f, 0.0f);
		int stepsN = 1000;
		for (int i = 0; i < stepsN; i++) {
			float frac = (float)i / (float)stepsN;
			V3 p01 = p0 + (p1 - p0)*frac;
			V3 p32 = p3 + (p2 - p3)*frac;
			V3 p02 = p0 + (p2 - p0)*frac;
			V3 p31 = p3 + (p1 - p3)*frac;
			fb->SetBGR(0xFFFFFFFF);
			V3 c1(0.0f, 1.0f, 0.0f);
			V3 c0(1.0f, 0.0f, 0.0f);
			fb->Draw2DSegment(p01, p32, c0, c0);
			fb->Draw2DSegment(p02, p31, c1, c1);
			fb->redraw();
			Fl::check();
		}
		return;

	}


	{

		M33 m;
		m[0] = V3(1.0f, 0.0f, 0.0f);
		m[1] = V3(0.0f, 1.0f, 0.0f);
		m[2] = V3(0.0f, 0.0f, 1.0f);
		cerr << m << endl;
		V3 v(2.0f, 10.0f, -1.0f);
		cerr << m * v << endl;
		return;

	}

	{
		int u0 = 100;
		int v0 = 200;
		int u1 = 500;
		int v1 = 400;
		float r = 34.3f;
		int stepsN = 100;
		unsigned int color = 0xFF00FFFF;
		for (int stepi = 0; stepi < stepsN; stepi++) {
			int curru = u0 + (u1 - u0)*stepi / stepsN;
			int currv = v0 + (v1 - v0)*stepi / stepsN;
			fb->SetBGR(0xFFFFFFFF);
			fb->DrawCircle(curru, currv, r, color);
			fb->redraw();
			Fl::check();
		}
		return;
	}

	{
		V3 v0(4.0f, 3.0f, 0.0f);
		V3 v1(5.0f, 2.0f, 1.0f);
		cerr << v0.Length() << endl;
		return;
		cerr << v0 << endl << v1 << endl << v0 - v1 << endl;
		return;
		v0[0] = 3.0f;
		cerr << "v0[0]= " << v0[0] << endl;
		return;
	}

	{
		int u0 = 20;
		int v0 = 40;
		int u1 = 400;
		int v1 = 200;
		unsigned int color = 0xFFFF0000;
		fb->Draw2DRectangle(u0, v0, u1, v1, color);
	}

	fb->redraw();
	return;

	for (int u = 0; u < fb->w; u++) {
		fb->Set(u, fb->h / 2, 0xFF000000);
	}

	fb->redraw();

}


void Scene::NewButton() {
	cerr << "INFO: pressed New Button" << endl;
	ppc->SaveToTextFile("view.txt");
}

void Scene::RenderAll() {

	//	RenderRT(fb, ppc);
	//	return;

	Render(fb, ppc);
	return;

	if (hwfb)
		hwfb->redraw();
	if (gpufb)
		gpufb->redraw();
	//	Render(fb3, ppc3);

}


void Scene::RenderHW() {

	// clear the framebuffer
	glClearColor(0.0, 0.0f, 0.5f, 1.0f);
	glEnable(GL_DEPTH_TEST);
	glClear(GL_COLOR_BUFFER_BIT |
		GL_DEPTH_BUFFER_BIT);

	// set view
	// set intrinsics
	ppc->SetIntrinsicsHW(1.0f, 1000.0f);
	// set extrinsics
	ppc->SetExtrinsicsHW();

	// render geometry
	for (int tmi = 0; tmi < tmsN; tmi++) {
		if (!tms[tmi].enabled)
			continue;
		tms[tmi].RenderHW();
	}

}

void Scene::RenderGPU() {

	// if the first time, call per session initialization
	if (cgi == NULL) {
		cgi = new CGInterface();
		cgi->PerSessionInit();
		soi = new ShaderOneInterface();
		soi->PerSessionInit(cgi);
	}

	// clear the framebuffer
	glClearColor(0.0, 0.0f, 0.5f, 1.0f);
	glEnable(GL_DEPTH_TEST);
	glClear(GL_COLOR_BUFFER_BIT |
		GL_DEPTH_BUFFER_BIT);

	// set view
	// set intrinsics
	ppc->SetIntrinsicsHW(1.0f, 1000.0f);
	// set extrinsics
	ppc->SetExtrinsicsHW();

	// per frame initialization
	cgi->EnableProfiles();
	soi->PerFrameInit();
	soi->BindPrograms();

	// render geometry
	for (int tmi = 0; tmi < tmsN; tmi++) {
		if (!tms[tmi].enabled)
			continue;
		tms[tmi].RenderHW();
	}

	cgi->DisableProfiles();


}


void Scene::RenderRT(FrameBuffer *currfb, PPC *currppc) {

	for (int v = 0; v < currfb->h; v++) {
		for (int u = 0; u < currfb->w; u++) {
			V3 ray = currppc->c + currppc->a*(.5f + (float)u) +
				currppc->b*(.5f + (float)v);
			ray = ray.UnitVector();
			V3 O = currppc->C, rO, rray;
			unsigned int col = TraceRay(0, O, ray, rO, rray), rcol;
			if (rO[0] != FLT_MAX) {
				V3 rrO, rrray;
				rcol = TraceRay(1, rO, rray, rrO, rrray);
				if (rcol == 0xFF00FF00)
					rcol = col;
				V3 c0; c0.SetColor(col);
				V3 c1; c1.SetColor(rcol);
				float diffk = 0.75f;
				col = (c0*diffk + c1 * (1.0f - diffk)).GetColor();
			}
			currfb->Set(u, v, col);
		}
		if (v < currfb->h - 1) {
			for (int u = 0; u < currfb->w; u++) {
				currfb->Set(u, v + 1, 0xFF0000FF);
			}
		}
		if ((v % 10) == 0 || v == currfb->h - 1) {
			currfb->redraw();
			Fl::check();
		}
	}
}

void Scene::Intersect(V3 Vs[3], V3 O, V3 ray, V3 &abc, float &t) {

	M33 V;
	V.SetColumn(0, Vs[0]);
	V.SetColumn(1, Vs[1]);
	V.SetColumn(2, Vs[2]);
	V = V.Inverted();
	V3 q0 = V * O;
	V3 q1 = V * ray;
	t = (1.0f - q0[0] - q0[1] - q0[2]) / (q1[0] + q1[1] + q1[2]);
	abc = q0 + q1 * t;
}

unsigned int Scene::TraceRay(int orderi, V3 O, V3 ray, V3 &rO, V3 &rray) {

	unsigned int ret = 0xFF00FF00;
	float tmin = FLT_MAX;
	rO[0] = FLT_MAX;
	for (int tmi = 0; tmi < tmsN; tmi++) {
		if (!tms[tmi].enabled)
			continue;
		TM &tm = tms[tmi];
		for (int tri = 0; tri < tm.trisN; tri++) {
			V3 Vs[3];
			Vs[0] = tm.verts[tm.tris[3 * tri + 0]];
			Vs[1] = tm.verts[tm.tris[3 * tri + 1]];
			Vs[2] = tm.verts[tm.tris[3 * tri + 2]];
			V3 Cs[3];
			Cs[0] = tm.cols[tm.tris[3 * tri + 0]];
			Cs[1] = tm.cols[tm.tris[3 * tri + 1]];
			Cs[2] = tm.cols[tm.tris[3 * tri + 2]];
			V3 Ns[3];
			if (tm.normals) {
				Ns[0] = tm.normals[tm.tris[3 * tri + 0]];
				Ns[1] = tm.normals[tm.tris[3 * tri + 1]];
				Ns[2] = tm.normals[tm.tris[3 * tri + 2]];
			}
			V3 abc; // barycentric coordinates of intersection point
			float t;
			Intersect(Vs, O, ray, abc, t);
			if (t < 0.01f || abc[0] < 0.0f || abc[1] < 0.0f || abc[2] < 0.0f ||
				t > tmin)
				continue;
			tmin = t;
			ret = (Cs[0] * abc[0] + Cs[1] * abc[1] + Cs[2] * abc[2]).GetColor();
			if (tm.normals && orderi < 1) {
				rO = O + ray * t;
				V3 normal = (Ns[0] * abc[0] + Ns[1] * abc[1] + Ns[2] * abc[2]).UnitVector();
				V3 ev = ray * -1.0f;
				V3 evn = normal * (ev*normal); // normal component of eye vector
				rray = (evn*2.0f - ev).UnitVector(); // reflected vector
			}
		}
	}

	return ret;

}
