/***************************************************************************
This c function is to be used by Matlab, which needs to be compiled in Matlab:
 > mex mexhsirgb.c
Then it can be called within Matlab same as any other m-file function:
 > rgb=mexhsirgb(hsi);
where hsi and rgb are 1x3 row vectors each containing the three values of H,S,I and R,G,B, respectively.

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

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "mex.h"

#define Pi 3.141592653589
#define glevel 256

hsi_rgb(H,S,I,R,G,B) // based on RGBCube model
float *R, *G, *B, H, S, I;
{ float r, g, b;
/* 
  printf("I=%f, H=%f, S=%f\n\n", I, H*180/Pi, S);
*/
  if (S>1) S=1;
  if (I>1) I=1;
  if (S==0)
     *R=*G=*B=I;
  else
    { 
      if ( (H >= 0) && (H <2*Pi/3) )
	{ b=(1-S)/3;
	  r=(1+S*cos(H)/cos(Pi/3-H))/3;
	  g=1-r-b;
	}
      else if ( (H>=2*Pi/3) && (H<4*Pi/3) )
	{ H=H-2*Pi/3;
	  r=(1-S)/3;
	  g=(1+S*cos(H)/cos(Pi/3-H))/3;
	  b=1-r-g;
	} 
      else if ( (H>=4*Pi/3) && (H<2*Pi) )
	{ H=H-4*Pi/3;
	  g=(1-S)/3;
	  b=(1+S*cos(H)/cos(Pi/3-H))/3;
	  r=1-b-g;
	}
      else
	{ printf("\nH out of range: %f\n",H*180/Pi); pause();}
      if (r<0 || g<0 || b<0) 
	{ printf("\n\nr,g,b: %f, %f, %f",r,g,b);
	  printf("  h,s,i: %f, %f, %f\n",H,S,I);
	  pause();
	}
      if (r < 0) r=0;   if (g < 0) g=0;   if (b < 0) b=0;
      *R=3*I*r; *G=3*I*g; *B=3*I*b;
      if (*R > 1) *R=1;   if (*G > 1) *G=1;   if (*B > 1) *B=1;
    }

/*  printf("R=%6.2f, G=%6.2f, B=%6.2f\n\n",*R,*G,*B);  */
}


HSI_RGB(h,s,v,r,g,b) // based on hexcone model
float *r, *g, *b, h, s, v;
{
  float f,p,q,t;
  int i;

  if (s>1.0) s=1.0;
  if (v>1.0) v=1.0;
  if (s==0.0) 
    { *r=*g=*b=v; 
      if (h!=0.0) { printf("Error, return..."); return 0; }
    }
  if (h>=2*Pi) h=0.0;
  h=h*3/Pi;
  i=h;
  f=h-i;
  p=v*(1-s);
  q=v*(1-s*f);
  t=v*(1-s*(1-f));
  switch (i){
  case 0:
    *r=v; *g=t; *b=p; break;
  case 1:
    *r=q; *g=v; *b=p; break;
  case 2:
    *r=p; *g=v; *b=t; break;
  case 3:
    *r=p; *g=q; *b=v; break;
  case 4:
    *r=t; *g=p; *b=v; break;
  case 5:
    *r=v; *g=p; *b=q; 
  }
}


void mexFunction(int Nout, mxArray *Aout[], int Nin, const mxArray *Ain[] )
{ 
  int m,n,M,N;
  float u,v,w,R,G,B,H,S,I;
  mxArray *Input;
  double *rgb,*hsi;
  //  printf("Ninput=%d, Noutput=%d\n",Nin,Nout);

  /*
  R=(float)(mxGetScalar(Ain[0]));
  G=(float)(mxGetScalar(Ain[1]));
  B=(float)(mxGetScalar(Ain[2]));
  */

  hsi=mxGetPr(Ain[0]);
  
  v=1.0/256;
  H=hsi[0];  S=hsi[1];  I=hsi[2];
  //  printf("HSI: (%.2f %.2f %.2f)\n",H,S,I);

  Aout[0]=mxCreateDoubleMatrix(1,3,mxREAL);

  
  //  R=rgb[0]; G=rgb[1]; B=rgb[2];
  
    /* Check for proper number of arguments */
  /*    
    if (nrhs != 2) { 
	mexErrMsgTxt("Two input arguments required."); 
    } else if (nlhs > 1) {
	mexErrMsgTxt("Too many output arguments."); 
    } 
  */

  hsi_rgb(H,S,I,&R,&G,&B);
  //  printf("hsi: (%.2f %.2f %.2f)\n",H,S,I);
  rgb=mxGetPr(Aout[0]);
  rgb[0]=R*glevel; rgb[1]=G*glevel; rgb[2]=B*glevel;
  
  return;
    
}