/**************************************************************************
 *{@C
 *      Copyright:      1988-2023 Paul Obermeier (obermeier@poSoft.de)
 *
 *      Module:         ImageProcessing
 *      Filename:       IP_Correlate.c
 *
 *      Author:         Paul Obermeier
 *
 *      Description:    Pattern matching functions.
 *
 *      Additional documentation:
 *                      None.
 *
 *      Exported functions:
 *                      IP_Correlate
 *                      IP_FindPattern
 *
 **************************************************************************/

#include <stdio.h>
#include <math.h>

#include "UT_Compat.h"
#include "UT_Const.h"
#include "UT_Macros.h"
#include "UT_Error.h"
#include "UT_Memory.h"
#include "UT_Parallel.h"

#define  USE_IMG_MACROS
#include "FF_Image.h"

#include "IP_Image.h"
#include "IP_ImagePrivate.h"

static Float32 *S;                     /* First pixel data rectangle */
static Int32   width_S, height_S;      /* Width and height of S in pixels */

static Float32 *D;                     /* Second pixel data rectangle */
static Int32   width_D, height_D;      /* Width and height of D in pixels */

/* A struct to pass parameters to the concurrently
   invoked "conc_correl" function */

typedef struct {
    Int32   nx, ny;   /* Number of different positions in x direction at which 
                         the correlation coefficient must be computed */
    Float32 ms, vs,   /* Mean and variance of the data in array S */
            *r;       /* Buffer to store the computed correlation coefficients */
} correlparms;


/* Amount of memory set aside for the concurrently
   invoked function's auto variable stack. */

#define stacksize (1 << 12)

/* Allocate a buffer, "R", for a rectangular array of "width_R" by
   "height_R" pixels, and copy the pixel data from a rectangular region
   of channel "chn" of image "pimg" into "R".  The position of lower left
   corner of the region to be copied in "pimg" is at (x1, y1); the position
   of the upper right corner is (x2, y2). */

static UT_Bool extract_data
    (FF_ImgDesc *pimg,
     FF_ImgChanType chn,
     Int32 x1, Int32 y1,
     Int32 x2, Int32 y2,
     Float32 **R,
     Int32 *width_R,
     Int32 *height_R)
{
    Float32       *buf, *bscan, *dst, *stop;
    const Float32 *fsrc;
    const UInt8   *ubsrc;
    Int32         width, height, y;

    /* Parameter check: If (x1, y1) and (x2, y2) specify other corners of
    the rectangle than the lower left and upper right ones, find the lower
    left and upper right corners.  Then check if the rectangle is completely
    covered by image "pimg". */

    if (x1 > x2)
        UT_SWAP (x1, x2, Int32);
    if (y1 > y2)
        UT_SWAP (y1, y2, Int32);

    if (x1 < 0 || x2 > pimg->desc.width || y1 < 0 || y1 > pimg->desc.height) {
        UT_ErrSetNum (UT_ErrParamRange, str_correl_rect);
        return UT_False;
    }

    /* Allocate a buffer for the pixel data extracted from the rectangle. */
    width =  x2 - x1 + 1;
    height = y2 - y1 + 1;

    if (!(buf = UT_MemTempArray (width * height, Float32)))
        return UT_False;

    /* Copy the pixel data from channel "chn" of the rectangle into the
    new buffer. Convert the data to FF_ImgFmtTypeFloat format if necessary. */
    
    switch (pimg->channel[chn]) {
    case FF_ImgFmtTypeNone:
        bscan = buf;
        for (y = y1; y <= y2; ++y) {
            dst = bscan;
            stop = dst + width;
            while (dst < stop)
                *dst++ = 0.0;
            bscan += width;
        }
        break;

    case FF_ImgFmtTypeUByte:
        bscan = buf;
        for (y = y1; y <= y2; ++y) {
            dst = bscan;
            stop = dst + width;
            ubsrc = FF_ImgUBytePixel (pimg, chn, x1, y);
            while (dst < stop)
                *dst++ = *ubsrc++ * (1.0 / 255.0);
            bscan += width;
        }
        break;

    case FF_ImgFmtTypeFloat:
        bscan = buf;
        for (y = y1; y <= y2; ++y) {
            dst = bscan;
            stop = dst + width;
            fsrc = FF_ImgFloatPixel (pimg, chn, x1, y);
            while (dst < stop)
                *dst++ = *fsrc++;
            bscan += width;
        }
        break;

    default:
        UT_ErrFatal ("extract_data", "invalid pixel data format");
    }

    *R = buf;
    *width_R = width;
    *height_R = height;
    return UT_True;
}

/* Compute the correlation coefficient of the data in two rectangular
   regions, "Rs", and "Rd".  The width and height of both, "Rs" and "Rd"
   are "width" and "height" pixels.  The lower left corner of "Rs" is at
   position (xs, ys) in array S; the lower left corner of "Rd" is at position
   (xd, yd) in array D.

   The correlation coefficient is computed straightforwardly by the book:
   Let "Rs" and "Rd" be two random variables (the two input data rectangles),
   and let E(X) be mathematical expectation of X.  The means, "ms" and "md"
   of "Rs" and "Rd" are

        ms = E(Rs),
        md = E(Rd).

   The variances, "vs" and "vd" of "Rs" and "Rd" are

        vs = E(Rs * Rs) - ms * ms,
        vd = E(Rd * Rd) - md * md,

   and their correlation coefficient, "r" is defined as

        r = E((Rs - ms) * (Rd - md)) / sqrt (vs * vd).

   If sqrt (vs * vd) is zero, the correlation coefficient does not exist,
   and "correl_1" returns -2.0. */

static Float32 correl_1
    (Int32 width, Int32 height,
     Int32 xs, Int32 ys,
     Int32 xd, Int32 yd)
{
    const Float32 *sscan, *dscan, *ssrc, *dsrc, *stop;
    Float64       ms, md, vs, vd, r, t, weight;
    Int32         y, ystop;

    /* Compute the means and the variances. */
    ms = 0.0;
    md = 0.0;
    vs = 0.0;
    vd = 0.0;

    y = ys;
    ystop = ys + height;
    sscan = S + ys * width_S;
    dscan = D + yd * width_D;

    while (y < ystop) {
        ssrc = sscan + xs;
        stop = ssrc + width;
        dsrc = dscan + xd;

        while (ssrc < stop) {
            ms += *ssrc;
            md += *dsrc;
            vs += *ssrc * *ssrc;
            vd += *dsrc * *dsrc;
            ++ssrc;
            ++dsrc;
        }
        sscan += width_S;
        dscan += width_D;
        ++y;
    }

    weight = 1.0 / (width * height);
    ms *= weight;
    md *= weight;
    vs = vs * weight - (ms * ms);
    vd = vd * weight - (md * md);

    if (vs * vd <= 0.0) {
        return -2.0;
    }

    /* Compute the correlation coefficient. */
    r = 0.0;

    y = ys;
    ystop = ys + height;
    sscan = S + ys * width_S;
    dscan = D + yd * width_D;

    while (y < ystop) {
        ssrc = sscan + xs;
        stop = ssrc + width;
        dsrc = dscan + xd;

        while (ssrc < stop)
            r += (*ssrc++ - ms) * (*dsrc++ - md);

        sscan += width_S;
        dscan += width_D;
        ++y;
    }

    r *= weight;
    t = sqrt (vs * vd);
    if (UT_ABS (r) >= MaxFloat32 * t)
        r = -2.0;
    else
        r = r / t;
    return r;
}

/* Compute the correlation coefficient of the data in two rectangular
   regions, "Rs", and "Rd", faster version.  "Correl_2" does the same as
   "correl_1", except for the mean "ms" and the variance "vs" of region
   "Rs".  "Correl_2" computes "ms" and "vs", whereas "correl_2" expects
   to be told what their values are. */

static Float32 correl_2
    (Int32 width, Int32 height,
     Int32 xs, Int32 ys,
     Int32 xd, Int32 yd,
     Float32 ms, Float32 vs)
{
    const Float32 *sscan, *dscan, *ssrc, *dsrc, *stop;
    Float64       md, vd, r, t, weight;
    Int32         y, ystop;

    /* Compute the mean and the variance of "Rd". */
    md = 0.0;
    vd = 0.0;

    y = yd;
    ystop = yd + height;
    dscan = D + yd * width_D;

    while (y < ystop) {
        dsrc = dscan + xd;
        stop = dsrc + width;

        while (dsrc < stop) {
            md += *dsrc;
            vd += *dsrc * *dsrc;
            ++dsrc;
        }

        dscan += width_D;
        ++y;
    }

    weight = 1.0 / (width * height);
    md *= weight;
    vd = vd * weight - (md * md);

    if (vs * vd <= 0.0)
        return -2.0;

    /* Compute the correlation coefficient. */
    r = 0.0;

    y = ys;
    ystop = ys + height;
    sscan = S + ys * width_S;
    dscan = D + yd * width_D;

    while (y < ystop) {
        ssrc = sscan + xs;
        stop = ssrc + width;
        dsrc = dscan + xd;

        while (ssrc < stop)
            r += (*ssrc++ - ms) * (*dsrc++ - md);

        sscan += width_S;
        dscan += width_D;
        ++y;
    }

    r *= weight;
    t = sqrt (vs * vd);
    if (UT_ABS (r) >= MaxFloat32 * t)
        r = -2.0;
    else
        r = r / t;
    return r;
}

/* Compute the mean "m" and the variance "v" of the data in a rectangular
   region. The width and height of the region are "width" and "height" pixels.
   The region's lower left corner is at position (xs, ys) in array S. */

static void mean_variance_S
    (Int32 width, Int32 height,
     Int32 xs, Int32 ys,
     Float32 *m, Float32 *v)
{
    const Float32 *sscan, *ssrc, *stop;
    Float64       ms, vs, weight;
    Int32         y, ystop;

    /* Compute the means and the variances. */
    ms = 0.0;
    vs = 0.0;

    y = ys;
    ystop = ys + height;
    sscan = S + ys * width_S;

    while (y < ystop) {
        ssrc = sscan + xs;
        stop = ssrc + width;

        while (ssrc < stop) {
            ms += *ssrc;
            vs += *ssrc * *ssrc;
            ++ssrc;
        }

        sscan += width_S;
        ++y;
    }

    weight = 1.0 / (width * height);
    ms *= weight;

    vs = vs * weight - (ms * ms);
    *m = ms;
    *v = vs;

    return;
}

/* Concurrent main loop for "find_pattern":  The pattern to be found
   is shifted across the search area pixel by pixel, and the correlation
   coefficients of the pattern and the pixels from the search area are
   computed. */

static void conc_correl (void *ptr, Int32 n_conc, Int32 i_conc)
{
    correlparms *parms;
    Int32       x, nx, y, ny, ymin, ymax;
    Float32     *r, ms, vs;

    parms = ptr;
    nx = parms->nx;
    ny = parms->ny;
    ms = parms->ms;
    vs = parms->vs;
    r =  parms->r;
    n_conc = (n_conc == 0? 1: n_conc);
    ymin = (ny * i_conc) / n_conc;
    ymax = (ny * (i_conc + 1)) / n_conc;

    for (y = ymin; y < ymax; ++y)
        for (x = 0; x < nx; ++x)
            r[y * nx + x] = correl_2 (width_S, height_S, 0, 0, x, y, ms, vs);
    return;
}

/* Compute the correlation coefficient for the pixels in two rectangular
   regions (see IP_Correlate for a description of the parameters). */

static UT_Bool correlate_pixels
    (IP_ImageId img1,
     IP_ImageId img2,
     FF_ImgChanType chn,
     Int32 xs1, Int32 ys1,
     Int32 xs2, Int32 ys2,
     Int32 xd1, Int32 yd1,
     Float32 *r)
{
    Int32 xd2, yd2;

    xd2 = xd1 + (xs2 - xs1);
    yd2 = yd1 + (ys2 - ys1);

    if (!extract_data (&img1->pimg, chn, xs1, ys1, xs2, ys2,
                       &S, &width_S, &height_S) ||
        !extract_data (&img2->pimg, chn, xd1, yd1, xd2, yd2,
                       &D, &width_D, &height_D))
        return UT_False;
    *r = correl_1 (width_S, height_S, 0, 0, 0, 0);
    return UT_True;
}

/* Find an occurrence of a given pattern in an image (see IP_FindPattern
   for a description of the parameters). */

static UT_Bool find_pattern
    (IP_ImageId img1,
     IP_ImageId img2,
     FF_ImgChanType chn,
     Int32 xs1, Int32 ys1,
     Int32 xs2, Int32 ys2,
     Int32 xd1, Int32 yd1,
     Int32 xd2, Int32 yd2,
     Float32 *xp, Float32 *yp,
     Float32 *rp)
{
    Int32       x, xm = 0, nx, y, ym = 0, ny;
    Float32     *r, rm;
    correlparms parms;

    /* Parameter check: Make sure that the region to be searched is
       at least as wide and as high as the pattern to be found in it. */

    nx = UT_ABS (xd2 - xd1) - UT_ABS (xs2 - xs1) + 1;
    ny = UT_ABS (yd2 - yd1) - UT_ABS (ys2 - ys1) + 1;

    if (nx <= 0 || ny <= 0) {
        UT_ErrSetNum (UT_ErrParamInvalid, str_find_rects);
        return UT_False;
    }

    /* Extract the pixel data to be processed from "img1" and "img2". */

    if (!extract_data (&img1->pimg, chn, xs1, ys1, xs2, ys2,
                       &S, &width_S, &height_S) ||
        !extract_data (&img2->pimg, chn, xd1, yd1, xd2, yd2,
                       &D, &width_D, &height_D)) {
        return UT_False;
    }

    /* Step pattern S across search region D in single-pixel increments
       and compute the correlation coefficients.  Store the correlation
       coefficients in array "r". */

    if (!(r = UT_MemTempArray (nx * ny, Float32))) {
        return UT_False;
    }

    parms.nx = nx;
    parms.ny = ny;
    parms.r = r;
    mean_variance_S (width_S, height_S, 0, 0, &parms.ms, &parms.vs);

    if (!UT_THREAD_EXEC (conc_correl, &parms, IP_StateNumThreads, stacksize)) {
        return UT_False;
    }

    /* Find the maximum correlation coefficient in array "r". */
    rm = -3.0;

    for (y = 0; y < ny; ++y) {
        for (x = 0; x < nx; ++x) {
            if (r[y * nx + x] > rm) {
                rm = r[y * nx + x];
                xm = x;
                ym = y;
            }
        }
    }
    *xp = (Float32) (xm + UT_MIN (xd1, xd2));
    *yp = (Float32) (ym + UT_MIN (yd1, yd2));
    *rp = rm;
    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           IP_Correlate
 *
 *      Usage:          Computes the correlation coefficient for two
 *                      sets of pixels.
 *
 *      Synopsis:       UT_Bool IP_Correlate
 *                              (IP_ImageId img1,
 *                               IP_ImageId img2,
 *                               FF_ImgChanType chn,
 *                               Int32 xs1, Int32 ys1,
 *                               Int32 xs2, Int32 ys2,
 *                               Int32 xd1, Int32 yd1,
 *                               Float32 *r)
 *
 *      Description:    Two pixel positions, (xs1, ys1) and (xs2, ys2),
 *                      specify the lower left and upper right corners of
 *                      a rectangle S in image "img1".
 *                      A second rectangle, D, in image "img2", with the
 *                      same size as S, is specified by the position of
 *                      the pixel at the lower left corner, (xd1,  yd1).
 *
 *                      IP_Correlate treats the pixel data in
 *                      channel "chn" of S and D as samples from two
 *                      random variables and computes their correlation
 *                      coefficient.
 *                      The correlation coefficient is in the range from
 *                      -1.0 to 1.0.  It can be used as a measure for the
 *                      similarity of the patterns formed by the pixels in
 *                      the two rectangles.
 *
 *                      For more detailed explanations of the correlation
 *                      coefficient and its significance to image processing,
 *                      see textbooks on these topics, e.g.
 *
 *                              Robert V. Hook, Allen T. Craig,
 *                              "Introduction to Mathematical Statistics",
 *                              Macmillan Publishing Co.,
 *                              4th ed. 1978,
 *                              pp 73 ff.
 *
 *                              Bernd Jaehne,
 *                              "Digitale Bildverarbeitung",
 *                              Springer Verlag,
 *                              2nd ed. 1991,
 *                              pp 225 ff.
 *
 *
 *      Return value:   If the correlation coefficient was computed
 *                      successfully, it is stored in "r", and
 *                      IP_Correlate returns UT_True.
 *
 *                      If the correlation coefficient could not be computed,
 *                      because the variance of the pixel data in S or D is
 *                      zero, some value less than -1.0 is stored in "r", and
 *                      IP_Correlate returns UT_True; no error code is
 *                      set.
 *
 *                      If the correlation coefficient could not be computed
 *                      because of an invalid parameter specification or an
 *                      arithmetic exception, IP_Correlate returns
 *                      UT_False; an error code is stored in UT_ErrNum.
 *
 *      See also:       IP_FindPattern
 *
 ***************************************************************************/

UT_Bool IP_Correlate
    (IP_ImageId img1,
     IP_ImageId img2,
     FF_ImgChanType chn,
     Int32 xs1, Int32 ys1,
     Int32 xs2, Int32 ys2,
     Int32 xd1, Int32 yd1,
     Float32 *r)
{
    UT_MemState memstate;
    UT_Bool     success;

    memstate = UT_MemRemember ();
    success = correlate_pixels (img1, img2, chn, xs1, ys1,
                                xs2, ys2, xd1, yd1, r);
    UT_MemRestore (memstate);
    return success;
}

/***************************************************************************
 *[@e
 *      Name:           IP_FindPattern
 *
 *      Usage:          Attempts to find a given pattern in an image.
 *
 *      Synopsis:       UT_Bool IP_FindPattern
 *                              (IP_ImageId img1,
 *                               IP_ImageId img2,
 *                               FF_ImgChanType chn,
 *                               Int32 xs1, Int32 ys1,
 *                               Int32 xs2, Int32 ys2,
 *                               Int32 xd1, Int32 yd1,
 *                               Int32 xd2, Int32 yd2,
 *                               Float32 *xp, Float32 *yp,
 *                               Float32 *rp)
 *
 *      Description:    Two pixel positions, (xs1, ys1) and (xs2, ys2),
 *                      specify the lower left and upper right corners of
 *                      a rectangle, S, in channel "chn" of image "img1".
 *                      A second rectangle, D, in channel "chn" of image
 *                      "img2", which must be at least as wide and high as
 *                      S, is specified by the positions of its lower left
 *                      and upper right corners, (xd1, yd1) and (xd2, yd2).
 *
 *                      The pixel data in S form a pattern.  IP_FindPattern
 *                      tries to find an occurrence of this pattern in the
 *                      pixel data in D:
 *                      The pixel at the lower left corner of S is aligned
 *                      with some pixel in D, and the correlation coefficient
 *                      of the pixels in S and the pixels in D at the same
 *                      positions is computed.
 *                      This process is repeated, shifting S across D pixel
 *                      by pixel, and computing the corresponding correlation
 *                      coefficients, until the maximum correlation between
 *                      S and D has been found.
 *                      The maximum correlation coeffcient, and the position
 *                      of the lower left corner of S (relative to the lower
 *                      left corner of "img2") are returned in "rp", "xp"
 *                      and "yp".
 *
 *                      "Xp", "yp" and "rp" can be interpreted as follows:
 *                      The pattern formed by the pixels in S also occurs in D.
 *                      The lower left corner of the pattern's occurence in D
 *                      is at position (xp, yp).
 *                      "Rp" indicates how well the pattern and its occurrence
 *                      in D match.  "Rp" is usually in the range from -1.0 to
 *                      1.0, with higher values indicating a better match.
 *                      "Rp" values of 0.0 and below suggest that the pattern
 *                      does not occur in D at all, and that the values of "xp"
 *                      and "yp" are not usable.
 *
 *                      Notes:
 *
 *                      - IP_FindPattern does not detect multiple occurences
 *                        of the pattern in D.  If the pattern occurs more than
 *                        once, IP_FindPattern selects an arbitray occurrence
 *                        and returns its position.
 *
 *                      - The computation time for IP_FindPattern is
 *                        proportional to the number of pixels in S and to
 *                        the number of pixels in D.  Finding an occurrence
 *                        of a big pattern or searching a large area can be
 *                        a lengthy process.  If the pattern to be found has
 *                        features significantly larger than a single pixel,
 *                        computation times can be reduced by scaling the
 *                        pattern and the region to be searched down to a
 *                        lower resolution and doing a rough search to limit
 *                        the search area before trying to find the exact
 *                        position of the original pattern in the original
 *                        image.
 *
 *                      - IP_FindPattern can be run concurrently on multiple
 *                        processors; see IP_SetNumThreads.
 *
 *      Return value:   If "xp", "yp" and "rp" were computed successfully,
 *                      IP_FindPattern returns UT_True.
 *
 *                      If the search for the pattern failed because
 *                      the variance of the pixels in S or in D is zero,
 *                      some value less than -1.0 is stored in "r", and
 *                      IP_FindPattern returns UT_True; no error code is
 *                      set.
 *
 *                      If the search failed because of an invalid
 *                      parameter specification or an arithmetic exception,
 *                      IP_FindPattern returns UT_False; an error code is
 *                      stored in UT_ErrNum.
 *
 *      See also:       IP_Correlate
 *
 ***************************************************************************/

UT_Bool IP_FindPattern
    (IP_ImageId img1,
     IP_ImageId img2,
     FF_ImgChanType chn,
     Int32 xs1, Int32 ys1,
     Int32 xs2, Int32 ys2,
     Int32 xd1, Int32 yd1,
     Int32 xd2, Int32 yd2,
     Float32 *xp, Float32 *yp,
     Float32 *rp)
{
    UT_MemState memstate;
    UT_Bool     success;

    memstate = UT_MemRemember ();
    success = find_pattern (img1, img2, chn, xs1, ys1, xs2, ys2,
                            xd1, yd1, xd2, yd2, xp, yp, rp);
    UT_MemRestore (memstate);
    return success;
}
