#include "ExpressionFilter.h"
#include <assert.h>
#include <stdio.h>

#include <iostream>
#include <sstream>

#define USE_BOOST_FILESYSTEM
#ifdef USE_BOOST_FILESYSTEM
#include <boost/filesystem/path.hpp> // For system independent path support
#include <boost/filesystem/exception.hpp>
#endif

// expression_num is used to specify which expression (e.g., AU 5, AU 12, etc.)
// this filter corresponds to. The corresponding likelihood table will be loaded
// from the given directory or the current working directory if none is specified.
ExpressionFilter::ExpressionFilter (int _expression_num, std::string dir ) : expression_num( _expression_num )
{
  for (int i = 0; i < NUM_STATES; i++) {
	probabilities[i] = PRIOR_PROBABILITIES[i];
  }

  // Load likelihood tables
  std::string fname = BuildFileName( dir, "present", expression_num, true );
  presentLikelihoods = LikelihoodTable( fname.c_str() );
  
  fname = BuildFileName( dir, "absent", expression_num, true );
  absentLikelihoods = LikelihoodTable( fname.c_str() );

  fname = BuildFileName( dir, "noface", expression_num, true );
  nonfaceLikelihoods = LikelihoodTable( fname.c_str() );
}

/** Generate file names for loading the likelihood tables.
 * Examples: "AU_1_present_likelihoods.txt" "AU_1_absent_likelihoods.txt" "nonface_likelihoods.txt"
 *   "AU_<exnum>_<prefix>_likelihoods.txt"
 * An optional directory can be used if the files are not in the current working directory.
 * A compile time option allows the use of Boost filesystem objects to make sure it works in a platform
 *  independent way.
 */
std::string
ExpressionFilter::BuildFileName( const std::string &dir, const std::string &prefix, int expression_num, bool face )
{
	std::ostringstream output_stream;
	
	if( face )
		output_stream << "AU_" << expression_num << "_";
	
	output_stream << prefix << "_likelihoods.txt";
		
	std::string filename;
	
#ifdef USE_BOOST_FILESYSTEM
	try
	{
		boost::filesystem::path fpath( output_stream.str() );
		if( !dir.empty() )
		{
			boost::filesystem::path nativeDir( dir.c_str(), boost::filesystem::native );
			fpath = nativeDir / fpath;
		}
		filename = fpath.native_file_string();
	}
	catch ( const boost::filesystem::filesystem_error & )
	{
		std::cerr << "Couldn't build valid path for file: " << output_stream.str() << "  with directory: " << dir << std::endl;
		return output_stream.str();
	}
#else
	if( dir.empty() )
		filename = output_stream.str();
	else
		filename = dir + "/" + output_stream.str();
#endif
	
	return filename;
}

// Calculates probability of transitioning from state "from" to "to".
double ExpressionFilter::TransitionProb (expr_state from, expr_state to)
{
  return TRANSITION_PROBABILITIES[from][to];
}

// Calculates likelihood of being in a particular state given the input
// vector of raw expression response, and whether face was found or not.
double ExpressionFilter::CalcLikelihood (expr_state state, InputVector input)
{
  if (input.face_found) {
	switch (state) {
	  case PRESENT:
		return HIT_PRESENT_RATE * presentLikelihoods.CalcLikelihood(input.raw_response);
	  case ABSENT:
		return HIT_ABSENT_RATE * absentLikelihoods.CalcLikelihood(input.raw_response);
	  case NO_FACE: // fallthrough
	  default:
		return FALSE_POSITIVE_RATE * nonfaceLikelihoods.CalcLikelihood(input.raw_response);
	}
  } else {
  	switch (state) {
	  case PRESENT:
		return MISS_PRESENT_RATE;
	  case ABSENT:
		return MISS_ABSENT_RATE;
	  case NO_FACE: // fallthrough
	  default:
		return CORRECT_REJECTION_RATE;
	}
  }
}

// Given the input vector (expression detector response plus whether the face-finder
// found a face), update the probabilities of all states.
void ExpressionFilter::NextTimeStep (InputVector input)
{
  // Calculate likelihoods
  double likelihoods[NUM_STATES];
  for (int i = 0; i < NUM_STATES; i++) {
	likelihoods[i] = CalcLikelihood((expr_state) i, input);
  }

  // Update current probabilities given the likelihoods
  NextTimeStepGivenLikelihoods(likelihoods);
}

// Updates the probability of being each state given the current likelihoods
// of all states.
void ExpressionFilter::NextTimeStepGivenLikelihoods (double likelihoods[NUM_STATES])
{
  double newProbabilities[NUM_STATES];
  for (int i = 0; i < NUM_STATES; i++) {
	double prior = 0;
	// Prior probability of state i = sum of probabilities of
	// transitioning from each state j to state i.
	for (int j = 0; j < NUM_STATES; j++) {
	  prior += probabilities[j] * TransitionProb((expr_state) j, (expr_state) i);
	  //printf("trans %d to %d=%f\n", j, i, TransitionProb((expr_state) j, (expr_state) i));
	}
	// Multiply prior by the likelihood.
    newProbabilities[i] = likelihoods[i] * prior;
	//printf("%d: like=%f prior=%f unnormprob=%f\n", i, likelihoods[i], prior, probabilities[i]);
  }
  // Normalize and store
  double sum = 0;
  for (int i = 0; i < NUM_STATES; i++) {
	sum += newProbabilities[i];
  }
  for (int i = 0; i < NUM_STATES; i++) {
	probabilities[i] = newProbabilities[i] / sum;
	//printf("prob[%d]=%f\n", i, probabilities[i]);
  }
}

// Returns the probability of currently residing in the specified state.
double ExpressionFilter::GetProbability (expr_state state)
{
  return probabilities[state];
}