//  Copyright (2013) Cédric Coussinet (cedric.coussinet@nomoseed.net)
//
//  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 3 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, see <http://www.gnu.org/licenses/>

#include "worldsquare.h"
#include "xmlutil.h"
#include <libxslt/xslt.h>
#include <string.h>
#include <libxslt/transform.h>

static xmlNodePtr root;

static xmlNsPtr ns = NULL;

static sensors* sensorsAgent = NULL;

typedef struct {
  xmlNodePtr cmd;
  int delay;
} actionDelay;

static actionDelay* delayMotor = NULL;
static actionDelay* delaySound = NULL;

static xmlDocPtr worldsquare = NULL;

static short agentsNumberMap;
static short agentsNumberUnit;
static short agentsNumberActive;

static xsltStylesheetPtr xslReset;
static xsltStylesheetPtr xslInput;
static xsltStylesheetPtr xslOutput;

static short xmlAgentNumber () {
  short cmp = 0;
  xmlNodePtr n;
  for (n=root->last;n;n=n->prev)
    if((n->type != XML_CDATA_SECTION_NODE) && (n->type != XML_TEXT_NODE)){
      if (!strcmp((const char*)n->name,"agent"))
        cmp++;
      else
        return cmp;
    }
  return cmp;
}

static float getData (const xmlNodePtr agent, const char* name, const char* att){
  float result;
  if (strcmp(att, "sum_sounds"))
    sscanf((const char*)xmlGetProp(xmlGetNodeByName(agent, name), (const xmlChar*) att), "%f", &result);
  else
    sscanf((const char*)xmlGetProp(xmlGetNodeByName (root, "sum_sounds"), (const xmlChar*) att), "%f", &result);
  return result;
}

static void writeCmd (const int delay, const short agent, const char* cmdMotor){
  if (delayMotor[agent].cmd)
    xmlFreeNode(delayMotor[agent].cmd);
  delayMotor[agent].cmd = xmlNewNode(ns, (const xmlChar*) cmdMotor);
  delayMotor[agent].delay = delay;
}

static void updateCmd (){
  short agent = agentsNumberMap - 1;
  xmlNodePtr n;
  for (n=root->last;n;n=n->prev)
    if((n->type != XML_CDATA_SECTION_NODE) && (n->type != XML_TEXT_NODE)){
      if (!strcmp((const char*)n->name, "agent")){
            if (agent < agentsNumberUnit){
              if (delayMotor[agent].cmd){
                if (delayMotor[agent].delay == 0){
                  xmlAddPrevSibling(xmlFirstElementChild(n), delayMotor[agent].cmd);
                  delayMotor[agent].cmd = NULL;
                }
                else
                  delayMotor[agent].delay--;
              }
              if (delaySound[agent].cmd != NULL){
                if (delaySound[agent].delay == 0){
                  xmlAddChild(n, delaySound[agent].cmd);
                  delaySound[agent].cmd = NULL;
                }
                else
                  delaySound[agent].delay--;
              }
            }
            agent--;
      }
      else{
        return;
      }
    }
}

static void resetWorldSquareXML(){
  xmlDocPtr temp;
  temp = xsltApplyStylesheet(xslReset, worldsquare, NULL);
  xmlFreeDoc(worldsquare);
  worldsquare = temp;
  root = xmlDocGetRootElement(worldsquare);
}

static void updateAgentsNumberWorldSquareXML(){
  agentsNumberMap = xmlAgentNumber();
  if (agentsNumberUnit > agentsNumberMap){
    int i;
    for(i=agentsNumberMap;i<agentsNumberUnit;i++){
        delayMotor[i].cmd = NULL;
        delaySound[i].cmd = NULL;
    }
    agentsNumberActive = agentsNumberMap;
  }
  else
    agentsNumberActive = agentsNumberUnit;
}

int initializeWorldSquareXML (const short agentsNumber){
  int i;
  xmlSubstituteEntitiesDefault (1);
  xmlLoadExtDtdDefaultValue = 1;
  ns = xmlNewNs(NULL,(xmlChar*)"http://www.nomoseed.org/worldsquare", NULL);
  xslReset = xsltParseStylesheetFile ((const xmlChar *) "web/benchmark/worldsquare_reset.xsl");
  if (!xslReset)
    return 0;
  xslInput = xsltParseStylesheetFile ((const xmlChar *) "web/benchmark/worldsquare_input.xsl");
  if (!xslInput)
    return 0;
  xslOutput = xsltParseStylesheetFile ((const xmlChar *) "web/benchmark/worldsquare_output.xsl");
  if (!xslOutput)
    return 0;
  agentsNumberUnit = agentsNumber;
  sensorsAgent = malloc(sizeof(sensors) * agentsNumberUnit);
  delayMotor = malloc(sizeof(actionDelay) * agentsNumberUnit);
  delaySound = malloc(sizeof(actionDelay) * agentsNumberUnit);
  for(i=0;i<agentsNumberUnit;i++){
      delayMotor[i].cmd = NULL;
      delaySound[i].cmd = NULL;
  }
  return 1;
}

short getAgentNumberActiveWorldSquareXML(){
  return agentsNumberActive;
}

sensors getAgentSensorsWorldSquareXML(const short id){
  return sensorsAgent[id];
}

void assumeWorldSquareXML (){
  xmlDocPtr temp;
  updateCmd();
  temp = xsltApplyStylesheet(xslOutput, worldsquare, NULL);
  xmlFreeDoc(worldsquare);
  worldsquare = xsltApplyStylesheet(xslInput, temp, NULL);
  xmlFreeDoc(temp);
  root = xmlDocGetRootElement(worldsquare);
}

void updateInputsWorldSquareXML (){
  short agent = agentsNumberMap - 1;
  xmlNodePtr n;
  for (n=root->last;n;n=n->prev)
    if((n->type != XML_CDATA_SECTION_NODE) && (n->type != XML_TEXT_NODE)){
      if (!strcmp((const char*)n->name,"agent")){
        if (agent < agentsNumberUnit){
          sensorsAgent[agent].hue[INPUT_HUE_WORLDSQUARE_VALUE] = getData (n, "hue", "value");
          sensorsAgent[agent].sphere[INPUT_SPHERES_NUMBER_WORLDSQUARE_VALUE] = getData (n, "sphere", "number");
          sensorsAgent[agent].bumper[INPUT_BUMPER_WORLDSQUARE_LEFT] = getData (n, "bumper", "left");
          sensorsAgent[agent].bumper[INPUT_BUMPER_WORLDSQUARE_FRONT] = getData (n, "bumper", "front");
          sensorsAgent[agent].bumper[INPUT_BUMPER_WORLDSQUARE_RIGHT] = getData (n, "bumper", "right");
          sensorsAgent[agent].bumper[INPUT_BUMPER_WORLDSQUARE_BACK] = getData (n, "bumper", "back");
          sensorsAgent[agent].sonar[INPUT_SONAR_WORLDSQUARE_LEFT] = getData (n, "sonar", "left");
          sensorsAgent[agent].sonar[INPUT_SONAR_WORLDSQUARE_FRONT] = getData (n, "sonar", "front");
          sensorsAgent[agent].sonar[INPUT_SONAR_WORLDSQUARE_RIGHT] = getData (n, "sonar", "right");
          sensorsAgent[agent].sonar[INPUT_SONAR_WORLDSQUARE_BACK] = getData (n, "sonar", "back");
          sensorsAgent[agent].random[INPUT_RANDOM_WORLDSQUARE_VALUE] = (float)rand()/(float)RAND_MAX;
          if (xmlHasProp(xmlGetNodeByName (root, "sum_sounds"), (const xmlChar*) "value"))
            sensorsAgent[agent].sound[INPUT_SOUND_WORLDSQUARE_VALUE] = getData (root, "sum_sounds", "value");
          else
            sensorsAgent[agent].sound[INPUT_SOUND_WORLDSQUARE_VALUE] = -1.0;
          if (!xmlGetNodeByName(n, "strength"))
            sensorsAgent[agent].strength[INPUT_STRENGTH_WORLDSQUARE_VALUE] = 0.0;
          else
            sensorsAgent[agent].strength[INPUT_STRENGTH_WORLDSQUARE_VALUE] = 1.0;
        }
      /*  FILE* f;
        f = fopen("input.dat","a");
        fprintf(f,"agent %d %f ", agent, sensorsAgent[agent].hue[INPUT_HUE_WORLDSQUARE_VALUE]);
        fprintf(f,"\n");
        fclose(f);*/
        agent--;
      }
      else{
        resetWorldSquareXML();
        return;
      }
  }
  resetWorldSquareXML();
}

void cleanWorldSquareXML(){
  xsltFreeStylesheet(xslReset);
  xsltFreeStylesheet(xslInput);
  xsltFreeStylesheet(xslOutput);
  xmlFreeDoc(worldsquare);
  if(ns)
    xmlFreeNs(ns);
  free(sensorsAgent);
  free(delayMotor);
  free(delaySound);
  xsltCleanupGlobals();
  xmlCleanupParser();
}

void commandMotorWorldSquareXML (const int delay, const short agent, const commandMotor cmd) {
    if (agent < agentsNumberActive){
      if (cmd == turn_left)
        writeCmd(delay, agent, "turn_left");
      else if (cmd == turn_right)
        writeCmd(delay, agent, "turn_right");
      else if (cmd == advance)
        writeCmd(delay, agent, "advance");
      else if (cmd == capture)
        writeCmd(delay, agent, "capture");
      else if (cmd == depose)
        writeCmd(delay, agent, "depose");
    }
}

void commandSoundWorldSquareXML (const int delay, const short agent, const int value) {
  xmlNodePtr sound = xmlNewNode(ns, (const xmlChar*) "sound");
  char str_value[70];
  memset(str_value, '\0', sizeof(str_value));
  sprintf(str_value, "%d", value);
  xmlSetProp (sound, (const xmlChar*) "value", (const xmlChar*) str_value);
  if (delaySound[agent].cmd)
    xmlFreeNode(delaySound[agent].cmd);
  delaySound[agent].cmd = sound;
  delaySound[agent].delay = delay;
}

void readWorldSquareXML (const char* filename) {
  if (worldsquare)
    xmlFreeDoc(worldsquare);
  worldsquare = xmlReadFile(filename, "UTF-8", 0 );
  root = xmlDocGetRootElement(worldsquare);
  updateAgentsNumberWorldSquareXML();
}

void saveWorldSquareXML (const char* filename) {
  xmlSaveFileEnc(filename, worldsquare, "UTF-8");
}
