//
// Based on the code by Paul Haeberli 1993
// 
#include "PajaTypes.h"
#include "math.h"


#define RLUM    (0.3086)
#define GLUM    (0.6094)
#define BLUM    (0.0820)

#define OFFSET_R        3
#define OFFSET_G        2
#define OFFSET_B        1
#define OFFSET_A        0

/* 
 *	matrixmult -	
 *		multiply two matricies
 */
void
matrixmult( float* a, float* b, float* c )
{
    int x, y;
    float temp[16];

    for(y=0; y<4 ; y++)
        for(x=0 ; x<4 ; x++) {
            temp[y * 4 + x] = b[y * 4 + 0] * a[0 * 4 + x]
                       + b[y * 4 + 1] * a[1 * 4 + x]
                       + b[y * 4 + 2] * a[2 * 4 + x]
                       + b[y * 4 + 3] * a[3 * 4 + x];
        }
    for(y=0; y<4; y++)
        for(x=0; x<4; x++)
            c[y * 4 + x] = temp[y * 4 + x];
}

/* 
 *	identmat -	
 *		make an identity matrix
 */
void
identmat( float *matrix )
{
    *matrix++ = 1.0;    /* row 1        */
    *matrix++ = 0.0;
    *matrix++ = 0.0;
    *matrix++ = 0.0;
    *matrix++ = 0.0;    /* row 2        */
    *matrix++ = 1.0;
    *matrix++ = 0.0;
    *matrix++ = 0.0;
    *matrix++ = 0.0;    /* row 3        */
    *matrix++ = 0.0;
    *matrix++ = 1.0;
    *matrix++ = 0.0;
    *matrix++ = 0.0;    /* row 4        */
    *matrix++ = 0.0;
    *matrix++ = 0.0;
    *matrix++ = 1.0;
}

/* 
 *	xformpnt -	
 *		transform a 3D point using a matrix
 */
void
xformpnt( float* matrix, float x, float y, float z, float *tx, float *ty, float *tz )
{
    *tx = x*matrix[0 * 4 + 0] + y*matrix[1 * 4 + 0] + z*matrix[2 * 4 + 0] + matrix[3 * 4 + 0];
    *ty = x*matrix[0 * 4 + 1] + y*matrix[1 * 4 + 1] + z*matrix[2 * 4 + 1] + matrix[3 * 4 + 1];
    *tz = x*matrix[0 * 4 + 2] + y*matrix[1 * 4 + 2] + z*matrix[2 * 4 + 2] + matrix[3 * 4 + 2];
}

/* 
 *	cscalemat -	
 *		make a color scale marix
 */
void
cscalemat( float* mat, float rscale, float gscale, float bscale )
{
    float mmat[16];

    mmat[0 * 4 + 0] = rscale;
    mmat[0 * 4 + 1] = 0.0;
    mmat[0 * 4 + 2] = 0.0;
    mmat[0 * 4 + 3] = 0.0;

    mmat[1 * 4 + 0] = 0.0;
    mmat[1 * 4 + 1] = gscale;
    mmat[1 * 4 + 2] = 0.0;
    mmat[1 * 4 + 3] = 0.0;


    mmat[2 * 4 + 0] = 0.0;
    mmat[2 * 4 + 1] = 0.0;
    mmat[2 * 4 + 2] = bscale;
    mmat[2 * 4 + 3] = 0.0;

    mmat[3 * 4 + 0] = 0.0;
    mmat[3 * 4 + 1] = 0.0;
    mmat[3 * 4 + 2] = 0.0;
    mmat[3 * 4 + 3] = 1.0;
    matrixmult(mmat,mat,mat);
}

/* 
 *	lummat -	
 *		make a luminance marix
 */
void
lummat( float* mat )
{
    float mmat[16];
    float rwgt, gwgt, bwgt;

    rwgt = RLUM;
    gwgt = GLUM;
    bwgt = BLUM;
    mmat[0 * 4 + 0] = rwgt;
    mmat[0 * 4 + 1] = rwgt;
    mmat[0 * 4 + 2] = rwgt;
    mmat[0 * 4 + 3] = 0.0;

    mmat[1 * 4 + 0] = gwgt;
    mmat[1 * 4 + 1] = gwgt;
    mmat[1 * 4 + 2] = gwgt;
    mmat[1 * 4 + 3] = 0.0;

    mmat[2 * 4 + 0] = bwgt;
    mmat[2 * 4 + 1] = bwgt;
    mmat[2 * 4 + 2] = bwgt;
    mmat[2 * 4 + 3] = 0.0;

    mmat[3 * 4 + 0] = 0.0;
    mmat[3 * 4 + 1] = 0.0;
    mmat[3 * 4 + 2] = 0.0;
    mmat[3 * 4 + 3] = 1.0;
    matrixmult(mmat,mat,mat);
}

/* 
 *	saturatemat -	
 *		make a saturation marix
 */
void
saturatemat( float* mat, float sat )
{
    float mmat[16];
    float a, b, c, d, e, f, g, h, i;
    float rwgt, gwgt, bwgt;

    rwgt = RLUM;
    gwgt = GLUM;
    bwgt = BLUM;

    a = (1.0-sat)*rwgt + sat;
    b = (1.0-sat)*rwgt;
    c = (1.0-sat)*rwgt;
    d = (1.0-sat)*gwgt;
    e = (1.0-sat)*gwgt + sat;
    f = (1.0-sat)*gwgt;
    g = (1.0-sat)*bwgt;
    h = (1.0-sat)*bwgt;
    i = (1.0-sat)*bwgt + sat;
    mmat[0 * 4 + 0] = a;
    mmat[0 * 4 + 1] = b;
    mmat[0 * 4 + 2] = c;
    mmat[0 * 4 + 3] = 0.0;

    mmat[1 * 4 + 0] = d;
    mmat[1 * 4 + 1] = e;
    mmat[1 * 4 + 2] = f;
    mmat[1 * 4 + 3] = 0.0;

    mmat[2 * 4 + 0] = g;
    mmat[2 * 4 + 1] = h;
    mmat[2 * 4 + 2] = i;
    mmat[2 * 4 + 3] = 0.0;

    mmat[3 * 4 + 0] = 0.0;
    mmat[3 * 4 + 1] = 0.0;
    mmat[3 * 4 + 2] = 0.0;
    mmat[3 * 4 + 3] = 1.0;
    matrixmult(mmat,mat,mat);
}

/* 
 *	offsetmat -	
 *		offset r, g, and b
 */
void
offsetmat( float* mat, float roffset, float goffset, float boffset )
{
    float mmat[16];

    mmat[0 * 4 + 0] = 1.0;
    mmat[0 * 4 + 1] = 0.0;
    mmat[0 * 4 + 2] = 0.0;
    mmat[0 * 4 + 3] = 0.0;

    mmat[1 * 4 + 0] = 0.0;
    mmat[1 * 4 + 1] = 1.0;
    mmat[1 * 4 + 2] = 0.0;
    mmat[1 * 4 + 3] = 0.0;

    mmat[2 * 4 + 0] = 0.0;
    mmat[2 * 4 + 1] = 0.0;
    mmat[2 * 4 + 2] = 1.0;
    mmat[2 * 4 + 3] = 0.0;

    mmat[3 * 4 + 0] = roffset;
    mmat[3 * 4 + 1] = goffset;
    mmat[3 * 4 + 2] = boffset;
    mmat[3 * 4 + 3] = 1.0;
    matrixmult(mmat,mat,mat);
}

/* 
 *	xrotate -	
 *		rotate about the x (red) axis
 */
void
xrotatemat( float* mat, float rs, float rc )
{
    float mmat[16];

    mmat[0 * 4 + 0] = 1.0;
    mmat[0 * 4 + 1] = 0.0;
    mmat[0 * 4 + 2] = 0.0;
    mmat[0 * 4 + 3] = 0.0;

    mmat[1 * 4 + 0] = 0.0;
    mmat[1 * 4 + 1] = rc;
    mmat[1 * 4 + 2] = rs;
    mmat[1 * 4 + 3] = 0.0;

    mmat[2 * 4 + 0] = 0.0;
    mmat[2 * 4 + 1] = -rs;
    mmat[2 * 4 + 2] = rc;
    mmat[2 * 4 + 3] = 0.0;

    mmat[3 * 4 + 0] = 0.0;
    mmat[3 * 4 + 1] = 0.0;
    mmat[3 * 4 + 2] = 0.0;
    mmat[3 * 4 + 3] = 1.0;
    matrixmult(mmat,mat,mat);
}

/* 
 *	yrotate -	
 *		rotate about the y (green) axis
 */
void
yrotatemat( float* mat, float rs, float rc )
{
    float mmat[16];

    mmat[0 * 4 + 0] = rc;
    mmat[0 * 4 + 1] = 0.0;
    mmat[0 * 4 + 2] = -rs;
    mmat[0 * 4 + 3] = 0.0;

    mmat[1 * 4 + 0] = 0.0;
    mmat[1 * 4 + 1] = 1.0;
    mmat[1 * 4 + 2] = 0.0;
    mmat[1 * 4 + 3] = 0.0;

    mmat[2 * 4 + 0] = rs;
    mmat[2 * 4 + 1] = 0.0;
    mmat[2 * 4 + 2] = rc;
    mmat[2 * 4 + 3] = 0.0;

    mmat[3 * 4 + 0] = 0.0;
    mmat[3 * 4 + 1] = 0.0;
    mmat[3 * 4 + 2] = 0.0;
    mmat[3 * 4 + 3] = 1.0;
    matrixmult(mmat,mat,mat);
}

/* 
 *	zrotate -	
 *		rotate about the z (blue) axis
 */
void
zrotatemat( float* mat, float rs, float rc )
{
    float mmat[16];

    mmat[0 * 4 + 0] = rc;
    mmat[0 * 4 + 1] = rs;
    mmat[0 * 4 + 2] = 0.0;
    mmat[0 * 4 + 3] = 0.0;

    mmat[1 * 4 + 0] = -rs;
    mmat[1 * 4 + 1] = rc;
    mmat[1 * 4 + 2] = 0.0;
    mmat[1 * 4 + 3] = 0.0;

    mmat[2 * 4 + 0] = 0.0;
    mmat[2 * 4 + 1] = 0.0;
    mmat[2 * 4 + 2] = 1.0;
    mmat[2 * 4 + 3] = 0.0;

    mmat[3 * 4 + 0] = 0.0;
    mmat[3 * 4 + 1] = 0.0;
    mmat[3 * 4 + 2] = 0.0;
    mmat[3 * 4 + 3] = 1.0;
    matrixmult(mmat,mat,mat);
}

/* 
 *	zshear -	
 *		shear z using x and y.
 */
void
zshearmat( float* mat, float dx, float dy )
{
    float mmat[16];

    mmat[0 * 4 + 0] = 1.0;
    mmat[0 * 4 + 1] = 0.0;
    mmat[0 * 4 + 2] = dx;
    mmat[0 * 4 + 3] = 0.0;

    mmat[1 * 4 + 0] = 0.0;
    mmat[1 * 4 + 1] = 1.0;
    mmat[1 * 4 + 2] = dy;
    mmat[1 * 4 + 3] = 0.0;

    mmat[2 * 4 + 0] = 0.0;
    mmat[2 * 4 + 1] = 0.0;
    mmat[2 * 4 + 2] = 1.0;
    mmat[2 * 4 + 3] = 0.0;

    mmat[3 * 4 + 0] = 0.0;
    mmat[3 * 4 + 1] = 0.0;
    mmat[3 * 4 + 2] = 0.0;
    mmat[3 * 4 + 3] = 1.0;
    matrixmult(mmat,mat,mat);
}

/* 
 *	simplehuerotatemat -	
 *		simple hue rotation. This changes luminance 
 */
void
simplehuerotatemat( float* mat, float rot )
{
    float mag;
    float xrs, xrc;
    float yrs, yrc;
    float zrs, zrc;

/* rotate the grey vector into positive Z */
    mag = sqrt(2.0);
    xrs = 1.0/mag;
    xrc = 1.0/mag;
    xrotatemat(mat,xrs,xrc);

    mag = sqrt(3.0);
    yrs = -1.0/mag;
    yrc = sqrt(2.0)/mag;
    yrotatemat(mat,yrs,yrc);

/* rotate the hue */
    zrs = sin(rot*M_PI/180.0);
    zrc = cos(rot*M_PI/180.0);
    zrotatemat(mat,zrs,zrc);

/* rotate the grey vector back into place */
    yrotatemat(mat,-yrs,yrc);
    xrotatemat(mat,-xrs,xrc);
}

/* 
 *	huerotatemat -	
 *		rotate the hue, while maintaining luminance.
 */
void
huerotatemat( float* mat, float rot )
{
    float mmat[16];
    float mag;
    float lx, ly, lz;
    float xrs, xrc;
    float yrs, yrc;
    float zrs, zrc;
    float zsx, zsy;

    identmat(mmat);

/* rotate the grey vector into positive Z */
    mag = sqrt(2.0);
    xrs = 1.0/mag;
    xrc = 1.0/mag;
    xrotatemat(mmat,xrs,xrc);
    mag = sqrt(3.0);
    yrs = -1.0/mag;
    yrc = sqrt(2.0)/mag;
    yrotatemat(mmat,yrs,yrc);

/* shear the space to make the luminance plane horizontal */
    xformpnt(mmat,RLUM,GLUM,BLUM,&lx,&ly,&lz);
    zsx = lx/lz;
    zsy = ly/lz;
    zshearmat(mmat,zsx,zsy);

/* rotate the hue */
    zrs = sin(rot*M_PI/180.0);
    zrc = cos(rot*M_PI/180.0);
    zrotatemat(mmat,zrs,zrc);

/* unshear the space to put the luminance plane back */
    zshearmat(mmat,-zsx,-zsy);

/* rotate the grey vector back into place */
    yrotatemat(mmat,-yrs,yrc);
    xrotatemat(mmat,-xrs,xrc);

    matrixmult(mmat,mat,mat);
}
