////////////////////////////////////////////////////////////////
//
//      Fast demosaicing algorithm
//
//      copyright (c) 2008-2010  Emil Martinec <ejmartin@uchicago.edu>
//
//
// code dated: August 26, 2010
//
//  fast_demo.cc is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <https://www.gnu.org/licenses/>.
//
////////////////////////////////////////////////////////////////

#include <cmath>
#include "rawimagesource.h"
#include "rtgui/multilangmgr.h"
#include "opthelper.h"

using namespace std;
using namespace rtengine;

namespace
{
unsigned fc(const unsigned int cfa[2][2], int r, int c) {
    return cfa[r & 1][c & 1];
}
}

#define TS 224
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

/*
LUTf RawImageSource::initInvGrad()
{
    LUTf invGrad (0x10000);

    //set up directional weight function
    for (int i=0; i<0x10000; i++)
        invGrad[i] = 1.0/SQR(1.0+i);

    return invGrad;
}
*/
#define INVGRAD(i) (16.0f/SQR(4.0f+i))
#ifdef __SSE2__
#define INVGRADV(i) (c16v*_mm_rcp_ps(SQRV(fourv+i)))
#endif
//LUTf RawImageSource::invGrad = RawImageSource::initInvGrad();

void RawImageSource::fast_demosaic()
{

    double progress = 0.0;
    const bool plistenerActive = plistener;

    //int winx=0, winy=0;
    //int winw=W, winh=H;

    if (plistener) {
        plistener->setProgressStr (Glib::ustring::compose(M("TP_RAW_DMETHOD_PROGRESSBAR"), M("TP_RAW_FAST")));
        plistener->setProgress (progress);
    }


    const unsigned int cfarray[2][2] = {{FC(0,0), FC(0,1)}, {FC(1,0), FC(1,1)}};
    const int bord = 5;

    float clip_pt = 4 * 65535 * initialGain;

    //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#ifdef _OPENMP
    #pragma omp parallel
#endif
    {

        char *buffer;
        float *greentile;
        float *redtile;
        float *bluetile;
#define CLF 1
        // assign working space
        buffer = (char *) calloc(3 * sizeof(float) * TS * TS + 3 * CLF * 64 + 63, 1);
        char    *data;
        data = (char*)( ( uintptr_t(buffer) + uintptr_t(63)) / 64 * 64);

        greentile = (float (*))         data; //pointers to array
        redtile   = (float (*))         ((char*)greentile + sizeof(float) * TS * TS + CLF * 64);
        bluetile  = (float (*))         ((char*)redtile + sizeof(float) * TS * TS + CLF * 64);

#ifdef _OPENMP
        #pragma omp sections
#endif
        {
#ifdef _OPENMP
            #pragma omp section
#endif
            {

                //first, interpolate borders using bilinear
                for (int i = 0; i < H; i++)
                {

                    float sum[6];
                    int imin = max(0, i - 1);
                    int imax = min(i + 2, H);

                    for (int j = 0; j < bord; j++) { //first few columns
                        for (int c = 0; c < 6; c++) {
                            sum[c] = 0;
                        }

                        int jmin = max(0, j - 1);

                        for (int i1 = imin; i1 < imax; i1++)
                            for (int j1 = jmin; j1 < j + 2; j1++) {
                                int c = fc(cfarray, i1, j1);
                                sum[c] += rawData[i1][j1];
                                sum[c + 3]++;
                            }

                        int c = fc(cfarray, i, j);

                        if (c == 1) {
                            red[i][j] = sum[0] / sum[3];
                            green[i][j] = rawData[i][j];
                            blue[i][j] = sum[2] / sum[5];
                        } else {
                            green[i][j] = sum[1] / sum[4];

                            if (c == 0) {
                                red[i][j] = rawData[i][j];
                                blue[i][j] = sum[2] / sum[5];
                            } else {
                                red[i][j] = sum[0] / sum[3];
                                blue[i][j] = rawData[i][j];
                            }
                        }
                    }//j

                    for (int j = W - bord; j < W; j++) { //last few columns
                        for (int c = 0; c < 6; c++) {
                            sum[c] = 0;
                        }

                        int jmax = min(j + 2, W);

                        for (int i1 = imin; i1 < imax; i1++)
                            for (int j1 = j - 1; j1 < jmax; j1++) {
                                int c = fc(cfarray, i1, j1);
                                sum[c] += rawData[i1][j1];
                                sum[c + 3]++;
                            }

                        int c = fc(cfarray, i, j);

                        if (c == 1) {
                            red[i][j] = sum[0] / sum[3];
                            green[i][j] = rawData[i][j];
                            blue[i][j] = sum[2] / sum[5];
                        } else {
                            green[i][j] = sum[1] / sum[4];

                            if (c == 0) {
                                red[i][j] = rawData[i][j];
                                blue[i][j] = sum[2] / sum[5];
                            } else {
                                red[i][j] = sum[0] / sum[3];
                                blue[i][j] = rawData[i][j];
                            }
                        }
                    }//j
                }//i

            }
            //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#ifdef _OPENMP
            #pragma omp section
#endif
            {

                for (int j = bord; j < W - bord; j++)
                {
                    float sum[6];

                    for (int i = 0; i < bord; i++) { //first few rows
                        for (int c = 0; c < 6; c++) {
                            sum[c] = 0;
                        }

                        for (int i1 = max(0, i - 1); i1 < i + 2; i1++)
                            for (int j1 = j - 1; j1 < j + 2; j1++) {
                                int c = fc(cfarray, i1, j1);
                                sum[c] += rawData[i1][j1];
                                sum[c + 3]++;
                            }

                        int c = fc(cfarray, i, j);

                        if (c == 1) {
                            red[i][j] = sum[0] / sum[3];
                            green[i][j] = rawData[i][j];
                            blue[i][j] = sum[2] / sum[5];
                        } else {
                            green[i][j] = sum[1] / sum[4];

                            if (c == 0) {
                                red[i][j] = rawData[i][j];
                                blue[i][j] = sum[2] / sum[5];
                            } else {
                                red[i][j] = sum[0] / sum[3];
                                blue[i][j] = rawData[i][j];
                            }
                        }
                    }//i

                    for (int i = H - bord; i < H; i++) { //last few rows
                        for (int c = 0; c < 6; c++) {
                            sum[c] = 0;
                        }

                        for (int i1 = i - 1; i1 < min(i + 2, H); i1++)
                            for (int j1 = j - 1; j1 < j + 2; j1++) {
                                int c = fc(cfarray, i1, j1);
                                sum[c] += rawData[i1][j1];
                                sum[c + 3]++;
                            }

                        int c = fc(cfarray, i, j);

                        if (c == 1) {
                            red[i][j] = sum[0] / sum[3];
                            green[i][j] = rawData[i][j];
                            blue[i][j] = sum[2] / sum[5];
                        } else {
                            green[i][j] = sum[1] / sum[4];

                            if (c == 0) {
                                red[i][j] = rawData[i][j];
                                blue[i][j] = sum[2] / sum[5];
                            } else {
                                red[i][j] = sum[0] / sum[3];
                                blue[i][j] = rawData[i][j];
                            }
                        }
                    }//i
                }//j

            }
        }
#ifdef _OPENMP
        #pragma omp single
#endif
        {
            if(plistenerActive) {
                progress += 0.1;
                plistener->setProgress(progress);
            }
        }
        //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        int progressCounter = 0;
        const double progressInc = 16.0 * (1.0 - progress) / ((H * W) / ((TS - 4) * (TS - 4)));

#ifdef _OPENMP
        #pragma omp for nowait
#endif

        for (int top = bord - 2; top < H - bord + 2; top += TS - (4))
            for (int left = bord - 2; left < W - bord + 2; left += TS - (4)) {
                int bottom = min(top + TS, H - bord + 2);
                int right  = min(left + TS, W - bord + 2);

#ifdef __SSE2__
                __m128 wtuv, wtdv, wtlv, wtrv;
                __m128 greenv, tempv, absv, abs2v;
                __m128 c16v = _mm_set1_ps( 16.0f );
                __m128 fourv = _mm_set1_ps( 4.0f );
                vmask selmask;
                vmask andmask = _mm_set_epi32( 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff );

                if(fc(cfarray, top, left) == 1) {
                    selmask = _mm_set_epi32( 0, 0xffffffff, 0, 0xffffffff );
                } else {
                    selmask = _mm_set_epi32( 0xffffffff, 0, 0xffffffff, 0 );
                }

#endif

                // interpolate G using gradient weights
                for (int i = top, rr = 0; i < bottom; i++, rr++) {
                    float   wtu, wtd, wtl, wtr;
#ifdef __SSE2__
                    selmask = (vmask)_mm_andnot_ps( (__m128)selmask, (__m128)andmask);
                    int j, cc;
                    for (j = left, cc = 0; j < right - 3; j += 4, cc += 4) {
                        tempv = LVFU(rawData[i][j]);
                        absv = vabsf(LVFU(rawData[i - 1][j]) - LVFU(rawData[i + 1][j]));
                        wtuv = INVGRADV(absv + vabsf(tempv - LVFU(rawData[i - 2][j])) + vabsf(LVFU(rawData[i - 1][j]) - LVFU(rawData[i - 3][j])));
                        wtdv = INVGRADV(absv + vabsf(tempv - LVFU(rawData[i + 2][j])) + vabsf(LVFU(rawData[i + 1][j]) - LVFU(rawData[i + 3][j])));
                        abs2v = vabsf(LVFU(rawData[i][j - 1]) - LVFU(rawData[i][j + 1]));
                        wtlv = INVGRADV(abs2v + vabsf(tempv - LVFU(rawData[i][j - 2])) + vabsf(LVFU(rawData[i][j - 1]) - LVFU(rawData[i][j - 3])));
                        wtrv = INVGRADV(abs2v + vabsf(tempv - LVFU(rawData[i][j + 2])) + vabsf(LVFU(rawData[i][j + 1]) - LVFU(rawData[i][j + 3])));
                        greenv = (wtuv * LVFU(rawData[i - 1][j]) + wtdv * LVFU(rawData[i + 1][j]) + wtlv * LVFU(rawData[i][j - 1]) + wtrv * LVFU(rawData[i][j + 1])) / (wtuv + wtdv + wtlv + wtrv);
                        _mm_store_ps(&greentile[rr * TS + cc], vself(selmask, greenv, tempv));
                        _mm_store_ps(&redtile[rr * TS + cc], tempv);
                        _mm_store_ps(&bluetile[rr * TS + cc], tempv);
                    }

                    for (; j < right; j++, cc++) {

                        if (fc(cfarray, i, j) == 1) {
                            greentile[rr * TS + cc] = rawData[i][j];

                        } else {
                            //compute directional weights using image gradients
                            wtu = INVGRAD((abs(rawData[i + 1][j] - rawData[i - 1][j]) + abs(rawData[i][j] - rawData[i - 2][j]) + abs(rawData[i - 1][j] - rawData[i - 3][j])));
                            wtd = INVGRAD((abs(rawData[i - 1][j] - rawData[i + 1][j]) + abs(rawData[i][j] - rawData[i + 2][j]) + abs(rawData[i + 1][j] - rawData[i + 3][j])));
                            wtl = INVGRAD((abs(rawData[i][j + 1] - rawData[i][j - 1]) + abs(rawData[i][j] - rawData[i][j - 2]) + abs(rawData[i][j - 1] - rawData[i][j - 3])));
                            wtr = INVGRAD((abs(rawData[i][j - 1] - rawData[i][j + 1]) + abs(rawData[i][j] - rawData[i][j + 2]) + abs(rawData[i][j + 1] - rawData[i][j + 3])));

                            //store in rgb array the interpolated G value at R/B grid points using directional weighted average
                            greentile[rr * TS + cc] = (wtu * rawData[i - 1][j] + wtd * rawData[i + 1][j] + wtl * rawData[i][j - 1] + wtr * rawData[i][j + 1]) / (wtu + wtd + wtl + wtr);
                        }

                        redtile[rr * TS + cc] = rawData[i][j];
                        bluetile[rr * TS + cc] = rawData[i][j];
                    }

#else

                    for (int j = left, cc = 0; j < right; j++, cc++) {
                        if (fc(cfarray, i, j) == 1) {
                            greentile[rr * TS + cc] = rawData[i][j];
                        } else {
                            //compute directional weights using image gradients
                            wtu = INVGRAD((abs(rawData[i + 1][j] - rawData[i - 1][j]) + abs(rawData[i][j] - rawData[i - 2][j]) + abs(rawData[i - 1][j] - rawData[i - 3][j])));
                            wtd = INVGRAD((abs(rawData[i - 1][j] - rawData[i + 1][j]) + abs(rawData[i][j] - rawData[i + 2][j]) + abs(rawData[i + 1][j] - rawData[i + 3][j])));
                            wtl = INVGRAD((abs(rawData[i][j + 1] - rawData[i][j - 1]) + abs(rawData[i][j] - rawData[i][j - 2]) + abs(rawData[i][j - 1] - rawData[i][j - 3])));
                            wtr = INVGRAD((abs(rawData[i][j - 1] - rawData[i][j + 1]) + abs(rawData[i][j] - rawData[i][j + 2]) + abs(rawData[i][j + 1] - rawData[i][j + 3])));

                            //store in rgb array the interpolated G value at R/B grid points using directional weighted average
                            greentile[rr * TS + cc] = (wtu * rawData[i - 1][j] + wtd * rawData[i + 1][j] + wtl * rawData[i][j - 1] + wtr * rawData[i][j + 1]) / (wtu + wtd + wtl + wtr);
                        }

                        redtile[rr * TS + cc] = rawData[i][j];
                        bluetile[rr * TS + cc] = rawData[i][j];
                    }

#endif
                }

#ifdef __SSE2__
                __m128 zd25v = _mm_set1_ps(0.25f);
                __m128 clip_ptv = _mm_set1_ps( clip_pt );
#endif

                for (int i = top + 1, rr = 1; i < bottom - 1; i++, rr++) {
                    if (fc(cfarray, i, left + (fc(cfarray, i, 2) & 1) + 1) == 0)
#ifdef __SSE2__
                        for (int j = left + 1, cc = 1; j < right - 1; j += 4, cc += 4) {
                            //interpolate B/R colors at R/B sites
                            _mm_storeu_ps(&bluetile[rr * TS + cc], LVFU(greentile[rr * TS + cc]) - zd25v * ((LVFU(greentile[(rr - 1)*TS + (cc - 1)]) + LVFU(greentile[(rr - 1)*TS + (cc + 1)]) + LVFU(greentile[(rr + 1)*TS + cc + 1]) + LVFU(greentile[(rr + 1)*TS + cc - 1])) -
                                          vminf(LVFU(rawData[i - 1][j - 1]) + LVFU(rawData[i - 1][j + 1]) + LVFU(rawData[i + 1][j + 1]) + LVFU(rawData[i + 1][j - 1]), clip_ptv)));
                        }

#else

                        for (int cc = (fc(cfarray, i, 2) & 1) + 1, j = left + cc; j < right - 1; j += 2, cc += 2) {
                            //interpolate B/R colors at R/B sites
                            bluetile[rr * TS + cc] = greentile[rr * TS + cc] - 0.25f * ((greentile[(rr - 1) * TS + (cc - 1)] + greentile[(rr - 1) * TS + (cc + 1)] + greentile[(rr + 1) * TS + cc + 1] + greentile[(rr + 1) * TS + cc - 1]) -
                                                     min(clip_pt, rawData[i - 1][j - 1] + rawData[i - 1][j + 1] + rawData[i + 1][j + 1] + rawData[i + 1][j - 1]));
                        }

#endif
                    else
#ifdef __SSE2__
                        for (int j = left + 1, cc = 1; j < right - 1; j += 4, cc += 4) {
                            //interpolate B/R colors at R/B sites
                            _mm_storeu_ps(&redtile[rr * TS + cc], LVFU(greentile[rr * TS + cc]) - zd25v * ((LVFU(greentile[(rr - 1)*TS + cc - 1]) + LVFU(greentile[(rr - 1)*TS + cc + 1]) + LVFU(greentile[(rr + 1)*TS + cc + 1]) + LVFU(greentile[(rr + 1)*TS + cc - 1])) -
                                          vminf(LVFU(rawData[i - 1][j - 1]) + LVFU(rawData[i - 1][j + 1]) + LVFU(rawData[i + 1][j + 1]) + LVFU(rawData[i + 1][j - 1]), clip_ptv)));
                        }

#else

                        for (int cc = (fc(cfarray, i, 2) & 1) + 1, j = left + cc; j < right - 1; j += 2, cc += 2) {
                            //interpolate B/R colors at R/B sites
                            redtile[rr * TS + cc] = greentile[rr * TS + cc] - 0.25f * ((greentile[(rr - 1) * TS + cc - 1] + greentile[(rr - 1) * TS + cc + 1] + greentile[(rr + 1) * TS + cc + 1] + greentile[(rr + 1) * TS + cc - 1]) -
                                                    min(clip_pt, rawData[i - 1][j - 1] + rawData[i - 1][j + 1] + rawData[i + 1][j + 1] + rawData[i + 1][j - 1]));
                        }

#endif
                }


#ifdef __SSE2__
                __m128 temp1v, temp2v, greensumv;
                selmask = _mm_set_epi32( 0xffffffff, 0, 0xffffffff, 0 );
#endif

                // interpolate R/B using color differences
                for (int i = top + 2, rr = 2; i < bottom - 2; i++, rr++) {
#ifdef __SSE2__

                    for (int cc = 2 + (fc(cfarray, i, 2) & 1), j = left + cc; j < right - 2; j += 4, cc += 4) {
                        // no need to take care about the borders of the tile. There's enough free space.
                        //interpolate R and B colors at G sites
                        greenv = LVFU(greentile[rr * TS + cc]);
                        greensumv = LVFU(greentile[(rr - 1) * TS + cc]) + LVFU(greentile[(rr + 1) * TS + cc]) + LVFU(greentile[rr * TS + cc - 1]) + LVFU(greentile[rr * TS + cc + 1]);

                        temp1v = LVFU(redtile[rr * TS + cc]);
                        temp2v = greenv - zd25v * (greensumv - LVFU(redtile[(rr - 1) * TS + cc]) - LVFU(redtile[(rr + 1) * TS + cc]) - LVFU(redtile[rr * TS + cc - 1]) - LVFU(redtile[rr * TS + cc + 1]));

                        _mm_storeu_ps( &redtile[rr * TS + cc], vself(selmask, temp1v, temp2v));

                        temp1v = LVFU(bluetile[rr * TS + cc]);

                        temp2v = greenv - zd25v * (greensumv - LVFU(bluetile[(rr - 1) * TS + cc]) - LVFU(bluetile[(rr + 1) * TS + cc]) - LVFU(bluetile[rr * TS + cc - 1]) - LVFU(bluetile[rr * TS + cc + 1]));

                        _mm_storeu_ps( &bluetile[rr * TS + cc], vself(selmask, temp1v, temp2v));
                    }

#else

                    for (int cc = 2 + (fc(cfarray, i, 2) & 1), j = left + cc; j < right - 2; j += 2, cc += 2) {
                        //interpolate R and B colors at G sites
                        redtile[rr * TS + cc] = greentile[rr * TS + cc] - 0.25f * ((greentile[(rr - 1) * TS + cc] - redtile[(rr - 1) * TS + cc]) + (greentile[(rr + 1) * TS + cc] - redtile[(rr + 1) * TS + cc]) +
                                                (greentile[rr * TS + cc - 1] - redtile[rr * TS + cc - 1]) + (greentile[rr * TS + cc + 1] - redtile[rr * TS + cc + 1]));
                        bluetile[rr * TS + cc] = greentile[rr * TS + cc] - 0.25f * ((greentile[(rr - 1) * TS + cc] - bluetile[(rr - 1) * TS + cc]) + (greentile[(rr + 1) * TS + cc] - bluetile[(rr + 1) * TS + cc]) +
                                                 (greentile[rr * TS + cc - 1] - bluetile[rr * TS + cc - 1]) + (greentile[rr * TS + cc + 1] - bluetile[rr * TS + cc + 1]));
                    }

#endif
                }


                for (int i = top + 2, rr = 2; i < bottom - 2; i++, rr++) {
#ifdef __SSE2__
                    int j, cc;
                    for (j = left + 2, cc = 2; j < right - 5; j += 4, cc += 4) {
                        _mm_storeu_ps(&red[i][j], vmaxf(LVFU(redtile[rr * TS + cc]), ZEROV));
                        _mm_storeu_ps(&green[i][j], vmaxf(LVFU(greentile[rr * TS + cc]), ZEROV));
                        _mm_storeu_ps(&blue[i][j], vmaxf(LVFU(bluetile[rr * TS + cc]), ZEROV));
                    }

                    for (; j < right - 2; j++, cc++) {
                        red[i][j] = std::max(0.f, redtile[rr * TS + cc]);
                        green[i][j] = std::max(0.f, greentile[rr * TS + cc]);
                        blue[i][j] = std::max(0.f, bluetile[rr * TS + cc]);
                    }

#else

                    for (int j = left + 2, cc = 2; j < right - 2; j++, cc++) {
                        red[i][j] = std::max(0.f, redtile[rr * TS + cc]);
                        green[i][j] = std::max(0.f, greentile[rr * TS + cc]);
                        blue[i][j] = std::max(0.f, bluetile[rr * TS + cc]);
                    }

#endif


                }

                if(plistenerActive && ((++progressCounter) % 16 == 0)) {
#ifdef _OPENMP
                    #pragma omp critical (updateprogress)
#endif
                    {
                        progress += progressInc;
                        progress = min(1.0, progress);
                        plistener->setProgress (progress);
                    }
                }

            }

        free(buffer);
    } // End of parallelization

    if(plistenerActive) {
        plistener->setProgress(1.00);
    }



}
#undef TS
#undef CLF