/*
 * SorgL.c
 *
 *  Created on: 2009/03/25
 *      Author: H.Shouno
 *  This is a wrapper program for Recognition layer of S-cells for self organization, using SorgL() 
 *  in Neocog.c by Prof.Fukushima.
 *
 */

#include <stdio.h>
#include <string.h>
#include <sys/stat.h>
#include "mex.h"
#include "neocog_proto.h"

void mexFunction( int nlhs, mxArray *lhs[], int nrhs, const mxArray *rhs[] );

/*
 * Input & Output Arguments
 */

#define A_OUT		lhs[0] // output of Us layer excitatory connections
                               // (cell matrix (size nKs x nKc), and each element is a squared matrix)
#define B_OUT		lhs[1] // output of Usv layer inhibitory connections
#define CAT_MAP_OUT	lhs[2] // output of new category map of Us plane

#define USSIZE_IN	rhs[0] // US plane size
#define UC_IN		rhs[1] // previous Uc layers.
#define A_IN		rhs[2] // excitatory connections (cell matrix (size nKs x nKc), and each element is a squared matrix)
#define B_IN		rhs[3] // inhibitory connections (double valued array ( size nKs ) )
#define C_IN		rhs[4] // fixed connections to inhibitory plane called USV
#define CAT_IN		rhs[5] // category class index for the current pattern
#define CAT_MAP_IN	rhs[6] // current category map of Us plane
#define TH_IN		rhs[7] // Threshold value (optional)
#define A_INI_IN	rhs[8] // base shape of A (size should be same as A.  optional)


//
// available expressions...
// [A B CATMAP] = Sres( USSIZE, UC, Aold, Bold, C, Category,
//               [Threshold for Us], [Aini]  );
//

#define KSMAX 512
#define NMAX 512
#define DEFAULT_THETA 0.5
#define DEFAULT_QS 100.0 // reinforcement factor
#define DEFAULT_AINI_RAD   4.9     // spatial radius of A (asini)
#define DEFAULT_AINI_GAMMA 0.6     // spatial decay factor (asini)
#define DEFAULT_AINI_COS   0.0001  // value of teh center (asini)
#define NDIMS 3

//
// Gateway function
//
void mexFunction( int nlhs, mxArray *lhs[], int nrhs, const mxArray *rhs[] )
{
	int i, j;
	int Cat, catmap[KSMAX];
	double ***us, ***uc;
	int  nks, nkc, ns, nc;
	double ***as[KSMAX], bs[KSMAX], **cs, **asini, **cl;
	int na = -1, nl;
	double theta;
	double *coefp;
	int ndims;
	int dims[NDIMS];
	const int *ucdims;
	int *mapp;

	if( nlhs != 3 )		mexErrMsgTxt( "SorgL() requires 3 outputs: [A B Category] = Sorg(...)" );
	if( nrhs < 6 ) 		mexErrMsgTxt( "SorgL() requires 6 or more inputs: Sorg( UsSize, UC, Aold, Bold, C, Category, [Thteta for Us], [Asini]  )" );
	
	// 1st Us plane size
	if( !mxIsDouble( USSIZE_IN ) ){
		mexErrMsgTxt( "1st argumtent is a Us plane size, which should be a double value " );
	}
	ns = (int)mxGetScalar( USSIZE_IN );
	if( ns < 0 ){
		mexErrMsgTxt( "Us plane size should be a positive integer." );
	}


	// 2nd Uc planes.
	ndims = mxGetNumberOfDimensions( UC_IN );
	if( ndims != 3 )	mexErrMsgTxt( "UC layer must be a 3 dimensional array sized as NC x NC x NKC" );
	ucdims = mxGetDimensions( UC_IN ); // Do not rewrite ucdims
	nc = ucdims[0];	nkc = ucdims[2];
	if( nc != ucdims[1] || !mxIsDouble( UC_IN ) ){
		mexErrMsgTxt( "UC should be a 3 dimensional array like a piled double valued square matrix." );
	}
	if( nc <= 0 || nkc <= 0 ){
		mexErrMsgTxt( "Uc size must be a positive value." );
	}
	uc = geta3D( nkc, nc, nc, nkc );
	coefp = mxGetPr( UC_IN );
	memmove( &uc[0][0][0], coefp, sizeof(double)*nc*nc*nkc );


	// 3rd Coefficient A
	if( !mxIsCell(A_IN) ){
		mexErrMsgTxt( "Coef A must be a Cell Matrix sized (nKs x nKc)." );
	}
	nks = mxGetM( A_IN );
	for( i = 0 ; i < KSMAX ; i++ ){ // initialyze coefficients. same as in function geta4D1();
		as[i] = NULL;
	}
	if( nks > 0 ){ // connection has already existed.
		if( nkc != mxGetN( A_IN ) ){
			mexPrintf( "Uc is (%d x %d x '%d'), but A is (%d x '%d'). Oops, size mismatch!", 
				   nc, nc, nkc, nks, mxGetM( A_IN ) );
			mexErrMsgTxt( "Coef size and Uc size are not identical." );
		}
		add4D2( as, nks, nkc, KSMAX );
		for( i = 0 ; i < nks ; i++ ){
			for( j = 0 ; j < nkc ; j++ ){
				mxArray *p = mxGetCell( A_IN, i + j * nks );
				if( p != NULL ){
					if( !mxIsDouble( p ) ){
						mexErrMsgTxt( "Coefficeint Matrix A should be double valued array." );
					}
					if( na < 0 )	na = mxGetN( p );
					else if( na != mxGetM( p ) ){
						mexErrMsgTxt( "some Coefficient matrix in A is not square." );
					}
					coefp = mxGetPr( p );
					as[i][j] = get2D( na, na );
					memmove( &as[i][j][0][0], coefp, na*na*sizeof(double) );
				}
				else{
					as[i][j] = NULL;
				}
			}
		}
	}


	// 4th Coefficient B
	if( nks > 0 ){
		if( mxGetM( B_IN ) != nks || mxGetN( B_IN ) != 1 ){
			mexErrMsgTxt( "Coefficients B: size mismatched" );
		}
		if( !mxIsDouble( B_IN ) ){
			mexErrMsgTxt( "Coefficients B should be a double valued array" );
		}	
		coefp = mxGetPr( B_IN );
		memmove( bs, coefp, nks*sizeof(double) );
	}


	// 5th Coef C  Uc --> Usv
	if( nks > 0 ){
		if( na != mxGetN( C_IN ) || na != mxGetM( C_IN ) ){
			mexErrMsgTxt( "Coefficeints C size is mismatched. it should be identical to the element of A" );
		}
	}
	else{
		na = mxGetM( C_IN );   // Initial setup of Matrix size of A
		if( na != mxGetN( C_IN ) ){
			mexErrMsgTxt( "Coefficient matrix C must be a square." );
		}
	}
	if( !mxIsDouble( C_IN ) ){
		mexErrMsgTxt( "Coefficients C must be a double valued matrix." );
	}
	coefp = mxGetPr( C_IN );
	cs = get2D( na, na );
	memmove( &cs[0][0], coefp, na*na*sizeof(double) );


	// 6th Category
	if( !mxIsInt32( CAT_IN ) ){
		mexErrMsgTxt( "Category value should be a 32bit integer value" );		
	}
	Cat = mxGetScalar( CAT_IN );


	// 7th Current Category map
	if( nks > 0 ){
		if( nks != mxGetM( CAT_MAP_IN ) || mxGetN( CAT_MAP_IN ) != 1 ){
			mexErrMsgTxt( "Category map size is not identical to the S cell planes." );
		}
		if( !mxIsInt32( CAT_MAP_IN ) ){
			mexErrMsgTxt( "Category map must be a 32bit integer array." );
		}
		mapp = (int*)mxGetPr( CAT_MAP_IN );
		memmove( catmap, mapp, nks * sizeof(int) );
	}


	// 8th Threshold value (optional)
	theta = DEFAULT_THETA;
	if( nrhs >= 8 ){
		if( !mxIsDouble( TH_IN ) ){
			mexErrMsgTxt( "Threshold value should be a double value" );
		}
		theta = mxGetScalar( TH_IN );
	}


	// 9th AsIni matrix
	asini = get2D( na, na );
	if( nrhs >= 9 ){
		if( ( na != mxGetM(A_INI_IN) ) || ( na != mxGetN(A_INI_IN) ) ){
			mexErrMsgTxt( "Coefficients size of AsIni should be identical to the coefficient matrix element of A." );
		}
		if( !mxIsDouble( A_INI_IN ) ){
			mexErrMsgTxt( "Coefficients AsIni must be a double valued matrix." );
		}
		coefp = mxGetPr( A_INI_IN );
		memmove( &asini[0][0], coefp, na*na*sizeof(double) );
	}
	else{
		make_c( asini, na, DEFAULT_AINI_RAD, DEFAULT_AINI_GAMMA, DEFAULT_AINI_COS );
	}

	
	// Call Self-Organization 
	if( nks > 0 ){
		us = geta3D( nks, ns, ns, KSMAX ); 
	}
	nks = SorgL(as, bs, catmap, Cat, uc, cs, asini, us,
		    nc, nkc, ns, nks, na, na, 
		    DEFAULT_QS, theta, KSMAX, KSMAX, nc, ns, NULL);

	// copy to output values
	A_OUT = mxCreateCellMatrix( nks, nkc );
	for( i = 0 ; i < nks ; i++ ){
		for( j = 0 ; j < nkc ; j++ ){
			double **p = as[i][j];
			if( p != NULL ){
				int index = i + j * nks;
				mxArray *q = mxCreateDoubleMatrix( na, na, mxREAL );
				
				mxSetCell( A_OUT, index, q );
				coefp = (double*)mxGetPr( q );
				memmove( coefp, p[0], na*na*sizeof(double) );
			}
		}
	}
	B_OUT = mxCreateDoubleMatrix( nks, 1, mxREAL );
	coefp = (double*)mxGetPr( B_OUT );
	memmove( coefp, bs, nks * sizeof(double) );

	CAT_MAP_OUT = mxCreateNumericMatrix( nks, 1, mxINT32_CLASS, mxREAL );
	mapp = (int*)mxGetPr( CAT_MAP_OUT );   // assume 32bit int class
	memmove( mapp, catmap, nks * sizeof(int) );

	return;
}
