/**************************************************************/
/*                                                            */
/*                  	house_library.C 					  */
/*                                                            */
/**************************************************************/

# include "nsl_include.h"

#ifdef NSL_PC
static const double PI = 3.14159;
static const double M_2_PI = 2.0/PI;
int rint(double f)
{
	return (int) (f+0.5);
}
#endif

/* 	M_2_PI = 2/pi */

/*      w = 3.0     	1/2 of interpupillary distance (cm) */
/*      yf = -10.0   	intersection of optical axes (0,yf) (cm) */
/*      l = 22.0     	distance of interpupillary line from origin (cm) */
/*      dmax = 0.25  	maximum disparity */
/*      sigma = 0.25 	spread parameter */
/*	dddi = 0.0125	disparity interval 1.0/(jmax-1)		*/
/*	dpdi = 0.0125 	retinal interval (A,D) 1.0/(imax-1) 	*/
/*	dqdi = 0.0125	retinal interval (RL,RR) 1.0/(rimax-1)	*/

/*      a0	       	angle of declension of optical axis from ordinate */
/*      x,y           	xy coordinates in view plane (cm) */
/*      ql,qr         	retina positions for left and right eyes */
/*      tl,tr         	retinal angles for left and right eyes */
/*      bl,br         	declension angles for input */


/* 'xyview_to_left_retina' converts world coordinates to left retina. */
            
float xyview_to_left_retina(float x,float y,float w,float yf,float l)
{
	float bl,ql,a0;

        a0 = atan(w/(yf+l));
        bl = atan((float)(x + w)/(y + l));
        ql = M_2_PI*(bl - a0);
	return(ql + 1.0);
}

// 'xyview_to_right_retina' converts world coordinates to right retina.

float xyview_to_right_retina(float x,float y,float w,float yf,float l)
{
	float br,qr,a0;

        a0 = atan(w/(yf+l));
        br = atan((float)(x - w)/(y + l));
        qr = M_2_PI*(br + a0);
	return(qr + 1.0);
}
void vector_fill(nsl_vector& Q,int i0,int i1)
{
        for (int i = i0; i <= i1; i++)
                Q.elem(i) = 1.0;
}
void view_to_right_retina(nsl_vector& ret,nsl_matrix& in,
	nsl_data& w_d,nsl_data& yf_d,nsl_data& l_d)
{
	int iq0,iq1;
	float val,q0,q1;
	float xc,yc,xc0,xc1;
	float w,yf,l;

	w = w_d.get_value();
	yf = yf_d.get_value();
	l = l_d.get_value();

	int imax = in.get_imax();
	int jmax = in.get_jmax();

// crossed x,y
	int yz = imax/2;	// x axis origin on matrix
	int xz = jmax/2;	// y axis origin on matrix

	float dx = 1.0; // x distance between adjacent horizontal units
	float dy = 1.0; // y distance betwenn adjacent vertical units

	int rimax = ret.get_imax();
	int midq = (rimax - rimax%2)/2;

	float k = 0.5;		// input threshold 
	float wx2 = 0.5;	// input object half width 
			
	for (int j = 0; j < jmax; j++)
	    for (int i = 0; i < imax; i++)
	    {
		val = in.elem(i,j);
		if (val >= k)
		{	// crossed x,y
			xc = (j-xz)*dx;		// x coordinate
			yc = (i-yz)*dy;		// y coordinate
			xc0 = xc - wx2;
			xc1 = xc + wx2;
	    		q0 = xyview_to_right_retina(xc0,yc,w,yf,l); // start
			iq0 = rint(midq*q0);
			q1 = xyview_to_right_retina(xc1,yc,w,yf,l); // end
			iq1 = rint(midq*q1);
			vector_fill(ret,iq0,iq1);
		}
	    }
}
void view_to_left_retina(nsl_vector& ret,nsl_matrix& in,
	nsl_data& w_d,nsl_data& yf_d,nsl_data& l_d)
{
	int iq0,iq1;
	float val,q0,q1;
	float xc,yc,xc0,xc1;
	float w,yf,l;

	w = w_d.elem();
	yf = yf_d.elem();
	l = l_d.elem();

	int imax = in.get_imax();
	int jmax = in.get_jmax();

// crossed x,y
	int xz = jmax/2;	// x axis origin on matrix
	int yz = imax/2;	// y axis origin on matrix

	float dx = 1.0; // x distance between adjacent horizontal units
	float dy = 1.0; // y distance betwenn adjacent vertical units

	int rimax = ret.get_imax();
	int midq = (rimax - rimax%2)/2;

	float k = 0.5;		// input threshold 
	float wx2 = 0.5;	// input object half width 
			
	for (int j = 0; j < jmax; j++)
	    for (int i = 0; i < imax; i++)
	    {
		val = in.elem(i,j);
		if (val >= k)
		{	// crossed x,y
			xc = (j-xz)*dx;		// x center coordinate
			yc = (i-yz)*dy;		// y center coordinate
			xc0 = xc - wx2;		// object left
			xc1 = xc + wx2;		// object right
	    		q0 = xyview_to_left_retina(xc0,yc,w,yf,l); // start
			iq0 = rint(midq*q0);
			q1 = xyview_to_left_retina(xc1,yc,w,yf,l); // end
			iq1 = rint(midq*q1);
			vector_fill(ret,iq0,iq1);
		}
	    }
}
void retina_to_accommodation(nsl_matrix& a,nsl_matrix& in,nsl_vector& ret,
	nsl_data& w_d,nsl_data& yf_d,nsl_data& l_d,
	nsl_data& dmax_d,nsl_data& sigma_d)
{
	float da,ql,qr;
        float d,r,s,val;
	int id,ip;
	float xc,yc,xc0,xc1,x;
	float w,yf,l,dmax,sigma;
	
	w = w_d.elem();
	yf = yf_d.elem();
	l = l_d.elem();
	dmax = dmax_d.elem();
	sigma = sigma_d.elem();

	int imax = a.get_imax();
	int jmax = a.get_jmax();

// crossed x,y
	int yz = imax/2;	// x axis origin on matrix
	int xz = jmax/2;	// y axis origin on matrix

	float dx = 1.0; // x distance between adjacent horizontal units
	float dy = 1.0; // y distance between adjacent vertical units

	int rimax = ret.get_imax();
	int midq = (rimax - rimax%2)/2;

// crossed x,y
	int midy = (imax - imax%2)/2;
	int midx = (jmax - jmax%2)/2;

	float dqdi = 1.0/(jmax-1);
	float dddi = 1.0/(imax-1);
        float dpdi = dqdi*midq/midx;
	float dipdiq = dqdi/dpdi;

	float k = 0.5;		// input threshold
	float xs = 0.5;		// input step for calculating input to retina
	float wx2 = 0.5;	// input object half width

	for (int j = 0; j < jmax; j++)
	    for (int i = 0; i < imax; i++)
	    {
		val = in.elem(i,j);
		if (val >= k)
		{	// crossed x,y
			xc = (j-xz)*dx;
			yc = (i-yz)*dy;
			xc0 = xc - wx2;		// object left
			xc1 = xc + wx2;		// object right
			for (x = xc0; x <= xc1; x += xs)
			{
	    			ql = xyview_to_left_retina(x,yc,w,yf,l); 
				qr = xyview_to_right_retina(x,yc,w,yf,l); 
				da = qr-ql;
//				ip = rint((float) (midq*(qr-1.0)*dipdiq));
				float ips = midq*(qr-1.0)*dipdiq;
				ip = (int) (ips + 0.5);
       				for (id= -midy; id<= midy; id++)
        			{
					d = id*dddi;
               				r = pow((d-da)/(sigma*dmax),2.0);
                			s = exp(-r/2.0);
               				a.elem(id+midy,ip+midx) = s;
				}
			}
	    	}
	    }
}
void retina_to_disparity(nsl_matrix& d,nsl_vector& rr,nsl_vector& rl)
{
        int iq,id,iql,ip;
	float val;

	int imax = d.get_imax();
	int jmax = d.get_jmax();

// crossed x,y
	int midy = (imax - imax%2)/2;
	int midx = (jmax - jmax%2)/2;
 
	int rimax = rr.get_imax();
	int midq = (rimax - rimax%2)/2;

	float dqdi = 1.0/(jmax-1);
	float dddi = 1.0/(imax-1);
        float dpdi = dqdi*midq/midx;
	float dipdiq = dqdi/dpdi;

        for (iq = -midq; iq <= midq; iq++)
	{
	    ip = rint(iq*dipdiq);
//	    q = iq*dqdi;
            for (id = -midy; id <= midy; id++)
	    {
//		d = id*dddi;
//		iql = rint((q-d)/dqdi);
		iql = iq - id;
		if (iql >= -midq && iql <= midq)
		{
                    val = rl.elem(iql+midq)*rr.elem(iq+midq);
		    if (d.elem(id+midy,ip+midx) == 0.0)
			d.elem(id+midy,ip+midx) = val;
		}
	    }
	}
}

/* 'output_view' converts a retinal,disparity coordinate matrix into
	a x,y matrix. */
/*
output_view(Q,imax,jmax,T,yf,w,l,k)
float *Q,*T;
int imax,jmax;
float w,l,k;
{
	int ip,id,midx,midy;

	midx = (jmax-1)/2;
	midy = (imax-1)/2;
	for (ip = 0; ip < jmax; ip++)
	    for (id = 0; id < imax; id++)
		if (T[id][ip] > k)
		    retina_to_xyview(Q,imax,jmax,ip,midx,id,midy,yf,w,l);
}
*/
/* 'retina_to_xyview' converts retinal coordinates to world coordinates. */
/*
retina_to_xyview(Q,imax,jmax,ip,midx,id,midy,yf,w,l)
int ip,id,midx,midy;
float *Q,yf,w,l;
int imax,jmax;
{
	int i,j;
	float d,x,y,a0,qr,tl,tr,dddi,dpdi; 

	dpdi = 1.0/(jmax-1);
	dddi = 1.0/(imax-1);

        a0 = atan(w/(yf+l));
	qr = (ip - midx)*dpdi;
	d = (id - midy)*dddi;
        tl = (qr-d)/M_2_PI;
        tr = qr/M_2_PI;
        y = 2*w/(tan(tl + a0) - tan(tr - a0)) - l;
        x = (y + l)*tan(tr - a0) + w;
	j = rint(x) + midx;
	i = rint(y) + midy;
	if (i>=0 && j>=0 && i<imax && j<jmax)
		Q[i][j] = 1.0;
}
*/