/*
 *  motion.c:           Motion compensation code
 *
 *  Written by:         Ullrich Hafner
 *                      Michael Unger
 *
 *  This file is part of FIASCO (Fractal Image And Sequence COdec)
 *  Copyright (C) 1994-2000 Ullrich Hafner
 */

/*
 *  $Date: 2000/06/14 20:50:51 $
 *  $Author: hafner $
 *  $Revision: 5.1 $
 *  $State: Exp $
 */

#include "config.h"

#include <string.h>

#include "pm_c_util.h"

#include "types.h"
#include "macros.h"
#include "error.h"

#include "image.h"
#include "misc.h"
#include "motion.h"

/*****************************************************************************

                                public code

*****************************************************************************/

void
restore_mc (int enlarge_factor, image_t *image, const image_t *past,
            const image_t *future, const wfa_t *wfa)
/*
 *  Restore motion compensated prediction of 'image' represented by 'wfa'.
 *  If 'enlarge_factor' != 0 then enlarge image by given amount.
 *  Reference frames are given by 'past' and 'future'.
 *
 *  No return values.
 */
{
   unsigned  state, label;
   unsigned  root_state;
   word_t   *mcblock1, *mcblock2;       /* MC blocks */

#define FX(v) ((image->format == FORMAT_4_2_0) && band != Y ? ((v) / 2) : v)

   mcblock1 = Calloc (size_of_level (MAX((int) wfa->wfainfo->p_max_level
                                          + 2 * enlarge_factor, 0)),
                      sizeof (word_t));
   mcblock2 = Calloc (size_of_level (MAX((int) wfa->wfainfo->p_max_level
                                          + 2 * enlarge_factor, 0)),
                      sizeof (word_t));

   if (!image->color)
      root_state = wfa->root_state;
   else
      root_state  = wfa->tree [wfa->tree [wfa->root_state][0]][0];

   for (state = wfa->basis_states; state <= root_state; state++)
      for (label = 0; label < MAXLABELS; label++)
         if (wfa->mv_tree[state][label].type != NONE)
         {
            color_e band;
            unsigned level  = wfa->level_of_state [state] - 1;
            unsigned width  = width_of_level (level);
            unsigned height = height_of_level (level);
            unsigned offset = image->width - width;

            switch (wfa->mv_tree [state][label].type)
            {
               case FORWARD:
                  for (band  = first_band (image->color);
                       band <= last_band (image->color); band++)
                  {
                     extract_mc_block (mcblock1, FX (width), FX (height),
                                       past->pixels [band], FX (past->width),
                                       wfa->wfainfo->half_pixel,
                                       FX (wfa->x [state][label]),
                                       FX (wfa->y [state][label]),
                                       FX (wfa->mv_tree [state][label].fx),
                                       FX (wfa->mv_tree [state][label].fy));
                     {
                        word_t   *mc1;  /* current pixel in MC block */
                        word_t   *orig; /* current pixel in original image */
                        unsigned  x, y; /* pixel coordinates */

                        mc1  = mcblock1;
                        orig = (word_t *) image->pixels [band]
                               + FX (wfa->x[state][label])
                               + FX (wfa->y[state][label]) * FX (image->width);

                        for (y = FX (height); y; y--)
                        {
                           for (x = FX (width); x; x--)
                              *orig++ += *mc1++;

                           orig += FX (offset);
                        }
                     }
                  }
                  break;
               case BACKWARD:
                  for (band  = first_band (image->color);
                       band <= last_band (image->color); band++)
                  {
                     extract_mc_block (mcblock1, FX (width), FX (height),
                                       future->pixels [band],
                                       FX (future->width),
                                       wfa->wfainfo->half_pixel,
                                       FX (wfa->x [state][label]),
                                       FX (wfa->y [state][label]),
                                       FX (wfa->mv_tree [state][label].bx),
                                       FX (wfa->mv_tree [state][label].by));
                     {
                        word_t   *mc1;  /* current pixel in MC block 1 */
                        word_t   *orig; /* current pixel in original image */
                        unsigned  x, y; /* pixel coordinates */

                        mc1  = mcblock1;
                        orig = (word_t *) image->pixels [band]
                               + FX (wfa->x[state][label])
                               + FX (wfa->y[state][label]) * FX (image->width);

                        for (y = FX (height); y; y--)
                        {
                           for (x = FX (width); x; x--)
                              *orig++ += *mc1++;

                           orig += FX (offset);
                        }
                     }
                  }
                  break;
               case INTERPOLATED:
                  for (band  = first_band (image->color);
                       band <= last_band (image->color); band++)
                  {
                     extract_mc_block (mcblock1, FX (width), FX (height),
                                       past->pixels [band], FX (past->width),
                                       wfa->wfainfo->half_pixel,
                                       FX (wfa->x[state][label]),
                                       FX (wfa->y[state][label]),
                                       FX (wfa->mv_tree[state][label].fx),
                                       FX (wfa->mv_tree[state][label].fy));
                     extract_mc_block (mcblock2, FX (width), FX (height),
                                       future->pixels [band],
                                       FX (future->width),
                                       wfa->wfainfo->half_pixel,
                                       FX (wfa->x[state][label]),
                                       FX (wfa->y[state][label]),
                                       FX (wfa->mv_tree[state][label].bx),
                                       FX (wfa->mv_tree[state][label].by));
                     {
                        word_t   *mc1;  /* current pixel in MC block 1 */
                        word_t   *mc2;  /* current pixel in MC block 1 */
                        word_t   *orig; /* current pixel in original image */
                        unsigned  x, y; /* pixel coordinates */

                        mc1  = mcblock1;
                        mc2  = mcblock2;
                        orig = (word_t *) image->pixels [band]
                               + FX (wfa->x[state][label])
                               + FX (wfa->y[state][label]) * FX (image->width);

                        for (y = FX (height); y; y--)
                        {
                           for (x = FX (width); x; x--)
#ifdef HAVE_SIGNED_SHIFT
                              *orig++ += (*mc1++ + *mc2++) >> 1;
#else /* not HAVE_SIGNED_SHIFT */
                           *orig++ += (*mc1++ + *mc2++) / 2;
#endif /* not HAVE_SIGNED_SHIFT */

                           orig += FX (offset);
                        }
                     }
                  }
                  break;
               default:
                  break;
            }
         }

   if (image->color)
   {
      unsigned    n;
      word_t     *ptr;
      static int *clipping = NULL;
      unsigned    shift    = image->format == FORMAT_4_2_0 ? 2 : 0;

      if (!clipping)                    /* initialize clipping table */
      {
         int i;

         clipping = Calloc (256 * 3, sizeof (int));
         for (i = -128; i < 128; i++)
            clipping [256 + i + 128] = i;
         for (i = 0; i < 256; i++)
            clipping [i] = clipping [256];
         for (i = 512; i < 512 + 256; i++)
            clipping [i] = clipping [511];
         clipping += 256 + 128;
      }

      ptr = image->pixels [Cb];
      for (n = (image->width * image->height) >> shift; n; n--, ptr++)
#ifdef HAVE_SIGNED_SHIFT
         *ptr = clipping [*ptr >> 4] << 4;
#else /* not HAVE_SIGNED_SHIFT */
         *ptr = clipping [*ptr / 16] * 16;
#endif /* not HAVE_SIGNED_SHIFT */
      ptr = image->pixels [Cr];
      for (n = (image->width * image->height) >> shift; n; n--, ptr++)
#ifdef HAVE_SIGNED_SHIFT
        *ptr = clipping [*ptr >> 4] << 4;
#else /* not HAVE_SIGNED_SHIFT */
        *ptr = clipping [*ptr / 16] * 16;
#endif /* not HAVE_SIGNED_SHIFT */
   }

   Free (mcblock1);
   Free (mcblock2);
}

void
extract_mc_block (word_t *mcblock, unsigned width, unsigned height,
                  const word_t *reference, unsigned ref_width,
                  bool_t half_pixel, unsigned xo, unsigned yo,
                  unsigned mx, unsigned my)
/*
 *  Extract motion compensation image 'mcblock' of size 'width'x'height'
 *  from 'reference' image (width is given by 'ref_width').
 *  Coordinates of reference block are given by ('xo' + 'mx', 'yo' + 'my').
 *  Use 'half_pixel' precision if specified.
 *
 *  No return value.
 *
 *  Side effects:
 *      'mcblock[]'     MCPE block is filled with reference pixels
 */
{
   if (!half_pixel)                     /* Fullpixel precision */
   {
      const word_t *rblock;             /* pointer to reference image */
      unsigned      y;                  /* current row */

      rblock  = reference + (yo + my) * ref_width + (xo + mx);
      for (y = height; y; y--)
      {
         memcpy (mcblock, rblock, width * sizeof (word_t));

         mcblock += width;
         rblock  += ref_width;
      }
   }
   else                                 /* Halfpixel precision */
   {
      unsigned      x, y;               /* current coordinates */
      unsigned      offset;             /* remaining pixels in row */
      const word_t *rblock;             /* pointer to reference image */
      const word_t *ryblock;            /* pointer to next line */
      const word_t *rxblock;            /* pointer to next column */
      const word_t *rxyblock;           /* pointer to next column & row */

      rblock   = reference + (yo + my / 2) * ref_width + (xo + mx / 2);
      ryblock  = rblock + ref_width;    /* pixel in next row */
      rxblock  = rblock + 1;            /* pixel in next column */
      rxyblock = ryblock + 1;           /* pixel in next row & column */
      offset   = ref_width - width;

      if ((mx & 1) == 0)
      {
         if ((my & 1) == 0)             /* Don't use halfpixel refinement */
            for (y = height; y; y--)
            {
               memcpy (mcblock, rblock, width * sizeof (word_t));

               mcblock += width;
               rblock  += ref_width;
            }
         else                           /* Halfpixel in y direction */
            for (y = height; y; y--)
            {
               for (x = width; x; x--)
#ifdef HAVE_SIGNED_SHIFT
                  *mcblock++ = (*rblock++ + *ryblock++) >> 1;
#else /* not HAVE_SIGNED_SHIFT */
                  *mcblock++ = (*rblock++ + *ryblock++) / 2;
#endif /* not HAVE_SIGNED_SHIFT */

               rblock  += offset;
               ryblock += offset;
            }
      }
      else
      {
         if ((my & 1) == 0)             /* Halfpixel in x direction */
            for (y = height; y; y--)
            {
               for (x = width; x; x--)
#ifdef HAVE_SIGNED_SHIFT
                  *mcblock++ = (*rblock++ + *rxblock++) >> 1;
#else /* not HAVE_SIGNED_SHIFT */
                  *mcblock++ = (*rblock++ + *rxblock++) / 2;
#endif /* not HAVE_SIGNED_SHIFT */

               rblock  += offset;
               rxblock += offset;
            }
         else                           /* Halfpixel in xy direction */
            for (y = height; y; y--)
            {
               for (x = width; x; x--)
#ifdef HAVE_SIGNED_SHIFT
                  *mcblock++ = (*rblock++ + *rxblock++
                                + *ryblock++ + *rxyblock++) >> 2;
#else /* not HAVE_SIGNED_SHIFT */
                  *mcblock++ = (*rblock++ + *rxblock++
                                + *ryblock++ + *rxyblock++) / 4;
#endif /* not HAVE_SIGNED_SHIFT */
               rblock   += offset;
               ryblock  += offset;
               rxblock  += offset;
               rxyblock += offset;
            }
      }
   }
}
