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

#ifndef THERMAL_BLOBBER_CAMERACORRECTION_H_
#define THERMAL_BLOBBER_CAMERACORRECTION_H_

#include "imageTrigTable.h"

#include <crosbot/config.hpp>
#include <crosbot/data.hpp>
#include <crosbot/handle.hpp>
#include <crosbot/thread.hpp>

#include <map>
#include <string>
#include <utility>

#define ELEMENT_CAMERA_CORRECTION  "cameraCorrection"

#define CORRECTION_RANGE_MIN        0.5
#define CORRECTION_RANGE_MAX        4.0

using namespace crosbot;
using std::pair;
using std::map;
using std::vector;

namespace casualty {

class CameraCorrection : public HandledObject {
public:
    CameraCorrection();
    ~CameraCorrection();

    // Loads the config
    void configure(ConfigElementPtr config);

    // Sets up the corrections based on the ranges provided in the constructor
    // The images are used to provide the pixel height/width
    void configureCorrection(ImagePtr thImg, ImagePtr srImg);

    /**
     * Corrects the swiss pixel into thermal space given the distance reading
     * @param p Point in SwissRanger space to convert
     * @return Resulting point is thermal camera space
     */
    Point2D correctThermalPixel(Point2D p);

    /**
     * Project the thermal pixel into the frame of the thermal camera
     */
    Point3D projectThermal(Point2D centre, double d);

protected:
    // Config
    Size2D distanceFOV;
    Size2D thermalFOV;
    double thermalDx;
    double thermalDy;
    double thermalDz;
    //double centreFraction;
    //double depthResolution;

    // Configuration of correction
    bool configured;
    Mutex configureMutex;

    // 2D array for lookup table - correction[x][y](Point2D)
    vector<vector<Point2D > > toDistCorrection;
    // Table of angles for distance image pixels dist[x][y](horiz, vert)
    // map is for efficient purposes
    vector<double> distPixelHorizAngles;
    vector<double> distPixelVertAngles;
    map<double, int> mapPixelHorizAngles;
    map<double, int> mapPixelVertAngles;

    // Useful global constants
    int thWidth, thHeight;
    int distWidth, distHeight;

    // Thermal cam centre bounding box
    //pair<int, int> centreTopL;
    //pair<int, int> centreBotR;

    // Convert pixel to angles
    void pixelToAngles(Point2D pixel, Size2D fov, int width, int height, double& horizAngle, double& vertAngle);
    // Convert angles to pixel
    void thAnglesToPixel(double horizAngle, double vertAngle, Point2D& pixel);

    // Fills out the corrections lookup table
    void fillCorrectionsTable(ImagePtr srImg, ImagePtr thImg);

    // Calculate pixel for given angle
    int findPixel(double angle, vector<double>& angleArr, map<double, int>& angleMap);
};
typedef Handle<CameraCorrection> CameraCorrectionPtr;

} // namespace casualty

#endif /* THERMAL_BLOBBER_CAMERACORRECTION_H_ */
