#include <iostream>
#include "opencv2/opencv.hpp"

#define DEBUG_OUTPUT 1

using namespace std;
using namespace cv;

Mat own_threshold(Mat conf_map, float lower, float upper){
    Mat temp1 = conf_map > lower;
    Mat temp2 = conf_map < upper;
    return temp1 & temp2;
}

int main()
{
    // Read in images
    Mat one = imread("/data/flower/up.jpg");
    Mat two = imread("/data/flower/right.jpg");
    Mat three = imread("/data/flower/down.jpg");
    Mat four = imread("/data/flower/left.jpg");

    vector<Mat> channels_one, channels_two, channels_three, channels_four;
    split(one, channels_one);
    split(two, channels_two);
    split(three, channels_three);
    split(four, channels_four);

    // Calculate the average value needed for further processing
    Mat int1(one.rows, one.cols, CV_32FC1), int2(two.rows, two.cols, CV_32FC1), int3(three.rows, three.cols, CV_32FC1), int4(four.rows, four.cols, CV_32FC1);
    for(int row = 0; row < one.rows; row++){
        for(int col =0; col < one.cols; col++){
            Scalar m1 = mean( Vec3f(channels_one[0].at<uchar>(row,col), channels_one[1].at<uchar>(row,col), channels_one[2].at<uchar>(row,col)) );
            Scalar m2 = mean( Vec3f(channels_two[0].at<uchar>(row,col), channels_two[1].at<uchar>(row,col), channels_two[2].at<uchar>(row,col)) );
            Scalar m3 = mean( Vec3f(channels_three[0].at<uchar>(row,col), channels_three[1].at<uchar>(row,col), channels_three[2].at<uchar>(row,col)) );
            Scalar m4 = mean( Vec3f(channels_four[0].at<uchar>(row,col), channels_four[1].at<uchar>(row,col), channels_four[2].at<uchar>(row,col)) );
            int1.at<float>(row, col) = m1[0];
            int2.at<float>(row, col) = m2[0];
            int3.at<float>(row, col) = m3[0];
            int4.at<float>(row, col) = m4[0];
        }
    }

    Scalar aint1 = mean(int1); Scalar aint2 = mean(int2); Scalar aint3 = mean(int3); Scalar aint4 = mean(int4);
    float avint1 = aint1[0];
    float avint2 = aint2[0];
    float avint3 = aint3[0];
    float avint4 = aint4[0];

    Scalar aint = mean( Vec4f(avint1, avint2, avint3, avint4) );
    float avint = aint[0];

    // Perform matrix operation on the average data using the average values
    // This is for normalizing the intensities
    int1 = (avint/avint1) * int1;
    int2 = (avint/avint2) * int2;
    int3 = (avint/avint3) * int3;
    int4 = (avint/avint4) * int4;

    // Now calculate the ambient images
    Mat temp1, temp2, maxint;
    max(int1, int2, temp1);
    max(int2, int3, temp2);
    max(int2, int3, temp2);
    max(temp1, temp2, maxint);

    Mat maxint_viz = maxint/255;

    imshow("ambient image on mean intensities", maxint_viz); waitKey(0);

    Mat tempC1, tempC2, maxrgb;
    max(one,two, tempC1);
    max(three,four, tempC2);
    max(tempC1, tempC2, maxrgb);

    imshow("ambient image on color data", maxrgb); waitKey(0);

    // Now calculate the ratio images
    Mat rad1, rad2, rad3, rad4;
    divide(int1, maxint, rad1); //imshow("ratio image up", rad1); waitKey(0);
    divide(int2, maxint, rad2); //imshow("ratio image right", rad2); waitKey(0);
    divide(int3, maxint, rad3); //imshow("ratio image down", rad3); waitKey(0);
    divide(int4, maxint, rad4); //imshow("ratio image left", rad4); waitKey(0);

    // Apply vertical and horizontal Sobel filters
    // Based on orientation of flash
    Mat edge1, edge2, edge3, edge4;
    Mat rad1b, rad2b, rad3b, rad4b;
    Sobel(rad1, edge1, CV_32F, 1, 0, -1); // Vertical Sobel
    Sobel(rad2, edge2, CV_32F, 0, 1, -1); // Horizontal Sobel
    Sobel(rad3, edge3, CV_32F, 1, 0, -1); // Vertical Sobel
    Sobel(rad4, edge4, CV_32F, 0, 1, -1); // Horizontal Sobel

    // Calculation of negative transitions
    Mat mask1, mask2, mask3, mask4;
    Mat mask1f, mask2f, mask3f, mask4f;

    mask1 = edge1 > 0; mask1.convertTo(mask1f, CV_32FC1); mask1f = mask1f/255;
    multiply(edge1, mask1f, edge1);

    mask2 = edge2 < 0; mask2.convertTo(mask2f, CV_32FC1); mask2f = mask2f/255;
    multiply(edge2, mask2f, edge2);
    edge2 = abs(edge2);

    mask3 = edge3 < 0; mask3.convertTo(mask3f, CV_32FC1); mask3f = mask3f/255;
    multiply(edge3, mask3f, edge3);
    edge3 = abs(edge3);

    mask4 = edge4 > 0; mask4.convertTo(mask4f, CV_32FC1); mask4f = mask4f/255;
    multiply(edge4, mask4f, edge4);

    imshow("edge map up", edge1); waitKey(0);
    imshow("edge map right", edge2); waitKey(0);
    imshow("edge map down", edge3); waitKey(0);
    imshow("edge map left", edge4); waitKey(0);

    // Now we want all edges combined into a single edge map by taking the maximal value of each map
    Mat tempConf1, tempConf2, conf;
    max(edge1, edge2, tempConf1);
    max(edge3, edge4, tempConf2);
    max(tempConf1, tempConf2, conf);

    imshow("confidence map", conf); waitKey(0);

    // Threshold to achieve a cleaner confidence map
    edge1 = own_threshold(edge1, 0.5, 1.0);
    edge2 = own_threshold(edge2, 0.5, 1.0);
    edge3 = own_threshold(edge3, 0.5, 1.0);
    edge4 = own_threshold(edge4, 0.5, 1.0);

    Mat edges = edge1 | edge2 | edge3 | edge4;

    imshow("edge map up - binarized", edge1); waitKey(0);
    imshow("edge map right - binarized", edge2); waitKey(0);
    imshow("edge map down - binarized", edge3); waitKey(0);
    imshow("edge map left - binarized", edge4); waitKey(0);
    imshow("edge map binarized combined", edges); waitKey(0);

    // Calculate texture edges
    Mat tedges;
    Sobel(maxint, tedges, CV_32F, 1, 1); // Combined into one call in OpenCV while Matlab code does 2 calls --> same output
    tedges = own_threshold(tedges/255, 0.3, 0.6);
    imshow("texture edges map", tedges); waitKey(0);

    // Remove depth
    tedges = tedges - edges;
    imshow("texture edges map - depth removed", tedges); waitKey(0);

    // All edges
    Mat alledges = edges + tedges;
    bitwise_not(alledges,alledges);

    imshow("all edges", alledges); waitKey(0);

    Mat canvas = maxrgb.clone();
    alledges.convertTo(alledges, CV_8UC1);
    // Blend the edge map with the original image
    for(int row = 0; row < alledges.rows; row++){
        for(int col =0; col < alledges.cols; col++){
               if(alledges.at<uchar>(row, col) == 0){
                    canvas.at<Vec3b>(row, col) = Vec3b(0,0,0);
               }
        }
    }
    imshow("final", canvas); waitKey(0);

    return 0;
}