// Class for emotional interaction

#include "../const.h"

#ifdef AIBO_YES

#include <OPENR/OSyslog.h>
#include <OPENR/ODataFormats.h>
#include <OPENR/OFbkImage.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/core_macro.h> 
#include <OPENR/OObserver.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <string.h>  
#include <stdio.h>
#include <math.h>
#include "Interaction.h"
#include "../common/include/MoNetData.h"



Interaction::Interaction(): interactionState(IDLE)
{
}

OStatus
Interaction::DoInit(const OSystemEvent& event)
{
    NEW_ALL_SUBJECT_AND_OBSERVER;
    REGISTER_ALL_ENTRY;
    SET_ALL_READY_AND_NOTIFY_ENTRY;
    
    // Initializing personality parameters...
    per_parameters = Init_per_para();

    // Initializing AIBO Personality
    bool result = InitPersonality();
    if (!result) {
        PRN(("Could not load AIBO's personality.\n"));
        return oFAIL;
    }

    return oSUCCESS;
}


OStatus
Interaction::DoStart(const OSystemEvent& event)
{
    ENABLE_ALL_SUBJECT;

    if (subject[sbjCommand]->IsReady() == true) {
        ExecuteMotion(STARTING);
        interactionState = START;
    } 
    else {
        interactionState = WAITING;
    }

    ASSERT_READY_TO_ALL_OBSERVER;
    return oSUCCESS;
}

OStatus
Interaction::DoStop(const OSystemEvent& event)
{
    interactionState = IDLE;
    DISABLE_ALL_SUBJECT;
    DEASSERT_READY_TO_ALL_OBSERVER;
    return oSUCCESS;
}

OStatus
Interaction::DoDestroy(const OSystemEvent& event)
{
    DELETE_ALL_SUBJECT_AND_OBSERVER;
    return oSUCCESS;
}


// This functions reads from a configuration file
// AIBO personality and save it in an appropriate data structure
bool
Interaction::InitPersonality()
{

int i, j, random;
bool result;
state current, next;
input emotion;
float prob;
char filename[MAX_LENGTH_FILE];
char per_files[PER_NUM][MAX_LENGTH_FILE] = PERFILES;
FILE* f;


#ifdef FUNC_TRACK
PRN(("Starting InitPersonality\n"));
#endif

// Allocating memory for Personality
per = Personality();
for (i = 0; i < STATE_NUM; i++) {
    for (j = 0; j < STATE_NUM; j++) {
          if (per.transition_prob[i][j] == NULL)
             return false;
     }
     if (per.transition_prob[i] == NULL)
             return false;
}
if (per.transition_prob == NULL)
   return false;

// Initializing random seed
SystemTime t;
GetSystemTime(&t);
srand((unsigned)t.useconds);


if (!per_parameters.per_random) {
    // Opening the selected Personality configuration file
    sprintf(filename, "%s%s", INIT_FOLDER, per_parameters.per_file);
}
else {
    // Choosing randomly a Personality configuration file
    random = (int) (PER_NUM * (rand() / (RAND_MAX + 1.0)));
    sprintf(filename, "%s%s", INIT_FOLDER, per_files[random]);
    PRN(("Selected Personality File: %s\n\n", per_files[random]));
}
f = fopen(filename, "r+");
if (f == 0) {
   PRN(("can't open %s\n", filename));
   return false;
}


// Reading configuration file
fscanf(f, "#####PERSONALITY_CONFIGURATION::%*s\n");
fscanf(f, "#####\n");
while(fscanf(f, "#STATE::%*s %d\n", &current)) {
   while (fscanf(f, "P(%d, %d) = %f\n", &emotion, &next, &prob))
         per.transition_prob[current][emotion][next] = prob;
   fscanf(f, "#####\n");
}
fscanf(f, "#####END_OF_FILE#####\n");
fclose(f);

// Checking probabilities correctness
result =  CheckProb();

#ifdef FUNC_TRACK
PRN(("Ending InitPersonality\n"));
#endif

return result;

}

// This function updates the transition probabilities according
// to the interaction history
void
Interaction::UpdatePersonality(Emotion& emotion)
{


#ifdef FUNC_TRACK
PRN(("Starting UpdatePersonality\n"));
#endif


// Updating interaction points...
switch(emotion.name) {
       // USER_NEUTRAL
       case 0:
             break;
       // USER_JOYFUL and USER_SURPRISED: nice interaction update
       case 1:
             per.nice_interaction_points += per_parameters.joyful_points;
             break;
       case 3:
             per.nice_interaction_points += per_parameters.surprised_points;
             break;
       // USER_SAD and USER_FEARFUL: sad interaction update
       case 2:
             per.sad_interaction_points += per_parameters.sad_points;
             break;
       case 5:
             per.sad_interaction_points += per_parameters.fearful_points;
             break;
       // USER_ANGRY and USER_DISGUSTED: bad interaction update
       case 4:
             per.bad_interaction_points += per_parameters.angry_points;
             break;
       case 6:
             per.bad_interaction_points += per_parameters.disgusted_points;
             break;
       default:
             break;
}

// Updating personality...
if (per_parameters.aibo_evolving) {
   if(per_parameters.aibo_imitating)
       UpdatePersonalityImitating();
   else
       UpdatePersonalityCompensating();
}

#ifdef FUNC_TRACK
PRN(("Ending UpdatePersonality\n"));
#endif

return;

}

void
Interaction::ReadyCommand(const OReadyEvent& event)
{

#ifdef FUNC_TRACK
PRN(("Starting ReadyCommand\n"));
#endif

if (interactionState == WAITING) {
   ExecuteMotion(STANDING);
   interactionState = START;
}

#ifdef FUNC_TRACK
PRN(("Ending ReadyCommand\n"));
#endif

}


// This function determines AIBO's next emotional state
// on the basis of AIBO's loaded personality and the user's
// emotional state given as an input.
void
Interaction::Interact(const ONotifyEvent& event)
{

int i, command;
double random, partial_sum;
Emotion emotion;

#ifdef FUNC_TRACK
PRN(("Starting Interact\n"));
#endif

// Getting emotional input...
emotion = *((Emotion*)event.Data(0));


if (interactionState == IDLE) {
        return; // do nothing
}

if (interactionState == WAITING) {
        OSYSLOG1((osyslogERROR, "Interaction::Interact() : interactionState ERROR\n"));
        observer[event.ObsIndex()]->AssertReady();
        return;
}

if (interactionState == START) {
    
    random = rand() / (RAND_MAX + 1.0);
    partial_sum = 0.0;
    
    // Selecting next emotional state
    i = 0;
    while (partial_sum <= random && i < STATE_NUM) {
       if (per.transition_prob[per.current_state][emotion.name][i])
          partial_sum += per.transition_prob[per.current_state][emotion.name][i];
       i++;
    }
    i--;
    per.current_state = (state)i;
    
     // Outputting AIBO state and activating the corresponding motion
    switch(per.current_state) {
         case AIBO_NEUTRAL:
                           PRN(("My current state is NEUTRAL\n"));
                           command = NEUTRAL_MOTION;
                           break;

         case AIBO_JOYFUL:
                           PRN(("My current state is JOYFUL\n"));
                           command = JOYFUL_MOTION;
                           break;

         case AIBO_SAD:
                           PRN(("My current state is SAD\n"));
                           command = SAD_MOTION;
                           break;
    
         case AIBO_ANGRY:
                           PRN(("My current state is ANGRY\n"));
                           command = ANGRY_MOTION;
                           break;
    
         default:
                           command = STANDING;
                           break;
    
    }
    ExecuteMotion(command);

    
    // Updating personality...
    if (per_parameters.aibo_evolving)
                UpdatePersonality(emotion);
    
    #ifdef FUNC_TRACK
    PRN(("Ending Interact\n"));
    #endif

}

observer[event.ObsIndex()]->AssertReady(event.SenderID());

}


// Personality is updated adopting an imitative attitude
void
Interaction::UpdatePersonalityImitating()
{

int i, j, s, k, end, num_update, num_subtract;
int to_update[STATE_NUM];                               // states whose entry probability will be increased
bool subtract[STATE_NUM];                               // states whose entry probability will be diminished
bool first[STATE_NUM];
float sum, add, total, delta, precision = 0.000001;     // 'precision' used for comparation of float values
bool updating = false;

#ifdef FUNC_TRACK
PRN(("Starting UpdatePersonalityImitating\n"));
#endif

// Many nice inputs occured, so we increase transition probabilities to AIBO_JOYFUL state
if ((per.nice_interaction_points + precision) >= per_parameters.update_thres) {
   updating = true;
   per.nice_interaction_points = 0.0;                                                 // resetting interaction points
   to_update[0] = AIBO_JOYFUL;
   end = 1;
   printf("\nI am updating my personality: I will be more joyful.\n\n");
}
// Many sad inputs occured, so we increase transition probabilities to AIBO_SAD state
else if ((per.sad_interaction_points + precision) >= per_parameters.update_thres) {
   updating = true;
   per.sad_interaction_points = 0.0;                                                 // resetting interaction points
   to_update[0] = AIBO_SAD;
   end = 1;
   printf("\nI am updating my personality: I will be sadder.\n\n");
}
// Many bad inputs occured, so we increase transition probabilities to AIBO_ANGRY state
else if ((per.bad_interaction_points + precision) >= per_parameters.update_thres) {
   updating = true;
   per.bad_interaction_points = 0.0;                                                 // resetting interaction points
   to_update[0] = AIBO_ANGRY;
   end = 1;
   printf("\nI am updating my personality: I will be angrier.\n\n");
}

if (updating) {
   for (i = 0; i < STATE_NUM; i++)
       subtract[i] = true;
   for (k = 0; k < end; k++)
       subtract[to_update[k]] = false;

   for(i = 0; i < EM_NUM + 1; i++) {
         for(s = 0; s < STATE_NUM; s++) {
                     
                     num_update = end;                              // Number of states whose entry probability will be increased
                     num_subtract = STATE_NUM - num_update;         // Number of states whose entry probability will be decreased
                     
                     total = per_parameters.update_value * num_update;  // Total probability amount to shift to "to_update" states
                                                                        // Every entry probability for those states will be increased,
                                                                        // if possible, by 'per_parameters.update_value' amount.


                     sum = 0.0;                                     // total amount of probability we can subtract from not_to_update states
                     for (j = 0; j < STATE_NUM; j++) {
                           if (subtract[j]) {
                                 sum += per.transition_prob[s][i][j];
                           }
                     }
                     if ((sum + precision) < total)                // we don't have enough probability! We are going to use
                        total = sum;                               // only the amount that is available

                        
                     delta = total/num_subtract;                   // Amount of probability that will be subtracted from "not_to_update" states
                     add = total/num_update;                       // Amount of probability that will be added to "to_update" states



                     for (j = 0; j < STATE_NUM; j++)
                            first[j] = true;
                     for (j = 0; j < STATE_NUM; j++) {
                           // If state j has entry probability < delta, delta must be recomputed.
                           // Every state will satisfy the if condition at most once, so we use 'first'
                           // array to assure that no state will be considered more than once.
                           if (subtract[j] && first[j] && (per.transition_prob[s][i][j] + precision) < delta) {
                                    delta *= num_subtract;
                                    num_subtract--;
                                    delta = (delta -  per.transition_prob[s][i][j])/num_subtract;
                                    first[j] = false;
                                    j = -1;
                           }
                     }
                     // "not_to_update" states have their probabilities decreased by 'delta'.
                     // If their probability is less than delta, it is zeroed.
                     for (j = 0; j < STATE_NUM; j++) {
                         if (subtract[j] && (per.transition_prob[s][i][j] + precision) >= delta) {
                                  per.transition_prob[s][i][j] -= delta;
                         }
                         else if (subtract[j] && (per.transition_prob[s][i][j] + precision) < delta)  {
                                  per.transition_prob[s][i][j] = 0.0;
                         }
                     }
                     
                     // "to_update" states have their probabilities increased by 'add'.
                     // If their probability, with this addition, would get bigger than 1,
                     // it is set to 1, and 'total' and 'add' values are recomputed.
                     for (k = 0; k < end; k++)   {
                            if ((1.0 - (per.transition_prob[s][i][to_update[k]] + add)) > precision)  {
                               per.transition_prob[s][i][to_update[k]] += add;
                               if (k < end - 1) {
                                  total -= add;
                                  add = total/(end - 1 - k);         // to avoid rounding errors...
                               }

                            }
                            else  {
                               per.transition_prob[s][i][to_update[k]] = 1.0;
                               if (k < end - 1) {
                                  total -= (1.0 -  per.transition_prob[s][i][to_update[k]]);
                                  add = total/(end - 1 - k);
                               }
                            }
                      }

          }

   }
}

// Debugging...
if (per_parameters.debug_per && updating) {
   for (s = 0; s < STATE_NUM; s++) {
       for(i = 0; i < EM_NUM + 1; i++) {
           for (j = 0; j < STATE_NUM; j++) {
               printf("P_%d(%d, %d) = %.3f\n", s, i, j, per.transition_prob[s][i][j]);
                }
           }
     }
}


// temp
bool b = CheckProb();
if (b) PRN(("OK\n"));
else PRN(("NO\n"));


if (updating)
   PRN(("Personality Updated!\n"));

#ifdef FUNC_TRACK
PRN(("Ending UpdatePersonalityImitating\n"));
#endif

return;

}


// Personality is updated adopting a compensatory attitude
void
Interaction::UpdatePersonalityCompensating()
{

int i, j, k, s, end, num_subtract, num_update;
int to_update[STATE_NUM];                                     // states whose entry probability will be increased
bool subtract[STATE_NUM];                                     // states whose entry probability will be diminished
bool first[STATE_NUM];
float sum, add, total, delta, precision = 0.000001;           // 'precision' used for comparation of float values
bool updating = false;

#ifdef FUNC_TRACK
PRN(("Starting UpdatePersonalityCompensating\n"));
#endif


// Many nice inputs occured, so we increase transition probabilities to AIBO_SAD and AIBO_ANGRY state
if ((per.nice_interaction_points + precision) >= per_parameters.update_thres) {
   updating = true;
   per.nice_interaction_points = 0.0;                                                 // resetting interaction points
   to_update[0] = AIBO_SAD;
   to_update[1] = AIBO_ANGRY;
   end = 2;
   printf("\nI am updating my personality: I will be sadder and angrier.\n\n");
}
// Many sad inputs occured, so we increase transition probabilities to AIBO_JOYFUL state
else if ((per.sad_interaction_points + precision) >= per_parameters.update_thres) {
   updating = true;
   per.sad_interaction_points = 0.0;                                                 // resetting interaction points
   to_update[0] = AIBO_JOYFUL;
   end = 1;
   printf("\nI am updating my personality: I will be more joyful.\n\n");
}
// Many bad inputs occured, so we increase transition probabilities to AIBO_JOYFUL state
else if ((per.bad_interaction_points + precision) >= per_parameters.update_thres) {
   updating = true;
   per.bad_interaction_points = 0.0;                                                 // resetting interaction points
   to_update[0] = AIBO_JOYFUL;
   end = 1;
   printf("\nI am updating my personality: I will be more joyful.\n\n");
}

if (updating) {
   for (i = 0; i < STATE_NUM; i++)
       subtract[i] = true;
   for (k = 0; k < end; k++)
       subtract[to_update[k]] = false;

   for(i = 0; i < EM_NUM + 1; i++) {
         for(s = 0; s < STATE_NUM; s++) {
                     
                     num_update = end;                              // Number of states whose entry probability will be increased
                     num_subtract = STATE_NUM - num_update;         // Number of states whose entry probability will be decreased
                     
                     total = per_parameters.update_value * num_update;  // Total probability amount to shift to "to_update" states
                                                                        // Every entry probability for those states will be increased,
                                                                        // if possible, by 'per_parameters.update_value' amount.


                     sum = 0.0;                                     // total amount of probability we can subtract from not_to_update states
                     for (j = 0; j < STATE_NUM; j++) {
                           if (subtract[j]) {
                                 sum += per.transition_prob[s][i][j];
                           }
                     }
                     if ((sum + precision) < total)                // we don't have enough probability! We are going to use
                        total = sum;                               // only the amount that is available

                        
                     delta = total/num_subtract;                   // Amount of probability that will be subtracted from "not_to_update" states
                     add = total/num_update;                       // Amount of probability that will be added to "to_update" states



                     for (j = 0; j < STATE_NUM; j++)
                            first[j] = true;
                     for (j = 0; j < STATE_NUM; j++) {
                           // If state j has entry probability < delta, delta must be recomputed.
                           // Every state will satisfy the if condition at most once, so we use 'first'
                           // array to assure that no state will be considered more than once.
                           if (subtract[j] && first[j] && (per.transition_prob[s][i][j] + precision) < delta) {
                                    delta *= num_subtract;
                                    num_subtract--;
                                    delta = (delta -  per.transition_prob[s][i][j])/num_subtract;
                                    first[j] = false;
                                    j = -1;
                           }
                     }
                     // "not_to_update" states have their probabilities decreased by 'delta'.
                     // If their probability is less than delta, it is zeroed.
                     for (j = 0; j < STATE_NUM; j++) {
                         if (subtract[j] && (per.transition_prob[s][i][j] + precision) >= delta) {
                                  per.transition_prob[s][i][j] -= delta;
                         }
                         else if (subtract[j] && (per.transition_prob[s][i][j] + precision) < delta)  {
                                  per.transition_prob[s][i][j] = 0.0;
                         }
                     }
                     
                     // "to_update" states have their probabilities increased by 'add'.
                     // If their probability, with this addition, would get bigger than 1,
                     // it is set to 1, and 'total' and 'add' values are recomputed.
                     for (k = 0; k < end; k++)   {
                            if ((1.0 - (per.transition_prob[s][i][to_update[k]] + add)) > precision)  {
                               per.transition_prob[s][i][to_update[k]] += add;
                               if (k < end - 1) {
                                  total -= add;
                                  add = total/(end - 1 - k);         // to avoid rounding errors...
                               }

                            }
                            else  {
                               per.transition_prob[s][i][to_update[k]] = 1.0;
                               if (k < end - 1) {
                                  total -= (1.0 -  per.transition_prob[s][i][to_update[k]]);
                                  add = total/(end - 1 - k);
                               }
                            }
                      }

          }

   }
}

// Debugging...
if (per_parameters.debug_per && updating) {
   for (s = 0; s < STATE_NUM; s++) {
       for(i = 0; i < EM_NUM + 1; i++) {
           for (j = 0; j < STATE_NUM; j++) {
               printf("P_%d(%d, %d) = %.3f\n", s, i, j, per.transition_prob[s][i][j]);
           }
        }
   }
}


// temp
bool b = CheckProb();
if (b) PRN(("OK\n"));
else PRN(("NO\n"));

if (updating)
   PRN(("Personality Updated!"));



#ifdef FUNC_TRACK
PRN(("Ending UpdatePersonalityCompensating\n"));
#endif

return;

}

void
Interaction::ExecuteMotion(int command)
{

#ifdef FUNC_TRACK
PRN(("Starting ExecuteMotion\n"));
#endif

MoNetCommand cmd(command);
subject[sbjCommand]->SetData(&cmd, sizeof(cmd));
subject[sbjCommand]->NotifyObservers();

#ifdef FUNC_TRACK
PRN(("Ending ExecuteMotion\n"));
#endif

return;

}

// For debugging purposes...
void
Interaction::NotifyResult(const ONotifyEvent& event)
{

#ifdef FUNC_TRACK
PRN(("Starting NotifyResult\n"));
#endif

MoNetResult* result = (MoNetResult*)event.Data(0);

OSYSDEBUG(("MONET RESULT: commandID %d, status %d, posture %d\n",
          result->commandID, result->status, result->posture));
          
observer[event.ObsIndex()]->AssertReady();

#ifdef FUNC_TRACK
PRN(("Ending NotifyResult\n"));
#endif

return;

}

// Checking transition probabilities correctness
bool
Interaction::CheckProb()
{

int i, j, s;
float sum, precision = 0.000001;

#ifdef FUNC_TRACK
PRN(("Starting CheckProb\n"));
#endif

for (s = 0; s < STATE_NUM; s++) {
      for (j = 0; j < EM_NUM + 1; j++) {
          sum = 0;
          for (i = 0; i < STATE_NUM; i++) {
              sum += per.transition_prob[s][j][i];
          }
          if (fabs(sum - 1) > precision) {
              return false;
          }
      }
}

#ifdef FUNC_TRACK
PRN(("Ending CheckProb\n"));
#endif

return true;

}


// Freeing Personality memory...
void
Interaction::FreePersonality()
{

int i, j;

for (i = 0; i < STATE_NUM; i++) {
    for (j = 0; j < EM_NUM + 1; j++) {
        if (per.transition_prob[i][j] != NULL)
           free(per.transition_prob[i][j]);
    }
    if (per.transition_prob[i] != NULL)
       free(per.transition_prob[i]);
}
if (per.transition_prob != NULL)
   free(per.transition_prob);

per.transition_prob = NULL;

return;
}


#endif
