/*
 * 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
    int fovx, fovy;
    ConfigElementPtr distConfig = config->getChild("distance");
    fovx = distConfig->getParamAsInt("fovx", 50);
    fovy = distConfig->getParamAsInt("fovy", 50);
    distanceFOV = Size2D(fovx, fovy);
    LOG("%s dist foxv = %d, fovy = %d\n", LOG_START, fovx, fovy);

    // Load thermal values
    ConfigElementPtr thermalConfig = config->getChild("thermal");
	fovx = thermalConfig->getParamAsInt("fovx", 50);
	fovy = thermalConfig->getParamAsInt("fovy", 50);
	thermalFOV = Size2D(fovx, fovy);
	thermalDx = thermalConfig->getParamAsDouble("dx", 0);
	thermalDy = thermalConfig->getParamAsDouble("dy", 0);
	thermalDz = thermalConfig->getParamAsDouble("dz", 0);
}

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

    // Get depth level
    unsigned int level = round(dist / depthResolution);
    if (level >= numResolution) level = numResolution - 1;

    // Create Point
    pair<int, int> ptPair = corrections[p.x][p.y][level];
    Point2D corrPt(ptPair.first, ptPair.second);

    return corrPt;
}

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

	double x_angle = thTable->tx[(int) centre.x];
    double y_angle = thTable->ty[(int) centre.y];
    //logger->log("\t (x_angle, y_angle) = (%.1lf, %.1lf)\n", RAD2DEG(x_angle), RAD2DEG(y_angle));

    // Calculate polar coordinates
    double theta = y_angle + M_PI_2;
    double phi   = M_PI_2 - x_angle;
    //logger->log("\t (d, theta, phi) = (%.1lf, %.1lf, %.1lf)\n", 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);
    //logger->log("\t (x, y, z) = (%.3lf, %.3lf, %.3lf)\n", point.x, point.y, point.z);

    return point;
}

// 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 srImg) {
    // Get configuration lock
    Lock configureLock(configureMutex);

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

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

    // Time = 0 to know if camera's have updated or not
    Time zeroT;

    if (thImg != NULL && srImg != NULL) {
        // Generate params
        numWidth = srImg->width;
        numHeight = srImg->height / 2.0;
        numResolution = maxSwissRangerValue() / depthResolution + 1;

        // Setup corrections 3D array
        corrections.resize(numWidth);
        for(unsigned int x = 0; x < numWidth; x++) {
            corrections[x].resize(numHeight);
            for(unsigned int y = 0; y < numHeight; y++) {
                corrections[x][y].resize(numResolution, pair<int, int>(0,0));
            }
        }

        // Fill corrections array
        fillCorrectionsTable(srImg, thImg);

        // Calculate the centre bounding box of thermal camera
        double halfCentreFrac = centreFraction / 2.0;
        int pixelHCFx = thImg->width * halfCentreFrac;
        int pixelHCFy = thImg->height * halfCentreFrac;
        int pixelCentx = thImg->width / 2;
        int pixelCenty = thImg->height / 2;
        centreTopL = pair<int, int>(pixelCentx - pixelHCFx, pixelCenty - pixelHCFy);
        centreBotR = pair<int, int>(pixelCentx + pixelHCFx, pixelCenty + pixelHCFy);
        //logger->log("%s Boundaries: topL(%d,%d) botT(%d,%d)\n", LOG_START,
        //            centreTopL.first, centreTopL.second, centreBotR.first, centreBotR.second);

        configured = true;
    } else {
    	LOG("%s Can't configure - cameras not active\n", LOG_START);
        if (srImg == NULL) {
		   LOG("%s ***** SwissImage is NULL\n", LOG_START);
	   }
	   if (thImg == NULL) {
		   LOG("%s ***** Thermal Image is NULL\n", LOG_START);
	   }
    }

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

/*
 * In this maths:
 *      - The camera are assumed to be looking along the y-axis
 *      - The angles used are theta (azimuthal angle) and phi (polar/zenith angle)
 *      - It is assume each image pixel corresponds to a unique theta/phi
 */
void CameraCorrection::fillCorrectionsTable(ImagePtr srImg, ImagePtr thImg) {
    //logger->log("%s Filling Corrections Table\n", LOG_START);
    //logger->log("%s SR: width (%d) height (%d) FOV (%.4lf, %.4lf)\n", LOG_START, srImg->getWidth(), srImg->getHeight() / 2, swissCam->getFOV().width, swissCam->getFOV().height);
    //logger->log("%s TH: width (%d) height (%d) FOV (%.4lf, %.4lf)\n", LOG_START, thImg->getWidth(), thImg->getHeight(), thermalCam->getFOV().width, thermalCam->getFOV().height);
    // Setup TrigTables for thermal and swiss images
    srTable = new _ImageTrigTable(srImg->width, srImg->height / 2.0, distanceFOV);
    thTable = new _ImageTrigTable(thImg->width, thImg->height, thermalFOV);
    //logger->log("%s Generated ImageTrigTables\n", LOG_START);

    // Displacement
    double Tx = thermalDx;
    double Ty = thermalDy;
    double Tz = thermalDz;

    // Map for storing calculated angle/pixel mappings
    map<int, int> txMap;
    map<int, int> tyMap;

    for(unsigned int Iy = 0; Iy < numHeight; Iy++) {
        for(unsigned int Ix = 0; Ix < numWidth; Ix++) {
            // SR pixel angles
            double x_angle = srTable->tx[Ix];
            double y_angle = srTable->ty[Iy];
            //logger->log("\t (x_angle, y_angle) = (%.1lf, %.1lf)\n", RAD2DEG(x_angle), RAD2DEG(y_angle));

            // Polar coord in SR
            double theta = y_angle + M_PI_2;
            double phi   = M_PI_2 - x_angle;
            //logger->log("\t (d, theta, phi) = (%.1lf, %.1lf, %.1lf)\n", d, RAD2DEG(theta), RAD2DEG(phi));

            for(unsigned int h = 1; h < numResolution; h++) {
                double d = resolutionDist(h);

                // Calculate correction for the x/y point and depth
                //logger->log("(d) (Ix, Iy) = (%.2lf) (%d, %d)\n", d, Ix, Iy);

                // Cartesian coord in SR
                double x = d * sin(theta) * cos(phi);
                double y = d * sin(theta) * sin(phi);
                double z = d * cos(theta);
                //logger->log("\t (x, y, z) = (%.3lf, %.3lf, %.3lf)\n", x, y, z);

                // Cartesian coord in thermal
                double x2 = x - Tx;
                double y2 = y - Ty;
                double z2 = z - Tz;
                //logger->log("\t (x2, y2, z2) = (%.3lf, %.3lf, %.3lf)\n", x2, y2, z2);

                // Polar coord in thermal
                double d2 = sqrt(x2 * x2 + y2 * y2 + z2 * z2);
                double theta2 = acos(z2 / d2);
                double phi2 = atan2(y2, x2);
                //logger->log("\t (d2, theta2, phi2) = (%.3lf, %.1lf, %.1lf)\n", d2, RAD2DEG(theta2), RAD2DEG(phi2));

                // TH pixel angles
                double x2_angle = M_PI_2 - phi2;
                double y2_angle = theta2 - M_PI_2;
                //logger->log("\t (x2_angle, y2_angle) = (%.1lf, %.1lf)\n", RAD2DEG(x2_angle), RAD2DEG(y2_angle));

                // Pixel in thermal
                int Ix2 = selectedPixel(x2_angle, thTable->tx, txMap, thTable->width);
                int Iy2 = selectedPixel(y2_angle, thTable->ty, tyMap, thTable->height);
                //logger->log("\t (Ix2, Iy2) = (%d, %d)\n", Ix2, Iy2);

                // Add to table
                corrections[Ix][Iy][h] = pair<int, int>(round(Ix2), round(Iy2));
            }
        }
    }
}

int CameraCorrection::selectedPixel(double angle, double* angleArr,
        std::map<int, int>& angleMap, unsigned int n) {
    // '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<int, 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 < n; p++) {
            tmpDiff = fabs(angleArr[p] - angle);
            if (tmpDiff < diff) {
                pixel = p;
                diff = tmpDiff;
            }
        }

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

    return pixel;
}

/*
void _CameraCorrection::fillCorrectionsTable(RobotImage srImg, RobotImage thImg) {
    // Configure correction constants
    double PIXELx = srImg->getWidth();
    double PIXELy = srImg->getHeight() / 2;
    PIXELx2 = thImg->getWidth();
    PIXELy2 = thImg->getHeight();
    double FOVx = swissCam->getFOV().width;
    double FOVy = swissCam->getFOV().height;
    FOVx2 = thermalCam->getFOV().width;
    FOVy2 = thermalCam->getFOV().height;
    double Tx = thermalCam->getPose().p.x;
    double Ty = thermalCam->getPose().p.y;
    double Tz = thermalCam->getPose().p.z;
    //logger->log("%s\tPx\tPy\tPx2\tPxy\tFOVx\tFOVy\tFOVx2\tFOVy2\tTx\tTy\tTz\n", LOG_START);
    //logger->log("%s\t%.0lf\t%.0lf\t%.0lf\t%.0lf\t%.5lf\t%.5lf\t%.5lf\t%.5lf\t%.2lf\t%.2lf\t%.2lf\n", LOG_START,
    //            PIXELx, PIXELy, PIXELx2, PIXELy2, FOVx, FOVy,FOVx2, FOVy2, Tx, Ty, Tz);

    // FOV constants
    double FOVx_ON2 = FOVx / 2.0;
    double FOVy_ON2 = FOVy / 2.0;
    double FOVx2_ON2 = FOVx2 / 2.0;
    double FOVy2_ON2 = FOVy2 / 2.0;
    //printf("FOV (x,y) = (%lf, %lf)\n", FOVx, FOVy);

    // APP constants
    double APPx = FOVx / PIXELx;
    double APPy = FOVy / PIXELy;
    double APPx2 = FOVx2 / PIXELx2;
    double APPy2 = FOVy2 / PIXELy2;
    //printf("APP (x,y) = (%lf, %lf)\n", APPx, APPy);

    for(unsigned int Ix = 0; Ix < numWidth; Ix++) {
        for(unsigned int Iy = 0; Iy < numHeight; Iy++) {
            for(unsigned int h = 0; h < numResolution; h++) {
                double d = resolutionDist(h);
                //logger->log("%s h:%d, d:%lf\n", LOG_START, h, d);

                // Calculate correction for the x/y point and depth
                //printf("\t(Ix, Iy) = (%.0lf, %.0lf)\n", Ix, Iy);

                // Polar coord in SR
                double theta = M_PI_2 - Ix * APPx + FOVx_ON2;
                double phi = M_PI_2 + Iy * APPy - FOVy_ON2;
                //printf("\t(d, theta, phi) = (%.1lf, %.5lf, %.5lf)\n", d, theta, phi);

                // Cartesian coord in SR
                double x = d * cos(theta) * sin(phi);
                double y = d * sin(theta) * sin(phi);
                double z = d * cos(phi);
                //printf("\t(x, y, z) = (%.3lf, %.3lf, %.3lf)\n", x, y, z);

                // Cartesian coord in thermal
                double x2 = x - Tx;
                double y2 = y - Ty;
                double z2 = z - Tz;
                //printf("\t(x2, y2, z2) = (%.1lf, %.1lf, %.1lf)\n", x2, y2, z2);

                // Polar coord in thermal
                double d2 = sqrt(x2 * x2 + y2 * y2 + z2 * z2);
                double theta2 = atan2(y2, x2);
                double phi2 = acos(z2 / d2);
                //printf("\t(d2, theta2, phi2) = (%.1lf, %.5lf, %.5lf)\n", d2, theta2, phi2);

                // Pixel in thermal
                double Ix2 = (M_PI_2 - theta2 + FOVx2_ON2) / APPx2;
                double Iy2 = (phi2 + FOVy2_ON2 - M_PI_2) / APPy2;
                double rIx2 = round(Ix2);
                double rIy2 = round(Iy2);

                // Error
                double Errx = rIx2 - Ix;
                double Erry = rIy2 - Iy;

                // Add to table
                corrections[Ix][Iy][h] = pair<int, int>(round(Errx), round(Erry));
            }
        }
    }
}
*/

double CameraCorrection::resolutionDist(unsigned int level) {
    return level * depthResolution;
}

} // namespace casualty
