/** 
* @file FitSigmoid.cpp
* @brief fit a sigmoid function accordings to the paper of platt
* @author Erik Rodner
* @date 03/05/2009

*/
#include <iostream>
#include <math.h>
#include <limits>
#include "FitSigmoid.h"

using namespace OBJREC;

using namespace std;


void FitSigmoid::fitSigmoid ( const vector<double> & t,
		  const vector<double> & out,
		  double startp,
		  double & A, double & B )
{
    const int maxiterations = 100;

    double lambda = 1e-3;
    double olderr = numeric_limits<double>::max();
    vector<double> pp ( out.size(), startp );
    
    double oldA;
    double oldB;
    int count = 0; // failure count
    for (int it = 1; it < maxiterations ; it++ )
    {
	double a = 0;
	double b = 0;
	double c = 0;
	double d = 0;
	double e = 0; 
	// First, compute Hessian & gradient of error function 
	// with respect to A & B
	
	int index = 0;
	for (vector<double>::const_iterator j = out.begin();
	     j != out.end(); j++, index++ )
	{
	    double pi = *j;
	    double d1 = pp[index]-t[index];
	    double d2 = pp[index]*(1-pp[index]);
	    a += pi*pi*d2;
	    b += d2;
	    c += pi*d2;
	    d += pi*d1;
	    e += d1;
	}

	// If gradient is really tiny, then stop

	if (fabs(d) < 1e-9 && fabs(e) < 1e-9)
	    break;
	    
	oldA = A;
	oldB = B; 
	double err = 0;
	// Loop until goodness of fit increases
    
	while (1) 
	{
	    double det = (a+lambda)*(b+lambda)-c*c;
	    if (fabs(det) < 1e-12) {
		// if determinant of Hessian is zero,
		// increase stabilizer
		lambda *= 10;
		continue;
	    }
	    A = oldA + ((b+lambda)*d-c*e)/det;
	    B = oldB + ((a+lambda)*e-c*d)/det;
	    // Now, compute the goodness of fit
	    err = 0; 
	    int index = 0;
	    for (vector<double>::const_iterator j = out.begin();
		j != out.end(); j++, index++ )
            {
		double pi = *j;
		double p = 1.0/(1.0+exp(pi*A+B));
		pp[index] = p;
		// At this step, make sure log(0) returns -200 
		double logp = p < 1e-12 ? -200 : log(p);
		double lognp = 1-p < 1e-12 ? -200 : log(1-p);
		err -= t[index]*logp+(1-t[index])*lognp;
	    }
	    if (err < olderr*(1+1e-7)) {
		lambda *= 0.1;
		break;
	    }
	    // error did not decrease: increase stabilizer by factor of 10 
	    // & try again 
	    lambda *= 10;
	    if (lambda > 1e6) // something is broken. Give up
		break;
	}
	double diff = err-olderr;
	double scale = 0.5*(err+olderr+1);
	if ((diff > -1e-3*scale) && (diff < 1e-7*scale))
	    count++; 
	else
	    count = 0;
	olderr = err;
	
	if (count == 3)
	    break;
    }
}


void FitSigmoid::fitProbabilities ( const vector<pair<int, double> > & results, 
	     double & A, double & B, double mlestimation )
{
    int prior0 = 0;
    int prior1 = 0;
    for (vector<pair<int, double> >::const_iterator j = results.begin();
	 j != results.end(); j++ )
	if ( j->first ) prior1++;
	else prior0++;

    // corresponds to MAP estimation instead of ML estimation
    // -> paper of platt
    double hiTarget;
    double loTarget;
    
    if ( mlestimation )
    {
	hiTarget = 1.0;
	loTarget = 0.0;
    } else {
	hiTarget = (prior1+1)/(double)(prior1+2);
	loTarget = 1.0/(prior0+2.0);
    }
    vector<double> t ( results.size() );
    vector<double> out ( results.size() );
    
    int index = 0;
    for (vector<pair<int, double> >::const_iterator j = results.begin();
	 j != results.end(); j++, index++ )
    {
	int yi = j->first;
	if ( yi )
	{
	    t[index] = hiTarget;
	} else {
	    t[index] = loTarget;
	}
	out[index] = j->second;
    }
    A = 0;
    B = log( (float)(prior0+1))-log( (float)(prior1+1) );
    double startp = (prior1+1)/(double)(prior0+prior1+2);

    fitSigmoid ( t, out, startp, A, B );
}