#include <math.h>
#include "colors.h"

/* documentation :http://www.brucelindbloom.com/Eqn_RGB_XYZ_Matrix.html
   following matrix is the transposed matrix of the CIE matrix described in this page */
static float rgb2xyz_matrix[3][3] = {
     {0.488718, 0.310680, 0.200602},
     {0.176204, 0.812985, 0.0108109},
     {0.000000, 0.0102048, 0.989795}
};

static float X_ref, Y_ref, Z_ref, u_ref, v_ref, epsilon, k_ref;

void compute_ref()
{
  guchar r_ref, g_ref, b_ref;
  r_ref = b_ref = g_ref = 255;
  point_rgb2xyz_normalized (&r_ref, &b_ref, &g_ref, &X_ref, &Y_ref, &Z_ref);
  xyz_normalized2uv_prime_normalized(&X_ref, &Y_ref, &Z_ref, &u_ref, &v_ref);
  epsilon = 0.008856;
  k_ref = 903.3;
}

/* Input:  RBG point in range 0-255
   Output: H S L values in range 0-255 */
void point_rgb2hsl( guchar *R, guchar *G, guchar *B, guchar *h, guchar *s, guchar *l )
{
     float r, g, b, mincolor, maxcolor, h_percent, s_percent, l_percent;
     /* convert RGB values to the range 0-1 */
     r = *R / 255.;
     g = *G / 255.;
     b = *B / 255.;
     mincolor = MIN(r, MIN(g, b));
     maxcolor = MAX(r, MAX(g, b));

     if (mincolor == maxcolor)
     {
	  /* color is kind of grey */
	  h_percent = 0; /* h is actually undefined */
	  s_percent = 0;
	  l_percent = maxcolor; /* l = (maxcolor+mincolor)/2 = maxcolor = mincolor */
     }
     else
     {
	  float MaxAddMin = maxcolor + mincolor;
	  float MaxDifMin = maxcolor - mincolor;
	  l_percent = MaxAddMin / 2;
	  if (l_percent < 0.5)
	       s_percent = MaxDifMin / MaxAddMin;
	  else
	       s_percent = MaxDifMin / (2 - MaxAddMin);
	  if (maxcolor == r)
	       h_percent = (g - b) / MaxDifMin;
	  else if (maxcolor == g)
	       h_percent = 2 + (b - r)/ MaxDifMin;
	  else /* if (maxcolor == b) */
	       h_percent = 4 + (r - g) / MaxDifMin;
     }

     /* move h_percent from [-1,5] to [0,1] */
     if ((h_percent /= 6) < 0)
	  h_percent += 1;

     *h = (guchar) (h_percent * 255);
     *s = (guchar) (s_percent * 255);
     *l = (guchar) (l_percent * 255);
}

/* Input:  RBG point in range 0-255
   Output: X Y Z values in range 0-1 */
void point_rgb2xyz_normalized( guchar *R, guchar *G, guchar *B, float *x, float *y, float *z )
{
     float r, g, b;
     /* convert RGB values to the range 0-1 */
     r = *R / 255.;
     g = *G / 255.;
     b = *B / 255.;

     *x = r*rgb2xyz_matrix[0][0] + g*rgb2xyz_matrix[0][1] + b*rgb2xyz_matrix[0][2];
     *y = r*rgb2xyz_matrix[1][0] + g*rgb2xyz_matrix[1][1] + b*rgb2xyz_matrix[1][2];
     *z = r*rgb2xyz_matrix[2][0] + g*rgb2xyz_matrix[2][1] + b*rgb2xyz_matrix[2][2];
}

/* Input:  RBG point in range 0-255
   Output: X Y Z values in range 0-255 */
void point_rgb2xyz( guchar *R, guchar *G, guchar *B, guchar *x, guchar *y, guchar *z )
{
     float x_norm, y_norm, z_norm;

     point_rgb2xyz_normalized (R, G, B, &x_norm, &y_norm, &z_norm);

     *x = (guchar) (x_norm * 255);
     *y = (guchar) (y_norm * 255);
     *z = (guchar) (z_norm * 255);
}

/* Input: X Y Z values in range 0-1
   Output: U V values in range 0-1 */
void xyz_normalized2uv_prime_normalized( float *x, float *y, float *z, float *u, float *v)
{
     float denum = *x + 15 * *y + 3 * *z;
     if (denum)
     {
	  *u = 4 * *x / denum;
	  *v = 9 * *y / denum;
     }
     else
	  *u = *v = 0;
}

/* Input: X Y Z values in range 0-1 
   Output: U V values in range 0-1 */
void xyz_normalized2uv_normalized( float *x, float *y, float *z, float *l_star, float *u, float *v)
{
     float u_prime, v_prime;
     xyz_normalized2uv_prime_normalized(x, y, z, &u_prime, &v_prime);
  
/*   *u = 13 * *l_star * (u_prime - u_ref); */
/*   *v = 13 * *l_star * (v_prime - v_ref); */
     *u = 2.8575 * *l_star * u_prime;
     *v = 1.766 * *l_star * v_prime;
}

/* Input: X Y Z values in range 0-1
   Output: A B values in range 0-1 */
void xyz_normalized2ab_normalized( float *x, float *y, float *z, float *a, float *b)
{ 
     float f_x, f_y, f_z;
     partial_lab_function( x, &X_ref, &f_x);
     partial_lab_function( y, &Y_ref, &f_y);
     partial_lab_function( z, &Z_ref, &f_z);

/*   *a = 500 * ( f_x - f_y); */
/*   *b = 200 * ( f_y - f_z); */
     *a  = 1.611 * (f_x - f_y) + 0.413;
     *b  = (f_y - f_z) / 1.555 + 0.4987;

}

/* Input: X Y Z values in range 0-1
   Output: l-star values in range 0-1 */
void xyz_normalized2l_star_normalized(float *x, float *y, float *z, float *l_star)
{
     float f_y;
     partial_lab_function( y, &Y_ref, &f_y); 
/*      *l_star = 116. * f_y - 16.; */
     *l_star = f_y;
}

/* Input: one values of X Y Z in range 0-1
   Output: ? */
void partial_lab_function( float *coord, float *ref, float *funct)
{
     float coord_r = *coord / *ref;

     if( coord_r > epsilon)
	  *funct = exp( 1./3. * log(coord_r));
     else
	  *funct = ( ( k_ref * coord_r) + 16.) / 116.;
}

/*Input: RGB point in range 0-255
  Output: LUV point in range 0-255*/
void point_rgb2luv( guchar *R, guchar *G, guchar *B, guchar *l_star, guchar *u, guchar *v )
{
  float x, y, z;
  float l_star_tmp, u_tmp, v_tmp;
  
  l_star_tmp = *l_star;
  u_tmp = *u;
  v_tmp = *v;

  point_rgb2xyz_normalized(R, G, B, &x, &y, &z);
  xyz_normalized2l_star_normalized(&x, &y, &z, &l_star_tmp);
  xyz_normalized2uv_normalized(&x, &y, &z, &l_star_tmp, &u_tmp, &v_tmp);

  *l_star = (guchar) (l_star_tmp * 255);
  *u = (guchar) (u_tmp * 255);
  *v = (guchar) (v_tmp * 255);
}

/*Input: RGB point in range 0-255
  Output: LAB point in range 0-255*/
void point_rgb2lab( guchar *R, guchar *G, guchar *B, guchar *l_star, guchar *a, guchar *b )
{
  float x, y, z;
  float l_star_tmp, a_tmp, b_tmp;
  
  l_star_tmp = *l_star;
  a_tmp = *a;
  b_tmp = *b;

  point_rgb2xyz_normalized(R, G, B, &x, &y, &z);
  xyz_normalized2l_star_normalized(&x, &y, &z, &l_star_tmp);
  xyz_normalized2ab_normalized(&x, &y, &z, &a_tmp, &b_tmp);

  *l_star = (guchar) (l_star_tmp * 255);
  *a = (guchar) (a_tmp * 255);
  *b = (guchar) (b_tmp * 255);
}

void point_rgb2hslxyzluvab( guchar *R, guchar *G, guchar *B, guchar *h, guchar *s, guchar *l, guchar *x, guchar *y, guchar *z, guchar *l_star, guchar *u_star, guchar *v_star, guchar *a_star, guchar *b_star )
{
     point_rgb2hsl(R, G, B, h, s, l);
     point_rgb2xyz(R, G, B, x, y, z);
     point_rgb2luv(R, G, B, l_star, u_star, v_star);
     point_rgb2lab(R, G, B, l_star, a_star, b_star);
}