/************************************************************/
/*    NAME: Xiaobin Zeng                                              */
/*    ORGN: MIT                                             */
/*    FILE: DataManagement.cpp                                        */
/*    DATE:                                                 */
/************************************************************/

#include <iterator>
#include <sys/stat.h>
#include "MBUtils.h"
#include "DataManagement.h"
#include <json/json.h>

using namespace std;

//---------------------------------------------------------
// Constructor
DataManagement::DataManagement()
{
  logEnable = false;

  motionControlInfo.desiredHeading = 0;
  motionControlInfo.desiredSpeed = 0;
  motionControlInfo.desiredDepth = 0;

  nDoublePrecision = 5;
  //AUV状态信息:0~99  
  logVarList["uMotion_pose_log"] = 0;
  //任务历史信息:100~199
  logVarList["uMission_task_log"] = 100;
  //客户端操作记录:200~299
  logVarList["uClient_plandbSet_log"] = 200;
  logVarList["uClient_plandbGet_log"] = 201;
  logVarList["uClient_parameterSet_log"] = 202;
  logVarList["uClient_planControl_log"] = 203;
  logVarList["uClient_manualEnable_log"] = 204;
  logVarList["uClient_manualDrive_log"] = 205;
  //错误日志:300~399
  logVarList["uFH_errorMsg_log"] = 300;
  //运控信息:400~499
  logVarList["uMotion_desired_log"] = 400;
}

//---------------------------------------------------------
// Destructor
DataManagement::~DataManagement()
{
  CloseOutputStream();   
}

//---------------------------------------------------------
// Procedure: OnNewMail
bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
{
  AppCastingMOOSApp::OnNewMail(NewMail);

  DoAsyncLog(NewMail);

  MOOSMSG_LIST::iterator p;
   
  for(p=NewMail.begin(); p!=NewMail.end(); p++) 
  {
    CMOOSMsg &msg = *p;

    string key   = msg.GetKey();
    string comm  = msg.GetCommunity();
    double dval  = msg.GetDouble();
    string sval  = msg.GetString(); 
    string msrc  = msg.GetSource();
    double mtime = msg.GetTime();
    bool   mdbl  = msg.IsDouble();
    bool   mstr  = msg.IsString();

    if (key == "uClient_logEnable_cmd")
    {
      if (sval == "true")
      {
        logEnable = true;
        OpenOutputStream();
      }
      else
      {
        logEnable = false;
        CloseOutputStream();
      }
    }

    if(key == "uDevice_monitor_fb")
    {
      std::string err;
      Json::Value estimatedStateData;
      std::istringstream iss(sval);  
      Json::CharReaderBuilder builder;
      bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err);
      if (!parsingResult) 
      {  
          std::cerr << "Failed to parse JSON string." << std::endl;  
          return false;  
      }     
      std::stringstream ss;
      ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","
        << estimatedStateData["driveMode"].asUInt() << ","
        << estimatedStateData["referenceLon"].asFloat() << ","
        << estimatedStateData["referenceLat"].asFloat() << ","
        << estimatedStateData["referenceAltitude"].asFloat() << ","
        << estimatedStateData["currentLon"].asFloat() << ","
        << estimatedStateData["currentLat"].asFloat() << ","
        << estimatedStateData["currentAltitude"].asFloat() << ","
        << estimatedStateData["north"].asFloat() << ","
        << estimatedStateData["east"].asFloat() << ","
        << estimatedStateData["depth"].asFloat() << ","
        << estimatedStateData["roll"].asFloat() << ","
        << estimatedStateData["pitch"].asFloat() << ","
        << estimatedStateData["yaw"].asFloat() << ","
        << estimatedStateData["insVX"].asFloat() << ","
        << estimatedStateData["insVY"].asFloat() << ","
        << estimatedStateData["insVZ"].asFloat() << ","
        << estimatedStateData["dvlVX"].asFloat() << ","
        << estimatedStateData["dvlVY"].asFloat() << ","
        << estimatedStateData["dvlVZ"].asFloat() << ","
        << estimatedStateData["height"].asFloat() << ","
        << estimatedStateData["rpm"].asInt() << ","
        << estimatedStateData["lightEnable"].asUInt() << ","
        << estimatedStateData["throwingLoadEnable"].asUInt() << ","
        << estimatedStateData["dvlStatus"].asUInt() << ","
        << estimatedStateData["iridium"].asUInt() << ","
        << estimatedStateData["batteryVoltage"].asUInt() << ","
        << estimatedStateData["batteryLevel"].asUInt() << ","
        << estimatedStateData["batteryTemp"].asFloat();
      Notify("uMotion_pose_log", ss.str());
    }

    if(key == "uMission_task_fb")
    {
      std::string err;
      Json::Value missionStatusObject;
      std::istringstream iss(sval);  
      Json::CharReaderBuilder builder;
      bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err);
      try
      {
        if (!parsingResult)
        {
          throw (std::string("parse error"));
        }
        if (missionStatusObject["state"].asInt() == RUN)
        {
          std::stringstream ss;
          ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","
            << missionStatusObject["state"].asInt() << ","
            << missionStatusObject["taskName"].asString() << ","
            << missionStatusObject["destName"].asString() << ","
            << missionStatusObject["errorCode"].asUInt();
          Notify("uMission_task_log", ss.str());
        }
      }
      catch(std::string s)
      {
        std::cout << s << std::endl;
      }
    }

    if((key == "uFH_errorMsg1_fb") || (key == "uFH_errorMsg2_fb") || (key == "uFH_errorMsg3_fb"))
    {
      std::string err;
      Json::Value errorStatus;
      std::istringstream iss(sval);  
      Json::CharReaderBuilder builder;
      bool parsingResult = Json::parseFromStream(builder, iss, &errorStatus, &err);  
      try
      {
        if (!parsingResult) 
        {
          throw (std::string("parse error"));
        }
        if (errorStatus["FaultMsgs"].empty())
        {
          throw (std::string("faultMsgs empty"));
        }

        std::stringstream ss;
        ss << std::fixed << std::setprecision(6) << MOOS::Time() << ",";
        ss << errorStatus["FaultLevel"].asInt() << ",";
        std::string prefix = ss.str();
        
        Json::Value faultMsgsObject = errorStatus["FaultMsgs"];
        std::vector<std::string> typeList = faultMsgsObject.getMemberNames();
        int typeSize = typeList.size();
        std::vector<std::string> contentList;
        for(int i=0; i< typeSize; i++)
        {
          ss.str("");
          std::string faultType = typeList.at(i);
          ss << faultType << ",";
          Json::Value faultIDArray = faultMsgsObject[faultType]["FaultID"];
          if (!faultIDArray.isArray())
          {
            throw (std::string(faultType + " FaultID is not array"));
          }
          ss << "[";
          for(int j=0; j< faultIDArray.size(); j++)
          {
            if (j == faultIDArray.size() - 1)
            {
              ss << faultIDArray[j].asInt();
            }
            else
            {
              ss << faultIDArray[j].asInt() << ",";              
            }
          }
          ss << "],";
          ss << faultMsgsObject[faultType]["FirstRecvTime"].asString() << ",";
          ss << faultMsgsObject[faultType]["Source"].asString();
          contentList.push_back(prefix + ss.str()); 
        }
        int contentSize = contentList.size();
        ss.str("");
        for (int i = 0; i < contentSize; i++)
        {

          if (i == contentSize - 1)
          {
            ss << contentList.at(i);
          }
          else
          {
            ss << contentList.at(i) << std::endl;            
          }          
        }
        Notify("uFH_errorMsg_log", ss.str());
      }
      catch(std::string s)
      {
        std::cout << s << std::endl;
      }
    }
    if(key == "DESIRED_HEADING")
    {
      motionControlInfo.desiredHeading = dval;
      std::stringstream ss;
      ss << std::fixed << std::setprecision(6) << MOOS::Time() << ",";
      ss << motionControlInfo.desiredHeading << ","
        << motionControlInfo.desiredSpeed << ","
        << motionControlInfo.desiredDepth;
      Notify("uMotion_desired_log", ss.str());                     
    }
    if(key == "DESIRED_SPEED")
    {
      motionControlInfo.desiredSpeed = dval;
      std::stringstream ss;
      ss << std::fixed << std::setprecision(6) << MOOS::Time() << ",";
      ss << motionControlInfo.desiredHeading << ","
        << motionControlInfo.desiredSpeed << ","
        << motionControlInfo.desiredDepth;
      Notify("uMotion_desired_log", ss.str());        
    }
    if(key == "DESIRED_DEPTH")
    {
      motionControlInfo.desiredDepth = dval;
      std::stringstream ss;
      ss << std::fixed << std::setprecision(6) << MOOS::Time() << ",";
      ss << motionControlInfo.desiredHeading << ","
        << motionControlInfo.desiredSpeed << ","
        << motionControlInfo.desiredDepth;
      Notify("uMotion_desired_log", ss.str());        
    }    
  }
	
  return(true);
}

bool DataManagement::DoAsyncLog(MOOSMSG_LIST &NewMail)
{
  MOOSMSG_LIST::iterator q;
  std::stringstream sStream;
  int saveFileIndex = -1;
  for(q = NewMail.begin();q!=NewMail.end();q++)
  {
    CMOOSMsg & rMsg = *q;
    if(logVarList.find(rMsg.m_sKey)!=logVarList.end()) 
    {
      std::map<std::string, int>::iterator q = logVarList.find(rMsg.m_sKey);
      saveFileIndex = q->second;

      std::stringstream sEntry;
      sEntry.setf(ios::left);
      sEntry.setf(ios::fixed);
      if(rMsg.IsDataType(MOOS_STRING) || rMsg.IsDataType(MOOS_DOUBLE))
      {
        sEntry << rMsg.GetAsString(12,nDoublePrecision) << ' ';
      }

      if ((saveFileIndex >= 0) && (saveFileIndex < 100))
      {
        if(auvDataStream.is_open())
        {
          auvDataStream << sEntry.str() << std::endl;
        }
      }
      if ((saveFileIndex >= 100) && (saveFileIndex < 200))
      {
        if(missionHistoryStream.is_open())
        {
          missionHistoryStream << sEntry.str() << std::endl;
        }
      }
      if ((saveFileIndex >= 200) && (saveFileIndex < 300))
      {
        if(clientCommandStream.is_open())
        {
          clientCommandStream << sEntry.str() << std::endl;
        }
      }
      if ((saveFileIndex >= 300) && (saveFileIndex < 400))
      {
        if(faultLogStream.is_open())
        {
          faultLogStream << sEntry.str() << std::endl;
        }
      }
      if ((saveFileIndex >= 400) && (saveFileIndex < 500))
      {
        if(motionControlStream.is_open())
        {
          motionControlStream << sEntry.str() << std::endl;
        }
      }
      contentFromStream = sEntry.str();
    }
  } 
  return true;
}

//---------------------------------------------------------
// Procedure: OnConnectToServer

bool DataManagement::OnConnectToServer()
{
   RegisterVariables();
   return(true);
}

//---------------------------------------------------------
// Procedure: Iterate()
//            happens AppTick times per second

bool DataManagement::Iterate()
{
  AppCastingMOOSApp::Iterate();

  auvDataStream.flush();
  missionHistoryStream.flush();
  clientCommandStream.flush();
  faultLogStream.flush();
  motionControlStream.flush();
  AppCastingMOOSApp::PostReport();  

  return(true);
}

//---------------------------------------------------------
// Procedure: OnStartUp()
//            happens before connection is open

bool DataManagement::OnStartUp()
{
  AppCastingMOOSApp::OnStartUp();

  // list<string> sParams;
  // m_MissionReader.EnableVerbatimQuoting(false);
  // if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
  //   list<string>::iterator p;
  //   for(p=sParams.begin(); p!=sParams.end(); p++) {
  //     string line  = *p;
  //     string param = tolower(biteStringX(line, '='));
  //     string value = line;
      
  //     if(param == "foo") {
  //       //handled
  //     }
  //     else if(param == "bar") {
  //       //handled
  //     }
  //   }
  // }

  m_MissionReader.GetValue("LogDir", saveLogDir);
  std::string vehicleName;
  m_MissionReader.GetValue("VehicleName", vehicleName);
  m_MissionReader.GetValue("LogEnable", logEnable);

  // std::string auvDataFile;
  // m_MissionReader.GetValue("AuvDataLog", auvDataFile);  
  // std::string missionHistoryFile;
  // m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile);    
  // std::string clientCommandFile;
  // m_MissionReader.GetValue("ClientCommandLog", clientCommandFile);  
  // std::string faultLogFile;
  // m_MissionReader.GetValue("FaultLog", faultLogFile);  
  // std::string motionControlFile;
  // m_MissionReader.GetValue("MotionControlLog", motionControlFile);  

  if (access(saveLogDir.c_str(), F_OK) == -1 )
  {
    mode_t mode = 0775;
    mkdir(saveLogDir.c_str(), mode);
  }
  saveLogDir += "/" + vehicleName; 
  if (access(saveLogDir.c_str(), F_OK) == -1 )
  {
    mode_t mode = 0775;
    mkdir(saveLogDir.c_str(), mode);
  }

  if (logEnable)
  {
    // std::string subDir;
    // std::string localTime;
    // GenerateFileName(subDir, localTime);
    // saveLogDir += "/" + subDir;    
    // if (access(saveLogDir.c_str(), F_OK) == -1 )
    // {
    //   mode_t mode = 0775;
    //   mkdir(saveLogDir.c_str(), mode);
    // }
    // saveLogDir += "/" + localTime;    
    // if (access(saveLogDir.c_str(), F_OK) == -1 )
    // {
    //   mode_t mode = 0775;
    //   mkdir(saveLogDir.c_str(), mode);
    // }
    // std::string auvDataSavePath = saveLogDir + "/" + auvDataFile;
    // std::string missionHistorySavePath = saveLogDir + "/" + missionHistoryFile;
    // std::string clientCommandSavePath = saveLogDir + "/" + clientCommandFile;
    // std::string faultLogSavePath = saveLogDir + "/" + faultLogFile;
    // std::string motionControlSavePath = saveLogDir + "/" + motionControlFile;

    // if(!OpenFile(auvDataStream, auvDataSavePath))
    //   return MOOSFail("Failed to Open auvData file");
    // if(!OpenFile(missionHistoryStream, missionHistorySavePath))
    //   return MOOSFail("Failed to Open missionHistory file");
    // if(!OpenFile(clientCommandStream, clientCommandSavePath))
    //   return MOOSFail("Failed to Open clientCommand file");
    // if(!OpenFile(faultLogStream, faultLogSavePath))
    //   return MOOSFail("Failed to Open faultLog file");
    // if(!OpenFile(motionControlStream, motionControlSavePath))
    //   return MOOSFail("Failed to Open faultLog file");

    // DoAuvDataLogBanner(auvDataStream);
    // DoMissionHistoryBanner(missionHistoryStream);
    // DoFaultHandleBanner(faultLogStream);
    // DoMotionControlBanner(motionControlStream);

    OpenOutputStream();
  }

  RegisterVariables();
  return(true);
}

//---------------------------------------------------------
// Procedure: RegisterVariables

void DataManagement::RegisterVariables()
{
  AppCastingMOOSApp::RegisterVariables();

  Register("uDevice_monitor_fb", 0);
  Register("uMission_task_fb", 0);
  Register("uMission_task_log", 0);
  Register("uMotion_pose_log", 0);
  Register("uClient_plandbSet_log", 0);
  Register("uClient_plandbGet_log", 0);
  Register("uClient_parameterSet_log", 0);
  Register("uClient_planControl_log", 0);
  Register("uClient_manualEnable_log", 0);
  Register("uClient_manualDrive_log", 0);
  Register("uFH_errorMsg1_fb", 0);
  Register("uFH_errorMsg2_fb", 0);
  Register("uFH_errorMsg3_fb", 0);
  Register("uFH_errorMsg_log", 0);
  Register("DESIRED_HEADING", 0);
  Register("DESIRED_SPEED", 0);
  Register("DESIRED_DEPTH", 0);
  Register("uMotion_desired_log", 0);
  Register("uClient_logEnable_cmd", 0); 
}

bool DataManagement::buildReport()
{
  m_msgs << contentFromStream << endl;
  return true;
}

double DataManagement::getTimeStamp()
{
  struct timeval tv;
  gettimeofday(&tv,NULL);
  double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000;
  return stamp;  
}

void DataManagement::DoAuvDataLogBanner(std::ofstream &os)
{
    os << "stamp" << ",";
    os << "mode" << ",";
    os << "refLon" << ",";
    os << "refLat" << ",";
    os << "refAlt" << ",";
    os << "curLon" << ",";
    os << "curLat" << ",";
    os << "curAlt" << ",";
    os << "north" << ",";
    os << "east" << ",";
    os << "depth" << ",";
    os << "roll" << ",";
    os << "pitch" << ",";
    os << "yaw" << ",";
    os << "insVX" << ",";
    os << "insVY" << ",";
    os << "insVZ" << ",";
    os << "dvlVX" << ",";
    os << "dvlVY" << ",";
    os << "dvlVZ" << ",";
    os << "height" << ",";
    os << "thrust" << ",";
    os << "light" << ",";
    os << "load" << ",";
    os << "dvl" << ",";
    os << "iridium" << ",";
    os << "batteryVol" << ",";
    os << "batteryLev" << ",";
    os << "batteryTemp" << std::endl;
}

void DataManagement::DoMissionHistoryBanner(std::ofstream &os)
{
  os << "stamp" << ",";
  os << "state" << ",";
  os << "taskName" << ",";
  os << "destName" << ",";
  os << "errorCode" << std::endl;
}

void DataManagement::DoFaultHandleBanner(std::ofstream &os)
{
  os << "stamp" << ",";
  os << "level" << ",";
  os << "type" << ",";
  os << "id" << ",";
  os << "first" << ","; 
  os << "source" << std::endl;
}

void DataManagement::DoMotionControlBanner(std::ofstream &os)
{
  os << "stamp" << ",";
  os << "desired_heading" << ",";
  os << "desired_speed" << ",";
  os << "desired_depth" << std::endl;
}

bool DataManagement::OpenFile(std::ofstream & of,const std::string & sName)
{
	of.open(sName.c_str());

  if(!of.is_open())
  {
      return false;
  }
  return true;
}

void DataManagement::GenerateFileName(std::string &fileDir, std::string &fileName)
{
  time_t now = time(0);
  tm *gmtm = localtime(&now);
  int year = 1900 + gmtm->tm_year;
  int monthTemp = gmtm->tm_mon+1;
  std::string month;
  if (monthTemp < 10)
  {
    month = "0" + std::to_string(monthTemp);
  }
  else
  {
    month = std::to_string(monthTemp);
  }
  int dayTemp = gmtm->tm_mday;  
  std::string day;
  if (dayTemp < 10)
  {
    day = "0" + std::to_string(dayTemp);
  }
  else
  {
    day = std::to_string(dayTemp);
  }
  int hourTemp = gmtm->tm_hour;
  std::string hour;
  if (hourTemp < 10)
  {
    hour = "0" + std::to_string(hourTemp);
  }
  else
  {
    hour = std::to_string(hourTemp);
  }
  int minuteTemp = gmtm->tm_min;
  std::string minute;
  if (minuteTemp < 10)
  {
    minute = "0" + std::to_string(minuteTemp);
  }
  else
  {
    minute = std::to_string(minuteTemp);
  }
  int secondTemp = gmtm->tm_sec;
  std::string second;
  if (secondTemp < 10)
  {
    second = "0" + std::to_string(secondTemp);
  }
  else
  {
    second = std::to_string(secondTemp);
  }
  std::stringstream ss;
  ss << year << "-" << month << "-" << day;
  fileDir = ss.str();
  ss.str("");
  ss << hour << minute << second;  
  fileName = ss.str();  
}

bool DataManagement::OpenOutputStream()
{
  std::string subDir;
  std::string localTime;
  GenerateFileName(subDir, localTime);
  std::string saveSubLogDir = saveLogDir + "/" + subDir;    
  if (access(saveSubLogDir.c_str(), F_OK) == -1 )
  {
    mode_t mode = 0775;
    mkdir(saveSubLogDir.c_str(), mode);
  }
  saveSubLogDir += "/" + localTime;    
  if (access(saveSubLogDir.c_str(), F_OK) == -1 )
  {
    mode_t mode = 0775;
    mkdir(saveSubLogDir.c_str(), mode);
  }

  std::string auvDataFile;
  m_MissionReader.GetValue("AuvDataLog", auvDataFile);  
  std::string missionHistoryFile;
  m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile);    
  std::string clientCommandFile;
  m_MissionReader.GetValue("ClientCommandLog", clientCommandFile);  
  std::string faultLogFile;
  m_MissionReader.GetValue("FaultLog", faultLogFile);  
  std::string motionControlFile;
  m_MissionReader.GetValue("MotionControlLog", motionControlFile);  

  std::string auvDataSavePath = saveSubLogDir + "/" + auvDataFile;
  std::string missionHistorySavePath = saveSubLogDir + "/" + missionHistoryFile;
  std::string clientCommandSavePath = saveSubLogDir + "/" + clientCommandFile;
  std::string faultLogSavePath = saveSubLogDir + "/" + faultLogFile;
  std::string motionControlSavePath = saveSubLogDir + "/" + motionControlFile;

  if(!OpenFile(auvDataStream, auvDataSavePath))
    return MOOSFail("Failed to Open auvData file");
  if(!OpenFile(missionHistoryStream, missionHistorySavePath))
    return MOOSFail("Failed to Open missionHistory file");
  if(!OpenFile(clientCommandStream, clientCommandSavePath))
    return MOOSFail("Failed to Open clientCommand file");
  if(!OpenFile(faultLogStream, faultLogSavePath))
    return MOOSFail("Failed to Open faultLog file");
  if(!OpenFile(motionControlStream, motionControlSavePath))
    return MOOSFail("Failed to Open faultLog file");

  DoAuvDataLogBanner(auvDataStream);
  DoMissionHistoryBanner(missionHistoryStream);
  DoFaultHandleBanner(faultLogStream);
  DoMotionControlBanner(motionControlStream);

  return true;
}


void DataManagement::CloseOutputStream()
{
  if(auvDataStream.is_open())
  {
    auvDataStream.close();
  }
  if(missionHistoryStream.is_open())
  {
    missionHistoryStream.close();
  }
  if(clientCommandStream.is_open())
  {
    clientCommandStream.close();
  }
  if(faultLogStream.is_open())
  {
    faultLogStream.close();
  }
  if(motionControlStream.is_open())
  {
    motionControlStream.close();
  }
}