/*
 * File         : cameraCorrection.cpp
 * Author       : Timothy Wiley
 * Version      :
 * Created      : 08/04/2010
 * Description  : Calculates the values to translate one camera pixels into swiss ranger pixels
 * Copyright    :
 */

#include "thermal_blobber/cameraCorrection.h"

#include <crosbot/utils.hpp>

#include <algorithm>
#include <cmath>
#include <limits>

// Constants
#define DEFAULT_CENTRE_FRACTION     0.333
#define DEFAULT_DEPTH_RESOLUTION    0.2
#define LOG_START                   "CameraCorrection ::"

// Macros
#define dimWidth(n) (n.first)
#define dimHeight(n) (n.second)
#define x(n) (n.first)
#define y(n) (n.second)

using std::map;
using std::string;

namespace casualty {

CameraCorrection::CameraCorrection() :
    configured(false)
{
}

CameraCorrection::~CameraCorrection() {

}

void CameraCorrection::configure(ConfigElementPtr config) {
    //centreFraction  = config->getParamAsDouble("centreFraction", DEFAULT_CENTRE_FRACTION);
    //depthResolution = config->getParamAsDouble("depthResolution", DEFAULT_DEPTH_RESOLUTION);

    // Load distance values
    double fovx, fovy;
    ConfigElementPtr distConfig = config->getChild("distance");
    fovx = DEG2RAD(distConfig->getParamAsInt("fovx", 50));
    fovy = DEG2RAD(distConfig->getParamAsInt("fovy", 50));
    distanceFOV = Size2D(fovx, fovy);
    //LOG("%s configure :: dist fovx = %lf, fovy = %lf\n", LOG_START, RAD2DEG(distanceFOV.width), RAD2DEG(distanceFOV.height));

    // Load thermal values
    ConfigElementPtr thermalConfig = config->getChild("thermal");
	fovx = DEG2RAD(thermalConfig->getParamAsInt("fovx", 50));
	fovy = DEG2RAD(thermalConfig->getParamAsInt("fovy", 50));
	thermalFOV = Size2D(fovx, fovy);
	thermalDx = thermalConfig->getParamAsDouble("dx", 0);
	thermalDy = thermalConfig->getParamAsDouble("dy", 0);
	thermalDz = thermalConfig->getParamAsDouble("dz", 0);
	//LOG("%s configure :: thermal fovx = %lf, fovy = %lf\n", LOG_START, RAD2DEG(thermalFOV.width), RAD2DEG(thermalFOV.height));
}


// Sets up the corrections based on the ranges provided in the constructor
// The images are used to provide the pixel height/width and FOV?
void CameraCorrection::configureCorrection(ImagePtr thImg, ImagePtr distImg) {
    // Get configuration lock
    Lock configureLock(configureMutex);

    // Ensure configuration is still required
    if (configured) return;

    LOG("%s Configure\n", LOG_START);

    if (thImg != NULL && distImg != NULL) {
        // Load additional constants
        thWidth = thImg->width;
        thHeight = thImg->height;
        distWidth = distImg->width;
        distHeight = distImg->height;

        // Setup distance angles array
        distPixelHorizAngles.resize(distWidth);
        distPixelVertAngles.resize(distHeight);

        // Fill out array of distance angles
        for(unsigned int Ix = 0; Ix < distWidth; Ix++) {
            // Get Angles in Distance Image
            Point2D pixel(Ix, 0);
            double horizAngle, vertAngle;
            pixelToAngles(pixel, distanceFOV, distWidth, distHeight, horizAngle, vertAngle);
            distPixelHorizAngles[Ix] = horizAngle;
            //LOG("%s \t Dist x: %d - horizAngle: %lf\n", LOG_START, Ix, RAD2DEG(horizAngle));
        }
        for(unsigned int Iy = 0; Iy < distHeight; Iy++) {
            // Get Angles in Distance Image
            Point2D pixel(0, Iy);
            double horizAngle, vertAngle;
            pixelToAngles(pixel, distanceFOV, distWidth, distHeight, horizAngle, vertAngle);
            distPixelVertAngles[Iy] = vertAngle;
            //LOG("%s \t Dist y: %d - vertzAngle: %lf\n", LOG_START, Iy, RAD2DEG(vertAngle));
        }

        // Setup corrections array
        toDistCorrection.resize(thWidth);
        for(unsigned int x = 0; x < thWidth; x++) {
            toDistCorrection[x].resize(thHeight, Point2D(0, 0));
        }

        // Iterate through each thermal pixel
        for(unsigned int Iy = 0; Iy < thHeight; Iy++) {
            for(unsigned int Ix = 0; Ix < thWidth; Ix++) {
                // Get Angles in Thermal Image
                Point2D thPixel(Ix, Iy);
                double horizAngle, vertAngle;
                pixelToAngles(thPixel, thermalFOV, thWidth, thHeight, horizAngle, vertAngle);

                // Get Point in Distance Image
                Point2D distPixel;
                thAnglesToPixel(horizAngle, vertAngle, distPixel);
                //LOG("%s \t Thermal pixel(%.0lf, %.0lf) hor: %lf, ver: %lf", LOG_START, thPixel.x, thPixel.y, RAD2DEG(horizAngle), RAD2DEG(vertAngle));
                //LOG(" to Dist pixel (%.0lf, %.0lf)\n", distPixel.x, distPixel.y);

                // Save pixel
                toDistCorrection[Ix][Iy] = Point2D((int) distPixel.x, (int) distPixel.y);
            }
        }

        configured = true;
    } else {
        LOG("%s Can't configure ::", LOG_START);
        if (distImg == NULL) {
            LOG(" SwissImage is NULL", LOG_START);
        }
        if (thImg == NULL) {
            LOG(" Thermal Image is NULL", LOG_START);
        }
        LOG("\n");
    }

    LOG("%s Configuration complete\n", LOG_START);
}

Point2D CameraCorrection::correctThermalPixel(Point2D p) {
    // If not configured then don't adjust the point
	if (!configured) {
        return p;
    }

	return toDistCorrection[p.x][p.y];
}

Point3D CameraCorrection::projectThermal(Point2D centre, double d) {
    if (!configured) {
    	return Point3D(centre.x, centre.y, 0);
    }

    //LOG("%s projectThermal()\n", LOG_START);
    //LOG("%s \t centre:(%.0lf, %.0lf)\n", LOG_START, centre.x, centre.y);

	//double x_angle = thTable->tx[(int) centre.x];
    //double y_angle = thTable->ty[(int) centre.y];
    double x_angle, y_angle;
    pixelToAngles(centre, thermalFOV, thWidth, thHeight, x_angle, y_angle);
    //LOG("%s \t (x_angle, y_angle) = (%.1lf, %.1lf)\n", LOG_START, RAD2DEG(x_angle), RAD2DEG(y_angle));

    // Calculate polar coordinates
    double theta = y_angle + M_PI_2;
    double phi   = M_PI_2 - x_angle;
    //LOG("%s \t (d, theta, phi) = (%.1lf, %.1lf, %.1lf)\n", LOG_START, d, RAD2DEG(theta), RAD2DEG(phi));

    // Calculate cartesian coordinates
    Point3D point;
    point.x = d * sin(theta) * cos(phi);
    point.y = d * sin(theta) * sin(phi);
    point.z = d * cos(theta);
    //LOG("%s \t (x, y, z) = (%.3lf, %.3lf, %.3lf)\n", LOG_START, point.x, point.y, point.z);

    return point;
}

void CameraCorrection::pixelToAngles(Point2D pixel, Size2D fov, int width, int height,
        double& horizAngle, double& vertAngle) {
    // Calculation from Adam Milstein

    // Location of the central pixel
    double cx = ((double) width) / 2.0;
    double cy = ((double) height) / 2.0;
    // Field of view (deflection) in each dimension (note that these are measured from the centerline).
    // Note that the aspect ratio of the field of view is OPPOSITE to that of the actual pixels.
    //double xView = 25;//22.5;
    //double yView = 22;//19.5;
    //double xd = (double)xView*3.14156/180.0;
    //double yd = (double)yView*3.14156/180.0;

    double xd = fov.width / 2;
    double yd = fov.height / 2;
    double fd;

    // Horizontal Angle
    fd = (width-cx)/tan(xd);
    horizAngle = atan2((pixel.x-cx),fd);

    // Vertical angle
    fd = (height-cy)/tan(yd);
    vertAngle = atan2((pixel.y-cy),fd);
}

void CameraCorrection::thAnglesToPixel(double horizAngle, double vertAngle, Point2D& pixel) {
    pixel.x = findPixel(horizAngle, distPixelHorizAngles, mapPixelHorizAngles);
    pixel.y = findPixel(vertAngle, distPixelVertAngles, mapPixelVertAngles);
}


int CameraCorrection::findPixel(double angle, vector<double>& angleArr, map<double, int>& angleMap) {
    // 'Round' angle to 4 decimal places
    //int angleRound = floorf(angle * 10000 + 0.5);
    int angleRound = floor(angle * 10000 + 0.5);

    // Pixel
    int pixel = 0;

    // Lookup map
    map<double, int>::iterator it = angleMap.find(angleRound);
    if (it != angleMap.end()) {
        pixel = it->second;
    } else {
        // Need to calculate pixel for angle
        // Min difference
        double diff = std::numeric_limits<double>::max();
        double tmpDiff = diff;

        for (unsigned int p = 0; p != angleArr.size(); p++) {
            tmpDiff = fabs(angleArr[p] - angle);
            if (tmpDiff < diff) {
                pixel = p;
                diff = tmpDiff;
            }
        }

        // Store pixel in map
        angleMap[angleRound] = pixel;
    }

    return pixel;
}

} // namespace casualty
