#include "reconstructor.h"

#include <iostream>

Reconstructor::Reconstructor(PerspectiveCamera camLeft, PerspectiveCamera camRight): camLeft(camLeft), camRight(camRight) {

type = SceneObjectType::ST_RECONSTRUCTOR;

f = camLeft.getFocalLength();

if (f != camRight.getFocalLength()) {

std::cout << "Haaaaaalt, die Focal Lengths beider Cameras stimmen nicht überein!\n";

}

b = (camRight.getCenterOfProjection() - camLeft.getCenterOfProjection()).length();

std::cout << "b sollte 2.04 sein: " << b << "\n";

reconstruct();

}

void Reconstructor::reconstruct() {

std::vector<std::array<QVector4D, 8>> rightObjects = camRight.getObjects();

std::vector<std::array<QVector4D, 8>> leftObjects = camLeft.getObjects();

QVector4D clcop = camLeft.getCenterOfProjection();

for (size_t i = 0; i < rightObjects.size(); i++) {

std::array<QVector4D, 8> reconstructedObject;

for (int j = 0; j < 8; j++) {

float x_parallax = leftObjects[i][j].x() - rightObjects[i][j].x() + b; //weil b x aligned ist dürfen wir das!

float z = -f * b / x_parallax;

float y = -z * leftObjects[i][j].y() / f;

float x = -z * leftObjects[i][j].x() / f;

reconstructedObject[j] = QVector4D(x,y,z,1) + clcop;

}

reconstructedObjects.push_back(reconstructedObject);

}

}

void Reconstructor::draw (const RenderCamera& renderer, const QColor&, float) const {

// render points of reconstructed cubes

for (auto obj : reconstructedObjects) {

for (auto point : obj) {

//std::cout << point[0] << ", " << point[1] << ", " << point[2] << ", " << point[3] << "\n";

renderer.renderPoint(point, QColorConstants::Blue, 8.0);

}

}

// render lines of reconstructed cubes

std::array<std::array<int, 2>, 12> lines = {{

{0,1}, {0,3}, {1,2}, {2,3},

{4,5}, {4,7}, {5,6}, {6,7},

{0,4}, {1,5}, {2,6}, {3,7}

}};

for (auto obj : reconstructedObjects) {

for (auto el : lines) {

renderer.renderLine(

obj[el[0]],

obj[el[1]],

QColorConstants::Blue,

0.5);

}

}

}

void Reconstructor::affineMap(const QMatrix4x4& M)

{

}