/***************************************************************************
 *   Copyright (C) 2005 by Robot Group Leipzig                             *
 *    martius@informatik.uni-leipzig.de                                    *
 *    fhesse@informatik.uni-leipzig.de                                     *
 *    der@informatik.uni-leipzig.de                                        *
 *    guettler@mis.mpg.de                                                  *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 *                                                                         *
 *   $Log: mutualinformationcontroller.cpp,v $
 *   Revision 1.10  2009/02/13 16:25:56  guettler
 *   fix: wrong letters in init method when RandGen added
 *
 *   Revision 1.9  2008/05/30 11:58:27  martius
 *   use cmath instead of math.h
 *
 *   Revision 1.8  2008/04/17 14:54:45  martius
 *   randomGen added, which is a random generator with long period and an
 *    internal state. Each Agent has an instance and passed it to the controller
 *    and the wiring. This is good for
 *   a) repeatability on agent basis,
 *   b) parallel execution as done in ode_robots
 *
 *   Revision 1.7  2008/01/17 10:01:23  der
 *   updated version
 *
 *   Revision 1.6  2007/12/12 10:26:13  der
 *   MI: calculation of H(X) and H(Y|X) added
 *
 *   Revision 1.5  2007/12/06 10:04:18  der
 *   minor changes
 *
 *   Revision 1.4  2007/09/25 07:50:12  robot3
 *   old MI calculation disabled (but is still present)
 *   new MI update rule is used, costs O(1)
 *
 *   Revision 1.3  2007/04/19 13:48:20  robot3
 *   inserted new MI update function - needs O(1) instead of O(n^2)
 *
 *   Revision 1.2  2007/03/22 11:35:12  der
 *   corrected some bugs
 *
 *   Revision 1.1  2007/03/22 08:16:52  robot3
 *   passive controller for calculating the mutual information from sensor values.
 *   See ode_robots/simulations/MI_Simu for an example.
 *
 
 *                                                                         *
 ***************************************************************************/
#include "mutualinformationcontroller.h"
#include "controller_misc.h"
#include <cmath>

MutualInformationController::MutualInformationController(int sensorIntervalCount, double minSensorValue, double maxSensorValue) : AbstractController("MutualInformation", "$Id: mutualinformationcontroller.cpp,v 1.10 2009/02/13 16:25:56 guettler Exp $")
{
  this->sensorIntervalCount=sensorIntervalCount;
  this->minSensorValue=minSensorValue;
  this->maxSensorValue=maxSensorValue;
  t=0;
  if (sensorIntervalCount<=10)
  {
    this->showF=1;
    this->showP=1;
  }
  else
  {
    this->showF=0;
    this->showP=0;
  }
  this->initialized=false;
  this->useXsiCalculation=false;

}

void MutualInformationController::init(int sensornumber, int motornumber, RandGen* randGen)
{
  this->sensorNumber=sensornumber;
  this->motorNumber=motornumber;
  // if(!randGen) randGen = new RandGen(); // this gives a small memory leak

  // initialize lists with matrices
  for( int i=0;i<sensorNumber ;i++ )
  {
    // matrix dimension is 1 more because we want to store
    // the sum of the rows and columns, so
    // freqMatrixList[sensorIntervalCount,sensorIntervalCount]
    // is equal to t
    this->freqMatrixList.push_back(new matrix::Matrix(sensorIntervalCount+1,sensorIntervalCount+1));
    this->probMatrixList.push_back(new matrix::Matrix(sensorIntervalCount+1,sensorIntervalCount+1));

  }
  // allocate memory for oldSensors
  this->oldSensorStates = (double*) malloc(sizeof(double) * sensorNumber);
  // allocate memory for MI
  this->MI = (double*) malloc(sizeof(double) * sensorNumber);
  // allocate memory for H_x
  this->H_x = (double*) malloc(sizeof(double) * sensorNumber);
  // allocate memory for H_yx
  this->H_yx = (double*) malloc(sizeof(double) * sensorNumber);

  // init them with sensorIntervalCount/2
  // init I and H with 0
  for( int i=0;i<sensorNumber ;i++ )
  {
    oldSensorStates[i]=sensorIntervalCount/2.0;
    MI[i]=0.0;
    H_x[i]=0.0;
  }
  if (this->useXsiCalculation)
  {
    // initialize the xsifreq list with matrices
    for( int i=0;i<sensorNumber ;i++ )
    {
      // matrix dimension is 1 more because we want to store
      // the sum of the rows and columns, so
      // xsifreqMatrixList[sensorIntervalCount]
      // is equal to t
      this->xsiFreqMatrixList.push_back(new matrix::Matrix(sensorIntervalCount+1,1));
    }
    // allocate memory for Xsi_x
    this->H_Xsi = (double*) malloc(sizeof(double) * sensorNumber);

    // init Xsi with 0
    for( int i=0;i<sensorNumber ;i++)
      H_Xsi[i]=0.0;
  }
  this->initialized=true; // tells wether the controller is already initialized by the agent

}

void MutualInformationController::setAandCandCalcH_xsi(double ainit, double cinit)
{
  if (this->initialized)
  {
    std::cerr << "ERROR: The method setAandCandCalcH_xsi(double ainit, double cinit) must be called before the initialization" << std::endl;
    std::cerr << "of the Agent!" << std::endl;
  }
  else
  {
    this->useXsiCalculation=true;
    // remember ainit and cinit
    this->cinit=cinit;
    this->ainit=ainit;
  }
}

void MutualInformationController::step(const sensor* sensors, int sensornumber, 
				       motor* motors, int motornumber)
{
  assert(initialized==true);
  assert(sensornumber==sensorNumber);
  assert(motornumber==motorNumber);
  t++;
  this->updateMIs(sensors);
  // update sensor frequency and probability matrices
  int i=0;
  int state;
  for (std::list<matrix::Matrix*>::iterator freqMatrix=freqMatrixList.begin();freqMatrix!=freqMatrixList.end();freqMatrix++)
  {
    state = getState(sensors[i]);

    // set frequencies
    (*freqMatrix)->val(this->getState(oldSensorStates[i]),state)++;

    // sum of frequency of oldSensorState[i] (t-1)
    (*freqMatrix)->val(this->getState(oldSensorStates[i]),sensorIntervalCount)++;
    // sum of frequency of state (t)
    (*freqMatrix)->val(sensorIntervalCount,state)++;
    // total sum of all frequency, is equal to t
    (*freqMatrix)->val(sensorIntervalCount,sensorIntervalCount)++;

    oldSensorStates[i]=sensors[i]; // update old sensor state
    i++;
  }

  // calculate H(x)
  this->calculateH_x(this->H_x);
  this->calculateH_yx(this->H_yx);
  // calculate Xsi(x)
  if (this->useXsiCalculation)
  {
    this->updateXsiFreqMatrixList(sensors);
    this->calculateH_Xsi( this->H_Xsi);
  }
  i=0;
  // store the old sensor state before leaving this method
  for (std::list<matrix::Matrix*>::iterator freqMatrix=freqMatrixList.begin();freqMatrix!=freqMatrixList.end();freqMatrix++)
  {
    oldSensorStates[i]=sensors[i]; // update old sensor state
  }



  //calculateMIs(MI);
  //std::cout << "MI[0]    = " << MI[0] << std::endl;
  //std::cout << "newMI[0] = " << newMI[0] << std::endl;
}

void MutualInformationController::stepNoLearning(const sensor* sensors, int sensornumber, motor* motors, int motornumber)
{
  step(sensors,sensornumber,motors,motornumber);
}


int MutualInformationController::getState(double sensorValue)
{
  // returns the state belonging to the sensor value
  int state = (int)
              (((double)(sensorIntervalCount-1))*((sensorValue+1.0)/2)+0.5);
  if (state>(sensorIntervalCount-1))
    return (sensorIntervalCount-1);
  if (state<0)
    return 0;
  return state;
}

void MutualInformationController::updateMIs(const sensor* sensors)
{
  /*
  	NEW: calculate MIs with O(1), needs to have the old value!
  	IMPORTANT: This calculation needs the OLD F matrix, so update the MI
  		with this function first before updating the F matrix!
  	calculation formula:
  	MI(t) = 1/t ((t-1) (MI(t-1)-log(t-1)) +dS) + log t
  	where t = number of timer_ticks
  	where dS = +F[n,newState] * log(F[n,newState])
  		+F[oldState,n] * log(F[oldState,n])
  		+(F[oldState,newState]+1) * log(F[oldState,newState]+1)
  		-(F[n,newState]+1) * log(F[n,newState]+1)
  		-(F[oldState,n]+1) * log(F[oldState,n]+1)
  		-F[oldState,newState] * log(F[oldState,newState])
  	where n = sensorIntervalCount
  	*/
  int i=0;
  for (std::list<matrix::Matrix*>::iterator F=freqMatrixList.begin();F!=freqMatrixList.end();F++)
  {
    int newState = getState(sensors[i]);
    int oldState = this->getState(oldSensorStates[i]);
    // calculate dS
    int n = sensorIntervalCount; // use only for more compact formula
    double dS=0.0;
    if (((*F)->val(n,newState))>0.0)
      dS=((*F)->val(n,newState)) * log(((*F)->val(n,newState)));
    if (((*F)->val(oldState,n))>0.0)
      dS+=((*F)->val(oldState,n)) * log(((*F)->val(oldState,n)));
    if (((*F)->val(oldState,newState))>0.0)
      dS-=((*F)->val(oldState,newState)) * log(((*F)->val(oldState,newState)));
    dS+=(((*F)->val(oldState,newState))+1) * log(((*F)->val(oldState,newState))+1)
        -(((*F)->val(n,newState))+1) * log(((*F)->val(n,newState))+1)
        -(((*F)->val(oldState,n))+1) * log(((*F)->val(oldState,n))+1);
    // updateMI with old MI and dS
    double t = (double)(this->t);
    double tminus1 = (double)(t-1);
    if ((this->t)==1)
    {
      // log(t-1) is infinite, use more simple formula
      MI[i]=dS / t + log(t);
    }
    else
    {
      MI[i]=((tminus1)*(MI[i] - log(tminus1)) + dS) / t + log(t);
    }

    /*if (t<=10) {
    	calculateMIs(MI);
    }*/


    i++;
  }

}

void MutualInformationController::updateXsiFreqMatrixList(const sensor* sensors)
{
  // update sensor frequency and probability matrices
  int i=0;
  int state;
  for (std::list<matrix::Matrix*>::iterator freqMatrix=xsiFreqMatrixList.begin();freqMatrix!=xsiFreqMatrixList.end();freqMatrix++)
  {
    // calculate actual Xsi
    double xsi = sensors[i] -ainit * tanh(cinit * this->oldSensorStates[i]);
    if (t%2==0)
      xsi=0.9;
    else
      xsi=-0.9;
    std::cout << "xsi=" << xsi << std::endl;
    state = getState(xsi); // this is only useful if xsi is normally between -1 and +1!!!
    // set frequencies
    (*freqMatrix)->val(state,0)++;
    // sum of frequency
    (*freqMatrix)->val(sensorIntervalCount,0)++;

    i++;
  }

}


void MutualInformationController::calculateH_Xsi(double* H_Xsi)
{
  int i=0;
  for (std::list<matrix::Matrix*>::iterator F=xsiFreqMatrixList.begin();F!=xsiFreqMatrixList.end();F++)
  {
    H_Xsi[i]=0.0;
    for(int x=0 ;x<sensorIntervalCount ;x++ )
    {
      if (((*F)->val(x,0))!=0)
      { // to avoid log(0)
        double val = ((*F)->val(x,0) /((double)t))
                     *log((*F)->val(x,0) /((double)t));
        (H_Xsi[i])-=val;
      }
    }
    i++;
  }
}

void MutualInformationController::calculateH_x(double* H)
{
  int i=0;
  for (std::list<matrix::Matrix*>::iterator F=freqMatrixList.begin();F!=freqMatrixList.end();F++)
  {
    H[i]=0.0;
    for(int x=0 ;x<sensorIntervalCount ;x++ )
    {
      if (((*F)->val(x,sensorIntervalCount))!=0)
      { // to avoid log(0)
        double val = ((*F)->val(x,sensorIntervalCount) /((double)t))
                     *log((*F)->val(x,sensorIntervalCount) /((double)t));
        (H_x[i])-=val;
      }
    }
    i++;
  }
}

void MutualInformationController::calculateH_yx(double* H_yx)
{
  //
  int i=0;
  for (std::list<matrix::Matrix*>::iterator F=freqMatrixList.begin();F!=freqMatrixList.end();F++)
  {
    H_yx[i]=0.0;
    for(int x=0 ;x<sensorIntervalCount ;x++ )
    {
      for (int y=0;y<sensorIntervalCount;y++)
      {
        if (((*F)->val(x,y))!=0)
        {
          double val = (*F)->val(x,y)
                       *log(((*F)->val(x,y))/
                            ((*F)->val(x,sensorIntervalCount)));
          (H_yx[i])-=val/((double)t);
        }
      }
    }
    i++;
  }
}

void MutualInformationController::calculateMIs(double* MI)
{


  /*
  old formula: I = sum(over t-1) p(t-1) sum(over t) p(t|t-1) ln (p(t|t-1)/p(t))
  = sum(over t-1, t) p(t-1,t) ln (p(t-1,t)/p(t))
  p(t-1,t) = #{s_t-1 --> s_t}/t = F[t-1,t]/t
  p(t-1) = P[t-1,sensorIntervalCount];
  p(t) = P[sensorIntervalCount,t];
  old formula update: we use completely F for calculating MI
  */
  int i=0;
  for (std::list<matrix::Matrix*>::iterator F=freqMatrixList.begin();F!=freqMatrixList.end();F++)
  {
    MI[i]=0.0;
    for(int x=0 ;x<sensorIntervalCount ;x++ )
    {
      for (int y=0;y<sensorIntervalCount;y++)
      {
        if (((*F)->val(x,y))!=0)
        {
          double val = (*F)->val(x,y)
                       *log(((*F)->val(x,y) * ((double)t) )/
                            ((*F)->val(sensorIntervalCount,y) * (*F)->val(x,sensorIntervalCount)));
          (MI[i])+=val/((double)t);
        }
      }
    }
    i++;
  }
}




/*******************************************************************/
/*  CONFIGURABLE                                                   */
/*******************************************************************/

Configurable::paramval MutualInformationController::getParam(const paramkey& key) const
{
  if(key == "showF") return showF;
  else if(key == "showP") return showP;
  else  return AbstractController::getParam(key) ;
}

bool MutualInformationController::setParam(const paramkey& key, paramval val)
{
  if(key == "showF") showF=val;
  else if(key == "showP") showP=val;
  else return AbstractController::setParam(key, val);
  return true;
}

Configurable::paramlist MutualInformationController::getParamList() const
{
  paramlist l;
  l += std::pair<paramkey, paramval> (std::string("showF"), showF);
  l += std::pair<paramkey, paramval> (std::string("showP"), showP);
  return l;
}


/*******************************************************************/
/*  INSPECTABLE                                                    */
/*******************************************************************/


std::list<Inspectable::iparamkey> MutualInformationController::getInternalParamNames() const
{
  // return names
  std::list<iparamkey> keyList;
  keyList+=std::string("timer_ticks");
  keyList+=getArrayNames(sensorNumber,"MI");
  keyList+=getArrayNames(sensorNumber,"H(x)");
  if (this->useXsiCalculation)
    keyList+=getArrayNames(sensorNumber,"H(Xsi)");
  int i=0;
  if (showF>0)
  {
    for(std::list<matrix::Matrix*>::const_iterator freqMatrix=freqMatrixList.begin();freqMatrix!=freqMatrixList.end() ;freqMatrix++ )
    {
      keyList+=storeMatrixFieldNames(**freqMatrix,"F");
      i++;
    }
  }
  if (showP>0)
  {
    for(std::list<matrix::Matrix*>::const_iterator probMatrix=probMatrixList.begin();probMatrix!=probMatrixList.end() ;probMatrix++ )
    {
      keyList+=storeMatrixFieldNames(**probMatrix, "P");
      i++;
    }
  }
  for(std::list<matrix::Matrix*>::const_iterator freqMatrix=xsiFreqMatrixList.begin();freqMatrix!=xsiFreqMatrixList.end() ;freqMatrix++ )
  {
    keyList+=storeMatrixFieldNames(**freqMatrix,"xsi");
    i++;
  }
  return keyList;
}

std::list<Inspectable::iparamval> MutualInformationController::getInternalParams() const
{
  std::list<iparamval> valList;
  valList+=(iparamval) t;
  valList+=convertArrayToList(MI,sensorNumber);
  valList+=convertArrayToList(H_x,sensorNumber);
  if (this->useXsiCalculation)
    valList+=convertArrayToList(H_Xsi,sensorNumber);
  int i=0;
  if (showF>0)
  {
    for(std::list<matrix::Matrix*>::const_iterator freqMatrix=freqMatrixList.begin();freqMatrix!=freqMatrixList.end() ;freqMatrix++ )
    {
      valList+=(*freqMatrix)->convertToList();
      i++;
    }
  }
  if (showP>0)
  {
    for(std::list<matrix::Matrix*>::const_iterator probMatrix=probMatrixList.begin();probMatrix!=probMatrixList.end() ;probMatrix++ )
    {
      valList+=(*probMatrix)->convertToList();
      i++;
    }
  }
  for(std::list<matrix::Matrix*>::const_iterator freqMatrix=xsiFreqMatrixList.begin();freqMatrix!=xsiFreqMatrixList.end() ;freqMatrix++ )
  {
    valList+=(*freqMatrix)->convertToList();
    i++;
  }
  return valList;
}
