/* GStreamer
 * Copyright (C) 2011 David Schleef <ds@entropywave.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 51 Franklin Street, Suite 500,
 * Boston, MA 02110-1335, USA.
 */
/**
 * SECTION:element-gstpatchdetect
 *
 * The patchdetect element detects color patches from a color
 * calibration chart.  Currently, the patches for the 24-square
 * Munsell ColorChecker are hard-coded into the element.  When
 * a color chart is detected in the video stream, a message is
 * sent to the bus containing the detected color values of each
 * of the patches.
 *
 * <refsect2>
 * <title>Example launch line</title>
 * |[
 * gst-launch -v dv1394src ! dvdemux ! dvdec ! patchdetect ! xvimagesink
 * ]|
 * </refsect2>
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gst/gst.h>
#include <gst/base/gstbasetransform.h>
#include <gst/video/video.h>
#include <math.h>
#include <string.h>
#include "gstpatchdetect.h"

GST_DEBUG_CATEGORY_STATIC (gst_patchdetect_debug_category);
#define GST_CAT_DEFAULT gst_patchdetect_debug_category

/* prototypes */


static void gst_patchdetect_set_property (GObject * object,
    guint property_id, const GValue * value, GParamSpec * pspec);
static void gst_patchdetect_get_property (GObject * object,
    guint property_id, GValue * value, GParamSpec * pspec);
static void gst_patchdetect_dispose (GObject * object);
static void gst_patchdetect_finalize (GObject * object);

static gboolean
gst_patchdetect_get_unit_size (GstBaseTransform * trans, GstCaps * caps,
    guint * size);
static gboolean
gst_patchdetect_set_caps (GstBaseTransform * trans, GstCaps * incaps,
    GstCaps * outcaps);
static gboolean gst_patchdetect_start (GstBaseTransform * trans);
static gboolean gst_patchdetect_stop (GstBaseTransform * trans);
static gboolean gst_patchdetect_event (GstBaseTransform * trans,
    GstEvent * event);
static GstFlowReturn gst_patchdetect_transform_ip (GstBaseTransform * trans,
    GstBuffer * buf);
static gboolean gst_patchdetect_src_event (GstBaseTransform * trans,
    GstEvent * event);

enum
{
  PROP_0
};

/* pad templates */

static GstStaticPadTemplate gst_patchdetect_sink_template =
GST_STATIC_PAD_TEMPLATE ("sink",
    GST_PAD_SINK,
    GST_PAD_ALWAYS,
    GST_STATIC_CAPS (GST_VIDEO_CAPS_YUV ("I420"))
    );

static GstStaticPadTemplate gst_patchdetect_src_template =
GST_STATIC_PAD_TEMPLATE ("src",
    GST_PAD_SRC,
    GST_PAD_ALWAYS,
    GST_STATIC_CAPS (GST_VIDEO_CAPS_YUV ("I420"))
    );


/* class initialization */

#define DEBUG_INIT(bla) \
  GST_DEBUG_CATEGORY_INIT (gst_patchdetect_debug_category, "patchdetect", 0, \
      "debug category for patchdetect element");

GST_BOILERPLATE_FULL (GstPatchdetect, gst_patchdetect, GstBaseTransform,
    GST_TYPE_BASE_TRANSFORM, DEBUG_INIT);

static void
gst_patchdetect_base_init (gpointer g_class)
{
  GstElementClass *element_class = GST_ELEMENT_CLASS (g_class);

  gst_element_class_add_static_pad_template (element_class,
      &gst_patchdetect_sink_template);
  gst_element_class_add_static_pad_template (element_class,
      &gst_patchdetect_src_template);

  gst_element_class_set_details_simple (element_class, "Color Patch Detector",
      "Video/Analysis", "Detects color patches from a color calibration chart",
      "David Schleef <ds@entropywave.com>");
}

static void
gst_patchdetect_class_init (GstPatchdetectClass * klass)
{
  GObjectClass *gobject_class = G_OBJECT_CLASS (klass);
  GstBaseTransformClass *base_transform_class =
      GST_BASE_TRANSFORM_CLASS (klass);

  gobject_class->set_property = gst_patchdetect_set_property;
  gobject_class->get_property = gst_patchdetect_get_property;
  gobject_class->dispose = gst_patchdetect_dispose;
  gobject_class->finalize = gst_patchdetect_finalize;
  base_transform_class->get_unit_size =
      GST_DEBUG_FUNCPTR (gst_patchdetect_get_unit_size);
  base_transform_class->set_caps = GST_DEBUG_FUNCPTR (gst_patchdetect_set_caps);
  base_transform_class->start = GST_DEBUG_FUNCPTR (gst_patchdetect_start);
  base_transform_class->stop = GST_DEBUG_FUNCPTR (gst_patchdetect_stop);
  base_transform_class->event = GST_DEBUG_FUNCPTR (gst_patchdetect_event);
  base_transform_class->transform_ip =
      GST_DEBUG_FUNCPTR (gst_patchdetect_transform_ip);
  base_transform_class->src_event =
      GST_DEBUG_FUNCPTR (gst_patchdetect_src_event);

}

static void
gst_patchdetect_init (GstPatchdetect * patchdetect,
    GstPatchdetectClass * patchdetect_class)
{
}

void
gst_patchdetect_set_property (GObject * object, guint property_id,
    const GValue * value, GParamSpec * pspec)
{
  g_return_if_fail (GST_IS_PATCHDETECT (object));

  switch (property_id) {
    default:
      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id, pspec);
      break;
  }
}

void
gst_patchdetect_get_property (GObject * object, guint property_id,
    GValue * value, GParamSpec * pspec)
{
  g_return_if_fail (GST_IS_PATCHDETECT (object));

  switch (property_id) {
    default:
      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id, pspec);
      break;
  }
}

void
gst_patchdetect_dispose (GObject * object)
{
  g_return_if_fail (GST_IS_PATCHDETECT (object));

  /* clean up as possible.  may be called multiple times */

  G_OBJECT_CLASS (parent_class)->dispose (object);
}

void
gst_patchdetect_finalize (GObject * object)
{
  g_return_if_fail (GST_IS_PATCHDETECT (object));

  /* clean up object here */

  G_OBJECT_CLASS (parent_class)->finalize (object);
}


static gboolean
gst_patchdetect_get_unit_size (GstBaseTransform * trans, GstCaps * caps,
    guint * size)
{
  int width, height;
  GstVideoFormat format;
  gboolean ret;

  ret = gst_video_format_parse_caps (caps, &format, &width, &height);
  *size = gst_video_format_get_size (format, width, height);

  return ret;
}

static gboolean
gst_patchdetect_set_caps (GstBaseTransform * trans, GstCaps * incaps,
    GstCaps * outcaps)
{
  GstPatchdetect *patchdetect = GST_PATCHDETECT (trans);
  int width, height;
  GstVideoFormat format;
  gboolean ret;

  ret = gst_video_format_parse_caps (incaps, &format, &width, &height);
  if (ret) {
    patchdetect->format = format;
    patchdetect->width = width;
    patchdetect->height = height;
  }

  return ret;
}

static gboolean
gst_patchdetect_start (GstBaseTransform * trans)
{

  return TRUE;
}

static gboolean
gst_patchdetect_stop (GstBaseTransform * trans)
{

  return TRUE;
}

static gboolean
gst_patchdetect_event (GstBaseTransform * trans, GstEvent * event)
{

  return TRUE;
}

typedef struct
{
  guint8 *y;
  int ystride;
  guint8 *u;
  int ustride;
  guint8 *v;
  int vstride;
  int width;
  int height;
  int t;
} Frame;

typedef struct
{
  int y, u, v;
  int diff_y, diff_u, diff_v;
  gboolean match;
  int patch_block;
  int color;
  int count;
  int sum_x;
  int sum_y;
} Stats;

typedef struct
{
  int r, g, b;
  int y, u, v;
} Color;

typedef struct
{
  int x, y;
  int patch1, patch2;
  gboolean valid;
} Point;

typedef struct
{
  int xmin, xmax;
  int ymin, ymax;
  int val;
  int y, u, v;
  int count;
  int cen_x, cen_y;
  gboolean valid;
} Patch;

static Color patch_colors[24] = {
  {115, 82, 68, 92, 119, 143},
  {194, 150, 130, 152, 115, 148},
  {98, 122, 157, 119, 146, 116},
  {87, 108, 67, 102, 112, 120},
  {133, 128, 177, 130, 149, 128},
  {103, 189, 170, 161, 128, 91},
  {214, 126, 44, 135, 83, 170},
  {80, 91, 166, 97, 162, 120},
  {193, 90, 99, 113, 122, 173},
  {94, 60, 108, 77, 146, 141},
  {157, 188, 64, 164, 77, 119},
  {224, 163, 46, 160, 70, 160},
  {56, 61, 150, 73, 168, 122},
  {70, 148, 73, 124, 103, 97},
  {175, 54, 60, 85, 118, 181},
  {231, 199, 31, 182, 51, 149},
  {187, 86, 149, 112, 146, 170},
  {8, 133, 161, 109, 153, 72},
  {243, 243, 243, 225, 128, 128},
  {200, 200, 200, 188, 128, 128},
  {160, 160, 160, 153, 128, 128},
  {122, 122, 122, 121, 128, 128},
  {85, 85, 85, 89, 128, 128},
  {52, 52, 52, 61, 128, 128}
};

static void
get_block_stats (Frame * frame, int x, int y, Stats * stats)
{
  int i, j;
  guint8 *data;
  int max;
  int min;
  int sum;

  max = 0;
  min = 255;
  sum = 0;
  for (j = 0; j < 8; j++) {
    data = frame->y + frame->ystride * (j + y) + x;
    for (i = 0; i < 8; i++) {
      max = MAX (max, data[i]);
      min = MIN (min, data[i]);
      sum += data[i];
    }
  }
  stats->y = sum / 64;
  stats->diff_y = MAX (max - stats->y, stats->y - min);

  max = 0;
  min = 255;
  sum = 0;
  for (j = 0; j < 4; j++) {
    data = frame->u + frame->ustride * (j + y / 2) + x / 2;
    for (i = 0; i < 4; i++) {
      max = MAX (max, data[i]);
      min = MIN (min, data[i]);
      sum += data[i];
    }
  }
  stats->u = sum / 16;
  stats->diff_u = MAX (max - stats->u, stats->u - min);

  max = 0;
  min = 255;
  sum = 0;
  for (j = 0; j < 4; j++) {
    data = frame->v + frame->vstride * (j + y / 2) + x / 2;
    for (i = 0; i < 4; i++) {
      max = MAX (max, data[i]);
      min = MIN (min, data[i]);
      sum += data[i];
    }
  }
  stats->v = sum / 16;
  stats->diff_v = MAX (max - stats->v, stats->v - min);

  stats->patch_block = -1;
  stats->match = FALSE;
#define MATCH 15
  if (stats->diff_y < MATCH && stats->diff_u < MATCH && stats->diff_v < MATCH) {
    stats->match = TRUE;
  }
}

static void
paint_block (Frame * frame, int x, int y, int value)
{
  int i, j;
  guint8 *data;

  for (j = 0; j < 8; j++) {
    data = frame->y + frame->ystride * (j + y) + x;
    for (i = 0; i < 8; i++) {
      data[i] = value;
    }
  }

  for (j = 0; j < 4; j++) {
    data = frame->u + frame->ustride * (j + y / 2) + x / 2;
    for (i = 0; i < 4; i++) {
      data[i] = 128;
    }
  }

  for (j = 0; j < 4; j++) {
    data = frame->v + frame->vstride * (j + y / 2) + x / 2;
    for (i = 0; i < 4; i++) {
      data[i] = 128;
    }
  }
}

static gboolean
patch_check (Frame * frame, guint8 * patchpix, int x, int y, int w, int h)
{
  int i, j;

  for (j = y; j < y + h; j++) {
    for (i = x; i < x + w; i++) {
      if (patchpix[j * frame->width + i] != 0)
        return FALSE;
    }
  }

  return TRUE;
}

static void
patch_start (Frame * frame, guint8 * patchpix, Patch * patch, int x, int y,
    int w, int h)
{
  int i, j;

  for (j = y; j < y + h; j++) {
    for (i = x; i < x + w; i++) {
      patchpix[j * frame->width + i] = patch->val;
    }
  }
  patch->xmin = MAX (1, x - 1);
  patch->xmax = MIN (x + w + 1, frame->width - 1);
  patch->ymin = MAX (1, y - 1);
  patch->ymax = MIN (y + h + 1, frame->height - 1);
  patch->count = w * h;

}

static void
patch_grow (Frame * frame, guint8 * patchpix, Patch * patch)
{
  gboolean growmore = FALSE;
  guint8 *ydata, *udata, *vdata;
  int i, j;
  int count = 5;

#define MAXDIFF 15
  do {
    for (j = patch->ymin; j < patch->ymax; j++) {
      ydata = frame->y + frame->ystride * j;
      udata = frame->u + frame->ustride * (j / 2);
      vdata = frame->v + frame->vstride * (j / 2);
      for (i = patch->xmin; i < patch->xmax; i++) {
        if (patchpix[j * frame->width + i] != 0)
          continue;

        if (patchpix[(j + 1) * frame->width + i] == patch->val ||
            patchpix[(j - 1) * frame->width + i] == patch->val ||
            patchpix[j * frame->width + i + 1] == patch->val ||
            patchpix[j * frame->width + i - 1] == patch->val) {
          int diff = ABS (ydata[i] - patch->y) +
              ABS (udata[i / 2] - patch->u) + ABS (vdata[i / 2] - patch->v);

          if (diff < MAXDIFF) {
            patchpix[j * frame->width + i] = patch->val;
            patch->xmin = MIN (patch->xmin, MAX (i - 1, 1));
            patch->xmax = MAX (patch->xmax, MIN (i + 2, frame->width - 1));
            patch->ymin = MIN (patch->ymin, MAX (j - 1, 1));
            patch->ymax = MAX (patch->ymax, MIN (j + 2, frame->height - 1));
            patch->count++;
            growmore = TRUE;
          }
        }
      }
    }
    for (j = patch->ymax - 1; j >= patch->ymin; j--) {
      ydata = frame->y + frame->ystride * j;
      udata = frame->u + frame->ustride * (j / 2);
      vdata = frame->v + frame->vstride * (j / 2);
      for (i = patch->xmax - 1; i >= patch->xmin; i--) {
        if (patchpix[j * frame->width + i] != 0)
          continue;

        if (patchpix[(j + 1) * frame->width + i] == patch->val ||
            patchpix[(j - 1) * frame->width + i] == patch->val ||
            patchpix[j * frame->width + i + 1] == patch->val ||
            patchpix[j * frame->width + i - 1] == patch->val) {
          int diff = ABS (ydata[i] - patch->y) +
              ABS (udata[i / 2] - patch->u) + ABS (vdata[i / 2] - patch->v);

          if (diff < MAXDIFF) {
            patchpix[j * frame->width + i] = patch->val;
            patch->xmin = MIN (patch->xmin, MAX (i - 1, 1));
            patch->xmax = MAX (patch->xmax, MIN (i + 2, frame->width - 1));
            patch->ymin = MIN (patch->ymin, MAX (j - 1, 1));
            patch->ymax = MAX (patch->ymax, MIN (j + 2, frame->height - 1));
            patch->count++;
            growmore = TRUE;
          }
        }
      }
    }

    count--;
  } while (growmore && count > 0);

#if 0
  for (j = patch->ymin; j < patch->ymax; j++) {
    guint8 *data;
    data = frame->y + frame->ystride * j;
    for (i = patch->xmin; i < patch->xmax; i++) {
      if (patchpix[j * frame->width + i] != patch->val)
        continue;
      if ((i + j + frame->t) & 0x4) {
        data[i] = 16;
      }
    }
  }
#endif

}

#if 0
static void
find_cluster (Point * points, int n_points, int *result_x, int *result_y)
{
  int dist;
  int ave_x, ave_y;
  int i;

  for (dist = 50; dist >= 10; dist -= 5) {
    int sum_x, sum_y;
    int n_valid;

    sum_x = 0;
    sum_y = 0;
    n_valid = 0;
    for (i = 0; i < n_points; i++) {
      if (!points[i].valid)
        continue;
      sum_x += points[i].x;
      sum_y += points[i].y;
      n_valid++;
    }
    ave_x = sum_x / n_valid;
    ave_y = sum_y / n_valid;

    for (i = 0; i < n_points; i++) {
      int d;
      if (!points[i].valid)
        continue;
      d = (points[i].x - ave_x) * (points[i].x - ave_x);
      d += (points[i].y - ave_y) * (points[i].y - ave_y);
      if (d > dist * dist)
        points[i].valid = FALSE;
    }
  }
  *result_x = ave_x;
  *result_y = ave_y;
}
#endif

typedef struct _Matrix Matrix;
struct _Matrix
{
  double m[4][4];
};

#if 0
static void
dump_4x4 (double a[4][4], double b[4][4])
{
  int j;
  int i;

  for (j = 0; j < 4; j++) {
    g_print ("[ ");
    for (i = 0; i < 4; i++) {
      g_print ("%8.2g", a[i][j]);
      if (i != 4 - 1)
        g_print (", ");
    }
    g_print ("|");
    for (i = 0; i < 4; i++) {
      g_print ("%8.2g", b[i][j]);
      if (i != 4 - 1)
        g_print (", ");
    }
    g_print ("]\n");
  }
  g_print ("\n");

}
#endif

static void
invert_matrix (double m[10][10], int n)
{
  int i, j, k;
  double tmp[10][10] = { {0} };
  double x;

  for (i = 0; i < n; i++) {
    tmp[i][i] = 1;
  }

  for (j = 0; j < n; j++) {
    for (k = 0; k < n; k++) {
      if (k == j)
        continue;

      x = m[j][k] / m[j][j];
      for (i = 0; i < n; i++) {
        m[i][k] -= x * m[i][j];
        tmp[i][k] -= x * tmp[i][j];
      }
    }

    x = m[j][j];
    for (i = 0; i < n; i++) {
      m[i][j] /= x;
      tmp[i][j] /= x;
    }
  }

  memcpy (m, tmp, sizeof (tmp));
}

static GstFlowReturn
gst_patchdetect_transform_ip (GstBaseTransform * trans, GstBuffer * buf)
{
  GstPatchdetect *patchdetect = GST_PATCHDETECT (trans);
  Frame frame;
  Point *points;
  int i, j;
  int blocks_x, blocks_y;
  int n_points;
  int n_patches;
  Patch *patches;
  guint8 *patchpix;
  int vec1_x, vec1_y;
  int vec2_x, vec2_y;
  Color detected_colors[24];
  gboolean detected = FALSE;

  frame.y = GST_BUFFER_DATA (buf);
  frame.ystride = gst_video_format_get_row_stride (patchdetect->format,
      0, patchdetect->width);
  frame.u =
      frame.y + gst_video_format_get_component_offset (patchdetect->format, 1,
      patchdetect->width, patchdetect->height);
  frame.ustride =
      gst_video_format_get_row_stride (patchdetect->format, 1,
      patchdetect->width);
  frame.v =
      frame.y + gst_video_format_get_component_offset (patchdetect->format, 2,
      patchdetect->width, patchdetect->height);
  frame.vstride =
      gst_video_format_get_row_stride (patchdetect->format, 2,
      patchdetect->width);
  frame.width = patchdetect->width;
  frame.height = patchdetect->height;
  frame.t = patchdetect->t;
  patchdetect->t++;

  blocks_y = (patchdetect->height & (~7)) / 8;
  blocks_x = (patchdetect->width & (~7)) / 8;

  patchpix = g_malloc0 (patchdetect->width * patchdetect->height);
  patches = g_malloc0 (sizeof (Patch) * 256);

  n_patches = 0;
  for (j = 0; j < blocks_y; j += 4) {
    for (i = 0; i < blocks_x; i += 4) {
      Stats block = { 0 };

      get_block_stats (&frame, i * 8, j * 8, &block);

      patches[n_patches].val = n_patches + 2;
      if (block.match) {
        if (patch_check (&frame, patchpix, i * 8, j * 8, 8, 8)) {
          patch_start (&frame, patchpix, patches + n_patches, i * 8, j * 8, 8,
              8);

          patches[n_patches].y = block.y;
          patches[n_patches].u = block.u;
          patches[n_patches].v = block.v;

          patch_grow (&frame, patchpix, patches + n_patches);
          n_patches++;
          g_assert (n_patches < 256);
        }
      }
    }
  }

  {
    int n;

    for (n = 0; n < n_patches; n++) {
      Patch *patch = &patches[n];
      int xsum;
      int ysum;

      if (patch->count > 10000)
        continue;
      patch->valid = TRUE;

      xsum = 0;
      ysum = 0;
      for (j = patch->ymin; j < patch->ymax; j++) {
        for (i = patch->xmin; i < patch->xmax; i++) {
          if (patchpix[j * frame.width + i] != patch->val)
            continue;
          xsum += i;
          ysum += j;
        }
      }

      patch->cen_x = xsum / patch->count;
      patch->cen_y = ysum / patch->count;
    }

  }

  points = g_malloc0 (sizeof (Point) * 1000);
  n_points = 0;

  for (i = 0; i < n_patches; i++) {
    for (j = i + 1; j < n_patches; j++) {
      int dist_x, dist_y;

      if (i == j)
        continue;

      dist_x = patches[i].cen_x - patches[j].cen_x;
      dist_y = patches[i].cen_y - patches[j].cen_y;

      if (dist_x < 0) {
        dist_x = -dist_x;
        dist_y = -dist_y;
      }
      if (ABS (2 * dist_y) < dist_x && dist_x < 100) {
        points[n_points].x = dist_x;
        points[n_points].y = dist_y;
        points[n_points].valid = TRUE;
        points[n_points].patch1 = i;
        points[n_points].patch2 = j;
        n_points++;
        g_assert (n_points < 1000);
      }
    }
  }

  {
    int dist;
    int ave_x = 0, ave_y = 0;
    for (dist = 50; dist >= 10; dist -= 5) {
      int sum_x, sum_y;
      int n_valid;

      sum_x = 0;
      sum_y = 0;
      n_valid = 0;
      for (i = 0; i < n_points; i++) {
        if (!points[i].valid)
          continue;
        sum_x += points[i].x;
        sum_y += points[i].y;
        n_valid++;
      }
      if (n_valid == 0)
        continue;
      ave_x = sum_x / n_valid;
      ave_y = sum_y / n_valid;

      for (i = 0; i < n_points; i++) {
        int d;
        if (!points[i].valid)
          continue;
        d = (points[i].x - ave_x) * (points[i].x - ave_x);
        d += (points[i].y - ave_y) * (points[i].y - ave_y);
        if (d > dist * dist)
          points[i].valid = FALSE;
      }
    }
    vec1_x = ave_x;
    vec1_y = ave_y;
  }

  n_points = 0;
  for (i = 0; i < n_patches; i++) {
    for (j = i + 1; j < n_patches; j++) {
      int dist_x, dist_y;

      if (i == j)
        continue;

      dist_x = patches[i].cen_x - patches[j].cen_x;
      dist_y = patches[i].cen_y - patches[j].cen_y;

      if (dist_y < 0) {
        dist_x = -dist_x;
        dist_y = -dist_y;
      }
      if (ABS (2 * dist_x) < dist_y && dist_y < 100) {
        points[n_points].x = dist_x;
        points[n_points].y = dist_y;
        points[n_points].valid = TRUE;
        points[n_points].patch1 = i;
        points[n_points].patch2 = j;
        n_points++;
        g_assert (n_points < 1000);
      }
    }
  }

  {
    int dist;
    int ave_x = 0, ave_y = 0;
    for (dist = 50; dist >= 10; dist -= 5) {
      int sum_x, sum_y;
      int n_valid;

      sum_x = 0;
      sum_y = 0;
      n_valid = 0;
      for (i = 0; i < n_points; i++) {
        if (!points[i].valid)
          continue;
        sum_x += points[i].x;
        sum_y += points[i].y;
        n_valid++;
      }
      if (n_valid == 0)
        continue;
      ave_x = sum_x / n_valid;
      ave_y = sum_y / n_valid;

      for (i = 0; i < n_points; i++) {
        int d;
        if (!points[i].valid)
          continue;
        d = (points[i].x - ave_x) * (points[i].x - ave_x);
        d += (points[i].y - ave_y) * (points[i].y - ave_y);
        if (d > dist * dist)
          points[i].valid = FALSE;
      }
    }
    vec2_x = ave_x;
    vec2_y = ave_y;
  }

#if 0
  for (i = 0; i < n_points; i++) {
    if (!points[i].valid)
      continue;
    paint_block (&frame, 4 * points[i].x, 240 + 4 * points[i].y, 16);
  }
#endif
#if 0
  paint_block (&frame, 360, 240, 16);
  paint_block (&frame, 360 + vec1_x, 240 + vec1_y, 16);
  paint_block (&frame, 360 + vec2_x, 240 + vec2_y, 16);
#endif

  {
    double m00, m01, m10, m11;
    double det;
    double v1, v2;
    double ave_v1 = 0, ave_v2 = 0;

    det = vec1_x * vec2_y - vec1_y * vec2_x;
    m00 = vec2_y / det;
    m01 = -vec2_x / det;
    m10 = -vec1_y / det;
    m11 = vec1_x / det;

    for (i = 0; i < n_patches - 1; i++) {
      int count = 0;
      double sum_v1 = 0;
      double sum_v2 = 0;

      if (!patches[i].valid)
        continue;

      n_points = 0;
      for (j = i + 1; j < n_patches; j++) {
        int diff_x = patches[j].cen_x - patches[i].cen_x;
        int diff_y = patches[j].cen_y - patches[i].cen_y;

        if (!patches[j].valid)
          continue;

        v1 = diff_x * m00 + diff_y * m01;
        v2 = diff_x * m10 + diff_y * m11;

        if (v1 > -0.5 && v1 < 5.5 && v2 > -0.5 && v2 < 3.5 &&
            ABS (v1 - rint (v1)) < 0.1 && ABS (v2 - rint (v2)) < 0.1) {
          sum_v1 += v1 - rint (v1);
          sum_v2 += v2 - rint (v2);
          count++;
        }
      }
      ave_v1 = sum_v1 / count;
      ave_v2 = sum_v2 / count;

      if (count > 20) {
        int k;
        for (j = 0; j < 4; j++) {
          for (k = 0; k < 6; k++) {
            Stats block;

            int xx;
            int yy;
            xx = patches[i].cen_x + (ave_v1 + k) * vec1_x + (ave_v2 +
                j) * vec2_x;
            yy = patches[i].cen_y + (ave_v1 + k) * vec1_y + (ave_v2 +
                j) * vec2_y;

            get_block_stats (&frame, xx - 4, yy - 4, &block);
            //GST_ERROR("%d %d: %d %d %d", k, j, block.y, block.u, block.v);

            detected_colors[k + j * 6].y = block.y;
            detected_colors[k + j * 6].u = block.u;
            detected_colors[k + j * 6].v = block.v;

            paint_block (&frame, xx - 4, yy - 4, 16);
          }
        }

        detected = TRUE;

#if 0
        for (j = i + 1; j < n_patches; j++) {
          int diff_x = patches[j].cen_x - patches[i].cen_x;
          int diff_y = patches[j].cen_y - patches[i].cen_y;
          int xx;
          int yy;

          if (!patches[j].valid)
            continue;

          v1 = diff_x * m00 + diff_y * m01;
          v2 = diff_x * m10 + diff_y * m11;

          if (v1 > -0.5 && v1 < 5.5 && v2 > -0.5 && v2 < 3.5 &&
              ABS (v1 - rint (v1)) < 0.1 && ABS (v2 - rint (v2)) < 0.1) {
            v1 = rint (v1);
            v2 = rint (v2);
            xx = patches[i].cen_x + (ave_v1 + v1) * vec1_x + (ave_v2 +
                v2) * vec2_x;
            yy = patches[i].cen_y + (ave_v1 + v1) * vec1_y + (ave_v2 +
                v2) * vec2_y;

            paint_block (&frame, patches[j].cen_x, patches[j].cen_y, 128);
            paint_block (&frame, xx, yy, 16);
          }
        }
        paint_block (&frame, patches[i].cen_x, patches[i].cen_y, 240);
#endif
        break;
      }
    }
  }

#define N 10
  if (detected) {
    int i, j, k;
    int n = N;
    double diff = 0;
    double matrix[10][10] = { {0} };
    double vy[10] = { 0 };
    double vu[10] = { 0 };
    double vv[10] = { 0 };
    double *by = patchdetect->by;
    double *bu = patchdetect->bu;
    double *bv = patchdetect->bv;
    double flip_diff = 0;

    for (i = 0; i < 24; i++) {
      diff += ABS (detected_colors[i].y - patch_colors[i].y);
      diff += ABS (detected_colors[i].u - patch_colors[i].u);
      diff += ABS (detected_colors[i].v - patch_colors[i].v);

      flip_diff += ABS (detected_colors[23 - i].y - patch_colors[i].y);
      flip_diff += ABS (detected_colors[23 - i].u - patch_colors[i].u);
      flip_diff += ABS (detected_colors[23 - i].v - patch_colors[i].v);
    }
    GST_ERROR ("uncorrected error %g (flipped %g)", diff / 24.0,
        flip_diff / 24.0);
    if (flip_diff < diff) {
      for (i = 0; i < 12; i++) {
        Color tmp;
        tmp = detected_colors[i];
        detected_colors[i] = detected_colors[23 - i];
        detected_colors[23 - i] = tmp;
      }
    }

    for (i = 0; i < 24; i++) {
      int dy = detected_colors[i].y - patch_colors[i].y;
      int du = detected_colors[i].u - patch_colors[i].u;
      int dv = detected_colors[i].v - patch_colors[i].v;
      int py = detected_colors[i].y - 128;
      int pu = detected_colors[i].u - 128;
      int pv = detected_colors[i].v - 128;
      int w = (i < 18) ? 1 : 2;
      double z[10];

      diff += ABS (dy) + ABS (du) + ABS (dv);

      z[0] = 1;
      z[1] = py;
      z[2] = pu;
      z[3] = pv;
      z[4] = py * py;
      z[5] = py * pu;
      z[6] = py * pv;
      z[7] = pu * pu;
      z[8] = pu * pv;
      z[9] = pv * pv;

      for (j = 0; j < n; j++) {
        for (k = 0; k < n; k++) {
          matrix[j][k] += w * z[j] * z[k];
        }

        vy[j] += w * dy * z[j];
        vu[j] += w * du * z[j];
        vv[j] += w * dv * z[j];
      }
    }

    invert_matrix (matrix, n);

    for (i = 0; i < n; i++) {
      by[i] = 0;
      bu[i] = 0;
      bv[i] = 0;
      for (j = 0; j < n; j++) {
        by[i] += matrix[i][j] * vy[j];
        bu[i] += matrix[i][j] * vu[j];
        bv[i] += matrix[i][j] * vv[j];
      }
    }

    //GST_ERROR("a %g %g %g b %g %g %g", ay, au, av, by, bu, bv);

    diff = 0;
    for (i = 0; i < 24; i++) {
      double cy, cu, cv;
      double z[10];
      int py = detected_colors[i].y - 128;
      int pu = detected_colors[i].u - 128;
      int pv = detected_colors[i].v - 128;

      z[0] = 1;
      z[1] = py;
      z[2] = pu;
      z[3] = pv;
      z[4] = py * py;
      z[5] = py * pu;
      z[6] = py * pv;
      z[7] = pu * pu;
      z[8] = pu * pv;
      z[9] = pv * pv;

      cy = 0;
      cu = 0;
      cv = 0;
      for (j = 0; j < n; j++) {
        cy += by[j] * z[j];
        cu += bu[j] * z[j];
        cv += bv[j] * z[j];
      }

      diff += fabs (patch_colors[i].y - (128 + py - cy));
      diff += fabs (patch_colors[i].u - (128 + pu - cu));
      diff += fabs (patch_colors[i].v - (128 + pv - cv));
    }
    GST_ERROR ("average error %g", diff / 24.0);
    patchdetect->valid = 3000;
  }

  if (patchdetect->valid > 0) {
    int n = N;
    guint8 *u1, *u2;
    guint8 *v1, *v2;
    double *by = patchdetect->by;
    double *bu = patchdetect->bu;
    double *bv = patchdetect->bv;

    patchdetect->valid--;
    u1 = g_malloc (frame.width);
    u2 = g_malloc (frame.width);
    v1 = g_malloc (frame.width);
    v2 = g_malloc (frame.width);

    for (j = 0; j < frame.height; j += 2) {
      for (i = 0; i < frame.width / 2; i++) {
        u1[2 * i + 0] = frame.u[(j / 2) * frame.ustride + i];
        u1[2 * i + 1] = u1[2 * i + 0];
        u2[2 * i + 0] = u1[2 * i + 0];
        u2[2 * i + 1] = u1[2 * i + 0];
        v1[2 * i + 0] = frame.v[(j / 2) * frame.vstride + i];
        v1[2 * i + 1] = v1[2 * i + 0];
        v2[2 * i + 0] = v1[2 * i + 0];
        v2[2 * i + 1] = v1[2 * i + 0];
      }
      for (i = 0; i < frame.width; i++) {
        int k;
        double z[10];
        double cy, cu, cv;
        int y, u, v;
        int py, pu, pv;

        y = frame.y[(j + 0) * frame.ystride + i];
        u = u1[i];
        v = v1[i];

        py = y - 128;
        pu = u - 128;
        pv = v - 128;

        z[0] = 1;
        z[1] = py;
        z[2] = pu;
        z[3] = pv;
        z[4] = py * py;
        z[5] = py * pu;
        z[6] = py * pv;
        z[7] = pu * pu;
        z[8] = pu * pv;
        z[9] = pv * pv;

        cy = 0;
        cu = 0;
        cv = 0;
        for (k = 0; k < n; k++) {
          cy += by[k] * z[k];
          cu += bu[k] * z[k];
          cv += bv[k] * z[k];
        }

        frame.y[(j + 0) * frame.ystride + i] = CLAMP (rint (y - cy), 0, 255);
        u1[i] = CLAMP (rint (u - cu), 0, 255);
        v1[i] = CLAMP (rint (v - cv), 0, 255);

        y = frame.y[(j + 1) * frame.ystride + i];
        u = u2[i];
        v = v2[i];

        py = y - 128;
        pu = u - 128;
        pv = v - 128;

        z[0] = 1;
        z[1] = py;
        z[2] = pu;
        z[3] = pv;
        z[4] = py * py;
        z[5] = py * pu;
        z[6] = py * pv;
        z[7] = pu * pu;
        z[8] = pu * pv;
        z[9] = pv * pv;

        cy = 0;
        cu = 0;
        cv = 0;
        for (k = 0; k < n; k++) {
          cy += by[k] * z[k];
          cu += bu[k] * z[k];
          cv += bv[k] * z[k];
        }

        frame.y[(j + 1) * frame.ystride + i] = CLAMP (rint (y - cy), 0, 255);
        u2[i] = CLAMP (rint (u - cu), 0, 255);
        v2[i] = CLAMP (rint (v - cv), 0, 255);
      }
      for (i = 0; i < frame.width / 2; i++) {
        frame.u[(j / 2) * frame.ustride + i] = (u1[2 * i + 0] +
            u1[2 * i + 1] + u2[2 * i + 0] + u2[2 * i + 1] + 2) >> 2;
        frame.v[(j / 2) * frame.vstride + i] = (v1[2 * i + 0] +
            v1[2 * i + 1] + v2[2 * i + 0] + v2[2 * i + 1] + 2) >> 2;
      }
    }

    g_free (u1);
    g_free (u2);
    g_free (v1);
    g_free (v2);
  }

  g_free (points);
  g_free (patches);
  g_free (patchpix);

  return GST_FLOW_OK;
}

static gboolean
gst_patchdetect_src_event (GstBaseTransform * trans, GstEvent * event)
{

  return TRUE;
}

static gboolean
plugin_init (GstPlugin * plugin)
{

  gst_element_register (plugin, "patchdetect", GST_RANK_NONE,
      gst_patchdetect_get_type ());

  return TRUE;
}

GST_PLUGIN_DEFINE (GST_VERSION_MAJOR,
    GST_VERSION_MINOR,
    "patchdetect",
    "patchdetect element",
    plugin_init, VERSION, "LGPL", PACKAGE_NAME, GST_PACKAGE_ORIGIN)