#include <math.h>
#include <sstream>
#include <fstream>
#include <Eigen/StdVector>

#include "OdmOrthoPhoto.hpp"

OdmOrthoPhoto::OdmOrthoPhoto()
    :log_(false){
    outputFile_ = "ortho.tif";
    logFile_    = "log.txt";
    outputCornerFile_ = "";
    bandsOrder = "red,green,blue";

    resolution_ = 0.0f;

    alphaBand = nullptr;
    currentBandIndex = 0;
}

OdmOrthoPhoto::~OdmOrthoPhoto()
{
}

int OdmOrthoPhoto::run(int argc, char *argv[])
{
    try
    {
        parseArguments(argc, argv);
        createOrthoPhoto();
    }
    catch (const OdmOrthoPhotoException& e)
    {
        log_.setIsPrintingInCout(true);
        log_ << e.what() << "\n";
        log_.print(logFile_);
        return EXIT_FAILURE;
    }
    catch (const std::exception& e)
    {
        log_.setIsPrintingInCout(true);
        log_ << "Error in OdmOrthoPhoto:\n";
        log_ << e.what() << "\n";
        log_.print(logFile_);
        return EXIT_FAILURE;
    }
    catch (...)
    {
        log_.setIsPrintingInCout(true);
        log_ << "Unknown error, terminating:\n";
        log_.print(logFile_);
        return EXIT_FAILURE;
    }
    
    log_.print(logFile_);
    
    return EXIT_SUCCESS;
}

void OdmOrthoPhoto::parseArguments(int argc, char *argv[])
{
    logFile_ = std::string(argv[0]) + "_log.txt";
    log_ << logFile_ << "\n\n";
    
    // If no arguments were passed, print help.
    if (argc == 1)
    {
        printHelp();
    }
    
    log_ << "Arguments given\n";
    for(int argIndex = 1; argIndex < argc; ++argIndex)
    {
        log_ << argv[argIndex] << '\n';
    }
    
    log_ << '\n';
    for(int argIndex = 1; argIndex < argc; ++argIndex)
    {
        // The argument to be parsed.
        std::string argument = std::string(argv[argIndex]);
        
        if(argument == "-help")
        {
            printHelp();
        }
        else if(argument == "-resolution")
        {
            ++argIndex;
            if (argIndex >= argc)
            {
                throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
            }
            std::stringstream ss(argv[argIndex]);
            ss >> resolution_;
            log_ << "Resolution count was set to: " << resolution_ << "pixels/meter\n";
        }
        else if(argument == "-verbose")
        {
            log_.setIsPrintingInCout(true);
        }
        else if (argument == "-logFile")
        {
            ++argIndex;
            if (argIndex >= argc)
            {
                throw OdmOrthoPhotoException("Missing argument for '" + argument + "'.");
            }
            logFile_ = std::string(argv[argIndex]);
            std::ofstream testFile(logFile_.c_str());
            if (!testFile.is_open())
            {
                throw OdmOrthoPhotoException("Argument '" + argument + "' has a bad value.");
            }
            log_ << "Log file path was set to: " << logFile_ << "\n";
        }
        else if(argument == "-inputFiles")
        {
            argIndex++;
            if (argIndex >= argc)
            {
                throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
            }
            std::string inputFilesArg = std::string(argv[argIndex]);
            std::stringstream ss(inputFilesArg);
            std::string item;
            while(std::getline(ss, item, ',')){
                inputFiles.push_back(item);
            }
        }
        else if(argument == "-bands")
        {
            argIndex++;
            if (argIndex >= argc)
            {
                throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
            }
            bandsOrder = std::string(argv[argIndex]);
        }
        else if(argument == "-outputFile")
        {
            argIndex++;
            if (argIndex >= argc)
            {
                throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
            }
            outputFile_ = std::string(argv[argIndex]);
            log_ << "Writing output to: " << outputFile_ << "\n";
        }
        else if(argument == "-outputCornerFile")
        {
            argIndex++;
            if (argIndex >= argc)
            {
                throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
            }
            outputCornerFile_ = std::string(argv[argIndex]);
            log_ << "Writing corners to: " << outputCornerFile_ << "\n";
        }
        else
        {
            printHelp();
            throw OdmOrthoPhotoException("Unrecognised argument '" + argument + "'");
        }
    }
    log_ << "\n";

    std::stringstream ss(bandsOrder);
    std::string item;
    while(std::getline(ss, item, ',')){
        if (item == "red" || item == "r"){
            colorInterps.push_back(GCI_RedBand);
        }else if (item == "green" || item == "g"){
            colorInterps.push_back(GCI_GreenBand);
        }else if (item == "blue" || item == "b"){
            colorInterps.push_back(GCI_BlueBand);
        }else{
            colorInterps.push_back(GCI_GrayIndex);
        }
    }
}

void OdmOrthoPhoto::printHelp()
{
    log_.setIsPrintingInCout(true);

    log_ << "odm_orthophoto\n\n";

    log_ << "Purpose\n";
    log_ << "Create an orthograpical photo from an oriented textured mesh.\n\n";

    log_ << "Usage:\n";
    log_ << "The program requires a path to an input OBJ mesh file and a resolution, as pixels/m. All other input parameters are optional.\n\n";

    log_ << "The following flags are available\n";
    log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n";
    log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output stream as well as in the log file.\n\n";

    log_ << "Parameters are specified as: \"-<argument name> <argument>\", (without <>), and the following parameters are configureable:\n";
    log_ << "\"-inputFiles <path>[,<path2>,<path3>,...]\" (mandatory)\n";
    log_ << "\"Input obj files that must contain a textured mesh.\n\n";

    log_ << "\"-outputFile <path>\" (optional, default: ortho.jpg)\n";
    log_ << "\"Target file in which the orthophoto is saved.\n\n";

    log_ << "\"-outputCornerFile <path>\" (optional)\n";
    log_ << "\"Target text file for boundary corner points, written as \"xmin ymin xmax ymax\".\n\n";

    log_ << "\"-resolution <pixels/m>\" (mandatory)\n";
    log_ << "\"The number of pixels used per meter.\n\n";

    log_ << "\"-bands red,green,blue,[...]\" (optional)\n";
    log_ << "\"Naming of bands to assign color interpolation values when creating output TIFF.\n\n";

    log_.setIsPrintingInCout(false);
}

void OdmOrthoPhoto::saveTIFF(const std::string &filename, GDALDataType dataType){
    GDALAllRegister();
    GDALDriverH hDriver = GDALGetDriverByName( "GTiff" );
    if (!hDriver){
        std::cerr << "Cannot initialize GeoTIFF driver. Check your GDAL installation." << std::endl;
        exit(1);
    }
    char **papszOptions = NULL;
    GDALDatasetH hDstDS = GDALCreate( hDriver, filename.c_str(), width, height,
                                      static_cast<int>(bands.size() + 1), dataType, papszOptions );
    GDALRasterBandH hBand;

    // Bands
    size_t i = 0;
    for (; i < bands.size(); i++){
        hBand = GDALGetRasterBand( hDstDS, static_cast<int>(i) + 1 );

        GDALColorInterp interp = GCI_GrayIndex;
        if (i < colorInterps.size()){
            interp = colorInterps[i];
        }
        GDALSetRasterColorInterpretation(hBand, interp );

        if (GDALRasterIO( hBand, GF_Write, 0, 0, width, height,
                    bands[i], width, height, dataType, 0, 0 ) != CE_None){
            std::cerr << "Cannot write TIFF to " << filename << std::endl;
            exit(1);
        }
    }

    // Alpha
    if (dataType == GDT_Float32){
        finalizeAlphaBand<float>();
    }else if (dataType == GDT_UInt16){
        finalizeAlphaBand<uint16_t>();
    }else if (dataType == GDT_Byte){
        finalizeAlphaBand<uint8_t>();
    }else{
        throw OdmOrthoPhotoException("Invalid data type");
    }

    // Alpha
    hBand = GDALGetRasterBand( hDstDS, static_cast<int>(i) + 1 );

    // Set alpha band
    GDALSetRasterColorInterpretation(hBand, GCI_AlphaBand );

    if (GDALRasterIO( hBand, GF_Write, 0, 0, width, height,
                alphaBand, width, height, dataType, 0, 0 ) != CE_None){
        std::cerr << "Cannot write TIFF (alpha) to " << filename << std::endl;
        exit(1);
    }

    GDALClose( hDstDS );
}

template <typename T>
inline T maxRange(){
    return static_cast<T>(pow(2, sizeof(T) * 8) - 1);
}

template <typename T>
void OdmOrthoPhoto::initBands(int count){
    size_t pixelCount = static_cast<size_t>(width) * static_cast<size_t>(height);

    // Channels
    for (int i = 0; i < count; i++){
        T *arr = new T[pixelCount];
        for (size_t j = 0; j < pixelCount; j++){
            arr[j] = maxRange<T>();
        }
        bands.push_back(static_cast<void *>(arr));
    }
}

template <typename T>
void OdmOrthoPhoto::initAlphaBand(){
     size_t pixelCount = static_cast<size_t>(width) * static_cast<size_t>(height);
     // Alpha
     if (alphaBand == nullptr){
         T *arr = new T[pixelCount];
         for (size_t j = 0; j < pixelCount; j++){
             arr[j] = 0.0;
         }
         alphaBand = static_cast<void *>(arr);
     }
}

template <typename T>
void OdmOrthoPhoto::finalizeAlphaBand(){
     // Adjust alpha band values, only pixels that have
     // values on all bands should be visible

     size_t pixelCount = static_cast<size_t>(width) * static_cast<size_t>(height);
     int channels = bands.size();

     T *arr = reinterpret_cast<T *>(alphaBand);
     for (size_t j = 0; j < pixelCount; j++){
         arr[j] = arr[j] >= channels ? 255.0 : 0.0;
     }
}


void OdmOrthoPhoto::createOrthoPhoto()
{
    if(inputFiles.size() == 0)
    {
        throw OdmOrthoPhotoException("Failed to create ortho photo, no texture meshes given.");
    }

    int textureDepth = -1;
    bool primary = true;
    Bounds bounds;

    for (auto &inputFile : inputFiles){
        log_ << "Reading mesh file... " << inputFile << "\n";

        std::vector<pcl::MTLReader> companions; /**< Materials (used by loadOBJFile). **/
        pcl::TextureMesh mesh;
        loadObjFile(inputFile, mesh, companions);
        log_ << "Mesh file read.\n\n";

        // Does the model have more than one material?
        bool multiMaterial_ = 1 < mesh.tex_materials.size();
        bool splitModel = false;

        if(multiMaterial_)
        {
            // Need to check relationship between texture coordinates and faces.
            if(!isModelOk(mesh))
            {
                splitModel = true;
            }
        }

        Bounds b = computeBoundsForModel(mesh);

        log_ << "Model bounds x : " << b.xMin << " -> " << b.xMax << '\n';
        log_ << "Model bounds y : " << b.yMin << " -> " << b.yMax << '\n';

        if (primary){
            bounds = b;
        }else{
            // Quick check
            if (b.xMin != bounds.xMin ||
                    b.xMax != bounds.xMax ||
                    b.yMin != bounds.yMin ||
                    b.yMax != bounds.yMax){
                throw OdmOrthoPhotoException("Bounds between models must all match, but they don't.");
            }
        }

        // The size of the area.
        float xDiff = bounds.xMax - bounds.xMin;
        float yDiff = bounds.yMax - bounds.yMin;
        log_ << "Model area : " << xDiff*yDiff << "m2\n";

        // The resolution necessary to fit the area with the given resolution.
        height = static_cast<int>(std::ceil(resolution_*yDiff));
        width = static_cast<int>(std::ceil(resolution_*xDiff));

        depth_ = cv::Mat::zeros(height, width, CV_32F) - std::numeric_limits<float>::infinity();
        log_ << "Model resolution, width x height : " << width << "x" << height << '\n';

        // Check size of photo.
        if(0 >= height*width)
        {
            if(0 >= height)
            {
                log_ << "Warning: ortho photo has zero area, height = " << height << ". Forcing height = 1.\n";
                height = 1;
            }
            if(0 >= width)
            {
                log_ << "Warning: ortho photo has zero area, width = " << width << ". Forcing width = 1.\n";
                width = 1;
            }
            log_ << "New ortho photo resolution, width x height : " << width << "x" << height << '\n';
        }

        // Contains the vertices of the mesh.
        pcl::PointCloud<pcl::PointXYZ>::Ptr meshCloud (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromPCLPointCloud2 (mesh.cloud, *meshCloud);

        // Split model and make copies of vertices and texture coordinates for all faces
        if (splitModel)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr meshCloudSplit (new pcl::PointCloud<pcl::PointXYZ>);
            std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > textureCoordinates = std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >(0);

            size_t vertexIndexCount = 0;
            for(size_t t = 0; t < mesh.tex_polygons.size(); ++t)
            {
                vertexIndexCount += 3 * mesh.tex_polygons[t].size();
            }
            textureCoordinates.reserve(vertexIndexCount);

            for(size_t t = 0; t < mesh.tex_polygons.size(); ++t)
            {

                for(size_t faceIndex = 0; faceIndex < mesh.tex_polygons[t].size(); ++faceIndex)
                {
                    pcl::Vertices polygon = mesh.tex_polygons[t][faceIndex];

                    // The index to the vertices of the polygon.
                    size_t v1i = polygon.vertices[0];
                    size_t v2i = polygon.vertices[1];
                    size_t v3i = polygon.vertices[2];

                    // The polygon's points.
                    pcl::PointXYZ v1 = meshCloud->points[v1i];
                    pcl::PointXYZ v2 = meshCloud->points[v2i];
                    pcl::PointXYZ v3 = meshCloud->points[v3i];

                    Eigen::Vector2f vt1 = mesh.tex_coordinates[0][3*faceIndex];
                    Eigen::Vector2f vt2 = mesh.tex_coordinates[0][3*faceIndex + 1];
                    Eigen::Vector2f vt3 = mesh.tex_coordinates[0][3*faceIndex + 2];

                    meshCloudSplit->points.push_back(v1);
                    textureCoordinates.push_back(vt1);
                    mesh.tex_polygons[t][faceIndex].vertices[0] = vertexIndexCount;

                    meshCloudSplit->points.push_back(v2);
                    textureCoordinates.push_back(vt2);
                    mesh.tex_polygons[t][faceIndex].vertices[1] = vertexIndexCount;

                    meshCloudSplit->points.push_back(v3);
                    textureCoordinates.push_back(vt3);
                    mesh.tex_polygons[t][faceIndex].vertices[2] = vertexIndexCount;
                }
            }

            mesh.tex_coordinates.clear();
            mesh.tex_coordinates.push_back(textureCoordinates);

            meshCloud = meshCloudSplit;
        }

        // Creates a transformation which aligns the area for the ortho photo.
        Eigen::Transform<float, 3, Eigen::Affine> transform = getROITransform(bounds.xMin, -bounds.yMax);
        log_ << "Translating and scaling mesh...\n";

        // Move the mesh into position.
        pcl::transformPointCloud(*meshCloud, *meshCloud, transform);
        log_ << ".. mesh translated and scaled.\n\n";

        // Flatten texture coordinates.
        std::vector<Eigen::Vector2f> uvs;
        uvs.reserve(mesh.tex_coordinates.size());
        for(size_t t = 0; t < mesh.tex_coordinates.size(); ++t)
        {
            uvs.insert(uvs.end(), mesh.tex_coordinates[t].begin(), mesh.tex_coordinates[t].end());
        }

        // The current material texture
        cv::Mat texture;

        // Used to keep track of the global face index.
        size_t faceOff = 0;

        log_ << "Rendering the ortho photo...\n";

        // Iterate over each part of the mesh (one per material).
        for(size_t t = 0; t < mesh.tex_materials.size(); ++t)
        {
            // The material of the current submesh.
            pcl::TexMaterial material = mesh.tex_materials[t];
            texture = cv::imread(material.tex_file, cv::IMREAD_ANYDEPTH | cv::IMREAD_UNCHANGED);

            // BGR to RGB when necessary
            if (texture.channels() == 3){
                cv::cvtColor(texture, texture, cv::COLOR_BGR2RGB);
            }

            // The first material determines the bit depth
            // Init ortho photo
            if (t == 0){
                if (primary) textureDepth = texture.depth();
                else if (textureDepth != texture.depth()) throw OdmOrthoPhotoException("Texture depth must be the same for all models");

                log_ << "Texture channels: " << texture.channels() << "\n";

                try{
                    if (textureDepth == CV_8U){
                        log_ << "Texture depth: 8bit\n";
                        initBands<uint8_t>(texture.channels());
                        if (primary) initAlphaBand<uint8_t>();
                    }else if (textureDepth == CV_16U){
                        log_ << "Texture depth: 16bit\n";
                        initBands<uint16_t>(texture.channels());
                        if (primary) initAlphaBand<uint16_t>();
                    }else if (textureDepth == CV_32F){
                        log_ << "Texture depth: 32bit (float)\n";
                        initBands<float>(texture.channels());
                        if (primary) initAlphaBand<float>();
                    }else{
                        std::cerr << "Unsupported bit depth value: " << textureDepth;
                        exit(1);
                    }
                }catch(const std::bad_alloc &){
                    std::cerr << "Couldn't allocate enough memory to render the orthophoto (" << width << "x" << height << " cells = " << ((long long)width * (long long)height * 4) << " bytes). Try to increase the --orthophoto-resolution parameter to a larger integer or add more RAM.\n";
                    exit(1);
                }
            }

            // Check for missing files.
            if(texture.empty())
            {
                log_ << "Material texture could not be read:\n";
                log_ << material.tex_file << '\n';
                log_ << "Could not be read as image, does the file exist?\n";
                continue; // Skip to next material.
            }

            // The faces of the current submesh.
            std::vector<pcl::Vertices> faces = mesh.tex_polygons[t];

            // Iterate over each face...
            for(size_t faceIndex = 0; faceIndex < faces.size(); ++faceIndex)
            {
                // The current polygon.
                pcl::Vertices polygon = faces[faceIndex];

                // ... and draw it into the ortho photo.
                if (textureDepth == CV_8U){
                    drawTexturedTriangle<uint8_t>(texture, polygon, meshCloud, uvs, faceIndex+faceOff);
                }else if (textureDepth == CV_16U){
                    drawTexturedTriangle<uint16_t>(texture, polygon, meshCloud, uvs, faceIndex+faceOff);
                }else if (textureDepth == CV_32F){
                    drawTexturedTriangle<float>(texture, polygon, meshCloud, uvs, faceIndex+faceOff);
                }
            }
            faceOff += faces.size();
            log_ << "Material " << t << " rendered.\n";
        }
        log_ << "... model rendered\n";

        currentBandIndex += texture.channels();
        primary = false;
    }

    log_ << '\n';
    log_ << "Writing ortho photo to " << outputFile_ << "\n";

    if (textureDepth == CV_8U){
        saveTIFF(outputFile_, GDT_Byte);
    }else if (textureDepth == CV_16U){
        saveTIFF(outputFile_, GDT_UInt16);
    }else if (textureDepth == CV_32F){
        saveTIFF(outputFile_, GDT_Float32);
    }else{
        std::cerr << "Unsupported bit depth value: " << textureDepth;
        exit(1);
    }

    if (!outputCornerFile_.empty())
    {
        log_ << "Writing corner coordinates to " << outputCornerFile_ << "\n";
        std::ofstream cornerStream(outputCornerFile_.c_str());
        if (!cornerStream.is_open())
        {
            throw OdmOrthoPhotoException("Failed opening output corner file " + outputCornerFile_ + ".");
        }
        cornerStream.setf(std::ios::scientific, std::ios::floatfield);
        cornerStream.precision(17);
        cornerStream << bounds.xMin << " " << bounds.yMin << " " << bounds.xMax << " " << bounds.yMax;
        cornerStream.close();
    }

    log_ << "Orthophoto generation done.\n";
}

Bounds OdmOrthoPhoto::computeBoundsForModel(const pcl::TextureMesh &mesh)
{
    log_ << "Set boundary to contain entire model.\n";

    // The boundary of the model.
    Bounds r;

    r.xMin = std::numeric_limits<float>::infinity();
    r.xMax = -std::numeric_limits<float>::infinity();
    r.yMin = std::numeric_limits<float>::infinity();
    r.yMax = -std::numeric_limits<float>::infinity();

    // Contains the vertices of the mesh.
    pcl::PointCloud<pcl::PointXYZ>::Ptr meshCloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2 (mesh.cloud, *meshCloud);

    for (size_t i = 0; i < meshCloud->points.size(); i++){
        pcl::PointXYZ v = meshCloud->points[i];
        r.xMin = std::min(r.xMin, v.x);
        r.xMax = std::max(r.xMax, v.x);
        r.yMin = std::min(r.yMin, v.y);
        r.yMax = std::max(r.yMax, v.y);
    }

    log_ << "Boundary points:\n";
    log_ << "Point 1: " << r.xMin << " " << r.yMin << "\n";
    log_ << "Point 2: " << r.xMin << " " << r.yMax << "\n";
    log_ << "Point 3: " << r.xMax << " " << r.yMax << "\n";
    log_ << "Point 4: " << r.xMax << " " << r.yMin << "\n";
    log_ << "\n";

    return r;
}

Eigen::Transform<float, 3, Eigen::Affine> OdmOrthoPhoto::getROITransform(float xMin, float yMin) const
{
    // The transform used to move the chosen area into the ortho photo.
    Eigen::Transform<float, 3, Eigen::Affine> transform;

    transform(0, 0) = resolution_;     // x Scaling.
    transform(1, 0) = 0.0f;
    transform(2, 0) = 0.0f;
    transform(3, 0) = 0.0f;

    transform(0, 1) = 0.0f;
    transform(1, 1) = -resolution_;     // y Scaling, mirrored for easier rendering.
    transform(2, 1) = 0.0f;
    transform(3, 1) = 0.0f;

    transform(0, 2) = 0.0f;
    transform(1, 2) = 0.0f;
    transform(2, 2) = 1.0f;
    transform(3, 2) = 0.0f;

    transform(0, 3) = -xMin * resolution_;    // x Translation
    transform(1, 3) = -yMin * resolution_;    // y Translation
    transform(2, 3) = 0.0f;
    transform(3, 3) = 1.0f;

    return transform;
}

template <typename T>
void OdmOrthoPhoto::drawTexturedTriangle(const cv::Mat &texture, const pcl::Vertices &polygon, const pcl::PointCloud<pcl::PointXYZ>::Ptr &meshCloud, const std::vector<Eigen::Vector2f> &uvs, size_t faceIndex)
{
    // The index to the vertices of the polygon.
    size_t v1i = polygon.vertices[0];
    size_t v2i = polygon.vertices[1];
    size_t v3i = polygon.vertices[2];

    // The polygon's points.
    pcl::PointXYZ v1 = meshCloud->points[v1i];
    pcl::PointXYZ v2 = meshCloud->points[v2i];
    pcl::PointXYZ v3 = meshCloud->points[v3i];

    if(isSliverPolygon(v1, v2, v3))
    {
        log_ << "Warning: Sliver polygon found at face index " << faceIndex << '\n';
        return;
    }

    // The face data. Position v*{x,y,z}. Texture coordinate v*{u,v}. * is the vertex number in the polygon.
    float v1x, v1y, v1z, v1u, v1v;
    float v2x, v2y, v2z, v2u, v2v;
    float v3x, v3y, v3z, v3u, v3v;

    // Barycentric coordinates of the currently rendered point.
    float l1, l2, l3;

    // The size of the photo, as float.
    float fRows, fCols;
    fRows = static_cast<float>(texture.rows);
    fCols = static_cast<float>(texture.cols);

    // Get vertex position.
    v1x = v1.x; v1y = v1.y; v1z = v1.z;
    v2x = v2.x; v2y = v2.y; v2z = v2.z;
    v3x = v3.x; v3y = v3.y; v3z = v3.z;

    // Get texture coordinates. 
    v1u = uvs[3*faceIndex][0]; v1v = uvs[3*faceIndex][1];
    v2u = uvs[3*faceIndex+1][0]; v2v = uvs[3*faceIndex+1][1];
    v3u = uvs[3*faceIndex+2][0]; v3v = uvs[3*faceIndex+2][1];

    // Check bounding box overlap.
    int xMin = static_cast<int>(std::min(std::min(v1x, v2x), v3x));
    if(xMin > width)
    {
        return; // Completely outside to the right.
    }
    int xMax = static_cast<int>(std::max(std::max(v1x, v2x), v3x));
    if(xMax < 0)
    {
        return; // Completely outside to the left.
    }
    int yMin = static_cast<int>(std::min(std::min(v1y, v2y), v3y));
    if(yMin > height)
    {
        return; // Completely outside to the top.
    }
    int yMax = static_cast<int>(std::max(std::max(v1y, v2y), v3y));
    if(yMax < 0)
    {
        return; // Completely outside to the bottom.
    }

    // Top point row and column positions
    float topR, topC;
    // Middle point row and column positions
    float midR, midC;
    // Bottom point row and column positions
    float botR, botC;

    // Find top, middle and bottom points.
    if(v1y < v2y)
    {
        if(v1y < v3y)
        {
            if(v2y < v3y)
            {
                // 1 -> 2 -> 3
                topR = v1y; topC = v1x;
                midR = v2y; midC = v2x;
                botR = v3y; botC = v3x;
            }
            else
            {
                // 1 -> 3 -> 2
                topR = v1y; topC = v1x;
                midR = v3y; midC = v3x;
                botR = v2y; botC = v2x;
            }
        }
        else
        {
            // 3 -> 1 -> 2
            topR = v3y; topC = v3x;
            midR = v1y; midC = v1x;
            botR = v2y; botC = v2x;
        }        
    }
    else // v2y <= v1y
    {
        if(v2y < v3y)
        {
            if(v1y < v3y)
            {
                // 2 -> 1 -> 3
                topR = v2y; topC = v2x;
                midR = v1y; midC = v1x;
                botR = v3y; botC = v3x;
            }
            else
            {
                // 2 -> 3 -> 1
                topR = v2y; topC = v2x;
                midR = v3y; midC = v3x;
                botR = v1y; botC = v1x;
            }
        }
        else
        {
            // 3 -> 2 -> 1
            topR = v3y; topC = v3x;
            midR = v2y; midC = v2x;
            botR = v1y; botC = v1x;
        }
    }

    // General appreviations:
    // ---------------------
    // tm : Top(to)Middle.
    // mb : Middle(to)Bottom.
    // tb : Top(to)Bottom.
    // c  : column.
    // r  : row.
    // dr : DeltaRow, step value per row.

    // The step along column for every step along r. Top to middle.
    float ctmdr;
    // The step along column for every step along r. Top to bottom.
    float ctbdr;
    // The step along column for every step along r. Middle to bottom.
    float cmbdr;

    ctbdr = (botC-topC)/(botR-topR);

    // The current column position, from top to middle.
    float ctm = topC;
    // The current column position, from top to bottom.
    float ctb = topC;

    // Check for vertical line between middle and top.
    if(FLT_EPSILON < midR-topR)
    {
        ctmdr = (midC-topC)/(midR-topR);

        // The first pixel row for the bottom part of the triangle.
        int rqStart = std::max(static_cast<int>(std::floor(topR+0.5f)), 0);
        // The last pixel row for the top part of the triangle.
        int rqEnd = std::min(static_cast<int>(std::floor(midR+0.5f)), height);

        // Traverse along row from top to middle.
        for(int rq = rqStart; rq < rqEnd; ++rq)
        {
            // Set the current column positions.
            ctm = topC + ctmdr*(static_cast<float>(rq)+0.5f-topR);
            ctb = topC + ctbdr*(static_cast<float>(rq)+0.5f-topR);

            // The first pixel column for the current row.
            int cqStart = std::max(static_cast<int>(std::floor(0.5f+std::min(ctm, ctb))), 0);
            // The last pixel column for the current row.
            int cqEnd = std::min(static_cast<int>(std::floor(0.5f+std::max(ctm, ctb))), width);

            for(int cq = cqStart; cq < cqEnd; ++cq)
            {
                // Get barycentric coordinates for the current point.
                getBarycentricCoordinates(v1, v2, v3, static_cast<float>(cq)+0.5f, static_cast<float>(rq)+0.5f, l1, l2, l3);

                if(0.f > l1 || 0.f > l2 || 0.f > l3)
                {
                    //continue;
                }
                
                // The z value for the point.
                float z = v1z*l1+v2z*l2+v3z*l3;

                // Check depth
                float depthValue = depth_.at<float>(rq, cq);
                if(z < depthValue)
                {
                    // Current is behind another, don't draw.
                    continue;
                }

                // The uv values of the point.
                float u, v;
                u = v1u*l1+v2u*l2+v3u*l3;
                v = v1v*l1+v2v*l2+v3v*l3;
                
                renderPixel<T>(rq, cq, u*fCols, (1.0f-v)*fRows, texture);
                
                // Update depth buffer.
                depth_.at<float>(rq, cq) = z;
            }
        }
    }

    if(FLT_EPSILON < botR-midR)
    {
        cmbdr = (botC-midC)/(botR-midR);

        // The current column position, from middle to bottom.
        float cmb = midC;

        // The first pixel row for the bottom part of the triangle.
        int rqStart = std::max(static_cast<int>(std::floor(midR+0.5f)), 0);
        // The last pixel row for the bottom part of the triangle.
        int rqEnd = std::min(static_cast<int>(std::floor(botR+0.5f)), height);

        // Traverse along row from middle to bottom.
        for(int rq = rqStart; rq < rqEnd; ++rq)
        {
            // Set the current column positions.
            ctb = topC + ctbdr*(static_cast<float>(rq)+0.5f-topR);
            cmb = midC + cmbdr*(static_cast<float>(rq)+0.5f-midR);

            // The first pixel column for the current row.
            int cqStart = std::max(static_cast<int>(std::floor(0.5f+std::min(cmb, ctb))), 0);
            // The last pixel column for the current row.
            int cqEnd = std::min(static_cast<int>(std::floor(0.5f+std::max(cmb, ctb))), width);

            for(int cq = cqStart; cq < cqEnd; ++cq)
            {
                // Get barycentric coordinates for the current point.
                getBarycentricCoordinates(v1, v2, v3, static_cast<float>(cq)+0.5f, static_cast<float>(rq)+0.5f, l1, l2, l3);

                if(0.f > l1 || 0.f > l2 || 0.f > l3)
                {
                    //continue;
                }

                // The z value for the point.
                float z = v1z*l1+v2z*l2+v3z*l3;

                // Check depth
                float depthValue = depth_.at<float>(rq, cq);
                if(z < depthValue)
                {
                    // Current is behind another, don't draw.
                    continue;
                }

                // The uv values of the point.
                float u, v;
                u = v1u*l1+v2u*l2+v3u*l3;
                v = v1v*l1+v2v*l2+v3v*l3;

                renderPixel<T>(rq, cq, u*fCols, (1.0f-v)*fRows, texture);

                // Update depth buffer.
                depth_.at<float>(rq, cq) = z;
            }
        }
    }
}

template <typename T>
void OdmOrthoPhoto::renderPixel(int row, int col, float s, float t, const cv::Mat &texture)
{
    // The offset of the texture coordinate from its pixel positions.
    float leftF, topF;
    // The position of the top left pixel.
    int left, top;
    // The distance to the left and right pixel from the texture coordinate.
    float dl, dt;
    // The distance to the top and bottom pixel from the texture coordinate.
    float dr, db;
    
    dl = modff(s, &leftF);
    dr = 1.0f - dl;
    dt = modff(t, &topF);
    db = 1.0f - dt;
    
    left = static_cast<int>(leftF);
    top = static_cast<int>(topF);
    
    // The interpolated color values.
    size_t idx = static_cast<size_t>(row) * static_cast<size_t>(width) + static_cast<size_t>(col);
    T *data = reinterpret_cast<T *>(texture.data); // Faster access
    int numChannels = texture.channels();

    for (int i = 0; i < numChannels; i++){
        float value = 0.0f;

        T tl = data[(top) * texture.cols * numChannels + (left) * numChannels + i];
        T tr = data[(top) * texture.cols * numChannels + (left + 1) * numChannels + i];
        T bl = data[(top + 1) * texture.cols * numChannels + (left) * numChannels + i];
        T br = data[(top + 1) * texture.cols * numChannels + (left + 1) * numChannels + i];

        value += static_cast<float>(tl) * dr * db;
        value += static_cast<float>(tr) * dl * db;
        value += static_cast<float>(bl) * dr * dt;
        value += static_cast<float>(br) * dl * dt;

        static_cast<T *>(bands[currentBandIndex + i])[idx] = static_cast<T>(value);
    }

    // Increment the alpha band if the pixel was visible for this band
    // the final alpha band will be set to 255 if alpha == num bands
    // (all bands have information at this pixel)
    static_cast<T *>(alphaBand)[idx] += static_cast<T>(numChannels);
}

void OdmOrthoPhoto::getBarycentricCoordinates(pcl::PointXYZ v1, pcl::PointXYZ v2, pcl::PointXYZ v3, float x, float y, float &l1, float &l2, float &l3) const
{
    // Diff along y.
    float y2y3 = v2.y-v3.y;
    float y1y3 = v1.y-v3.y;
    float y3y1 = v3.y-v1.y;
    float yy3  =  y  -v3.y;
    
    // Diff along x.
    float x3x2 = v3.x-v2.x;
    float x1x3 = v1.x-v3.x;
    float xx3  =  x  -v3.x;
    
    // Normalization factor.
    float norm = (y2y3*x1x3 + x3x2*y1y3);
    
    l1 = (y2y3*(xx3) + x3x2*(yy3)) / norm;
    l2 = (y3y1*(xx3) + x1x3*(yy3)) / norm;
    l3 = 1 - l1 - l2;
}

bool OdmOrthoPhoto::isSliverPolygon(pcl::PointXYZ v1, pcl::PointXYZ v2, pcl::PointXYZ v3) const
{
    // Calculations are made using doubles, to minize rounding errors.
    Eigen::Vector3d a = Eigen::Vector3d(static_cast<double>(v1.x), static_cast<double>(v1.y), static_cast<double>(v1.z));
    Eigen::Vector3d b = Eigen::Vector3d(static_cast<double>(v2.x), static_cast<double>(v2.y), static_cast<double>(v2.z));
    Eigen::Vector3d c = Eigen::Vector3d(static_cast<double>(v3.x), static_cast<double>(v3.y), static_cast<double>(v3.z));
    Eigen::Vector3d dummyVec = (a-b).cross(c-b);

    // Area smaller than, or equal to, floating-point epsilon.
    return std::numeric_limits<float>::epsilon() >= static_cast<float>(std::sqrt(dummyVec.dot(dummyVec))/2.0);
}

bool OdmOrthoPhoto::isModelOk(const pcl::TextureMesh &mesh)
{
    // The number of texture coordinates in the model.
    size_t nTextureCoordinates = 0;
    // The number of faces in the model.
    size_t nFaces = 0;
    
    for(size_t t = 0; t < mesh.tex_coordinates.size(); ++t)
    {
        nTextureCoordinates += mesh.tex_coordinates[t].size();
    }
    for(size_t t = 0; t < mesh.tex_polygons.size(); ++t)
    {
        nFaces += mesh.tex_polygons[t].size();
    }
    
    log_ << "Number of faces in the model " << nFaces << '\n';
    
    return 3*nFaces == nTextureCoordinates;
}


bool OdmOrthoPhoto::loadObjFile(std::string inputFile, pcl::TextureMesh &mesh, std::vector<pcl::MTLReader> &companions)
{
    int data_type;
    unsigned int data_idx;
    int file_version;
    int offset = 0;
    Eigen::Vector4f origin;
    Eigen::Quaternionf orientation;

    if (!readHeader(inputFile, mesh.cloud, origin, orientation, file_version, data_type, data_idx, offset, companions))
    {
        throw OdmOrthoPhotoException("Problem reading header in modelfile!\n");
    }

    std::ifstream fs;

    fs.open (inputFile.c_str (), std::ios::binary);
    if (!fs.is_open () || fs.fail ())
    {
        //PCL_ERROR ("[pcl::OBJReader::readHeader] Could not open file '%s'! Error : %s\n", file_name.c_str (), strerror(errno));
        fs.close ();
        log_<<"Could not read mesh from file ";
        log_ << inputFile.c_str();
        log_ <<"\n";

        throw OdmOrthoPhotoException("Problem reading mesh from file!\n");
    }

    // Seek at the given offset
    fs.seekg (data_idx, std::ios::beg);

    // Get normal_x field indices
    int normal_x_field = -1;
    for (std::size_t i = 0; i < mesh.cloud.fields.size (); ++i)
    {
        if (mesh.cloud.fields[i].name == "normal_x")
        {
            normal_x_field = i;
            break;
        }
    }

    std::size_t v_idx = 0;
    std::size_t vn_idx = 0;
    std::size_t vt_idx = 0;
    std::size_t f_idx = 0;
    std::string line;
    std::vector<std::string> st;
    std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > coordinates;
    std::vector<Eigen::Vector2f> allTexCoords;

    std::map<int, int> f2vt;

    try
    {
        while (!fs.eof ())
        {
            getline (fs, line);
            // Ignore empty lines
            if (line == "")
                continue;

            // Tokenize the line
            std::stringstream sstream (line);
            sstream.imbue (std::locale::classic ());
            line = sstream.str ();
            boost::trim (line);
            boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on);

            // Ignore comments
            if (st[0] == "#")
                continue;
            // Vertex
            if (st[0] == "v")
            {
                try
                {
                    for (int i = 1, f = 0; i < 4; ++i, ++f)
                    {
                        float value = boost::lexical_cast<float> (st[i]);
                        memcpy (&mesh.cloud.data[v_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset], &value, sizeof (float));
                    }

                    ++v_idx;
                }
                catch (const boost::bad_lexical_cast &e)
                {
                    log_<<"Unable to convert %s to vertex coordinates!\n";
                    throw OdmOrthoPhotoException("Unable to convert %s to vertex coordinates!");
                }
                continue;
            }
            // Vertex normal
            if (st[0] == "vn")
            {
                try
                {
                    for (int i = 1, f = normal_x_field; i < 4; ++i, ++f)
                    {
                        float value = boost::lexical_cast<float> (st[i]);
                        memcpy (&mesh.cloud.data[vn_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset],
                        &value,
                        sizeof (float));
                    }
                    ++vn_idx;
                }
                catch (const boost::bad_lexical_cast &e)
                {
                    log_<<"Unable to convert %s to vertex normal!\n";
                    throw OdmOrthoPhotoException("Unable to convert %s to vertex normal!");
                }
                continue;
            }
            // Texture coordinates
            if (st[0] == "vt")
            {
                try
                {
                    Eigen::Vector3f c (0, 0, 0);
                    for (std::size_t i = 1; i < st.size (); ++i)
                        c[i-1] = boost::lexical_cast<float> (st[i]);

                    if (c[2] == 0)
                        coordinates.push_back (Eigen::Vector2f (c[0], c[1]));
                    else
                        coordinates.push_back (Eigen::Vector2f (c[0]/c[2], c[1]/c[2]));
                    ++vt_idx;

                }
                catch (const boost::bad_lexical_cast &e)
                {
                    log_<<"Unable to convert %s to vertex texture coordinates!\n";
                    throw OdmOrthoPhotoException("Unable to convert %s to vertex texture coordinates!");
                }
                continue;
            }
            // Material
            if (st[0] == "usemtl")
            {
                mesh.tex_polygons.push_back (std::vector<pcl::Vertices> ());
                mesh.tex_materials.push_back (pcl::TexMaterial ());
                for (std::size_t i = 0; i < companions.size (); ++i)
                {
                    std::vector<pcl::TexMaterial>::const_iterator mat_it = companions[i].getMaterial (st[1]);
                    if (mat_it != companions[i].materials_.end ())
                    {
                        mesh.tex_materials.back () = *mat_it;
                        break;
                    }
                }
                // We didn't find the appropriate material so we create it here with name only.
                if (mesh.tex_materials.back ().tex_name == "")
                    mesh.tex_materials.back ().tex_name = st[1];
                mesh.tex_coordinates.push_back (coordinates);
                coordinates.clear ();
                continue;
            }
            // Face
            if (st[0] == "f")
            {
                //We only care for vertices indices
                pcl::Vertices face_v; face_v.vertices.resize (st.size () - 1);
                for (std::size_t i = 1; i < st.size (); ++i)
                {
                    int v;
                    sscanf (st[i].c_str (), "%d", &v);
                    v = (v < 0) ? v_idx + v : v - 1;
                    face_v.vertices[i-1] = v;

                    int v2, vt, vn;
                    sscanf (st[i].c_str (), "%d/%d/%d", &v2, &vt, &vn);
                    f2vt[3*(f_idx) + i-1] = vt-1;
                }
                mesh.tex_polygons.back ().push_back (face_v);
                ++f_idx;
                continue;
            }
        }
    }
    catch (const char *exception)
    {
        fs.close ();
        log_<<"Unable to read file!\n";
        throw OdmOrthoPhotoException("Unable to read file!");
    }

    if (vt_idx != v_idx)
    {
        std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texcoordinates = std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >(0);
        texcoordinates.reserve(3*f_idx);

        for (size_t faceIndex = 0; faceIndex < f_idx; ++faceIndex)
        {
            for(size_t i = 0; i < 3; ++i)
            {
                Eigen::Vector2f vt = mesh.tex_coordinates[0][f2vt[3*faceIndex+i]];
                texcoordinates.push_back(vt);
            }
        }

        mesh.tex_coordinates.clear();
        mesh.tex_coordinates.push_back(texcoordinates);
    }

    fs.close();
    return (0);
}

bool OdmOrthoPhoto::readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
                                Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
                                int &file_version, int &data_type, unsigned int &data_idx,
                                const int offset,
                                std::vector<pcl::MTLReader> &companions)
{
    origin       = Eigen::Vector4f::Zero ();
    orientation  = Eigen::Quaternionf::Identity ();
    file_version = 0;
    cloud.width  = cloud.height = cloud.point_step = cloud.row_step = 0;
    cloud.data.clear ();
    data_type = 0;
    data_idx = offset;

    std::ifstream fs;
    std::string line;

    if (file_name == "" || !boost::filesystem::exists (file_name))
    {
        return false;
    }

    // Open file in binary mode to avoid problem of
    // std::getline() corrupting the result of ifstream::tellg()
    fs.open (file_name.c_str (), std::ios::binary);
    if (!fs.is_open () || fs.fail ())
    {
        fs.close ();
        return false;
    }

    // Seek at the given offset
    fs.seekg (offset, std::ios::beg);

    // Read the header and fill it in with wonderful values
    bool vertex_normal_found = false;
    bool vertex_texture_found = false;
    // Material library, skip for now!
    // bool material_found = false;
    std::vector<std::string> material_files;
    std::size_t nr_point = 0;
    std::vector<std::string> st;

    try
    {
        while (!fs.eof ())
        {
            getline (fs, line);
            // Ignore empty lines
            if (line == "")
            continue;

            // Tokenize the line
            std::stringstream sstream (line);
            sstream.imbue (std::locale::classic ());
            line = sstream.str ();
            boost::trim (line);
            boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on);
            // Ignore comments
            if (st.at (0) == "#")
                continue;

            // Vertex
            if (st.at (0) == "v")
            {
                ++nr_point;
                continue;
            }

            // Vertex texture
            if ((st.at (0) == "vt") && !vertex_texture_found)
            {
                vertex_texture_found = true;
                continue;
            }

            // Vertex normal
            if ((st.at (0) == "vn") && !vertex_normal_found)
            {
                vertex_normal_found = true;
                continue;
            }

            // Material library, skip for now!
            if (st.at (0) == "mtllib")
            {
                material_files.push_back (st.at (1));
                continue;
            }
        }
    }
    catch (const char *exception)
    {
        fs.close ();
        return false;
    }

    if (!nr_point)
    {
        fs.close ();
        return false;
    }

    int field_offset = 0;
    for (int i = 0; i < 3; ++i, field_offset += 4)
    {
        cloud.fields.push_back (pcl::PCLPointField ());
        cloud.fields[i].offset   = field_offset;
        cloud.fields[i].datatype = pcl::PCLPointField::FLOAT32;
        cloud.fields[i].count    = 1;
    }

    cloud.fields[0].name = "x";
    cloud.fields[1].name = "y";
    cloud.fields[2].name = "z";

    if (vertex_normal_found)
    {
        std::string normals_names[3] = { "normal_x", "normal_y", "normal_z" };
        for (int i = 0; i < 3; ++i, field_offset += 4)
        {
            cloud.fields.push_back (pcl::PCLPointField ());
            pcl::PCLPointField& last = cloud.fields.back ();
            last.name     = normals_names[i];
            last.offset   = field_offset;
            last.datatype = pcl::PCLPointField::FLOAT32;
            last.count    = 1;
        }
    }

    if (material_files.size () > 0)
    {
        for (std::size_t i = 0; i < material_files.size (); ++i)
        {
            pcl::MTLReader companion;

            if (companion.read (file_name, material_files[i]))
            {
                log_<<"Problem reading material file.";
            }

            companions.push_back (companion);
        }
    }

    cloud.point_step = field_offset;
    cloud.width      = nr_point;
    cloud.height     = 1;
    cloud.row_step   = cloud.point_step * cloud.width;
    cloud.is_dense   = true;
    cloud.data.resize (cloud.point_step * nr_point);
    fs.close ();
    return true;
}
