/* ipm.cc
 * 
 * Copyright (C) 2013 VisLab
 * 
 * This file is part of lib3dv; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, see <http://www.gnu.org/licenses/>.
 */

#include "lib3dv/image.h"
#include "lib3dv/ipm.h"

#include <cmath>
#include <iostream>
#include <limits>

lib3dv::inverse_perspective_mapping::inverse_perspective_mapping(const calibration::camera_intrinsics& intrinsics, float baseline) :
    m_intrinsics(intrinsics),
    m_baseline(baseline),
    m_transformation_matrix(NULL)
{}
            
lib3dv::inverse_perspective_mapping::inverse_perspective_mapping(const calibration::camera_intrinsics& intrinsics, const calibration::position& position, const calibration::orientation& orientation, float baseline) :
    m_intrinsics(intrinsics),
    m_baseline(baseline)
{
    m_transformation_matrix = new float[12];
    
    const float u0 = intrinsics.m_u0;
    const float v0 = intrinsics.m_v0;
    const float ku = intrinsics.m_ku;
    const float kv = intrinsics.m_kv;

    const float x = position.m_x;
    const float y = position.m_y;
    const float z = position.m_z;
    
    const float yaw = orientation.m_yaw;
    const float pitch = orientation.m_pitch;
    const float roll = orientation.m_roll;
    
    m_transformation_matrix[0] = cos(pitch) * cos(yaw);
    m_transformation_matrix[1] = sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw);
    m_transformation_matrix[2] = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw);

    m_transformation_matrix[4] = cos(pitch) * sin(yaw);
    m_transformation_matrix[5] = sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw);
    m_transformation_matrix[6] = cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw);

    m_transformation_matrix[8] = -sin(pitch);
    m_transformation_matrix[9] = sin(roll) * cos(pitch);
    m_transformation_matrix[10] = cos(roll) * cos(pitch);

    m_transformation_matrix[3] = x;
    m_transformation_matrix[7] = y;
    m_transformation_matrix[11] = z;

    m_transformation_matrix[0] *= ku * baseline;
    m_transformation_matrix[1] *= -baseline;
    m_transformation_matrix[2] *= -baseline * ku / kv;
    m_transformation_matrix[4] *= ku * baseline;
    m_transformation_matrix[5] *= -baseline;
    m_transformation_matrix[6] *= -baseline * ku / kv;
    m_transformation_matrix[8] *= ku * baseline;
    m_transformation_matrix[9] *= -baseline;
    m_transformation_matrix[10] *= -baseline * ku / kv;
}

lib3dv::inverse_perspective_mapping::~inverse_perspective_mapping()
{
    delete[] m_transformation_matrix;
}

bool lib3dv::inverse_perspective_mapping::operator()(boost::shared_ptr<const lib3dv::image> dsi, std::vector<lib3dv::point3>& world_points)
{
    if(dsi->m_type != lib3dv::image::type::DSI || dsi->m_format != lib3dv::image::format::MONO16 || dsi->m_bpp != 16 || dsi->m_channels != 1)
    {
        std::cerr << "[EE] lib3dv : invalid DSI supplied" << std::endl;
        return false;
    }
    
    world_points.reserve(world_points.size() + dsi->m_width * dsi->m_height);
    const uint16_t* buffer = reinterpret_cast<const uint16_t*>(dsi->m_buffer.data());

    if(!m_transformation_matrix)
    {    
        const float u0 = m_intrinsics.m_u0;
        const float v0 = m_intrinsics.m_v0;
        const float ku = m_intrinsics.m_ku;
        const float kv = m_intrinsics.m_kv;

        const float ku_b = ku * m_baseline;
        const float inv_kv = 1.0f / kv;
        const float ku_b_inv_kv = ku_b * inv_kv;

        for(unsigned int i = 0; i < dsi->m_height; ++i)
        {
            const float iv0_ku_b_inv_kv = (i - v0) * ku_b_inv_kv;

            for(unsigned int j = 0; j < dsi->m_width; ++j)
            {
                const uint16_t raw_disparity = buffer[i * dsi->m_width + j];

                lib3dv::point3 point;

                if(raw_disparity != 0xFFFF)
                {
                    const float inv_disparity = 256.0f / raw_disparity;
                    
                    point.m_x = (j - u0) * m_baseline * inv_disparity;
                    point.m_y = iv0_ku_b_inv_kv * inv_disparity;
                    point.m_z = ku_b * inv_disparity;
                }
                else
                {   
                    point.m_x = 0;
                    point.m_y = 0;
                    point.m_z = std::numeric_limits<float>::max();
                }
                
                world_points.push_back(point);
            }
        }
    }
    else
    {
        for(unsigned int i = 0; i < dsi->m_height; ++i)
        {
            for(unsigned int j = 0; j < dsi->m_width; ++j)
            {
                const uint16_t raw_disparity = buffer[i * dsi->m_width + j];

                point3 point;

                if(raw_disparity != 0xFFFF)
                {
                    const float inv_disparity = 256.0f / raw_disparity;
                    
                    const float x = inv_disparity;
                    const float y = (j - m_intrinsics.m_u0) * inv_disparity;
                    const float z = (i - m_intrinsics.m_v0) * inv_disparity;
                    
                    point.m_x = m_transformation_matrix[0] * x +  m_transformation_matrix[1] * y + m_transformation_matrix[2] * z + m_transformation_matrix[3];
                    point.m_y = m_transformation_matrix[4] * x +  m_transformation_matrix[5] * y + m_transformation_matrix[6] * z + m_transformation_matrix[7];
                    point.m_z = m_transformation_matrix[8] * x +  m_transformation_matrix[9] * y + m_transformation_matrix[10] * z + m_transformation_matrix[11];
                }
                else
                {   
                    point.m_x = std::numeric_limits<float>::max();
                    point.m_y = 0;
                    point.m_z = 0;
                }
                
                world_points.push_back(point);
            }
        }
    }
    
    return true;
}