#include <Slicer360ToPerspective.hpp>
#include <frame_conversions.hpp>
#include <csv_parser.hpp>
#include <iostream>

#include <omp.h>

Slicer360ToPerspective::Slicer360ToPerspective(Image const& panorama_image_) : panorama_image(panorama_image_), output_image_width(2), interpolation_method(Image::InterpMethod::BILINEAR), output_folder(".")
{}

Slicer360ToPerspective & Slicer360ToPerspective::SetOutputImageSize(int width, int height) {
    output_image_width = (width > 0) ? width : 1024;
    output_image_height = (height > 0) ? height : output_image_width;
    return *this;
}

Slicer360ToPerspective & Slicer360ToPerspective::SetInterpolationMethod(Image::InterpMethod const& interpolation_method_) {
    interpolation_method = interpolation_method_;
    return *this;
}

Slicer360ToPerspective & Slicer360ToPerspective::SetOutputFolder(std::string const& folder) {
    output_folder = folder;
    return *this;
}

Slicer360ToPerspective & Slicer360ToPerspective::SetCameras(std::vector<Camera> const& cameras_) {
    cameras = cameras_;
    return *this;
}

Image Slicer360ToPerspective::ComputeCoverage(int width, bool raw) const {
    int height = width/2;
    Image coverage(width, height, 1);

    // Compute the coverage of the 360-degree image by the cameras added to the slicer.
    #pragma omp parallel for
    for(int i = 0; i < height; i++) {
        for(int j = 0; j < width; j++) {
            // Compute view vector in inertial frame from equirectangular coordinates
            Eigen::Vector3d ray_dir_sph  = equirectangular2sph(i, j, width, height);
            Eigen::Vector3d ray_dir_cart = sph2cart(ray_dir_sph);
            int count = 0;
            for(Camera const& camera : cameras)
            {
                if(camera.IsPointVisibleInertialFrame(ray_dir_cart))
                    count++;
            }
            coverage.SetPixelValue(i, j, 0, count);
        }
    }

    if(raw)
        return coverage;
    else
        return coverage.Normalized().Colorized(Image::Colormap::PARULA);
}

Eigen::Vector3i Slicer360ToPerspective::ProjectToCameraPixel(Camera const& camera, int i, int j) const {
    // Get the ray direction in inertial frame by projecting the pixel to the sphere
    // Eigen::Vector2d Camera::PixelToNormalizedCoordinates(unsigned int i, unsigned int j, unsigned int width, unsigned int height) {
    Eigen::Vector2d p_sensor = Camera::PixelToNormalizedCoordinates(i, j, output_image_width, output_image_height);
    p_sensor[0] *= -1;
    p_sensor[1] *= ((double)output_image_height)/((double)output_image_width);// Take aspect ratio into account.

    // Compute ray direction from sensor coordinates
    Eigen::Vector3d ray_dir      = camera.ComputeRayDirInInertialFrame(p_sensor);
    Eigen::Vector3d ray_dir_sph  = cart2sph(ray_dir);
    Eigen::Vector2d p_equi       = sph2equirectangular_d(ray_dir_sph, panorama_image.GetWidth(), panorama_image.GetHeight());
    return panorama_image.GetPixelInterp(p_equi, interpolation_method);
}

Image Slicer360ToPerspective::ProjectToCamera(Camera const& camera) const {
    Image image(output_image_width, output_image_height, 3);
    #pragma omp parallel for
    for(int i = 0 ; i < output_image_height ; i++)
        for(int j = 0 ; j < output_image_width ; j++)
            image.SetPixel(i, j, ProjectToCameraPixel(camera, i, j));
    return image;
}

std::vector<Image> Slicer360ToPerspective::ProjectToCameras(std::vector<Camera> const& cameras_) const {
    std::vector<Image> images(cameras_.size());
    #pragma omp parallel for
        for(unsigned int i = 0 ; i < cameras_.size(); i++)
            images[i] = ProjectToCamera(cameras_[i]);
    return images;
}

std::vector<Image> Slicer360ToPerspective::ProjectToAllCameras() const {
    return ProjectToCameras(cameras);
}

Slicer360ToPerspective & Slicer360ToPerspective::LoadCamerasFromFile(std::string const& filename) {
    std::vector<std::vector<double>> camera_specs = parse_csv_file(filename);
    if(camera_specs.size() == 0) {
        std::cerr << "Error: no camera specification found in file " << filename << std::endl;
        return *this;
    } else if(camera_specs[0].size() < 4) {
        std::cerr << "Error: camera specification in file " << filename << " must have 4 columns : yaw, pitch, roll, FOV (all in degrees)." << std::endl;
        return *this;
    } else if(camera_specs[0].size() > 4) {
        std::cerr << "Warning: camera specification in file " << filename << " has more than 4 columns. Only the first 4 columns will be used." << std::endl;
    }
    // Load cameras from decoded CSV file
    cameras.clear();
    for(auto& camera_spec : camera_specs)
        cameras.push_back(Camera::FromYawPitchRollFOV(camera_spec));
    return *this;
}