/* #########################################
 * Traffic monitoring using Raspberry PI
 * Author: Michal Zacpal
 * Library: Image processing tools
 * 2015
 * ######################################### */
#include "ImageProcessing.h"
/*
 *  Convert mat to QImage
 */
QImage Mat2QImage(Mat img, int iWidth, int iHeight)
{
    /*if (img.empty())
        return null;*/
    cv::resize(img,img,Size(iWidth, iHeight));                                              // Resize input picture to size of screen object
    switch (img.channels()){
    case 1: cvtColor(img,img,CV_GRAY2RGB,0);break;                                          // convert color Qimage works with RGB and opencv with BGR or else
    case 3: cvtColor(img,img,CV_BGR2RGB,0);break;                                                // convert color Qimage works with RGB and opencv with BGR or else
    }
    return (QImage((uchar*)img.data, img.cols,img.rows,img.step,QImage::Format_RGB888));    // Convert from MAT to QImage
}

TVTM computeTVTM(Mat InImg, double H, double f, double theta)
{
    TVTM OutTVTM;
    theta = theta * 0.0174; // Convert from  to rad :theta * 3.14/180;

    Mat TVTMImg = Mat::zeros(2*InImg.rows,2*InImg.cols, CV_8UC1);
    int iRows = InImg.rows;
    int iCols = InImg.cols;
    double d = abs((H*(sin(theta)+f*cos(theta))/(f*sin(theta)-cos(theta)))+1);
    OutTVTM.lut = new Point[InImg.rows*InImg.cols]();
    int iP = 0;
    for (int x = 0; x < iRows; x++)
    {
        for (int y = 0; y < iCols; y++)
        {
            int x_ = (int)(H*((x*sin(theta)+f*cos(theta))/(-x*cos(theta)+f*sin(theta)))+d);
            int y_ = (int)(H*((y*sin(theta)+f*cos(theta))/(-x*cos(theta)+f*sin(theta)))+d);
            if (x_<=0)  x_ = 0;
            if (y_<=0)  y_=0;
            if (x_>=2*iRows) x_ = 2*iRows;
            if (y_>=2*iCols) y_ = 2*iCols;
            OutTVTM.lut[iP].x = x_;
            OutTVTM.lut[iP].y = y_;
            iP++;
            TVTMImg.at<uchar>(x_,y_) = InImg.at<uchar>(x,y);
        }
    }
    // Crop image
    int iMaxRow = 0;
    int iMaxCol = 0;
    for (int x = 0; x < TVTMImg.rows; x++)
    {
        for (int y = 0; y < TVTMImg.cols; y++ )
        {
            if (TVTMImg.at<uchar>(x,y) > 0)
            {
                if (x > iMaxRow) iMaxRow = x;
                if (y >iMaxCol) iMaxCol = y;
            }
        }
    }
    Rect rectROI(0,0,iMaxCol,iMaxRow);  // Region to croped
    Mat crTVTM; // Croped TVTM image
    crTVTM = TVTMImg(rectROI).clone();
    // get a
    for(int i = 0; i < crTVTM.cols; i++)
    {
        if (crTVTM.at<uchar>(crTVTM.rows-1,i) > 0)
        {
            OutTVTM.a = i;
            break;
        }
    }
    // get b
    for (int i = crTVTM.cols-1; i != 0; i--)
    {
        if (crTVTM.at<uchar>(crTVTM.rows-1,i) > 0)
        {
            OutTVTM.b = i;
            break;
        }
    }
    OutTVTM.TvtmImg = crTVTM;
    // get c
    OutTVTM.c = OutTVTM.b - OutTVTM.a;
    // get b;
    OutTVTM.b = crTVTM.cols - OutTVTM.b;
    return OutTVTM;
}
/*
 *  Estimation of parameters H, f and theta
 */
TVTM OptimizeTVTM(Mat TVTMImg, double H, double f, double theta)
{
    TVTM tvtm = computeTVTM(TVTMImg, H, f, theta);
    int iCols = tvtm.TvtmImg.cols;
    // Learning parameters
    double dAlfa = 1; double dBeta = 2; double dGama = 0.5; int iE = 20;int iPrecs = 60;
    int iM  = iCols / 3;
    int iM1 = iM - iE;
    int iM2 = iM + iE;
    int iEnd = 190;   // ending coeff.
    int  i = 0;
    while ( !( (iM1 < tvtm.a  &&   tvtm.a < iM2) &&(iM1 < tvtm.b  &&   tvtm.b < iM2) && (iM1 < tvtm.c  &&   tvtm.c < iM2)))
    {
        tvtm.TvtmImg.release();
        tvtm.a = 0;
        tvtm.b = 0;
        tvtm.c = 0;
        tvtm.lut = NULL;
        tvtm = computeTVTM(TVTMImg, H, f, theta);
        int iCols = tvtm.TvtmImg.cols;
        if (tvtm.a < tvtm.b )
        {
            H = H + dAlfa + dGama;
            theta = theta + dAlfa;
        }
        if (tvtm.a > tvtm.b)
        {
            H = H - dAlfa + dGama;
            theta = theta - dAlfa;
        }
        if (abs((tvtm.a-tvtm.b)) > iPrecs)
        {
            dAlfa = 1;
            dGama = 0.5;
        }
        else
        {
            dAlfa = 0.1;
            dGama = 0.05;
        }
        if (abs((tvtm.a-tvtm.b)) <= 2* iE)
        {
            if (tvtm.c < tvtm.b){ f = f + dBeta;}
            else{ f = f- dBeta;}
            if (tvtm.c > abs(tvtm.c-tvtm.b)){ dBeta = 5;}
            else{ dBeta = 1;}
        }
        iM = iCols/3;
        iM1 = iM - iE;
        iM2 = iM + iE;
        i++;
        if (i > iEnd) break;
    }
    return tvtm;
}
/*
 *      Transform image to TVTM using LUT
 */
Mat RetTVImg(Mat inputImg, TVTM TVTModel, bool bCrop)
{
    Mat OutImg_ = Mat::zeros(TVTModel.TvtmImg.rows,TVTModel.TvtmImg.cols,inputImg.type());
    int iPrev = 0;
    int iRow = 0;
    int iCol = 0;
    for (int i = 0; i < inputImg.rows;i++)
    {
        for (int j = 0; j < inputImg.cols;j++ )
        {
            iRow = TVTModel.lut[iPrev+j].x;
            iCol = TVTModel.lut[iPrev+j].y;
            OutImg_.at<uchar>(iRow,iCol)  = inputImg.at<uchar>(i,j);
        }
        iPrev = iPrev + inputImg.cols;
    }
    if (bCrop){
        Rect RR(TVTModel.a,0,TVTModel.c,OutImg_.rows-1);
        Mat OutImg;
        OutImg_(RR).copyTo(OutImg);
        return (OutImg);}
    return (OutImg_);
}
/*
 *  Fill empty pixels by duplicating neighbour
 */
Mat InterpolTVTM(Mat InputImg){

    for (int i = 1; i < InputImg.rows; i++)
    {
        for (int j = 0; j < InputImg.cols; j++)
        {
            if (InputImg.at<uchar>(i,j) == 0)
                InputImg.at<uchar>(i,j) = InputImg.at<uchar>(i-1,j);
        }
    }

    for (int i = 0; i < InputImg.rows; i++)
    {
        for (int j = 1; j < InputImg.cols; j++)
        {
            if (InputImg.at<uchar>(i,j) == 0)
                InputImg.at<uchar>(i,j) = InputImg.at<uchar>(i,j-1);

        }
    }
    return InputImg;
}
Mat ImEdge(Mat InImg)
{
    // Filter1          Filter2
    /*                  *
     * |-1 0 1|         |1 0 -1|
     * |-1 0 1|         |1 0 -1|
     * |-1 0 1|         |1 0 -1|
     */
   Mat ef1 = (Mat_<float>(3,3) << -1,0,1,
                                  -1,0,1,
                                  -1,0,1);
    Mat ef2 = (Mat_<float>(3,3) << 1,0,-1,
                                  1,0,-1,
                                  1,0,-1);
        Mat out1,out2,OutputIm;
        Point anchor;
        double delta;
        int ddepth;
        anchor = Point( -1, -1 );
        delta = 0;
        ddepth = InImg.depth();
        filter2D(InImg, out1, ddepth , ef1, anchor, delta, BORDER_CONSTANT);
        filter2D(InImg, out2, ddepth , ef2, anchor, delta, BORDER_CONSTANT);
        OutputIm = out1 + out2;
        return (OutputIm);
}
/*
 * Compute horizontal histogram * for binary picture
 */
vector<int> HorizHist(Mat InImg)
{
    vector<int> h(InImg.cols);

    for (int i = 3; i  < InImg.cols-3; i++)
    {
        h[i] = 0;
        for (int j = 0; j < InImg.rows; j++)
        {
            if (InImg.at<uchar>(j,i) > 0)
            {
                h[i] = h[i] + 1;
            }
        }
    }
    return h;
}
/*
 * Compute horizontal histogram * for uchar picture
 */
vector<int> HorizHistU(Mat InImg)
{
    vector<int> h(InImg.cols);
    int iMax = 0;
    for (int i = 5; i  < InImg.cols-5; i++)
    {
        h[i] = 0;
        for (int j = 0; j < InImg.rows; j++)
        {
            if (InImg.at<uchar>(j,i) > 0)
            {
                h[i] = h[i] + InImg.at<uchar>(j,i);
            }
        }
        if (h[i] > iMax) iMax = h[i];
    }

    for (int i = 0; i < (int)h.size(); i++){
        h[i] = (int)(((double)h[i]/(double)iMax)*(double)InImg.rows);
    }
    return h;
}
/*
 *  Crop possible lanes from picture
 */
Mat CropLane(Mat InImg, int iPosition, int iLaneOffset)
{
    Mat OutImg;
    if ((iPosition != -1))
    {
        if ( (iPosition - iLaneOffset) > 0 && (iPosition + iLaneOffset) < InImg.cols)
        {
            Rect rect(iPosition-iLaneOffset,0, 2* iLaneOffset, InImg.rows);
            InImg(rect).copyTo(OutImg);
        }
        else if((iPosition - iLaneOffset) <= 0)
        {
            Rect rect(0,0,iPosition + iLaneOffset, InImg.rows);
            InImg(rect).copyTo(OutImg);
        }
        else if( (iPosition + iLaneOffset) >= InImg.cols)
        {
            Rect rect(iPosition-iLaneOffset,0,InImg.cols - iPosition +  iLaneOffset, InImg.rows);
            InImg(rect).copyTo(OutImg);
        }

    }
    return OutImg;
}
/*
 *  Get lane coordinates from right and left side
 */
LaneCoordinates LaneBorder(Mat InImg, int iMinLaneWidth, int iMaxLaneWidth, int iPosition, int  iLaneOffset)
{
    vector<LaneCoordinates> ff;
    ff.push_back(LaneCoordinates());
    LaneCoordinates LaneCoor;
    bool found;
    for (int i = 0; i < InImg.rows; i = i + 5)
    {
        found = false;
        for (int j = 0; j < InImg.cols; j++)
        {
            if (InImg.at<uchar>(i,j) == 255)
            {
                for (int k = InImg.cols-1; k >= j; k--)
                {
                    if (InImg.at<uchar>(i,k) > 0 && k-j >=iMinLaneWidth && k-j <= iMaxLaneWidth)
                    {
                        LaneCoor.L.push_back(Point(j+iPosition-iLaneOffset,i));
                        LaneCoor.R.push_back(Point(k+iPosition-iLaneOffset,i));
                        found = true;
                        break;
                    }
                }
                if (found) break;
            }
        }
    }
    return LaneCoor;
}
/*
 *  Image threshold iteration
 */
Mat ImageThresholdIter(Mat inputImg, int iMaxIter)
{
    vector<int> histSimple(256);     // Simple histogram
    vector<int> histWeight(256);    // Simple weighted histogram
    vector<int> histCum(256);       // Cumulative histogram
    vector<int> histCumWeight(256);  // Cumulative weighted histogram
    // Init
    for (int i = 0; i < 256; i++)
    {
        histSimple[i] = 0;
        histWeight[i] = 0;
        histCum[i] = 0;
        histCumWeight[i] = 0;

    }
    // Simple
    for (int i = 0; i < inputImg.rows; i++){
        for (int j = 0; j < inputImg.cols; j++){
            histSimple[inputImg.at<uchar>(i,j)]++;
        }
    }
    // Simple Weighted
    for (int i = 0; i < (int)histWeight.size(); i++){
        histWeight[i] = histSimple[i] * i;
    }
    // Cumulative
    for (int i = 1; i < (int)histCum.size(); i++){
        histCum[i] = histCum[i-1] + histSimple[i];
    }
    // Cumulative Weighted
    for (int i = 1; i < (int)histCumWeight.size(); i++){
        histCumWeight[i] = histCumWeight[i-1] + histWeight[i];
    }
    // Iterative threshold
    int i = 0;
    vector<int> T;
    T.push_back(1);
    int Ebg = 0;
    int Efg = 0;
    bool done = false;

    while(!done){
        i++;
        if (histCum[T[i-1]] == 0)
            Ebg = 1;
        else
            Ebg = histCumWeight[T[i-1]]/histCum[T[i-1]];
        Efg = (histCumWeight[254] - histCumWeight[T[i-1]]) / (histCum[254] - histCum[T[i-1]]);
        T.push_back((Ebg+Efg)/2);
        if (T[i] == T[i-1]){
            done = true;
        }
        if (i > iMaxIter){
            done = true;
            //T[i] = 100;
        }

    }
    Mat Out;
    threshold(inputImg,Out,(double)T[i],255,THRESH_BINARY);
    inputImg.release();
    return Out;
}
