/************************************************************************
 * mjpeg functions                                                      *
 ************************************************************************/

#include "mjpeg.h"

/**
 * debug print
 */
void dbgprintf(int urgency, const char* format, ...) {
    va_list argp;
    if (urgency & VERBOSITY != 0) {
        va_start(argp, format);
        printf(format, argp);
    }
}

/**
 * unzigzag block
 */
int* unZigZag(int dest[64], int src[64]) {
  for (int i = 0; i < 64; i++) {
    dest[ZIGZAG_COEFFS[i]] = src[i] ;
  }
  return dest;
}

/**
 * unquantify block
 */
int* unquantify(int block[64], unsigned char qtable[64]) {
  for (int i = 0; i < 64; i++) {
      block[i] = block[i] * qtable[i];
  }
  return block;
}

/**
 *
 */
int intceil(int N, int D) {
   int i = N / D;
   if (N > D * i) i++;
   return i;
}

/**
 *
 */
int intfloor(int N, int D) {
   int i = N / D;
   if (N < D * i) i--;
   return i;
}

/**
 * transform JPEG number format into usual 2's complement format
 */
int reformat(unsigned long s, int good) {
    unsigned int st;
    if (!good)
        return 0;

    st = 1 << (good - 1); //2^(good - 1)
    if (s < st) {
        return (s + 1 + ((-1) << good));
    } else {
        return s;
    }
}

/**
 *
 */
inline int DESCALE(int x, int n) {
    return (x + (1 << (n - 1)) - (x < 0)) >> n;
}

/**
 *
 */
inline int ADD(int x, int y) {
  int mini = 0;
  int maxi = 0;
  int r = x + y;

  if (r > maxi)    maxi = r;
  if (r < mini)    mini = r;
  return r; //in effect: &0x0000FFFF
}

/**
 *
 */
inline int SUB(int x, int y) {
  int mini = 0;
  int maxi = 0;
  int r = x - y;

  if (r > maxi) maxi = r;
  if (r < mini) mini = r;
  return r; //in effect: &0x0000FFFF
}

/**
 *
 */
inline int CMUL(int c, int x) {
  int mini = 0;
  int maxi = 0;
  int r = c * x;
  // less accurate rounding here also works fine
  r = (r + (1 << (C_BITS - 1))) >> C_BITS;
  if (r > maxi)    maxi = r;
  if (r < mini)    mini = r;
  return r;
}

/**
 * rotate (x,y) over angle k * pi / 16 (counter-clockwise) and scale with f
 */
inline void rot(int f, int k, int x, int y, int *rx, int *ry) {
  int COS[2][8] = {
      {c0_1, c1_1, c2_1, c3_1, c4_1, c5_1, c6_1, c7_1},
      {c0_s2, c1_s2, c2_s2, c3_s2, c4_s2, c5_s2, c6_s2, c7_s2}
  };
#define Cos(k) COS[f][k]
#define Sin(k) Cos(8-k)
  *rx = SUB(CMUL(Cos(k), x), CMUL(Sin(k), y));
  *ry = ADD(CMUL(Sin(k), x), CMUL(Cos(k), y));
#undef Cos
#undef Sin
}


/**
 * inverse 1-D discrete cosine transform. The result Y is scaled
 * up by factor sqrt(8). original Loeffler algorithm
 */
inline void idct_1d(int *Y) {
    int z1[8], z2[8], z3[8];

    //stage 1
    but(Y[0], Y[4], z1[1], z1[0]);
    rot(1, 6, Y[2], Y[6], &z1[2], &z1[3]);
    but(Y[1], Y[7], z1[4], z1[7]);
    z1[5] = CMUL(sqrt2, Y[3]);
    z1[6] = CMUL(sqrt2, Y[5]);

    //stage 2
    but(z1[0], z1[3], z2[3], z2[0]);
    but(z1[1], z1[2], z2[2], z2[1]);
    but(z1[4], z1[6], z2[6], z2[4]);
    but(z1[7], z1[5], z2[5], z2[7]);

    //stage 3
    z3[0] = z2[0];
    z3[1] = z2[1];
    z3[2] = z2[2];
    z3[3] = z2[3];
    rot(0, 3, z2[4], z2[7], &z3[4], &z3[7]);
    rot(0, 1, z2[5], z2[6], &z3[5], &z3[6]);

    //final stage 4
    but(z3[0], z3[7], Y[7], Y[0]);
    but(z3[1], z3[6], Y[6], Y[1]);
    but(z3[2], z3[5], Y[5], Y[2]);
    but(z3[3], z3[4], Y[4], Y[3]);
}

/**
 * inverse 2-D discrete cosine transform
 */
void IDCT(int input[8][8], unsigned char output[8][8]) {
#define Y(i,j) Y[8*i+j]
#define X(i,j) (output[i][j])
    int Y[64];
    int k, l;
    int mini = INT_MAX2;
    int maxi = INT_MIN2;

    for (k = 0; k < 8; k++) { //pass 1: process rows.
        for (l = 0; l < 8; l++) { //prescale k-th row:
            Y(k, l) = SCALE(input[k][l], S_BITS);
        }
        idct_1d(&Y(k, 0)); //1-D IDCT on k-th row
        //result Y is scaled up by factor sqrt(8) * 2^S_BITS.
    }
    for (l = 0; l < 8; l++) { //pass 2: process columns.
      int Yc[8];

      for (k = 0; k < 8; k++) {
          Yc[k] = Y(k, l);
      }
      idct_1d(Yc); //1-D IDCT on l-th column
      for (k = 0; k < 8; k++) { //result is once more scaled up by a factor sqrt(8)
          int r = 128 + DESCALE(Yc[k], S_BITS + 3);
          r = r > 0 ? (r < 255 ? r : 255) : 0; //clip to 8 bits unsigned
          X(k, l) = r;
      }
    }
#undef X
#undef Y
}
