/***********************************************************************
   (C) Copyright, 1990 by Dean Rubine, Carnegie Mellon University
    Permission to use this code for noncommercial purposes is hereby granted.
    Permission to copy and distribute this code is hereby granted provided
    this copyright notice is retained.  All other rights reserved.
 **********************************************************************/
#ifdef MAC
#include <Types.h>
#include <Quickdraw.h>
#include <Packages.h>
#include <Memory.h>
#include <SANE.h>
#else
#include <math.h>
#endif
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "util.h"
#include "bitvector.h"
#include "matrix.h"
#include "sc.h"

#define	EPS	(1.0e-6)	/* for singular matrix check */

sClassifier
sNewClassifier()
{
    register sClassifier sc = allocate(1, struct sclassifier);
	sc->nfeatures = -1;
	sc->nclasses = 0;
	sc->classdope = allocate(MAXSCLASSES, sClassDope);
	sc->w = NULL;
	return sc;
}

void
sFreeClassifier(sc)
register sClassifier sc;
{
	register int i;
	register sClassDope scd;

	for(i = 0; i < sc->nclasses; i++) {
		scd = sc->classdope[i];
		if(scd->name) DisposPtr ( (Ptr) scd->name);
		DisposPtr ( (Ptr) scd);
		if(sc->w && sc->w[i]) FreeVector (sc->w[i]);
		if(scd->sumcov) FreeMatrix (scd->sumcov); 
		if(scd->average) FreeVector (scd->average);
	}
	DisposPtr ((Ptr) sc->classdope);
	if(sc->w) DisposPtr ((Ptr) sc->w);
	if(sc->cnst) FreeVector(sc->cnst);
	if(sc->invavgcov) FreeMatrix(sc->invavgcov);
	DisposPtr ((Ptr) sc);
}

sClassDope
sClassNameLookup(sc, classname)
register sClassifier sc;
register char *classname;
{
	register int i;
	register sClassDope scd;
	/* static sClassifier lastsc = 0;
	static sClassDope lastscd = 0;

	/* quick check for last class name
	if(lastsc == sc && STREQ(lastscd->name, classname))
		return lastscd; ****/

	/* linear search through all classes for name */
	for(i = 0; i < sc->nclasses; i++) {
		scd = sc->classdope[i];
		if(STREQ(scd->name, classname))
			return /* lastsc = sc, lastscd = */ scd;
	}
	return NULL;
}

sClassDope
sAddClass(sc, classname)
register sClassifier sc;
char *classname;
{
	register sClassDope scd;

	sc->classdope[sc->nclasses] = scd = allocate(1, struct sclassdope);
	scd->name = scopy(classname);
	scd->number = sc->nclasses;
	scd->nexamples = 0;
	scd->sumcov = NULL;
	++sc->nclasses;
	return scd;
}

void
sAddExample(sc, classname, y)
register sClassifier sc;
char *classname;
Vector y;
{
	register sClassDope scd;
	register int i, j;
	FLOAT nfv[50];
	FLOAT nm1on, recipn;

	scd = sClassNameLookup(sc, classname);
	if(scd == NULL)
		scd = sAddClass(sc, classname);

	if(sc->nfeatures == -1)
		sc->nfeatures = NROWS(y);

	if(scd->nexamples == 0) {
		scd->average = NewVector (sc->nfeatures);
		ZeroVector(scd->average);
		scd->sumcov = NewMatrix (sc->nfeatures, sc->nfeatures);
		ZeroMatrix(scd->sumcov);
	}

	if(sc->nfeatures != NROWS(y)) {
		error ("sAddExample: funny feature vector nrows!=%d", sc->nfeatures, 0, 0, 0, 0, 0, 0, 0); 
		return;
	}

	scd->nexamples++;
	nm1on = ((FLOAT) (scd->nexamples - 1)) / (FLOAT) scd->nexamples;
	recipn = 1.0/scd->nexamples;

	/* incrementally update covariance matrix */
	for(i = 0; i < sc->nfeatures; i++)
		nfv[i] = y[i] - scd->average[i];

	/* only upper triangular part computed */
    for(i = 0; i < sc->nfeatures; i++)
		for(j = i; j < sc->nfeatures; j++)
			scd->sumcov[i][j] += nm1on * nfv[i] * nfv[j];

	/* incrementally update mean vector */
	for(i = 0; i < sc->nfeatures; i++)
		scd->average[i] = nm1on * scd->average[i] + recipn * y[i];

}

void
FixClassifier(sc, avgcov)
register sClassifier sc;
Matrix avgcov;
{
	int i;
	FLOAT det;
	BitVector bv;
	Matrix m, r;

	/* just add the features one by one, discarding any that cause
	   the matrix to be non-invertible */
	CLEAR_BIT_VECTOR(bv);
	for(i = 0; i < sc->nfeatures; i++) {
		BIT_SET(i, bv);
		m = SliceMatrix(avgcov, bv, bv);
		r = NewMatrix(NROWS(m), NCOLS(m));
		det = InvertMatrix(m, r);
		if(fabs(det) <= EPS)
			BIT_CLEAR(i, bv);
		FreeMatrix(m);
		FreeMatrix(r);
	}

	m = SliceMatrix(avgcov, bv, bv);
	r = NewMatrix(NROWS(m), NCOLS(m));
	det = InvertMatrix(m, r);
	if(fabs(det) <= EPS)
		error("Can't fix classifier!");
	DeSliceMatrix(r, 0.0, bv, bv, sc->invavgcov);

	FreeMatrix(m);
	FreeMatrix(r);

}

void
sDoneAdding(sc)
register sClassifier sc;
{
	register int i, j;
	int c;
	int ne, denom;
	FLOAT oneoverdenom;
	register Matrix s;
	register Matrix avgcov;
	FLOAT det;
	register sClassDope scd;

	if(sc->nclasses == 0) 
		error("sDoneAdding: No classes\n");

	/* Given covariance matrices for each class (* number of examples - 1)
	    compute the average (common) covariance matrix */

	avgcov = NewMatrix(sc->nfeatures, sc->nfeatures);
	ZeroMatrix(avgcov);
	ne = 0;
	for(c = 0; c < sc->nclasses; c++) {
		scd = sc->classdope[c];
		ne += scd->nexamples;
		s = scd->sumcov;
		
		for(i = 0; i < sc->nfeatures; i++)
			for(j = i; j < sc->nfeatures; j++)
				avgcov[i][j] += s[i][j]; 
	}

	denom = ne - sc->nclasses;
	if(denom <= 0) {
		/* printf("no examples, denom=%d\n", denom); */
		return;
	}

	oneoverdenom = 1.0 / denom;
	for(i = 0; i < sc->nfeatures; i++)
		for(j = i; j < sc->nfeatures; j++)
			avgcov[j][i] = avgcov[i][j] *= oneoverdenom;

	/* invert the avg covariance matrix */

	sc->invavgcov = NewMatrix(sc->nfeatures, sc->nfeatures);
	det = InvertMatrix (avgcov, sc->invavgcov);

	if(fabs(det) <= EPS)
		FixClassifier(sc, avgcov);
	
	/* now compute discrimination functions */
	sc->w = allocate(sc->nclasses, Vector);
	sc->cnst = NewVector(sc->nclasses);
	for(c = 0; c < sc->nclasses; c++) {
		scd = sc->classdope[c];
		sc->w[c] = NewVector(sc->nfeatures);
		VectorTimesMatrix(scd->average, sc->invavgcov, sc->w[c]);
		sc->cnst[c] = -0.5 * InnerProduct(sc->w[c], scd->average);
		/* could add log(priorprob class c) to cnst[c] */
	}

	FreeMatrix(avgcov);
	return;
}

sClassDope
sClassify(sClassifier sc, Vector fv) {
	return sClassifyAD(sc, fv, (FLOAT*) NULL, (FLOAT*) NULL);
}

sClassDope
sClassifyAD(sc, fv, ap, dp)
sClassifier sc;
Vector fv;
FLOAT *ap;
FLOAT *dp;
{
	register int i, maxclass;
	FLOAT disc[MAXSCLASSES];
	FLOAT denom;
	register sClassDope scd;
	FLOAT d;

	if(sc->w == NULL) Debugger ();

	for(i = 0; i < sc->nclasses; i++) 
		disc[i] = InnerProduct (sc->w[i], fv) + sc->cnst[i];
	maxclass = 0;
	for(i = 1; i < sc->nclasses; i++)
		if(disc[i] > disc[maxclass])
			maxclass = i;

	scd = sc->classdope[maxclass];

	if(ap) {	/* calculate probability of non-ambiguity */
		for(denom = 0, i = 0; i < sc->nclasses; i++)
			/* quick check to avoid computing negligible term */
			if((d = disc[i] - disc[maxclass]) > -7.0)
				denom += exp(d);
		*ap = 1.0 / denom;
	}

	if(dp) 	/* calculate distance to mean of chosen class */
		*dp = MahalanobisDistance(fv, scd->average, sc->invavgcov);

	return scd;
}

/*
 Compute (v-u)' sigma (v-u)
 */

FLOAT
MahalanobisDistance(v, u, sigma)
register Vector v, u;
register Matrix sigma;
{
	register i;
	/* static */ Vector space = NULL;
	FLOAT result;

	if(space == NULL || NROWS(space) != NROWS(v)) {
		if(space) FreeVector(space);
		space = NewVector(NROWS(v));
	}
	for(i = 0; i < NROWS(v); i++)
		space[i] = v[i] - u[i];
	result =  QuadraticForm(space, sigma);
	return result;
}

void
sDistances (sc, nclosest)
register sClassifier sc; int nclosest;
{
	register Matrix d = NewMatrix(sc->nclasses, sc->nclasses);
	register int i, j;
	FLOAT min, max = 0;
	int n, mi, mj;

	for(i = 0; i < NROWS(d); i++) {
		for(j = i+1; j < NCOLS(d); j++) {
			d[i][j] = MahalanobisDistance(
						sc->classdope[i]->average,
						sc->classdope[j]->average,
						sc->invavgcov);
			if(d[i][j] > max) max = d[i][j];
		}
	}

	for(n = 1; n <= nclosest; n++) {
		min = max;
		mi = mj = -1;
		for(i = 0; i < NROWS(d); i++) {
			for(j = i+1; j < NCOLS(d); j++) {
				if(d[i][j] < min)
					min = d[mi=i][mj=j];
			}
		}
		if(mi == -1)
			break;

		d[mi][mj] = max+1;
	}
	FreeMatrix(d);
}

void
sDumpClassifier(sc)
register sClassifier sc;
{
	 register sClassIndex c;
#ifndef MAC

	printf("\n----Classifier %x, %d features:-----\n", sc, sc->nfeatures);
	printf("%d classes: ", sc->nclasses);
	for(c = 0; c < sc->nclasses; c++)
		printf("%s  ", sc->classdope[c]->name);
	printf("Discrimination functions:\n");
	for(c = 0; c < sc->nclasses; c++)
		PrintVector(sc->w[c], "%s: %g + ", sc->classdope[c]->name, 
			    sc->cnst[c],  0, 0, 0, 0, 0, 0);
	printf("Mean vectors:\n");
		PrintVector(sc->classdope[c]->average, "%s: ", 
			    sc->classdope[c]->name, 0, 0, 0, 0, 0, 0, 0);
	PrintMatrix(sc->invavgcov, "Inverse average covariance matrix:\n", 
		    0, 0, 0, 0, 0, 0, 0, 0);
	printf("\n---------\n\n");
#else
	char ch[2048], *s;
	s = ch;
	for(c = 0; c < sc->nclasses; c++) {
		numtostring ((long) sc->w[c], s);
		*++s = 32; *++s = 0;
		s = ch + strlen(ch);
	}
	numtostring ((long) sc->cnst, s);
	s = ch + strlen(ch);
	*++s = 32; *++s = 0;
	numtostring ((long) sc->invavgcov, s);
	s = ch + strlen(ch);
	*++s = 32; *++s = 0;
	debugstr (ch);
#endif
}

#ifdef MAC

Handle
sCompile (sc) sClassifier sc;
{
	int i, j;
	sClassDope scd;
	char *c;
	FLOAT* d;
	Handle h = NewHandle (sizeof (int) * 2
						+ sizeof (char) * 20 * sc->nclasses
						+ sizeof (FLOAT) * sc->nclasses * 2 * sc->nfeatures
						+ sizeof (FLOAT) * sc->nclasses
						+ sizeof (FLOAT) * sc->nfeatures * sc->nfeatures);

if (MemError()) Debugger();
	HLock(h);
	*((int**) h)[0] = sc->nclasses;
	(*((int**) h))[1] = sc->nfeatures;
	c = ((char*) *h) + 2 * sizeof(int);
	for(i = 0; i < sc->nclasses; i++) {
		scd = sc->classdope[i];
		strcpy (c, scd->name);
		c += 20;
	}
	d = (FLOAT*) c;
	for(i = 0; i < sc->nclasses; i++) {
		scd = sc->classdope[i];
		for (j = 0; j <sc->nfeatures; j++) d[j] = scd->average[j];
		d += sc->nfeatures;
		for (j = 0; j <sc->nfeatures; j++) Ęd[j] = sc->w[i][j];
		d += sc->nfeatures;
	}
	for (j = 0; j < sc->nclasses; j++)  d[j] = sc->cnst[j];
	d += sc->nclasses;
	for (i = 0; i < sc->nfeatures; i++) {
		for (j = 0 ; j < sc->nfeatures; j++)
			d[j] = sc->invavgcov[i][j];
		d += sc->nfeatures;
	}
	HUnlock (h);
	return h;
}

sClassifier
sLoad (Handle h) 
{
		sClassifier sc;
		sClassDope scd;
		int nc, nf, i, j;
		char* c; FLOAT* d;
		
		sc = sNewClassifier ();
		HLock(h);
		nc = (*((int**) h))[0]; 
		nf = (*((int**) h))[1]; 
		sc->nfeatures = nf;
		c = ((char*) *h) + 2 * sizeof(int);
		for (i = 0; i < nc; i++) { sAddClass (sc, c); c += 20; }
		sc->w = (Vector*) NewPtr (nc*sizeof(Vector));
		d = (FLOAT*) c;
		for(i = 0; i < sc->nclasses; i++) {
			scd = sc->classdope[i];
			scd->average = NewVector (nf);
			for (j = 0; j <sc->nfeatures; j++)  scd->average[j] = d[j];
			d += sc->nfeatures;
			sc->w[i] = NewVector (nf);
			for (j = 0; j <sc->nfeatures; j++) Ęsc->w[i][j] = d[j];
			d += sc->nfeatures;
		}
		sc->cnst = NewVector (nc);
		for (j = 0; j < sc->nclasses; j++) sc->cnst[j] = d[j];
		d += sc->nclasses;
		sc->invavgcov = NewMatrix (nf, nf);
		for (i = 0; i < sc->nfeatures; i++) {
			for (j = 0 ; j < sc->nfeatures; j++)
					sc->invavgcov[i][j]= d[j];
			d += sc->nfeatures;
		}
		HUnlock (h);
		return sc;
}

#else

void
sWrite (outfile, sc) FILE *outfile; sClassifier sc;
{
	int i;
	register sClassDope scd;

	fprintf(outfile, "%d classes\n", sc->nclasses);
	for(i = 0; i < sc->nclasses; i++) {
		scd = sc->classdope[i];
		fprintf(outfile, "%s\n", scd->name);
	}
	for(i = 0; i < sc->nclasses; i++) {
		scd = sc->classdope[i];
		OutputVector(outfile, scd->average);
		OutputVector(outfile, sc->w[i]);

	}
	OutputVector(outfile, sc->cnst);
	OutputMatrix(outfile, sc->invavgcov);
}


sClassifier
sRead (infile) FILE *infile;
{
	int i, n;
	register sClassifier sc;
	register sClassDope scd;
	char buf[100];

	printf("Reading classifier "), fflush(stdout); 
	sc = sNewClassifier();
	fgets(buf, 100, infile);
	if(sscanf(buf, "%d", &n) != 1) error("sRead 1");
	printf("%d classes ", n), fflush(stdout);
	for(i = 0; i < n; i++) {
		fscanf(infile, "%s", buf);
		scd = sAddClass(sc, buf);
		printf("%s ", scd->name), fflush(stdout);
	}
	sc->w = allocate(sc->nclasses, Vector);
	for(i = 0; i < sc->nclasses; i++) {
		scd = sc->classdope[i];
		scd->average = InputVector(infile);
		sc->w[i] = InputVector(infile);
	}
	sc->cnst = InputVector(infile);
	sc->invavgcov = InputMatrix(infile);
	return sc;
}

#endif