/************************************************************************
 *                                                                      *
 *  Program package         T O O L D I A G                             *
 *                                                                      *
 *  Version   1.5                                                       *
 *  Date:     Tue Feb  8 13:39:08 1994                                  *
 *                                                                      *
 *  NOTE: This program package is copyrighted in the sense that it      *
 *  may be used for scientific purposes. The package as a whole, or     *
 *  parts thereof, cannot be included or used in any commercial         *
 *  application without written permission granted by the author.       *
 *  No programs contained in this package may be copied for commercial  *
 *  distribution.                                                       *
 *                                                                      *
 *  All comments  concerning this program package may be sent to the    *
 *  e-mail address 'tr@fct.unl.pt'.                                     *
 *                                                                      *
 ************************************************************************/



#include <stdio.h>
#include <math.h>
#include "def.h"
#include "matrix.h"

extern universe *U;

static str80 buf;

#define PEEPMAT(M) printf("M=\n");print_Matrix(&M);DBG;   /**/
/* #define PEEPMAT(M)   /**/
#define xxxVERIFY
#ifdef VERIFY
#define EPSILON (0.01)
static float eps;
#endif


void mcovar_mats( FSV, numFeat, T, A, W )
FeatSelectVector FSV;
int numFeat;
Matrix *T, *A, *W;
{
 FeatVector FV;
 int c, i, d, p, total, j;
 MatElem scalar;
 Matrix M, Mk, MAux;
 Matrix Aux1, Aux2, Aux3;

#ifdef VERIFY
 Matrix x_c, x_cT, TT, TT_mat, TT_mat2;
 init_Matrix( &x_c ); x_c.row = numFeat; x_c.col = 1; Matrix_Alloc( &x_c );
 init_Matrix( &x_cT ); init_Matrix( &TT ); init_Matrix( &TT_mat );
 init_Matrix( &TT_mat2 );
 TT.row = numFeat; TT.col = numFeat; Matrix_Alloc( &TT );
 Reset_Matrix( &TT );
#endif

 /* calculate the matrices */
 
 init_Matrix( &Aux1 ); init_Matrix( &Aux2 ); init_Matrix( &Aux3 );
 init_Matrix( &M ); M.row = numFeat; M.col = 1; Matrix_Alloc( &M );
 init_Matrix( &Mk ); Mk.row = numFeat; Mk.col = 1; Matrix_Alloc( &Mk );
 init_Matrix( &MAux ); MAux.row = numFeat; MAux.col = 1;
     Matrix_Alloc( &MAux );

 Matrix_Free( A ); A->row = numFeat; A->col = A->row;
 Matrix_Alloc( A ); Reset_Matrix( A );
 Matrix_Free( W ); W->row = numFeat; W->col = W->row;
 Matrix_Alloc( W ); Reset_Matrix( W );
 
 Reset_Matrix( &M );
 /* grand centroid */
 total = 0;
 for( c = 0; c < U->nrClass; c++ )
 {
   total += U->C[c].numSampl;
   for( i = 0; i < U->C[c].numSampl; i++ )
   {
     FV = &(U->C[c].S[i*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       M.Elem[d] += FV[FSV[d].rank];
     }
   }
 }
 scalar = (MatElem)(1.0/(MatElem)total); 
 ScalarMult_Matrix( scalar, &M );
 /* printf("Grand centroid:\n"); print_Matrix( &M ); gets(buf);/**/

#ifdef VERIFY
 for( c = 0; c < U->nrClass; c++ )
 {
   for( i = 0; i < U->C[c].numSampl; i++ )
   {
     FV = &(U->C[c].S[i*U->nrFeat]);
     /* showFV( U->nrFeat, FV );	/**/
     for( d = 0; d < numFeat; d++ )
     {
       x_c.Elem[d] =  FV[FSV[d].rank];	/**/
       /* x_c.Elem[d] =  FV[FSV[d].rank] - M.Elem[d];	/**/
     }
     Transpose_Matrix( &x_c, &x_cT );
     Mult_Matrix( &x_c, &x_cT, &TT_mat );
     Add_Matrix( &TT_mat, &TT, &TT_mat2 );
     Copy_Matrix( &TT_mat2, &TT );
   }
 }
#endif

 /* A , W */
 for( c = 0; c < U->nrClass; c++ )
 {
   /* local centroid */
   Reset_Matrix( &Mk );
   for( i = 0; i < U->C[c].numSampl; i++ )
   {
     FV = &(U->C[c].S[i*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       Mk.Elem[d] += FV[FSV[d].rank];
     }
   }
   scalar = (MatElem)(1.0/(MatElem)U->C[c].numSampl);
   ScalarMult_Matrix( scalar, &Mk );
   /* printf("Local centroid of class %d:\n",c);
   print_Matrix( &Mk ); gets(buf);/**/

   /* calculate summand of A for one class */

   Subtract_Matrix( &Mk, &M, &Aux1 );
   Transpose_Matrix( &Aux1, &Aux2 );
   Mult_Matrix( &Aux1, &Aux2, &Aux3 );
   scalar = (MatElem)U->C[c].numSampl;
   ScalarMult_Matrix( scalar, &Aux3 );
   Add_Matrix( A, &Aux3, &Aux1 );
   Copy_Matrix( &Aux1, A );

   /* calculate summand of W for one class */

   for( i = 0; i < U->C[c].numSampl; i++ )
   {
     FV = &(U->C[c].S[i*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       MAux.Elem[d] = ( FV[FSV[d].rank] - Mk.Elem[d] - M.Elem[d] );
     }
     Transpose_Matrix( &MAux, &Aux1 );
     Mult_Matrix( &MAux, &Aux1, &Aux2 );
     Add_Matrix( W, &Aux2, &Aux3 );
     Copy_Matrix( &Aux3, W );
   }

 }/* for all classes */
 Add_Matrix( A, W, T );
 /* printf("A:"); print_Matrix( A ); printf("W:"); print_Matrix( W );
 printf("T:"); print_Matrix( T ); /**/
#ifdef VERIFY
 for( i = 0; i < TT.row; i++ )
   for( j = 0; j < TT.col; j++)
   {
     eps = (float)fabs(T->Elem[i*T->col+j] - TT.Elem[i*TT.col+j]);
     if( eps > EPSILON )
     {
       printf("\nT:\n"); print_Matrix( T );
       printf("TT:\n"); print_Matrix( &TT );
       printf("Different Matrices T[%d,%d] and TT-exit.. diff=%f\n",
		i, j, eps ); exit(1);
     }
   }
 Matrix_Free( &x_c ); Matrix_Free( &x_cT ); Matrix_Free( &TT );
 Matrix_Free( &TT_mat ); Matrix_Free( &TT_mat2 );
#endif
 Matrix_Free( &M ); Matrix_Free( &Mk ); Matrix_Free( &MAux );
 Matrix_Free( &Aux1 ); Matrix_Free( &Aux2 ); Matrix_Free( &Aux3 );
}



/*
 *  Generate the scatter matrices from a set of features
 *  see Duda and Hart, pp. 221
 *   The difference to the Kittler case is evidently that the matrix
 *   is multiplicated with the number of samples
*/
void scatter_mats_duda( FSV, numFeat, T, SB, SW )
FeatSelectVector FSV;
int numFeat;
Matrix *T, *SB, *SW;
{
 FeatVector FV;
 int k, i, d, p, j;
 MatElem scalar;
 Matrix M, Mi, MAux;
 Matrix Aux1, Aux2, Aux3, TT;

 init_Matrix( &Aux1 ); init_Matrix( &Aux2 ); init_Matrix( &Aux3 );
 init_Matrix( &M ); M.row = numFeat; M.col = 1; Matrix_Alloc( &M );
 init_Matrix( &Mi ); Mi.row = numFeat; Mi.col = 1; Matrix_Alloc( &Mi );
 init_Matrix( &MAux ); MAux.row = numFeat; MAux.col = 1;
     Matrix_Alloc( &MAux );

 Matrix_Free( SB ); SB->row = numFeat; SB->col = SB->row;
 Matrix_Alloc( SB ); Reset_Matrix( SB );
 Matrix_Free( SW ); SW->row = numFeat; SW->col = SW->row;
 Matrix_Alloc( SW ); Reset_Matrix( SW );
 
 Reset_Matrix( &M );
 /* grand centroid */
 for( i = 0; i < U->nrClass; i++ )
 {
   for( k = 0; k < U->C[i].numSampl; k++ )
   {
     FV = &(U->C[i].S[k*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       M.Elem[d] += FV[FSV[d].rank];
     }
   }
 }
 scalar = (MatElem)(1.0/(MatElem)U->sumSampl); 
 ScalarMult_Matrix( scalar, &M );
 /* printf("Grand centroid:\n"); print_Matrix( &M ); gets(buf);/**/


 /* SB , SW */
 for( i = 0; i < U->nrClass; i++ )
 {
   /* local centroid */
   Reset_Matrix( &Mi );
   for( k = 0; k < U->C[i].numSampl; k++ )
   {
     FV = &(U->C[i].S[k*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       Mi.Elem[d] += FV[FSV[d].rank];
     }
   }
   scalar = (MatElem)(1.0/(MatElem)U->C[i].numSampl);
   ScalarMult_Matrix( scalar, &Mi );
   /* printf("Local centroid of class %d:\n",i);
   print_Matrix( &Mi ); gets(buf);/**/

   /* calculate summand of SB for one class */

   Subtract_Matrix( &Mi, &M, &Aux1 );
   Transpose_Matrix( &Aux1, &Aux2 );
   Mult_Matrix( &Aux1, &Aux2, &Aux3 );
   scalar = (MatElem)U->C[i].numSampl;
   ScalarMult_Matrix( scalar, &Aux3 );
   Add_Matrix( SB, &Aux3, &Aux1 );
   Copy_Matrix( &Aux1, SB );

   /* calculate summand of SW for one class */

   for( k = 0; k < U->C[i].numSampl; k++ )
   {
     FV = &(U->C[i].S[k*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       MAux.Elem[d] = ( FV[FSV[d].rank] - Mi.Elem[d] );
     }
     Transpose_Matrix( &MAux, &Aux1 );
     Mult_Matrix( &MAux, &Aux1, &Aux2 );
     Add_Matrix( SW, &Aux2, &Aux3 );
     Copy_Matrix( &Aux3, SW );
   }

 }/* for all classes */
 Add_Matrix( SB, SW, T );
 /* printf("SB:\n"); print_Matrix( SB ); printf("SW:\n"); print_Matrix( SW );
 printf("T:\n"); print_Matrix( T );  /**/
#ifdef VERIFY
 /* check the total */
 init_Matrix( &TT ); TT.row = numFeat; TT.col = numFeat; Matrix_Alloc( &TT );
 Reset_Matrix( &TT );
 for( i = 0; i < U->nrClass; i++ )
 {
   for( k = 0; k < U->C[i].numSampl; k++ )
   {
     FV = &(U->C[i].S[k*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       MAux.Elem[d] = ( FV[FSV[d].rank] - M.Elem[d] );
     }
     Transpose_Matrix( &MAux, &Aux1 );
     Mult_Matrix( &MAux, &Aux1, &Aux2 );
     Add_Matrix( &TT, &Aux2, &Aux3 );
     Copy_Matrix( &Aux3, &TT );
   }
 }
 for( i = 0; i < TT.row; i++ )
   for( j = 0; j < TT.col; j++)
   {
     eps = (float)fabs(T->Elem[i*T->col+j] - TT.Elem[i*TT.col+j]);
     if( eps > EPSILON )
     {
       printf("\nT:\n"); print_Matrix( T );
       printf("TT:\n"); print_Matrix( &TT );
       printf("Different Matrices T[%d,%d] and TT-exit.. diff=%f\n",
		i, j, eps ); exit(1);
     }
   }
 Matrix_Free( &TT );
#endif
 Matrix_Free( &M ); Matrix_Free( &Mi ); Matrix_Free( &MAux );
 Matrix_Free( &Aux1 ); Matrix_Free( &Aux2 ); Matrix_Free( &Aux3 );
}


/*
 *  Generate the scatter matrices from a set of features
 *  see Devijver, P. A., and Kittler, J.,
 *  "Pattern Recognition --- A Statistical Approach,"
 *   Prentice/Hall Int., London, 1982. 
*/
void scatter_mats_kittler( FSV, numFeat, T, SB, SW )
FeatSelectVector FSV;
int numFeat;
Matrix *T, *SB, *SW;
{
 FeatVector FV;
 int k, i, d, p, j;
 MatElem scalar;
 Matrix M, Mi, MAux, SWi;
 Matrix Aux1, Aux2, Aux3, TT;

 init_Matrix( &Aux1 ); init_Matrix( &Aux2 ); init_Matrix( &Aux3 );
 init_Matrix( &M ); M.row = numFeat; M.col = 1; Matrix_Alloc( &M );
 init_Matrix( &Mi ); Mi.row = numFeat; Mi.col = 1; Matrix_Alloc( &Mi );
 init_Matrix( &MAux ); MAux.row = numFeat; MAux.col = 1;
     Matrix_Alloc( &MAux );
 init_Matrix( &SWi ); SWi.row = numFeat; SWi.col = numFeat;
     Matrix_Alloc( &SWi );

 Matrix_Free( SB ); SB->row = numFeat; SB->col = SB->row;
 Matrix_Alloc( SB ); Reset_Matrix( SB );
 Matrix_Free( SW ); SW->row = numFeat; SW->col = SW->row;
 Matrix_Alloc( SW ); Reset_Matrix( SW );
 
 Reset_Matrix( &M );
 /* grand centroid */
 for( i = 0; i < U->nrClass; i++ )
 {
   for( k = 0; k < U->C[i].numSampl; k++ )
   {
     FV = &(U->C[i].S[k*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       M.Elem[d] += FV[FSV[d].rank];
     }
   }
 }
 scalar = (MatElem)(1.0/(MatElem)U->sumSampl); 
 ScalarMult_Matrix( scalar, &M );
 /* printf("Grand centroid:\n"); print_Matrix( &M ); gets(buf);/**/


 /* SB , SW */
 for( i = 0; i < U->nrClass; i++ )
 {
   /* local centroid */
   Reset_Matrix( &Mi );
   for( k = 0; k < U->C[i].numSampl; k++ )
   {
     FV = &(U->C[i].S[k*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       Mi.Elem[d] += FV[FSV[d].rank];
     }
   }
   scalar = (MatElem)(1.0/(MatElem)U->C[i].numSampl);
   ScalarMult_Matrix( scalar, &Mi );
   /* printf("Local centroid of class %d:\n",i);
   print_Matrix( &Mi ); gets(buf);/**/

   /* calculate summand of SB for one class */

   Subtract_Matrix( &Mi, &M, &Aux1 );
   Transpose_Matrix( &Aux1, &Aux2 );
   Mult_Matrix( &Aux1, &Aux2, &Aux3 );
   scalar = (MatElem)U->C[i].a_priori_prob;
   ScalarMult_Matrix( scalar, &Aux3 );
   Add_Matrix( SB, &Aux3, &Aux1 );
   Copy_Matrix( &Aux1, SB );

   /* calculate summand of SW for one class */

   Reset_Matrix( &SWi );
   for( k = 0; k < U->C[i].numSampl; k++ )
   {
     FV = &(U->C[i].S[k*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       MAux.Elem[d] = ( FV[FSV[d].rank] - Mi.Elem[d] );
     }
     Transpose_Matrix( &MAux, &Aux1 );
     Mult_Matrix( &MAux, &Aux1, &Aux2 );
     Add_Matrix( &SWi, &Aux2, &Aux3 );
     Copy_Matrix( &Aux3, &SWi );
   }
   scalar = (MatElem)U->C[i].a_priori_prob / (MatElem)U->C[i].numSampl;
   ScalarMult_Matrix( scalar, &SWi );
   Add_Matrix( &SWi, SW, &Aux3 );
   Copy_Matrix( &Aux3, SW );

 }/* for all classes */

 Add_Matrix( SB, SW, T );
 /* printf("SB:\n"); print_Matrix( SB ); printf("SW:\n"); print_Matrix( SW );
 printf("T:\n"); print_Matrix( T );  /**/
#ifdef VERIFY
 /* check the total */
 init_Matrix( &TT ); TT.row = numFeat; TT.col = numFeat; Matrix_Alloc( &TT );
 Reset_Matrix( &TT );
 for( i = 0; i < U->nrClass; i++ )
 {
   for( k = 0; k < U->C[i].numSampl; k++ )
   {
     FV = &(U->C[i].S[k*U->nrFeat]);
     for( d = 0; d < numFeat; d++ )
     {
       MAux.Elem[d] = ( FV[FSV[d].rank] - M.Elem[d] );
     }
     Transpose_Matrix( &MAux, &Aux1 );
     Mult_Matrix( &MAux, &Aux1, &Aux2 );
     Add_Matrix( &TT, &Aux2, &Aux3 );
     Copy_Matrix( &Aux3, &TT );
   }
 }
 scalar = 1.0 / (MatElem)U->sumSampl;
 ScalarMult_Matrix( scalar, &TT );
 for( i = 0; i < TT.row; i++ )
   for( j = 0; j < TT.col; j++)
   {
     eps = (float)fabs(T->Elem[i*T->col+j] - TT.Elem[i*TT.col+j]);
     if( eps > EPSILON )
     {
       printf("\nT:\n"); print_Matrix( T );
       printf("TT:\n"); print_Matrix( &TT );
       printf("Different Matrices T[%d,%d] and TT-exit.. diff=%f\n",
		i, j, eps ); exit(1);
     }
   }
 Matrix_Free( &TT );
#endif
 Matrix_Free( &M ); Matrix_Free( &Mi ); Matrix_Free( &MAux );
 Matrix_Free( &Aux1 ); Matrix_Free( &Aux2 ); Matrix_Free( &Aux3 );
 Matrix_Free( &SWi );
}