#include <math.h>
#include <stdio.h> // for debugging

#include "readloop-cvt.h"

/*
 * [ R' ]   [ to_rgb_R_Y to_rgb_R_Cr to_rgb_R_Cb ] [ Y'-to_rgb_Y0 ]
 * [ G' ] = [ to_rgb_G_Y to_rgb_G_Cr to_rgb_G_Cb ] [    Cr-128    ] / 65536
 * [ B' ]   [ to_rgb_B_Y to_rgb_B_Cr to_rgb_B_Cb ] [    Cb-128    ]
 */
static unsigned int to_rgb_R_Y;
static unsigned int to_rgb_R_Cb;
static unsigned int to_rgb_R_Cr;
static unsigned int to_rgb_G_Y;
static unsigned int to_rgb_G_Cb;
static unsigned int to_rgb_G_Cr;
static unsigned int to_rgb_B_Y;
static unsigned int to_rgb_B_Cb;
static unsigned int to_rgb_B_Cr;
static unsigned int to_rgb_Y0;

/*
 * In analog terms,
 *
 *	Y' = Kr R' + (1 - Kr - Kb) G' + Kb B'
 *	Pr = .5 (R' - Y') / (1 - Kr)
 *	Pb = .5 (B' - Y') / (1 - Kb)
 *
 * Expanding this to create the matrix form, we get, successively,
 *
 *	Y' = Kr R' + (1-Kr-Kb) G' + Kb B'
 *	Pr = .5 (R' - (Kr R' + (1-Kr-Kb) G' + Kb B')) / (1-Kr)
 *	Pb = .5 (B' - (Kr R' + (1-Kr-Kb) G' + Kb B')) / (1-Kb)
 *
 *	Y' = Kr R' + (1-Kr-Kb) G' + Kb B'
 *	Pr = .5 (R' - Kr R' - (1-Kr-Kb) G' - Kb B') / (1-Kr)
 *	Pb = .5 (B' - Kr R' - (1-Kr-Kb) G' - Kb B') / (1-Kb)
 *
 *	Y' = Kr R' + (1-Kr-Kb) G' + Kb B'
 *	Pr = ½ R' - ½(1-Kr-Kb)/(1-Kr) G' - ½Kb/(1-Kr) B'
 *	Pb = - ½Kr/(1-Kb) R' - ½(1-Kr-Kb)/(1-Kb) G' + ½ B'
 *
 *	[ Y' ]   [  Kr         1-Kr-Kb             Kb         ] [ R' ]
 *	[ Pr ] = [  ½          -½(1-Kr-Kb)/(1-Kr) -½Kb/(1-Kr) ] [ G' ]
 *	[ Pb ]   [ -½Kr/(1-Kb) -½(1-Kr-Kb)/(1-Kb)  ½          ] [ B' ]
 *
 * Because R'/G'/B' and Y'/Pr/Pb use the same scale (a range of 1,
 *  [0,1] for Y'/R'/G'/B' and [-½,½] for Pr/Pb) exactly the same matrix
 *  works for [0..255] Y'/R'/G'/B' and [-128..127] Cr/Cb.  When
 *  limited-range clipping is turned on, we have to adjust for it.
 *
 * In any case, the above is not the matrix we want, because it's to go
 *  _from_ RGB triples, not _to_ RGB triples.  We have to invert it.
 *  We do this numerically when the parameters change, because it's
 *  easier and they change seldom.
 */

/*
 * To invert the above matrix, we write (where Kg = 1 - Kr - Kb)
 *
 * [  Kr         Kg           Kb         1 0 0 ]
 * [  ½          -½Kg/(1-Kr) -½Kb/(1-Kr) 0 1 0 ]
 * [ -½Kr/(1-Kb) -½Kg/(1-Kb)  ½          0 0 1 ]
 *
 *  and then do arithmetic to convert the left half to an identity
 *  matrix, during which the right half becomes the inverse.  The
 *  invariant here is that, at any stage,
 *
 *                         [   R'   ]
 * [ 0 ]   [ · · · · · · ] [   G'   ]
 * [ 0 ] = [ · · · · · · ] [   B'   ]
 * [ 0 ]   [ · · · · · · ] [ Y0-Y'  ]
 *                         [ 128-Cr ]
 *                         [ 128-Cb ]
 */
void reset_cvt(const CVTPARAMS *p)
{
 double fY;
 double fC;
 double Kg;
 double M[6][3];
 double t;
 int i;
 int j;

#ifdef CMAT_DEBUG
 void dumpM(void)
  { printf("%10g %10g %10g | %10g %10g %10g\n",M[0][0],M[1][0],M[2][0],M[3][0],M[4][0],M[5][0]);
    printf("%10g %10g %10g | %10g %10g %10g\n",M[0][1],M[1][1],M[2][1],M[3][1],M[4][1],M[5][1]);
    printf("%10g %10g %10g | %10g %10g %10g\n",M[0][2],M[1][2],M[2][2],M[3][2],M[4][2],M[5][2]);
  }
#endif

 fY = (p->maxY + 1 - p->minY) / 256.0;
 fC = p->maxCC / 127.0;
 Kg = 1 - p->Kr - p->Kb;
 M[0][0] = fY * p->Kr;
 M[1][0] = fY * Kg;
 M[2][0] = fY * p->Kb;
 M[3][0] = 1;
 M[4][0] = 0;
 M[5][0] = 0;
 M[0][1] = fC * .5;
 M[1][1] = fC * -.5 * Kg / (1 - p->Kr);
 M[2][1] = fC * -.5 * p->Kb / (1 - p->Kr);
 M[3][1] = 0;
 M[4][1] = 1;
 M[5][1] = 0;
 M[0][2] = fC * -.5 * p->Kr / (1 - p->Kb);
 M[1][2] = fC * -.5 * Kg / (1 - p->Kb);
 M[2][2] = fC * .5;
 M[3][2] = 0;
 M[4][2] = 0;
 M[5][2] = 1;
#ifdef CMAT_DEBUG
 dumpM();
#endif
 j = 0;
 if (fabs(M[0][1]) > fabs(M[0][0])) j = 1;
 if (fabs(M[0][2]) > fabs(M[0][j])) j = 2;
#ifdef CMAT_DEBUG
 printf("using row %d\n",j);
#endif
 if (j != 0)
  { for (i=6-1;i>=0;i--)
     { t = M[i][0];
       M[i][0] = M[i][j];
       M[i][j] = t;
     }
  }
#ifdef CMAT_DEBUG
 dumpM();
#endif
 t = M[0][0];
 for (i=6-1;i>=0;i--) M[i][0] /= t;
 t = M[0][1];
 for (i=6-1;i>=0;i--) M[i][1] -= t * M[i][0];
 t = M[0][2];
 for (i=6-1;i>=0;i--) M[i][2] -= t * M[i][0];
#ifdef CMAT_DEBUG
 printf("after fixing row 0\n");
 dumpM();
#endif
 if (fabs(M[1][2]) > fabs(M[1][1]))
  { for (i=6-1;i>=0;i--)
     { t = M[i][1];
       M[i][1] = M[i][2];
       M[i][2] = t;
     }
#ifdef CMAT_DEBUG
    printf("using row 2\n");
#endif
  }
#ifdef CMAT_DEBUG
 else
  { printf("using row 1\n");
  }
 dumpM();
#endif
 t = M[1][1];
 for (i=6-1;i>=1;i--) M[i][1] /= t;
 t = M[1][0];
 for (i=6-1;i>=1;i--) M[i][0] -= t * M[i][1];
 t = M[1][2];
 for (i=6-1;i>=1;i--) M[i][2] -= t * M[i][1];
#ifdef CMAT_DEBUG
 printf("after fixing row 1\n");
 dumpM();
#endif
 t = M[2][2];
 for (i=6-1;i>=2;i--) M[i][2] /= t;
 t = M[2][0];
 for (i=6-1;i>=2;i--) M[i][0] -= t * M[i][2];
 t = M[2][1];
 for (i=6-1;i>=2;i--) M[i][1] -= t * M[i][2];
#ifdef CMAT_DEBUG
 printf("after fixing row 2\n");
 dumpM();
#endif
 to_rgb_R_Y = M[3][0] * 65536;
 to_rgb_R_Cr = M[4][0] * 65536;
 to_rgb_R_Cb = M[5][0] * 65536;
 to_rgb_G_Y = M[3][1] * 65536;
 to_rgb_G_Cr = M[4][1] * 65536;
 to_rgb_G_Cb = M[5][1] * 65536;
 to_rgb_B_Y = M[3][2] * 65536;
 to_rgb_B_Cr = M[4][2] * 65536;
 to_rgb_B_Cb = M[5][2] * 65536;
 to_rgb_Y0 = p->minY;
}

RGB cvt_YCbCr_to_RGB(unsigned char Y, unsigned char Cb, unsigned char Cr)
{
 int y;
 int cr;
 int cb;
 int r;
 int g;
 int b;

 y = Y - to_rgb_Y0;
 cr = Cr - 128;
 cb = Cb - 128;
 r = (to_rgb_R_Y * y) + (to_rgb_R_Cr * cr) + (to_rgb_R_Cb * cb);
 g = (to_rgb_G_Y * y) + (to_rgb_G_Cr * cr) + (to_rgb_G_Cb * cb);
 b = (to_rgb_B_Y * y) + (to_rgb_B_Cr * cr) + (to_rgb_B_Cb * cb);
 if (r < 0) r = 0; else if (r >= 0x1000000) r = 255; else r >>= 16;
 if (g < 0) g = 0; else if (g >= 0x1000000) g = 255; else g >>= 16;
 if (b < 0) b = 0; else if (b >= 0x1000000) b = 255; else b >>= 16;
 return((RGB){.r=r,.g=g,.b=b});
}