

#include <math.h>

#include "viglarea.h"
#include "g12buffer.h"
#include "cloudviewdialog.h"


ViGLArea::ViGLArea(QWidget *parent)
    : QGLWidget(parent)
{
	ui.setupUi(this);

	focal = 1;
	disp = 0;
	disparity = NULL;
	image = NULL;

	//object = 0;
	xRot = 0;
	yRot = 0;
	zRot = 0;

	grid = 0;
	gridOn = 1;

	isTracking = 0;


	camera =  getShift3D( 0, 0, -30);

	/*positon = vectorCreate(0,0,-10);

	positon.x = 0;  positon.y = 0; positon.z = -10;
	view.x = 0.0; view.y = 0.0; view.z = 1.0;
	view.x = 0.0; view.y = 0.0; view.z = 1.0;*/

}

void ViGLArea::leftRotate (void)
{
	camera = multiplyBy3D(camera, getRotation3DY(0.05));
	//xRot += 1;
	this->update();

}
void ViGLArea::rightRotate (void)
{
	camera = multiplyBy3D(camera, getRotation3DY(-0.05));
	//xRot -= 1;
	this->update();

}
void ViGLArea::upRotate (void)
{
	camera = multiplyBy3D(camera, getRotation3DX(0.05));

//	yRot += 1;
	this->update();

}
void ViGLArea::downRotate (void)
{
	camera = multiplyBy3D(camera, getRotation3DX(-0.05));
	yRot -= 1;
	this->update();
}


QSize ViGLArea::minimumSizeHint() const
{
  return QSize(50, 50);
}

QSize ViGLArea::sizeHint() const
{
  return QSize(400, 400);
}

#define GRID_STEP 5
#define GRID_SIZE 5

void ViGLArea::initializeGL()
{
	qglClearColor(Qt::black);
	//object = makeObject();
	glShadeModel(GL_FLAT);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_CULL_FACE);

	grid = glGenLists(1);
	int k,i,j;

	glNewList(grid,GL_COMPILE);

    glPointSize(2);
	glBegin(GL_POINTS);

	for (i = -GRID_SIZE; i < GRID_SIZE; i++)
		for (j = -GRID_SIZE; j < GRID_SIZE; j++)
			for (k = -GRID_SIZE; k < GRID_SIZE; k++)
			{
				glColor3f(
						(double)(i + GRID_SIZE) / (2 * GRID_SIZE + 1) ,
						(double)(j + GRID_SIZE) / (2 * GRID_SIZE + 1),
						(double)(k + GRID_SIZE) / (2 * GRID_SIZE + 1));
				glVertex3d(i * GRID_STEP,j * GRID_STEP,k * GRID_STEP);
			}

	glEnd();
	glEndList();

}


void ViGLArea::resizeGL(int width, int height)
{
   int side = qMin(width, height);
   glViewport((width - side) / 2, (height - side) / 2, side, side);

   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
   //glOrtho(-15, +15, +15, -15, 1.0, 1500.0);
   gluPerspective(30.0f, width/height, 1.0f, 1500.0f);


   glMatrixMode(GL_MODELVIEW);
}


void ViGLArea::setBuffersSlot(G12Buffer *image, DisparityBuffer *disparity, double focal)
{
	this->image = image;
	this->disparity = disparity;
	this->focal = focal;
}


void ViGLArea::setFocal (double focal)
{
	this->focal = focal;
	this->update();
}

void ViGLArea::setDisplacement (double disp)
{
	this->disp = disp;
	this->update();
}

void ViGLArea::setGridOnOff(int val)
{
	gridOn = val;
	this->update();
}

void ViGLArea::mouseMoveEvent(QMouseEvent *event)
{
	uint32_t buttons = event->buttons();

	if (buttons & Qt::LeftButton)
	{
		double xs = (event->x() - track.x()) / 500.0;
		double ys = (event->y() - track.y()) / 500.0;

		//camera = multiplyBy3D(camera, getRotation3DY(xs));
		//camera = multiplyBy3D(camera, getRotation3DX(ys));

		camera = multiplyBy3D(getRotation3DY(xs), camera);
		camera = multiplyBy3D(getRotation3DX(ys), camera);

	}

	if (buttons & Qt::MidButton)
	{
		double xs = (event->x() - track.x()) / 10.0;
		double ys = (event->y() - track.y()) / 10.0;

		//camera = multiplyBy3D(camera, getShift3D(xs, ys, 0));
		camera = multiplyBy3D(getShift3D(xs, ys, 0), camera);
	}

	if (buttons & Qt::RightButton)
	{
		double xs = (event->x() - track.x()) / 10.0;
		double ys = (event->y() - track.y()) / 10.0;

		//camera = multiplyBy3D(camera, getShift3D(0, 0, xs));
		camera = multiplyBy3D(getShift3D(0, 0, xs), camera);
	}


	/*xRot += (event->x() - track.x()) * 3.0;
	yRot += (event->y() - track.y()) * 3.0;*/

	track = event->pos();
	this->update();
}

void ViGLArea::mousePressEvent(QMouseEvent *event)
{
	isTracking = 1;
	track = event->pos();
}

void ViGLArea::mouseReleaseEvent(QMouseEvent *event)
{
	isTracking = 0;

}

void ViGLArea::paintGL()
{
   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
   glLoadIdentity();
   /*glTranslated(0.0, 0.0, -300.0);
   glRotated(xRot / 16.0, 1.0, 0.0, 0.0);
   glRotated(yRot / 16.0, 0.0, 1.0, 0.0);
   glRotated(zRot / 16.0, 0.0, 0.0, 1.0);*/
   affineTransform3D cameraT = affine3Dtranspose(camera);
   glLoadMatrixd(&(cameraT.data[0]));


   if (grid != 0 && gridOn)
    	glCallList(grid);

   if (image == NULL || disparity ==  NULL)
	   return;

    glPointSize(3);
   	glBegin(GL_POINTS);
   	double maxz = -100000;
   	double minz =  100000;
   	int i,j;
   	for ( i = 0; i < image->h; i++)
   	{
   		for ( j = 0; j < image->w; j++)
   		{


   			int color = G12ELEM(image,j,i) >> 5;
   			glColor3b(color, color, color);
   			//glVertex2d(j - I1->w / 2, i - I1->h/2);
   			double x = (double)(j - (image->w / 2))/ image->w;
   			double y = (double)(i - (image->h / 2))/ image->h;
   			double z = 1;

   			//double disparity = (maxdisp - disp->data[i * disp->w + j]) / (maxdisp - mindisp);
   			uint16_t rawDisp = G12ELEM(disparity,j, i);
   			if (rawDisp == DISPARITY_UNKNOWN || rawDisp > DISPARITY_MAX_SANE)
   			{
   				continue;
   			}
   			double disparityVal = ((double)rawDisp / disparity->maxValue ) * 40;
   			disparityVal += disp;

   			double len =  focal / disparityVal;
   			//double len = 100;
   			x *= len;
   			y *= len;
   			z *= len;
   			//z -= 80;
   			/*if (z > maxz)
   				maxz = z;
   			if (z < minz)
   				minz = z;*/
   			glVertex3d(x,-y,-z);
   			//printf("%lf %lf %lf %lf\n", x, y, z, disparity);
   		}
   	}
   	glEnd();


}

ViGLArea::~ViGLArea()
{

}
