/*
 * 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 <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::vector;
using std::pair;

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);

    /**
     * The maximum SwissRanger pixel value
     * @return Maximum SwissRanger pixel value
     */
    inline float maxSwissRangerValue() {
        return 7.0;
    }

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

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

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

    /**
     * Mutex to ensure component is only being configured in one thread
     */
    Mutex configureMutex;

    // 3D array for lookup table - correction[x][y][depth](x correction, y correction)
    vector<vector<vector<pair<int, int> > > > corrections;
    bool configured;

    // Corrections array dimensions
    unsigned int numWidth;
    unsigned int numHeight;
    unsigned int numResolution;

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

    // Image Trig Tables
    ImageTrigTable srTable;
    ImageTrigTable thTable;

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

    // Calculate pixel for given angle
    int selectedPixel(double angle, double* angleArr, std::map<int, int>& angleMap, unsigned int n);

    // Distance for resolution depth level
    double resolutionDist(unsigned int level);

    // Necessary global constants
    double PIXELx2;
    double PIXELy2;
    double FOVx2;
    double FOVy2;
};
typedef Handle<CameraCorrection> CameraCorrectionPtr;

} // namespace casualty

#endif /* THERMAL_BLOBBER_CAMERACORRECTION_H_ */
