text:
#include "TopologyOracle.hpp"
#include "Scheduler.hpp"
#include "boost/random/discrete_distribution.hpp"
#include "boost/random/uniform_int.hpp"
#include <boost/lexical_cast.hpp>
#include <initializer_list>
#include <boost/random/mersenne_twister.hpp>
#include <cmath>
#define ILOUSESTL
#define IL_STD
#include "ilcplex/ilocplex.h"

// random generator
extern boost::mt19937 gen;

TopologyOracle::TopologyOracle(Topology* topo, po::variables_map vm, uint roundDuration) {
  this->roundDuration = roundDuration;
  this->cachingOpt = vm["optimize-caching"].as<bool>();
  this->ponCardinality = vm["pon-cardinality"].as<uint>();
  this->policy = (CachePolicy) vm["cache-policy"].as<uint>();
  this->maxCacheSize = vm["ucache-size"].as<uint>() * 8000; // input is in GB, variable in Mb
  this->maxLocCacheSize = vm["lcache-size"].as<uint>()  * 8000; // input is in GB, variable in Mb;
  this->reducedCaching = vm["reduced-caching"].as<bool>();
  this->topo = topo;
  this->mode = (SimMode) vm["sim-mode"].as<uint>();
  this->avgHoursPerUser = vm["avg-hours-per-user"].as<double>();
  this->avgContentLength = vm["content-length"].as<double>(); // in minutes
  this->devContentLength = vm["content-dev"].as<double>();
  this->peakReqRatio = vm["peak-req-ratio"].as<uint>();
  this->bitrate = vm["bitrate"].as<uint>();
  this->preCaching = vm["pre-caching"].as<bool>();
  //FIXME: chunkSize is integer here but we compare it with sizeDownloaded which is double
  this->chunkSize = vm["chunk-size"].as<uint>();
  this->bufferSize = vm["buffer-size"].as<uint>();
  //FIXME: this assumes constant bitrate for upload and a 10GPON
  this->maxUploads = std::floor(10240 / (ponCardinality * bitrate));
  this->avgReqLength = (avgContentLength * 60 *  // in seconds
          std::accumulate(sessionLength.begin(), sessionLength.end(), 0)/sessionLength.size());
  uint rounds = vm["rounds"].as<uint>();
  flowStats.avgFlowDuration.assign(rounds, 0);
  flowStats.avgPeerFlowDuration.assign(rounds, 0);
  flowStats.avgCacheFlowDuration.assign(rounds, 0);
  flowStats.avgUserCacheOccupancy.assign(rounds, 0);
  flowStats.avgASCacheOccupancy.assign(rounds, 0);
  flowStats.completedRequests.assign(rounds, 0);
  flowStats.localRequests.assign(rounds, 0);
  flowStats.servedRequests.assign(rounds, 0);
  flowStats.fromASCache.assign(rounds, 0);
  flowStats.fromPeers.assign(rounds, 0);
  flowStats.fromCentralServer.assign(rounds, 0);
  flowStats.congestionBlocked.assign(rounds, 0);
  flowStats.cacheOptimized.assign(rounds, 0);
  this->userCacheMap = new UserCacheMap;
  this->asidContentMap = new AsidContentMap;
  for (uint i = 0; i < topo->getNumASes(); i++) {
    ChunkMap map;
    asidContentMap->insert(std::make_pair(i,map));
  }

  // Initialize user cache map
    VertexVec ponNodes = topo->getPonNodes();
    BOOST_FOREACH(Vertex v, ponNodes) {
    for(uint i = 0; i < topo->getPonCustomers(v); i++) {
        PonUser user = std::make_pair(v, i);
        ChunkCache cache(maxCacheSize, policy);
        userCacheMap->insert(std::make_pair(user, cache));
      }
    }
  // Initialize local cache nodes
  this->localCacheMap = new LocalCacheMap;
  if (!reducedCaching) {
    VertexVec cacheNodes = topo->getLocalCacheNodes();
    BOOST_FOREACH(Vertex v, cacheNodes) {
      ChunkCache cache(maxLocCacheSize, policy);
      localCacheMap->insert(std::make_pair(v, cache));
    }
  } else {
    Vertex v = topo->getCentralServer();
    ChunkCache cache(maxLocCacheSize, policy);
    localCacheMap->insert(std::make_pair(v, cache));
  }

  //Initialize dailyRanking
  RankingTable<ContentElement*> ranking;
  dailyRanking.resize(7, ranking);

}

TopologyOracle::~TopologyOracle() {
  UserCacheMap::iterator ucit;
  for (ucit = userCacheMap->begin(); ucit != userCacheMap->end(); ucit++) {
    ucit->second.clearCache();
  }
  userCacheMap->clear();
  delete this->userCacheMap;
  asidContentMap->clear();
  delete this->asidContentMap;
  localCacheMap->clear();
  delete this->localCacheMap;
  dailyRanking.clear();
}

void TopologyOracle::addToCache(PonUser user, ChunkPtr chunk, SimTime time) {
  if (!chunk) {
    BOOST_LOG_TRIVIAL(error) << "TopologyOracle::addToCache() - invalid ChunkPtr";
    abort();
  }
  ContentElement* content = chunk->getContent();
  uint chunkIndex = chunk->getIndex();
  // update the chunkMap for content retrieval
  uint asid = topo->getAsid(user);
  ChunkMap::iterator cIt = asidContentMap->at(asid).find(chunk);
  std::pair<bool, std::set<ChunkPtr> > addResult;
  if (cIt == asidContentMap->at(asid).end()) {
    // This is the first time the oracle sees this chunk, add it to the map
    // Note: this should not happen now, throw an error
    BOOST_LOG_TRIVIAL(error) << time << ":ERROR: TopologyOracle::addToCache() - "
            "unknown chunk for content " << content->getName() <<
            ", chunkId " << chunkIndex;
    abort();
    // std::set<PonUser> sourceNodes;
    // cIt = asidContentMap->at(asid).insert(std::make_pair(content, sourceNodes)).first;
  }
  // update userCacheMap according to the specified cache policy
  BOOST_LOG_TRIVIAL(trace) << time << ": caching chunk " <<
          chunk->getIndex() << " of content " << content->getName() <<
          " at User " << user.first << "," << user.second;
  addResult = userCacheMap->at(user).addToCache(chunk, chunk->getSize(), time);
  if (!addResult.first) {
    if (userCacheMap->at(user).getMaxSize() >= chunk->getSize()) {
      BOOST_LOG_TRIVIAL(warning) << time << ": WARNING: failed to cache chunk "
            << chunk->getIndex() << " of content "
            << content->getName()
            << " at User "  << user.first << "," << user.second;
    }
  } else {
   /* if caching at the user was successful, add the user to the asidCacheMap
    * for its AS (cIt is the iterator to the right position in the map, see above)
    * note that the user might be already there if he had a copy of the content
    * with a different size
    */
    if (cIt->second.find(user) == cIt->second.end()) {
      bool insert = cIt->second.insert(user).second;
      if (!insert) {
        BOOST_LOG_TRIVIAL(warning) << time << ": WARNING: failed to insert User "
                << user.first << "," << user.second << " as a source for chunk "
                << chunk->getIndex() << " of content "
                << content->getName() << " in AS " << asid;
      }
    }
  }
  BOOST_FOREACH (ChunkPtr e, addResult.second) {
    // erase the content from the ContentMap entry of the user
    this->removeFromCMap(e, user);
    // update the asidContentMap too
    cIt = asidContentMap->at(asid).find(e);
    if (cIt != asidContentMap->at(asid).end())
      cIt->second.erase(user);
  }

  if (!reducedCaching && !preCaching) {
    // also update the local Cache associated to this user ASID
    // (the flow is not simulated as it is assumed it was cached as it transited
    // towards the user)
    Vertex lCache = topo->getLocalCache(user.first);
    if (!localCacheMap->at(lCache).isCached(chunk)) {
      // debug info
      BOOST_LOG_TRIVIAL(trace) << time << ": caching chunk " <<
          chunk->getIndex() << " of content " << content->getName() <<
          " at local cache " << lCache;
      addResult = localCacheMap->at(lCache).addToCache(chunk, chunk->getSize(), time);
      if (!addResult.first) {
        BOOST_LOG_TRIVIAL(warning) << time << ": WARNING: failed to cache caching chunk " <<
                chunk->getIndex() << " of content " << content->getName()
                << " at AS cache "  << lCache;
      }
    }
  }
}

void TopologyOracle::clearUserCache() {
  // clear the actual caches
  UserCacheMap::iterator uIt;
  for (uIt = userCacheMap->begin(); uIt != userCacheMap->end(); uIt++) {
    uIt->second.clearCache();
  }
  // clear the chunkMap to be consistent with the user caches
  AsidContentMap::iterator aIt;
  for (aIt = asidContentMap->begin(); aIt != asidContentMap->end(); aIt++) {
    ChunkMap::iterator cIt;
    for (cIt = aIt->second.begin(); cIt != aIt->second.end(); cIt++) {
      cIt->second.clear();
    }
  }
}

void TopologyOracle::clearLocalCache() {
  LocalCacheMap::iterator lIt;
  for (lIt = localCacheMap->begin(); lIt != localCacheMap->end(); lIt++) {
    lIt->second.clearCache();
  }
}

/* Attempts to find a source for the requested content. The priority is:
 * user's own cache -> other local peers -> local AS cache -> non local peers -> central server
 */
bool TopologyOracle::serveRequest(Flow* flow, Scheduler* scheduler) {
  PonUser destination = flow->getDestination();
  PonUser closestSource = UNKNOWN;
  SimTime time = scheduler->getSimTime();
  int currentDay = scheduler->getCurrentRound();
  ContentElement* content = flow->getContent();
  std::string contentName = content->getName();
  uint chunkId = flow->getChunkId();
  const ChunkPtr chunk = content->getChunkById(chunkId);
  BOOST_LOG_TRIVIAL(trace) << time << ": fetching source for chunk " << chunkId
            << " of content " << contentName
            << " to user " << destination.first << "," << destination.second;
  // update the number of requests for this content if it's the first chunk
  if (chunk->getIndex() == 0)
    dailyRanking.at(currentDay-content->getReleaseDay()).hit(content);
  // also increase the number of hits of the chunk (currently not used)
  // chunk->increaseViewsThisRound();
  // Set the flow as a transfer, since we are now assigning the source
  flow->setFlowType(FlowType::TRANSFER);
  // First check if the content is already in the user cache: if it is, there's
  // no need to simulate the data exchange and we only need to update the
  // cache stats.
  if (checkIfCached(destination, chunk)) {
    // signal to the scheduler that this flow is "virtual"
    flow->setSource(destination);
    flow->setEta(time);
    // update this user's cacheMap entry (for LRU/LFU)
    bool result = userCacheMap->at(destination).getFromCache(chunk,
            (scheduler->getCurrentRound()+1)*time, true);
    assert(result);
    flowStats.servedRequests.at(scheduler->getCurrentRound())++;
    flowStats.completedRequests.at(scheduler->getCurrentRound())++;
    flowStats.localRequests.at(scheduler->getCurrentRound())++;
    flowStats.fromPeers.at(scheduler->getCurrentRound())++;
    // debug info
    BOOST_LOG_TRIVIAL(debug) << time << ": user " << destination.first
            << "," << destination.second
            << " had a local copy of chunk " << chunkId << " from content "
            << contentName;
    return true;
  }
  // try to find a local peer first
  uint asid = topo->getAsid(destination);
  std::set<PonUser> localSources = asidContentMap->at(asid).at(chunk);
  // copy the set to a vector which can then be randomized to reduce the chance
  // the same peer will be selected over and over until congestion is reached
  std::vector<PonUser> randSources(localSources.begin(), localSources.end());
  std::random_shuffle(randSources.begin(), randSources.end());
  localSources.clear();
  bool foundSource = false;
  // Attempts to find the closest source whose route to the destination is not congested
  while (!randSources.empty() && !foundSource) {
    closestSource = randSources.back();
    // check if the identified source has enough content to serve this request
    if (this->checkIfCached(closestSource, chunk)
            && !topo->isCongested(closestSource, destination))
      foundSource = true;
    else {
      // erase closestSource from randSources
      randSources.pop_back();
      closestSource = UNKNOWN;
    }
  }
  if (!foundSource)
  {
    // Couldn't find a local source for the required content, check for the
    // local AS cache (unless we're in reducedCaching mode)

    if (!reducedCaching)
    {
      Vertex lCache = topo->getLocalCache(flow->getDestination().first);
      if (checkIfCached(lCache, chunk)) {
        // NOTE: We should check here if the downlink is congested, but with our
        // current hypothesis it should never be the case
        assert(topo->isCongested(std::make_pair(lCache, 0), destination) == false);
        // debug info
        BOOST_LOG_TRIVIAL(trace) << time << ": user " << destination.first
                << "," << destination.second
                << " downloading chunk " << chunkId
                << " of content " << contentName
                << " from AS cache node " << lCache
                << " (closestSource=" << closestSource.first
                << "," << closestSource.second << ")";
        this->getFromLocalCache(lCache, chunk,
                (scheduler->getCurrentRound()+1)*time);
        flow->setSource(std::make_pair(lCache, 0));
        flowStats.servedRequests.at(scheduler->getCurrentRound())++;
        flowStats.localRequests.at(scheduler->getCurrentRound())++;
        flow->setP2PFlow(false);
        scheduler->schedule(flow);
        topo->updateCapacity(flow, scheduler, true);
        return true;
      }
    }
    // no local peer and no micro cache, look for non local peer
    BOOST_LOG_TRIVIAL(trace) << "No local source found for chunk " << chunkId
            << " of content "+
            contentName+" , searching for non-local P2P sources";
    uint minDistance, distance, asIndex;
    ChunkMap::iterator cIt;
    // vector of ASes which have already explored for viable sources
    std::vector<bool> exploredASes(topo->getNumASes(), false);
    exploredASes[asid] = true;
    foundSource = false;
    while (std::find(exploredASes.begin(), exploredASes.end(), false) != exploredASes.end()
            && !foundSource) {
      // identify an AS with minimum distance from destination
      minDistance = INF_TIME;
      for (uint i = 0; i < topo->getNumASes(); i++) {
        if (exploredASes[i])
          continue;
        cIt = asidContentMap->at(i).find(chunk);
        if (cIt == asidContentMap->at(i).end() || cIt->second.empty()) {
          // this AS has no source with the required chunk, mark it as explored
          // to save time at a future iteration
          BOOST_LOG_TRIVIAL(trace) << "No viable source for chunk " << chunkId
            << " of content " + contentName + " in asid " << i;
          exploredASes[i] = true;
        } else {
          distance = topo->getRoute(*(cIt->second.begin()), destination).size();
          if (distance < minDistance) {
            minDistance = distance;
            asIndex = i;
          }
        }
      }
      // scan available sources in the identified closer AS
      if (minDistance != INF_TIME) {
        // Attempts to find the closest source whose route to the destination is not congested
        localSources.clear();
        localSources = asidContentMap->at(asIndex).at(chunk);
        randSources.clear();
        randSources.assign(localSources.begin(), localSources.end());
        std::random_shuffle(randSources.begin(), randSources.end());
        localSources.clear();
        while (!randSources.empty() && !foundSource) {
          closestSource = randSources.back();
          // check if the identified source has enough content to serve this request
          if (this->checkIfCached(closestSource, chunk)
                  && !topo->isCongested(closestSource, destination))
              foundSource = true;
          else {
            // erase closestSource from randSources
            randSources.pop_back();
            closestSource = UNKNOWN;
          }
        }
        exploredASes[asIndex] = true;
      }
    }
  }
  // if the closestSource is still unknown, we have to go to the central server
  if (closestSource == UNKNOWN) {
    // Ensure the central server was specified in the topology file
    Vertex centralServer = topo->getCentralServer();
    assert(centralServer != std::numeric_limits<unsigned int>::max());
    // The 1 in the second PonUser field is to differentiate it from an AS Cache
    closestSource = std::make_pair(centralServer, 1);
    if (topo->isCongested(closestSource, destination)) {
      // This was our last hope: there is no uncongested route to any source
      flowStats.congestionBlocked.at(scheduler->getCurrentRound())++;
     BOOST_LOG_TRIVIAL(info) << time << ": user " << destination.first
            << "," << destination.second
            << " could not find an un-congested route to chunk " << chunkId
            << " of content " << contentName;
      return false;
    }
    // If we're in reducedCaching mode, check if the content is stored on the
    // central cache, otherwise fetch it off-network
    if (reducedCaching) {
      if (!checkIfCached(centralServer, chunk)) {
        localCacheMap->at(centralServer).addToCache(chunk,
                chunk->getSize(),
                (scheduler->getCurrentRound()*roundDuration)+time);
      }
      // update LFU/LRU stats
      this->getFromLocalCache(centralServer, chunk,
              (scheduler->getCurrentRound()+1)*time);
    }
    flow->setP2PFlow(false);
    // debug info
    BOOST_LOG_TRIVIAL(trace) << time << ": user " << destination.first
            << "," << destination.second
            << " downloading chunk " << chunkId
            << " of content " << contentName
            << " from central server";
  } else {
    // p2p flow, update user cache statistics (for LFU/LRU purposes)
    flow->setP2PFlow(true);
    bool result = userCacheMap->at(closestSource).getFromCache(chunk,
            (scheduler->getCurrentRound()+1)*time, false);
    assert(result);
    // check for locality is done here to avoid central server to be mistakenly
    // identified as local
    if (topo->isLocal(destination.first, closestSource.first))
      flowStats.localRequests.at(scheduler->getCurrentRound())++;
    // debug info
    BOOST_LOG_TRIVIAL(trace) << time << ": user " << destination.first
            << "," << destination.second << " (asid " << topo->getAsid(destination)
            << ") downloading chunk " << chunkId
            << " of content " << contentName
            << " from peer " << closestSource.first << ","
            << closestSource.second << " (asid " << topo->getAsid(destination)
            << ")";
  }
  // Assign the closest source to the flow
  flow->setSource(closestSource);
  // Update flow statistics
  flowStats.servedRequests.at(scheduler->getCurrentRound())++;
  // schedule flow with INF_TIME ETA so that it's in the queue for future updating
  scheduler->schedule(flow);
  // update network load and estimate ETA based on available bandwidth
  topo->updateCapacity(flow, scheduler, true);
  return true;
}

void TopologyOracle::notifyCompletedFlow(Flow* flow, Scheduler* scheduler) {
  PonUser dest = flow->getDestination();
  SimTime time = scheduler->getSimTime();
  uint round = scheduler->getCurrentRound();
  const ChunkPtr chunk = flow->getContent()->getChunkById(flow->getChunkId());
  switch(flow->getFlowType()) {
    case FlowType::TRANSFER:
    {
      if (flow->getSource() != dest) {
      BOOST_LOG_TRIVIAL(debug) << "At time " << time << " user " << dest.first
              << "," << dest.second << " completed download of chunk "
              << chunk->getIndex() << " of content " << chunk->getContent()->getName();
      flow->updateSizeDownloaded(time);
      if (flow->getSizeDownloaded() < chunkSize) {
        // ensure that this is a result of a discrete time scale (approximation to previous second)
        assert(chunk->getSize() - flow->getSizeDownloaded() <= flow->getBandwidth());
        BOOST_LOG_TRIVIAL(trace) << time << ": completed flow has sizeDownloaded (" <<
                flow->getSizeDownloaded() << ") < Chunk Size (" <<
                chunkSize << ") due to time approximation, fixing this";
        flow->setSizeDownloaded(chunkSize);
      }
      // update flow statistics
      SimTime flowDuration = time - flow->getStart();
      // Update average flow duration stats, both generic and p2p / cache ones
      flowStats.avgFlowDuration.at(round) = (flowStats.avgFlowDuration.at(round)
              * flowStats.completedRequests.at(round)
              + flowDuration) / (flowStats.completedRequests.at(round) + 1);
      flowStats.completedRequests.at(round)++;
      BOOST_LOG_TRIVIAL(trace) << "Flow duration: " << flowDuration <<
              "; avgFlowDuration: " << flowStats.avgFlowDuration.at(round);

      if (flow->isP2PFlow()) {
        flowStats.avgPeerFlowDuration.at(round) = (flowStats.avgPeerFlowDuration.at(round)
                * flowStats.fromPeers.at(round)
                + flowDuration) / (flowStats.fromPeers.at(round) + 1);
        flowStats.fromPeers.at(round)++;
      } else {
        flowStats.avgCacheFlowDuration.at(round) = (flowStats.avgCacheFlowDuration.at(round) *
                (flowStats.fromASCache.at(round) + flowStats.fromCentralServer.at(round)) + flowDuration)
                / (flowStats.fromASCache.at(round) + flowStats.fromCentralServer.at(round) + 1);
        if (flow->getSource().first == topo->getCentralServer() &&
                flow->getSource().second == 1)
          flowStats.fromCentralServer.at(round)++;
        else
          flowStats.fromASCache.at(round)++;
      }
      topo->updateLoadMap(flow);
      // notify the source cache that it has completed this upload
      PonUser source = flow->getSource();
      bool retValue(false);
      if (flow->isP2PFlow())
        retValue = userCacheMap->at(source).uploadCompleted(chunk);
      else
        retValue = localCacheMap->at(source.first).uploadCompleted(chunk);
      assert(retValue);
      // update cache info (unless the content has expired, e.g. a flow carried over
      // from the previous round)
      /* FIXME: checking for a nullptr here does not make any sense, as we have
       already accessed it earlier in the method - check the code to see if we need
       to do it earlier or it's not needed at all*/
      if (flow->getContent() != nullptr) {
        /* here we need to ask the oracle to decide whether we should cache the new
         * chunk. Because the optimization will also tell us what to delete, we
         * have to remove those chunks manually before invoking addToCache
         * Note that in the popularity estimation branch, we should try to optimize
         * only when we know enough about the content - e.g. a round.
         */
        if (round == 0 || !cachingOpt) {
          this->addToCache(dest, chunk, round * roundDuration + time);
        } else {
          std::pair<bool, bool> optResult = this->optimizeCaching(dest, chunk,
                  time, round);
          /* we need to add the requested element if the optimization failed OR if
           * it succeeded and it determined that we need to. The difference is that
           * in the second case elements that need to be removed have already been erased
           */
          if (!optResult.first || (optResult.first && optResult.second)) {
            this->addToCache(dest, chunk, round * roundDuration + time);
          }
          // also record if the cache optimization was successful
          if (optResult.first == true)
            flowStats.cacheOptimized.at(round)++;
        }
      }
      // free resources in the topology
      topo->updateCapacity(flow, scheduler, false);
      }
      // if the flow is carried over from the previous round, the rest should be skipped
      if (flow->getContent() == userWatchMap.at(dest).content) {
        // add this chunk to the streaming buffer of the user
        // FIXME: we are not checking if the buffer is full, although we do check when we fetch
        auto insResult = userWatchMap.at(dest).buffer.insert(chunk);
        assert(insResult.second == true);
        /* here we previously generated a new request, however this is no longer related to
         * downloads, but rather to watching. the only thing we need to do is starting
         * a watch event if the user was waiting for the chunk we just downloaded
         */
        if (userWatchMap.at(dest).waiting
                && userWatchMap.at(dest).currentChunk == chunk->getIndex()) {
          // if we are waiting for a chunk we can't be done, just make sure
          assert(chunk->getIndex() < userWatchMap.at(dest).chunksToBeWatched);
          // start a new watching flow for the chunk we just got
          BOOST_LOG_TRIVIAL(debug) << "User was waiting for this chunk, starting a WATCH flow";
          // if it was not the first chunk, display a debugging message to track rebuffering events
          if (chunk->getIndex() > 0) {
            BOOST_LOG_TRIVIAL(info) << "At time " << scheduler->getSimTime()
                    << " user " << dest.first << "," << dest.second
                    << " stopped waiting for chunk " << chunk->getIndex() << " of content "
                    << chunk->getContent()->getName();
          }
          SimTime eta = scheduler->getSimTime() +
                  std::ceil(chunk->getSize() / this->bitrate);
          Flow* watchEvent = new Flow(flow->getContent(), dest, eta, chunk->getIndex(),
                  FlowType::WATCH);
          scheduler->schedule(watchEvent);
          userWatchMap.at(dest).waiting = false;
        }
      } else {
        BOOST_LOG_TRIVIAL(debug) << "Carried over transfer of chunk "
              << chunk->getIndex() << " of content " << chunk->getContent()->getName()
              << " to user " << dest.first << "," << dest.second
              << " from previous round";
      }
    }
    break;

    case FlowType::WATCH:
    {
      BOOST_LOG_TRIVIAL(debug) << "At time " << time << " user " << dest.first
              << "," << dest.second << " finished watching chunk "
              << chunk->getIndex() << " of content " << chunk->getContent()->getName();
      // is it a carried over flow?
      if (flow->getContent() != userWatchMap.at(dest).content) {
        /* the userWatchMap for this watching session has been overwritten now,
         * and a new session has been programmed, so there is no reason to
         * keep going with this but to complete the flows that were still hanging
         */
        BOOST_LOG_TRIVIAL(info) << "carried over watch flow for content " <<
                flow->getContent()->getName() << ", currently watching " <<
                userWatchMap.at(dest).content->getName();
        return;
      }
      // are we done with this content?
      uint completedChunk = userWatchMap.at(dest).currentChunk;
      if (time >= userWatchMap.at(dest).dailySessionInterval.getEnd() ||
              completedChunk >= userWatchMap.at(dest).chunksToBeWatched-1) {
        // yes, we are; free buffer and reset watching info
        BOOST_LOG_TRIVIAL(debug) << "At time " << time << " user " << dest.first
                << "," << dest.second << " finished watching content "
                << flow->getContent()->getName() << " with chunk "
                << completedChunk << "/" << flow->getContent()->getTotalChunks()-1;
        userWatchMap.at(dest).reset();
        // generate a new request (if the daily session is over, it's going to be checked there)
        this->generateNewRequest(dest, time, scheduler);
        // and that's all
        return;
      }
      // still more watching to do for this content
      // free space in the buffer now that the chunk has been watched
      userWatchMap.at(dest).buffer.erase(flow->getContent()->getChunkById(completedChunk));
      // pre-fetch as many new chunks as we can, since there's space in the buffer now
      uint bufferSlots = this->bufferSize - userWatchMap.at(dest).buffer.size();
      // there should be at least one free slot if nothing is wrong!
      assert (bufferSlots > 0);
      while (bufferSlots > 0 &&
              userWatchMap.at(dest).highestChunkFetched < flow->getContent()->getTotalChunks()-1) {
        BOOST_LOG_TRIVIAL(debug) << "There's " << bufferSlots << " slots in the buffer, "
                "pre-fetching chunk " << userWatchMap.at(dest).highestChunkFetched+1;
        Flow* requestChunk = new Flow(flow->getContent(), dest, time,
                userWatchMap.at(dest).highestChunkFetched+1);
        scheduler->schedule(requestChunk);
        userWatchMap.at(dest).highestChunkFetched++;
        bufferSlots--;
      }

      // first make sure that we are not attempting to fetch a chunk that does not exist
      assert(completedChunk != flow->getContent()->getTotalChunks()-1);
      // update the chunk we are currently interested into
      userWatchMap.at(dest).currentChunk++;
      // check if we've got the next chunk so we can start watching straight away
      if (userWatchMap.at(dest).buffer.find(flow->getContent()->getChunkById(completedChunk+1))
              != userWatchMap.at(dest).buffer.end()) {
        // start a new watching flow
        BOOST_LOG_TRIVIAL(debug) << "We had the next chunk (" << completedChunk+1
                << ") in the buffer, starting a new WATCH flow";
        SimTime eta = scheduler->getSimTime() +
                std::ceil(flow->getContent()->getChunkById(completedChunk+1)->getSize() / this->bitrate);
        Flow* watchEvent = new Flow(flow->getContent(), dest, eta, completedChunk+1,
                FlowType::WATCH);
        scheduler->schedule(watchEvent);
        // and remove the waiting status in case it was set
        userWatchMap.at(dest).waiting = false;
      } else {
        // there are chunks we want to watch and we do not have them. ALARM!
        BOOST_LOG_TRIVIAL(info) << "At time " << scheduler->getSimTime()
                << " user " << dest.first << "," << dest.second
                << " started waiting for chunk " << completedChunk+1 << " of content "
                << flow->getContent()->getName();
        BOOST_LOG_TRIVIAL(info) << "Highest chunk fetched so far: "
                << userWatchMap.at(dest).highestChunkFetched;
        userWatchMap.at(dest).waiting = true;
      }
    }
    break;
    default:
      BOOST_LOG_TRIVIAL(error) << "notifyCompletedFlow for non TRANSFER, non WATCH event.";
      abort();
  }

}

// Implemented for future use (e.g. distributed cache updates between rounds)
void TopologyOracle::notifyEndRound(uint endingRound) {
  // reset load on each link of the topology to prepare for next round's collection
  this->topo->resetLoadMap();
  // update the contentRateVec with the views observed on the current round
  double old = contentRateVec.at(0).at(0);
  for (uint day = 0; day < 7; day++) {
    for (auto i = 0; i < dailyRanking.at(day).size(); i++) {
      contentRateVec.at(day).at(i) = ((contentRateVec.at(day).at(i) *
              endingRound) + dailyRanking.at(day).getRoundHitsByRank(i))/(endingRound+1);
    }
  }
  BOOST_LOG_TRIVIAL(trace) << "old rate for day 0 rank 0: " << old << ", new: "
          << contentRateVec.at(0).at(0) << ", daily hits: " << dailyRanking.at(0).getHitsByRank(0);
  return;
}

std::set<PonUser> TopologyOracle::getSources(const ChunkPtr& chunk,
        uint asid) {
  return asidContentMap->at(asid).at(chunk);
}

void TopologyOracle::printStats(uint currentRound) {
  double localPctg, ASPctg, P2PPctg, CSPctg, blockPctg;
  localPctg = (flowStats.localRequests.at(currentRound)  /
          (double) flowStats.completedRequests.at(currentRound)) * 100;
  ASPctg = (flowStats.fromASCache.at(currentRound) /
          (double) flowStats.completedRequests.at(currentRound)) * 100;
  P2PPctg = (flowStats.fromPeers.at(currentRound) /
          (double) flowStats.completedRequests.at(currentRound)) * 100;
  CSPctg = (flowStats.fromCentralServer.at(currentRound) /
          (double) flowStats.completedRequests.at(currentRound)) * 100;
  blockPctg = (flowStats.congestionBlocked.at(currentRound) * 100) /
          (double) (flowStats.servedRequests.at(currentRound) +
          flowStats.congestionBlocked.at(currentRound));
  // check how much caching space is used on average in user caches
  double avgUserCacheOccupancy(0.0), avgMetroCacheOccupancy(0.0), temp(0.0);
  uint numCaches = 0;
  SimTime extTime = (currentRound + 1 ) * roundDuration;
  for (auto uIt = userCacheMap->begin(); uIt != userCacheMap->end() &&
          this->maxCacheSize > 0; uIt++) {
    temp = uIt->second.getAvgOccupancy(extTime);
    assert(temp != DBL_MAX);
    BOOST_LOG_TRIVIAL(trace) << "User " << uIt->first.first << "," <<  uIt->first.second
              << " has a cache occupancy of " << temp << "% (currentSize: "
              << uIt->second.getCurrentSize() << ", maxSize: "
              << uIt->second.getMaxSize() << ") with "
              << uIt->second.getNumElementsCached() << " elements";
    avgUserCacheOccupancy += temp;
    numCaches++;
    uIt->second.resetOccupancy(extTime);
  }
  if (numCaches != 0)
    avgUserCacheOccupancy = avgUserCacheOccupancy / numCaches;
  else
    avgUserCacheOccupancy = 0.0;
  this->flowStats.avgUserCacheOccupancy.at(currentRound) = avgUserCacheOccupancy;
  // check how much caching space is used on average in metro/core caches
  numCaches = 0;
  for (auto uIt = localCacheMap->begin(); uIt != localCacheMap->end() &&
          this->maxLocCacheSize > 0; uIt++) {
    // central server cache should not be included in the average computation
    if (uIt->first != topo->getCentralServer()) {
      temp = uIt->second.getAvgOccupancy(extTime);
      assert(temp != DBL_MAX);
      BOOST_LOG_TRIVIAL(trace) << "AS cache " << uIt->first
              << " has a cache occupancy of " << temp << "% (currentSize: "
              << uIt->second.getCurrentSize() << ", maxSize: "
              << uIt->second.getMaxSize() << ") with "
              << uIt->second.getNumElementsCached() << " elements";
      avgMetroCacheOccupancy += temp;
      numCaches++;
    }
  uIt->second.resetOccupancy(extTime);
  }
  if (numCaches != 0)
    avgMetroCacheOccupancy = avgMetroCacheOccupancy / numCaches;
  else
    avgMetroCacheOccupancy = 0.0;
  this->flowStats.avgASCacheOccupancy.at(currentRound) = avgMetroCacheOccupancy;

  // print stats to screen for human visualization
  std::cout << "Completed " << flowStats.completedRequests.at(currentRound)
          << " out of " << flowStats.servedRequests.at(currentRound)
          << " requests, of which " << flowStats.localRequests.at(currentRound)
          << " locally ("
          << localPctg << "%). " << std::endl
          << "Successful cache optimizations: " << flowStats.cacheOptimized.at(currentRound)
          << " (" << 100 * flowStats.cacheOptimized.at(currentRound) / (double) flowStats.completedRequests.at(currentRound)
          << "% of all completed requests)"
          << std::endl << "P2P flows: "
          << flowStats.fromPeers.at(currentRound) << " (" << P2PPctg
          << "%), AS Cache Flows: "
          << flowStats.fromASCache.at(currentRound) << " ("
          << ASPctg << "%), Central Server Flows: "
          << flowStats.fromCentralServer.at(currentRound) << " (" << CSPctg
          << "%); blocked due to congestion: "
          << flowStats.congestionBlocked.at(currentRound) << " ("
          << blockPctg << "%)" << std::endl;
  std::cout << "Average flow duration: " << flowStats.avgFlowDuration.at(currentRound)
            << "; Average P2P flow duration: " << flowStats.avgPeerFlowDuration.at(currentRound)
            << "; Average Cache flow duration: " << flowStats.avgCacheFlowDuration.at(currentRound)
            << std::endl;
  std::cout << "Average User Cache Occupancy: " << flowStats.avgUserCacheOccupancy.at(currentRound)
            << "%; Average AS Cache Occupancy: " <<flowStats.avgASCacheOccupancy.at(currentRound)
            << "%" << std::endl << std::endl;
}

/* addContent performs the maintenance steps required when adding a new element
 * to the catalog, i.e., creating entries in the AsidContentMap for each chunk
 * and adding it to the central repository if we are in reducedCaching mode.
 */
void TopologyOracle::addContent(ContentElement* content, uint elapsedRounds) {
  SimTime time = elapsedRounds * roundDuration;
  AsidContentMap::iterator aIt;
  std::vector<ChunkPtr>::iterator cIt;
  std::vector<ChunkPtr> chunks = content->getChunks();
  for (aIt = asidContentMap->begin(); aIt != asidContentMap->end(); aIt++) {
    std::set<PonUser> users;
    for (cIt = chunks.begin(); cIt != chunks.end(); cIt++) {
      aIt->second.insert(std::make_pair(*cIt, users));
    }
  }
  if (this->reducedCaching) {
    for (LocalCacheMap::iterator lit = localCacheMap->begin();
            lit != localCacheMap->end(); lit++) {
      for (cIt = chunks.begin(); cIt != chunks.end(); cIt++) {
        lit->second.addToCache(*cIt, (*cIt)->getSize(), time);
      }
    }
  }
}

/* removeContent does the reverse of addContent; it should be called when an
 * element is being removed from the catalog, as it deletes all the copies
 * of its chunks stored in caches across the system and removes the related
 * entries in the chunkMaps.
 */
void TopologyOracle::removeContent(ContentElement* content, uint roundsElapsed) {
  // erase the expiring content from all user and local caches
  SimTime time = roundsElapsed * roundDuration;
  AsidContentMap::iterator aIt;
  std::vector<ChunkPtr>::iterator cIt;
  std::vector<ChunkPtr> chunks = content->getChunks();
  for (aIt = asidContentMap->begin(); aIt != asidContentMap->end(); aIt++) {
    for (cIt = chunks.begin(); cIt != chunks.end(); cIt++) {
      BOOST_FOREACH(PonUser user, aIt->second.at(*cIt)) {
        userCacheMap->at(user).removeFromCache(*cIt, time);
      }
    aIt->second.erase(*cIt);
    }
  }
  for (LocalCacheMap::iterator lit = localCacheMap->begin();
          lit != localCacheMap->end(); lit++) {
    for (cIt = chunks.begin(); cIt != chunks.end(); cIt++) {
      lit->second.removeFromCache(*cIt, time);
    }
  }
}

bool TopologyOracle::checkIfCached(PonUser user, const ChunkPtr& chunk) {
  return userCacheMap->at(user).isCached(chunk);
}

bool TopologyOracle::checkIfCached(Vertex lCache, const ChunkPtr& chunk) {
  return localCacheMap->at(lCache).isCached(chunk);
}

void TopologyOracle::getFromLocalCache(Vertex lCache, const ChunkPtr& chunk,
        SimTime time) {
  if (localCacheMap->at(lCache).getMaxSize() >= chunk->getSize()) {
    bool result = localCacheMap->at(lCache).getFromCache(chunk, time, false);
    // the central server should have inifinite capacity but this is not implemented
    // so we should not get worried if the result is false
    if (lCache != topo->getCentralServer())
      assert (result);
  }
}

void TopologyOracle::removeFromCMap(const ChunkPtr& chunk, PonUser user) {
  uint asid = topo->getAsid(user);
  std::set<PonUser>::iterator uIt = asidContentMap->at(asid).at(chunk).find(user);
  if (uIt == asidContentMap->at(asid).at(chunk).end()) {
    BOOST_LOG_TRIVIAL(trace) << "Attempted to remove missing chunk " << chunk->getIndex()
              << " from the cache of user " << user.first << ","
              << user.second;
    return;
  } else
    asidContentMap->at(asid).at(chunk).erase(uIt);
}

std::pair<bool, bool> TopologyOracle::optimizeCaching(PonUser reqUser,
        const ChunkPtr& chunk, SimTime time, uint currentRound) {
  SimTime absTime = time + (currentRound*roundDuration);
  /* This should no longer apply as chunks are either cached entirely or not cached
   *
   * there is an inconsistence in our assumptions here, in that we add
   * an optimization variable for the requested content in addition to the
   * variables we have for the elements cached, but there is a chance that the
   * requested content was already cached (albeit with a different size). I need
   * to either make sure that this case is covered or that it never happens (e.g.,
   * by deleting it). In the next few lines I attempt to do so.

  if (checkIfCached(reqUser, content) == true) {
    userCacheMap->at(reqUser).removeFromCache(content, absTime);
    removeFromCMap(content, reqUser);
  }
  */
  int hour = std::floor(time / 3600);
  /* attempting to fix a potential bug in case we enter this function when it's
  /* midnight of the following day
   */
  if (hour >= usrPctgByHour.size())
    hour = usrPctgByHour.size() - 1;
  IloEnv env;
  int asid = topo->getAsid(reqUser);
  try {
    IloModel model(env);
    IloNumVarArray c(env);
    auto cachedVec = userCacheMap->at(reqUser).getCacheMap();
    // add a boolean variable for each of the elements cached
    // char varName[24];
    for (auto it = cachedVec.begin(); it != cachedVec.end(); it++) {
      // varName = ((string)("c_" + it->first->getName())).c_str();
      c.add(IloNumVar(env, 0, 1, ILOINT));
    }
    // add a boolean variable for the element requested
    // varName = ((string)("c_" + content->getName())).c_str();
    c.add(IloNumVar(env, 0, 1, ILOINT));
    // Define the constraints
    IloRangeArray constraints(env);
    // Initialize the objective function expression
    IloNumExpr exp(env);
    // add the size of the elements cached, if kept in the cache
    int i = 0;
    for (auto it = cachedVec.begin(); it != cachedVec.end(); it++) {
      ChunkPtr chunkIt = it->first;
      ContentElement* contIt = chunkIt->getContent();
      exp += c[i] * it->second.size;
      // also make sure that we do not erase a content if we are uploading it
      if  (it->second.uploads > 0) {
        constraints.add(c[i] == 1);
        i++;
        continue;
      }
      // build the constraint on rates if required
      /* the contentRateVec holds the number of requests expected for each content
       * per day. To get the number of requests per hour per AS we need
       * to get the % of requests in this particular hour via usrPctgByHour and
       * multiply it by the fraction of users in the AS of the requester. The avg
       * concurrent request number is calculated as reqPerHour * avgReqLength. To
       * get the peak of concurrent users, which is what we need, we multiply
       * that value by a pakReqRatio, which is taken as an integer input parameter
       * (-k).
       */
      uint dayIndex = currentRound - contIt->getReleaseDay();
      assert(0 <= dayIndex && dayIndex < 7);
      /* if it's a young content (released today and with only a few hours of life)
       * make sure it is not erased, as ranking is still too volatile to be accurate
       */
      if (dayIndex == 0 && hour < 6) {
        constraints.add(c[i] == 1);
        i++;
        continue;
      }
      uint rank = dailyRanking.at(dayIndex).getRankOf(contIt);
      double rate = contentRateVec.at(dayIndex).at(rank);
      double avgReqPerHour = (rate * usrPctgByHour.at(hour) / 100) *
          (topo->getASCustomers(asid) / topo->getNumCustomers());
      BOOST_LOG_TRIVIAL(trace) << "avgReqPerHour(" << contIt->getName() <<
              ":" << chunkIt->getIndex() << "," << hour
              << ") = (" << rate << " * " << usrPctgByHour.at(hour)
              << " / 100) * (" << topo->getASCustomers(asid) << " / " << topo->getNumCustomers()
              << ") = " << avgReqPerHour;
      if (avgReqPerHour < 1) {
        BOOST_LOG_TRIVIAL(trace) << "avgReqPerHour for chunk " << chunkIt->getIndex()
              << " of content " << contIt->getName()
              << " is less than 1 (" << avgReqPerHour << "), setting it to 1";
        avgReqPerHour = 1;
      }
      int chunkRate = std::floor((peakReqRatio * avgReqPerHour * (avgReqLength / 3600)) + 0.5);
      // ensure that the peakReqRatio does not inflate the requests to more than the users we have
      chunkRate = std::min(chunkRate, static_cast<int>(topo->getASCustomers(asid)));
      // ensure that we keep at least one copy of each content in each AS
      chunkRate = std::max(1, chunkRate);
      BOOST_LOG_TRIVIAL(trace) << "chunkRate = std::floor((" << peakReqRatio
              << " * " << avgReqPerHour
              << " * " << avgReqLength << " / 3600) + 0.5) = " << chunkRate;
      IloNumExpr cExp(env, 0);
      for (auto uit = asidContentMap->at(asid).at(chunkIt).begin();
              uit != asidContentMap->at(asid).at(chunkIt).end(); uit++) {
        if (reqUser != *uit) {
          cExp += maxUploads - userCacheMap->at(*uit).getTotalUploads()
                  + userCacheMap->at(*uit).getCurrentUploads(chunkIt);
        } else {
          // multiply the term by the caching variable so that we lose it if
          // we decide to erase the element from the cache
          cExp += c[i]* (IloInt) (maxUploads - userCacheMap->at(reqUser).getTotalUploads()
                  + userCacheMap->at(reqUser).getCurrentUploads(chunkIt));
        }
      }
      constraints.add(cExp >= chunkRate);
      i++;
    }
    // same for the chunk we just downloaded
    exp += c[cachedVec.size()] * chunk->getSize();
    // add a chunkRate constraint for the downloaded chunk if needed
    uint dayIndex = currentRound - chunk->getContent()->getReleaseDay();
    assert(0 <= dayIndex && dayIndex < 7);
    if (dayIndex == 0 && hour < 6) {
      constraints.add(c[cachedVec.size()] == 1);
    } else {
      uint rank = dailyRanking.at(dayIndex).getRankOf(chunk->getContent());
      double rate = contentRateVec.at(dayIndex).at(rank);
      double avgReqPerHour = (rate * usrPctgByHour.at(hour) / 100) *
              (topo->getASCustomers(asid) / topo->getNumCustomers());
      BOOST_LOG_TRIVIAL(trace) << "avgReqPerHour(" << chunk->getContent()->getName()
              << ":" << chunk->getIndex() << "," << hour
              << ") = (" << rate << " * " << usrPctgByHour.at(hour)
              << " / 100) * (" << topo->getASCustomers(asid) << " / " << topo->getNumCustomers()
              << ") = " << avgReqPerHour;
      if (avgReqPerHour < 1) {
        BOOST_LOG_TRIVIAL(trace) << "avgReqPerHour for chunk " << chunk->getIndex()
              << " of content " << chunk->getContent()->getName()
              << " is less than 1 (" << avgReqPerHour << "), setting it to 1";
        avgReqPerHour = 1;
      }
      int chunkRate = std::floor((peakReqRatio * avgReqPerHour * (avgReqLength / 3600)) + 0.5);
      // ensure that the peakReqRatio does not inflate the requests to more than the users we have
      chunkRate = std::min(chunkRate, static_cast<int>(topo->getASCustomers(asid)));
      // ensure that we keep at least one copy of each content in each AS
      chunkRate = std::max(1, chunkRate);
      BOOST_LOG_TRIVIAL(trace) << "chunkRate = std::floor((" << peakReqRatio
              << " * " << avgReqPerHour
              << " * " << avgReqLength << " / 3600) + 0.5) = " << chunkRate;
      IloNumExpr cExp(env, 0);
      for (auto uit = asidContentMap->at(asid).at(chunk).begin();
              uit != asidContentMap->at(asid).at(chunk).end(); uit++) {
        if (reqUser != *uit) {
          cExp += maxUploads - userCacheMap->at(*uit).getTotalUploads()
                  + userCacheMap->at(*uit).getCurrentUploads(chunk);
        } else {
          cExp += c[cachedVec.size()]* (IloInt) (maxUploads - userCacheMap->at(reqUser).getTotalUploads()
                  + userCacheMap->at(reqUser).getCurrentUploads(chunk));
        }
      }
      // if the requesting user was not already caching the content, we must add
      // its term to the sum in case we decide to cache the requested content
      if (userCacheMap->at(reqUser).isCached(chunk) == false) {
        cExp += c[cachedVec.size()]* (IloInt) (maxUploads - userCacheMap->at(reqUser).getTotalUploads()
                + userCacheMap->at(reqUser).getCurrentUploads(chunk));
      }
      constraints.add(cExp >= chunkRate);
    }
    //finally add a constraint on the maximum size of the cache
    constraints.add(exp <= userCacheMap->at(reqUser).getMaxSize());

    // specify that we are interested in minimizing the objective
    IloObjective obj = IloMinimize(env, exp);
    // Add the objectives and the constraints to the model
    model.add(c);
    model.add(obj);
    model.add(constraints);

    // ask CPLEX to solve the problem
    IloCplex cplex(model);
    cplex.setOut(env.getNullStream());
    if (!cplex.solve()) {
      BOOST_LOG_TRIVIAL(trace) << "Failed to optimize caching for chunk " <<
              chunk->getIndex() << " of content " <<
              chunk->getContent()->getName() << " at user " << reqUser.first << "," <<
              reqUser.second << "; reverting to standard cache policies";
      env.end();
      return std::make_pair(false, false);
    }

    IloNumArray vals(env);
    BOOST_LOG_TRIVIAL(trace) << "Solution status = " << cplex.getStatus();
    BOOST_LOG_TRIVIAL(trace) << "Solution value  = " << cplex.getObjValue();
    cplex.getValues(vals, c);
    BOOST_LOG_TRIVIAL(trace) << "Values        = " << vals;

    // check if any element has to be deleted from the cache
    i = 0;
    for (auto it = cachedVec.begin(); it != cachedVec.end(); it++) {
      if (vals[i] == 0) {
        userCacheMap->at(reqUser).removeFromCache(it->first, absTime);
        // also delete the user from the content map
        removeFromCMap(it->first, reqUser);
      }
      i++;
    }
    // check if reqContent has to be added to the cache
    bool shouldCache = (vals[cachedVec.size()] == 1);
    env.end();
    return std::make_pair(true, shouldCache);
    /*
    // print the new content of the cache for debugging purposes
    cachedVec = caches[reqUser].getCachedVec();
    cout << "Cache content after optimization: " ;
    for (auto it = cachedVec.begin(); it != cachedVec.end(); it++)
      cout << *it << " ";
    cout << endl << "Total size: " << caches[reqUser].getCurrentSize() << endl;
     * */
  } catch (IloException& e) {
    BOOST_LOG_TRIVIAL(warning) << "Concert exception caught: " << e;
  }
  env.end();
  return std::make_pair(false, false);
}

max_stars_repo_name: manuhalo/PLACeS

max_issues_repo_name: manuhalo/PLACeS

max_forks_repo_name: manuhalo/PLACeS


text:
#include "WMSLegendGraphicSettings.h"

#include <vector>

#include <algorithm>

#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <macgyver/Exception.h>

namespace SmartMet
{
namespace Plugin
{
namespace WMS
{
unsigned int numberOfLanguages = 193;
const char* language_codes[] = {
    "ab", "aa", "af",      "ak",      "sq", "am", "ar", "an", "hy", "as",   "av", "ae", "ay", "az",
    "bm", "ba", "eu",      "be",      "bn", "bh", "bi", "bs", "br", "bg",   "my", "ca", "ch", "ce",
    "ny", "zh", "zh-Hans", "zh-Hant", "cv", "kw", "co", "cr", "hr", "cs",   "da", "dv", "nl", "dz",
    "en", "eo", "et",      "ee",      "fo", "fj", "fi", "fr", "ff", "gl",   "gd", "gv", "ka", "de",
    "el", "kl", "gn",      "gu",      "ht", "ha", "he", "hz", "hi", "ho",   "hu", "is", "io", "ig",
    "id", "in", "ia",      "ie",      "iu", "ik", "ga", "it", "ja", "jv",   "kl", "kn", "kr", "ks",
    "kk", "km", "ki",      "rw",      "rn", "ky", "kv", "kg", "ko", "ku",   "kj", "lo", "la", "lv",
    "li", "ln", "lt",      "lu",      "lg", "lb", "gv", "mk", "mg", "ms",   "ml", "mt", "mi", "mr",
    "mh", "mo", "mn",      "na",      "nv", "ng", "nd", "ne", "no", "nb",   "nn", "ii", "oc", "oj",
    "cu", "or", "om",      "os",      "pi", "ps", "fa", "pl", "pt", "pa",   "qu", "rm", "ro", "ru",
    "se", "sm", "sg",      "sa",      "sr", "sh", "st", "tn", "sn", "Yiii", "sd", "si", "ss", "sk",
    "sl", "so", "nr",      "es",      "su", "sw", "ss", "sv", "tl", "ty",   "tg", "ta", "tt", "te",
    "th", "bo", "ti",      "to",      "ts", "tr", "tk", "tw", "ug", "uk",   "ur", "uz", "ve", "vi",
    "vo", "wa", "cy",      "wo",      "fy", "xh", "yi", "ji", "yo", "za",   "zu"};

WMSLegendGraphicSettings::WMSLegendGraphicSettings(const libconfig::Config& config)
{
  // default layout settings
  layout.param_name_xoffset = 20;
  layout.param_name_yoffset = 20;
  layout.param_unit_xoffset = 30;
  layout.param_unit_yoffset = 40;
  layout.legend_xoffset = 30;
  layout.legend_yoffset = 40;
  layout.symbol_group_x_padding = 30;
  layout.symbol_group_y_padding = 30;
  layout.legend_width = 10;
  layout.output_document_width = 500;
  layout.output_document_height = 500;
  expires = -1;  // No caching

  if (config.exists("wms.get_legend_graphic"))
  {
    const auto& legend_graphic_config = config.lookup("wms.get_legend_graphic");
    if (legend_graphic_config.isGroup())
    {
      // symbols to ignore
      if (config.exists("wms.get_legend_graphic.symbols_to_ignore"))
      {
        std::string symbolNames = config.lookup("wms.get_legend_graphic.symbols_to_ignore");
        boost::algorithm::split(symbolsToIgnore, symbolNames, boost::algorithm::is_any_of(","));
      }

      // Control cache expiration
      if (config.exists("wms.get_legend_graphic.expire"))
        expires = config.lookup("wms.get_legend_graphic.expire");

      if (config.exists("wms.get_legend_graphic.parameters"))
      {
        // parameter settings
        const auto& parameter_settings = config.lookup("wms.get_legend_graphic.parameters");

        if (!parameter_settings.isList())
          throw Fmi::Exception(BCP, "wms.get_legend_graphic.parameters must be a list");

        for (int i = 0; i < parameter_settings.getLength(); i++)
        {
          const auto& param = parameter_settings[i];

          std::string data_name = (param.exists("data_name") ? param["data_name"] : "");
          std::string name = (param.exists("name") ? param["name"] : "");
          std::string unit = (param.exists("unit") ? param["unit"] : "");
          bool hide_title = (param.exists("hide_title") ? (bool)param["hide_title"] : false);
          std::vector<std::string> param_names;

          if (!data_name.empty())
          {
            boost::algorithm::split(param_names, data_name, boost::algorithm::is_any_of(","));

            for (const auto& param_name : param_names)
            {
              parameters.insert(std::make_pair(
                  param_name, LegendGraphicParameter(data_name, name, unit, hide_title)));
            }
          }

          if (param.exists("translation"))
          {
            const auto& translation = param["translation"];
            for (unsigned int k = 0; k < numberOfLanguages; k++)
            {
              if (translation.exists(language_codes[k]))
              {
                std::string language_code = language_codes[k];
                std::string tr = translation[language_code.c_str()];
                for (const auto& p : param_names)
                {
                  LegendGraphicParameter& lgp = parameters[p];
                  lgp.translations.insert(std::make_pair(language_code, tr));
                }
              }
            }
          }
        }
      }

      if (config.exists("wms.get_legend_graphic.symbols"))
      {
        // symbol settings
        const auto& symbol_settings = config.lookup("wms.get_legend_graphic.symbols");

        if (!symbol_settings.isList())
          throw Fmi::Exception(BCP, "wms.get_legend_graphic.symbols must be a list");

        for (int i = 0; i < symbol_settings.getLength(); i++)
        {
          const auto& symbol = symbol_settings[i];

          std::string symbol_name = (symbol.exists("name") ? symbol["name"] : "");

          if (symbol_name.empty())
            continue;

          symbols.insert(std::make_pair(symbol_name, LegendGraphicSymbol(symbol_name)));

          if (symbol.exists("translation"))
          {
            const auto& translation = symbol["translation"];
            for (unsigned int k = 0; k < numberOfLanguages; k++)
            {
              if (translation.exists(language_codes[k]))
              {
                std::string language_code = language_codes[k];
                std::string tr = translation[language_code.c_str()];
                LegendGraphicSymbol& lgs = symbols[symbol_name];
                lgs.translations.insert(std::make_pair(language_code, tr));
              }
            }
          }
        }
      }

      // defaults can be overwritten from configuration
      if (config.exists("wms.get_legend_graphic.layout.param_name_xoffset"))
        layout.param_name_xoffset =
            config.lookup("wms.get_legend_graphic.layout.param_name_xoffset");
      if (config.exists("wms.get_legend_graphic.layout.param_name_yoffset"))
        layout.param_name_yoffset =
            config.lookup("wms.get_legend_graphic.layout.param_name_yoffset");
      if (config.exists("wms.get_legend_graphic.layout.param_unit_xoffset"))
        layout.param_unit_xoffset =
            config.lookup("wms.get_legend_graphic.layout.param_unit_xoffset");
      if (config.exists("wms.get_legend_graphic.layout.param_unit_yoffset"))
        layout.param_unit_yoffset =
            config.lookup("wms.get_legend_graphic.layout.param_unit_yoffset");
      if (config.exists("wms.get_legend_graphic.layout.legend_xoffset"))
        layout.legend_xoffset = config.lookup("wms.get_legend_graphic.layout.legend_xoffset");
      if (config.exists("wms.get_legend_graphic.layout.legend_yoffset"))
        layout.legend_yoffset = config.lookup("wms.get_legend_graphic.layout.legend_yoffset");
      if (config.exists("wms.get_legend_graphic.layout.legend_width"))
        layout.legend_width = config.lookup("wms.get_legend_graphic.layout.legend_width");
      if (config.exists("wms.get_legend_graphic.layout.symbol_group_x_padding"))
        layout.symbol_group_x_padding =
            config.lookup("wms.get_legend_graphic.layout.symbol_group_x_padding");
      if (config.exists("wms.get_legend_graphic.layout.symbol_group_y_padding"))
        layout.symbol_group_y_padding =
            config.lookup("wms.get_legend_graphic.layout.symbol_group_y_padding");
      if (config.exists("wms.get_legend_graphic.layout.output_document_width"))
        layout.output_document_width =
            config.lookup("wms.get_legend_graphic.layout.output_document_width");
      if (config.exists("wms.get_legend_graphic.layout.output_document_height"))
        layout.output_document_height =
            config.lookup("wms.get_legend_graphic.layout.output_document_height");
    }
  }
}

}  // namespace WMS
}  // namespace Plugin
}  // namespace SmartMet

ext: cpp

max_stars_repo_name: fmidev/smartmet-plugin-wms

max_issues_repo_name: fmidev/smartmet-plugin-wms

max_forks_repo_name: fmidev/smartmet-plugin-wms


text:
// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/concept/struct.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/find_if.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/optional.hpp>

#include "minimal_struct.hpp"
#include <laws/base.hpp>
namespace hana = boost::hana;
using hana::test::ct_eq;

template <int i = 0>
struct undefined { };

int main() {
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(obj(), undefined<>{}),
        hana::nothing
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(obj(ct_eq<0>{}), hana::equal.to(hana::int_c<0>)),
        hana::just(ct_eq<0>{})
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(obj(undefined<1>{}), hana::equal.to(hana::int_c<1>)),
        hana::nothing
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(obj(ct_eq<0>{}, ct_eq<1>{}), hana::equal.to(hana::int_c<0>)),
        hana::just(ct_eq<0>{})
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(obj(ct_eq<0>{}, ct_eq<1>{}), hana::equal.to(hana::int_c<1>)),
        hana::just(ct_eq<1>{})
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(obj(undefined<0>{}, undefined<1>{}), hana::equal.to(hana::int_c<2>)),
        hana::nothing
    ));
}


text:
/*
 *  Copyright 2007-2015 The OpenMx Project
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *       http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 */

/***********************************************************
*
*  omxStateSpaceExpectation.c
*
*  Created: Michael D. Hunter 	Date: 2012-10-28 20:07:36
*
*  Contains code to calculate the objective function for a
*   state space model.  Currently, this is done with a
*   Kalman filter in separate Predict and Update steps.
*   Later this could be done with one of several Kalman
*   filter-smoothers (a forward-backward algorithm).
*
**********************************************************/

#include "omxExpectation.h"
#include "omxBLAS.h"
#include "omxFIMLFitFunction.h"
#include "omxStateSpaceExpectation.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <unsupported/Eigen/MatrixFunctions>
#include <iostream>

void omxCallStateSpaceExpectation(omxExpectation* ox, const char *, const char *) {
    if(OMX_DEBUG) { mxLog("State Space Expectation Called."); }
	omxStateSpaceExpectation* ose = (omxStateSpaceExpectation*)(ox->argStruct);

	omxRecompute(ose->A, NULL);
	omxRecompute(ose->B, NULL);
	omxRecompute(ose->C, NULL);
	omxRecompute(ose->D, NULL);
	omxRecompute(ose->Q, NULL);
	omxRecompute(ose->R, NULL);

	// Probably should loop through all the data here!!!
	if(ose->t == NULL){
		omxKalmanPredict(ose);
	} else {
		omxKalmanBucyPredict(ose);
	}
	omxKalmanUpdate(ose);
}

void omxDestroyStateSpaceExpectation(omxExpectation* ox) {

	if(OMX_DEBUG) { mxLog("Destroying State Space Expectation."); }

	omxStateSpaceExpectation* argStruct = (omxStateSpaceExpectation*)(ox->argStruct);

	omxFreeMatrix(argStruct->r);
	omxFreeMatrix(argStruct->s);
	omxFreeMatrix(argStruct->z);
	omxFreeMatrix(argStruct->x);
	omxFreeMatrix(argStruct->y);
	omxFreeMatrix(argStruct->K);
	omxFreeMatrix(argStruct->P);
	omxFreeMatrix(argStruct->S);
	omxFreeMatrix(argStruct->Y);
	omxFreeMatrix(argStruct->Z);
	omxFreeMatrix(argStruct->det);
	omxFreeMatrix(argStruct->covInfo);
	omxFreeMatrix(argStruct->cov);
	omxFreeMatrix(argStruct->means);
	omxFreeMatrix(argStruct->smallC);
	omxFreeMatrix(argStruct->smallD);
	omxFreeMatrix(argStruct->smallR);
	omxFreeMatrix(argStruct->smallr);
	omxFreeMatrix(argStruct->smallK);
	omxFreeMatrix(argStruct->smallS);
	omxFreeMatrix(argStruct->smallY);

	delete argStruct;

}

void omxPopulateSSMAttributes(omxExpectation *ox, SEXP algebra) {
	if(OMX_DEBUG) { mxLog("Populating State Space Attributes.  Currently this does very little!"); }

	/* Initialize */
	omxSetExpectationComponent(ox, NULL, "Reset", NULL); //maybe shoulde be on ose?  after next line?
	omxStateSpaceExpectation* ose = (omxStateSpaceExpectation*)(ox->argStruct);

	if( !(ose->returnScores) ){
		if(OMX_DEBUG) { mxLog("Not asking for attributes, this is being skipped!"); }
		return;
	}

	SEXP xpred, ypred, ppred, spred, xupda, pupda, xsmoo, psmoo;
	// yupda, supda

	omxRecompute(ose->A, NULL);
	omxRecompute(ose->B, NULL);
	omxRecompute(ose->C, NULL);
	omxRecompute(ose->D, NULL);
	omxRecompute(ose->Q, NULL);
	omxRecompute(ose->R, NULL);

	// allocate matrices to be returned
	if(OMX_DEBUG_ALGEBRA) { mxLog("Allocating initial population matrices ..."); }
	int nx = ose->C->cols;
	int ny = ose->C->rows;
	if(OMX_DEBUG_ALGEBRA) { mxLog("Find number of rows of data ..."); }
	//int nt = ox->data->dataMat->rows;
	int nt = ox->data->numObs;
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... numObs:\n" << nt << std::endl; }
	if(OMX_DEBUG_ALGEBRA) { mxLog("Done Finding rows of data ..."); }
	int np = ((nx+1)*nx)/2;
	int ns = ((ny+1)*ny)/2;
	Rf_protect(xpred = Rf_allocMatrix(REALSXP, nt+1, nx));
	Rf_protect(ypred = Rf_allocMatrix(REALSXP, nt+1, ny));
	Rf_protect(ppred = Rf_allocMatrix(REALSXP, nt+1, np));
	Rf_protect(spred = Rf_allocMatrix(REALSXP, nt+1, ns));
	Rf_protect(xupda = Rf_allocMatrix(REALSXP, nt+1, nx));
	//Rf_protect(yupda = Rf_allocMatrix(REALSXP, nt+1, ny));
	Rf_protect(pupda = Rf_allocMatrix(REALSXP, nt+1, np));
	//Rf_protect(supda = Rf_allocMatrix(REALSXP, nt+1, ns));
	Rf_protect(xsmoo = Rf_allocMatrix(REALSXP, nt+1, nx));
	Rf_protect(psmoo = Rf_allocMatrix(REALSXP, nt+1, np));

	if(OMX_DEBUG_ALGEBRA) { mxLog("Setting zeroth row ..."); }
	// Set first row of xpred to x0
	int row = 0;
	for(int col = 0; col < nx; col++){
		REAL(xpred)[col * (nt+1) + row] =
			omxMatrixElement(ose->x, col, 0);
		REAL(xupda)[col * (nt+1) + row] =
			omxMatrixElement(ose->x, col, 0);
	}

	// Set first row of ppred to vech(P0)
	int counter = 0;
	for(int i = 0; i < ose->P->cols; i++) {
		for(int j = i; j < ose->P->rows; j++) {
			REAL(ppred)[counter * (nt+1) + row] = omxMatrixElement(ose->P, j, i);
			REAL(pupda)[counter * (nt+1) + row] = omxMatrixElement(ose->P, j, i);
			counter++;
		}
	}

	Eigen::VectorXd oldDefs;
	oldDefs.resize(ox->data->defVars.size());
	oldDefs.setConstant(NA_REAL);

	// Probably should loop through all the data here!!!
	if(OMX_DEBUG_ALGEBRA) { mxLog("Beginning forward loop ..."); }
	for(row=1; row < (nt+1); row++){
		if(OMX_DEBUG_ALGEBRA) { mxLog("Setting first data row ..."); }
		// Set row of data
		for(int i = 0; i < ny; i++) {
			omxSetMatrixElement(ose->y, i, 0, omxDoubleDataElement(ox->data, row-1, i));
		}

		// handle definition variables
		int numVarsFilled = 0;
		numVarsFilled = ox->data->handleDefinitionVarList(ox->currentState, row-1, oldDefs.data());

		/* Run Kalman prediction */
		if(ose->t == NULL){
			omxKalmanPredict(ose);
		} else {
			omxKalmanBucyPredict(ose);
		}

		// Copy latent state
		for(int col = 0; col < nx; col++)
			REAL(xpred)[col * (nt+1) + row] =
				omxMatrixElement(ose->x, col, 0);

		// Copy latent cov
		counter = 0;
		for(int i = 0; i < ose->P->cols; i++) {
			for(int j = i; j < ose->P->rows; j++) {
				REAL(ppred)[counter * (nt+1) + row] = omxMatrixElement(ose->P, j, i);
				counter++;
			}
		}

		// Create Full observed cov prediction
		if(OMX_DEBUG_ALGEBRA) { mxLog("Hand prediction of full observed cov ..."); }
		omxDSYMM(FALSE, 1.0, ose->P, ose->C, 0.0, ose->Y); // Y = C P
		omxCopyMatrix(ose->S, ose->R); // S = R
		omxDGEMM(FALSE, TRUE, 1.0, ose->Y, ose->C, 1.0, ose->S); // S = Y C^T + S THAT IS C P C^T + R

		// Copy observed cov
		counter = 0;
		for(int i = 0; i < ose->S->cols; i++) {
			for(int j = i; j < ose->S->rows; j++) {
				REAL(spred)[counter * (nt+1) + row] = omxMatrixElement(ose->S, j, i);
				counter++;
			}
		}

		/* Run Kalman update */
		omxKalmanUpdate(ose);

		// Copy latent state
		for(int col = 0; col < nx; col++)
			REAL(xupda)[col * (nt+1) + row] =
				omxMatrixElement(ose->x, col, 0);

		// Copy latent cov
		counter = 0;
		for(int i = 0; i < ose->P->cols; i++) {
			for(int j = i; j < ose->P->rows; j++) {
				REAL(pupda)[counter * (nt+1) + row] = omxMatrixElement(ose->P, j, i);
				counter++;
			}
		}

		// Copy observed means prediction
		for(int col = 0; col < ny; col++)
			REAL(ypred)[col * (nt+1) + row] =
				omxMatrixElement(ose->s, col, 0);

		// TODO Add m2ll calculation here.
		// Probably like this
		/*m2ll = y^T S y */ // n.b. y originally is the data row but becomes the data residual!
		//omxDSYMV(1.0, S, y, 0.0, s); // s = S y
		//m2ll = omxDDOT(y, s); // m2ll = y s THAT IS y^T S y
		//m2ll += det; // m2ll = m2ll + det THAT IS m2ll = log(det(S)) + y^T S y
		// Note: this leaves off the S->cols * log(2*pi) THAT IS k*log(2*pi)
	}

	/* TODO Add Backward pass through data for Kalman smoother*/
	// Initialize end of smoothed latents to last updated latents
	// P = last updated P
	// x = last updated x

	// Copy x and P as smoothed estimates for export
	row = nt;
	// Copy latent state
	for(int col = 0; col < nx; col++)
		REAL(xsmoo)[col * (nt+1) + row] =
			omxMatrixElement(ose->x, col, 0);

	// Copy latent cov
	counter = 0;
	for(int i = 0; i < ose->P->cols; i++) {
		for(int j = i; j < ose->P->rows; j++) {
			REAL(psmoo)[counter * (nt+1) + row] = omxMatrixElement(ose->P, j, i);
			counter++;
		}
	}

	// loop backwars through all data
	if(OMX_DEBUG_ALGEBRA) { mxLog("Beginning backward loop ..."); }
	for(row = nt-1; row > -1; row--){
		// handle definition variables
		int numVarsFilled = 0;
		numVarsFilled = ox->data->handleDefinitionVarList(ox->currentState, row, oldDefs.data());

		// Copy Z = updated P from pupda
		counter = 0;
		for(int i = 0; i < nx; i++) {
			for(int j = i; j < nx; j++) {
				double next = REAL(pupda)[counter * (nt+1) + row];
				omxSetMatrixElement(ose->Z, i, j, next);
				omxSetMatrixElement(ose->Z, j, i, next);
				counter++;
			}
		}

		// Copy z = updated x from xupda
		for(int col = 0; col < nx; col++)
			omxSetMatrixElement(ose->z, col, 0, REAL(xupda)[col * (nt+1) + row]);

		// Copy eigenExpA = predicted P from later row of ppred
		counter = 0;
		for(int i = 0; i < nx; i++) {
			for(int j = i; j < nx; j++) {
				double next = REAL(ppred)[counter * (nt+1) + row + 1];
				ose->eigenExpA(i, j) = next;
				ose->eigenExpA(j, i) = next;
				counter++;
			}
		}

		// Copy eigenPreX = predicted x from later row of xpred
		for(int col = 0; col < nx; col++)
			ose->eigenPreX(col, 0) =  REAL(xpred)[col * (nt+1) + row + 1];

		// Run Smoother
		omxRauchTungStriebelSmooth(ose);

		// Copy latent state to xsmoo
		for(int col = 0; col < nx; col++)
			REAL(xsmoo)[col * (nt+1) + row] =
				omxMatrixElement(ose->x, col, 0);

		// Copy latent cov to psmoo
		counter = 0;
		for(int i = 0; i < ose->P->cols; i++) {
			for(int j = i; j < ose->P->rows; j++) {
				REAL(psmoo)[counter * (nt+1) + row] = omxMatrixElement(ose->P, j, i);
				counter++;
			}
		}
	}

	// TODO check on definition variable population
	//  I suspect this does yet work properly for def vars.

	Rf_setAttrib(algebra, Rf_install("xPredicted"), xpred);
	Rf_setAttrib(algebra, Rf_install("yPredicted"), ypred);
	Rf_setAttrib(algebra, Rf_install("PPredicted"), ppred);
	Rf_setAttrib(algebra, Rf_install("SPredicted"), spred);
	Rf_setAttrib(algebra, Rf_install("xUpdated"), xupda);
	Rf_setAttrib(algebra, Rf_install("PUpdated"), pupda);
	Rf_setAttrib(algebra, Rf_install("xSmoothed"), xsmoo);
	Rf_setAttrib(algebra, Rf_install("PSmoothed"), psmoo);

	/*
	omxMatrix *expCovInt, *expMeanInt;
	expCovInt = argStruct->cov;
	expMeanInt = argStruct->means;

	Rf_protect(expCovExt = Rf_allocMatrix(REALSXP, expCovInt->rows, expCovInt->cols));
	for(int row = 0; row < expCovInt->rows; row++)
		for(int col = 0; col < expCovInt->cols; col++)
			REAL(expCovExt)[col * expCovInt->rows + row] =
				omxMatrixElement(expCovInt, row, col);
	if (expMeanInt != NULL && expMeanInt->rows > 0  && expMeanInt->cols > 0) {
		Rf_protect(expMeanExt = Rf_allocMatrix(REALSXP, expMeanInt->rows, expMeanInt->cols));
		for(int row = 0; row < expMeanInt->rows; row++)
			for(int col = 0; col < expMeanInt->cols; col++)
				REAL(expMeanExt)[col * expMeanInt->rows + row] =
					omxMatrixElement(expMeanInt, row, col);
	} else {
		Rf_protect(expMeanExt = Rf_allocMatrix(REALSXP, 0, 0));
	}

	Rf_setAttrib(algebra, Rf_install("expCov"), expCovExt);
	Rf_setAttrib(algebra, Rf_install("expMean"), expMeanExt);

	if(argStruct->populateRowDiagnostics){
		omxMatrix *rowLikelihoodsInt = argStruct->rowLikelihoods;
		Rf_protect(rowLikelihoodsExt = Rf_allocVector(REALSXP, rowLikelihoodsInt->rows));
		for(int row = 0; row < rowLikelihoodsInt->rows; row++)
			REAL(rowLikelihoodsExt)[row] = omxMatrixElement(rowLikelihoodsInt, row, 0);
		Rf_setAttrib(algebra, Rf_install("likelihoods"), rowLikelihoodsExt);
	}
	*/

}

void omxKalmanPredict(omxStateSpaceExpectation* ose) {
	if(OMX_DEBUG) { mxLog("Kalman Predict Called."); }
	/* Creat local copies of State Space Matrices */
	omxMatrix* A = ose->A;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(A, "....State Space: A"); }
	omxMatrix* B = ose->B;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(B, "....State Space: B"); }
	omxMatrix* Q = ose->Q;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Q, "....State Space: Q"); }
	omxMatrix* u = ose->u;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(u, "....State Space: u"); }
	omxMatrix* x = ose->x;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x"); }
	omxMatrix* z = ose->z;
	omxMatrix* P = ose->P;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P"); }
	//omxMatrix* S = ose->S;
	//omxMatrix* Y = ose->Y;
	omxMatrix* Z = ose->Z;

	/* x = A x + B u */
	omxDGEMV(FALSE, 1.0, A, x, 0.0, z); // x = A x
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(z, "....State Space: z = A x"); }
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(A, "....State Space: A"); }
	omxDGEMV(FALSE, 1.0, B, u, 1.0, z); // x = B u + x THAT IS x = A x + B u
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(z, "....State Space: z = A x + B u"); }
	omxCopyMatrix(x, z); // x = z THAT IS x = A x + B u
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x = A x + B u"); }

	/* P = A P A^T + Q */
	omxDSYMM(FALSE, 1.0, P, A, 0.0, Z); // Z = A P
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Z, "....State Space: Z = A P"); }
	omxCopyMatrix(P, Q); // P = Q
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P = Q"); }
	omxDGEMM(FALSE, TRUE, 1.0, Z, A, 1.0, P); // P = Z A^T + P THAT IS P = A P A^T + Q
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P = A P A^T + Q"); }
}

void omxKalmanUpdate(omxStateSpaceExpectation* ose) {
	//TODO: Clean up this hack of function.
	if(OMX_DEBUG) { mxLog("Kalman Update Called."); }
	/* Creat local copies of State Space Matrices */
	omxMatrix* C = ose->C;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(C, "....State Space: C"); }
	omxMatrix* D = ose->D;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(D, "....State Space: D"); }
	omxMatrix* r = ose->r;
	omxMatrix* s = ose->s;
	omxMatrix* u = ose->u;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(u, "....State Space: u"); }
	omxMatrix* x = ose->x;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x"); }
	omxMatrix* y = ose->y;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(y, "....State Space: y"); }
	omxMatrix* P = ose->P;
	// omxMatrix* Cov = ose->cov; //unused
	omxMatrix* Means = ose->means;
	omxMatrix* Det = ose->det;
	*Det->data = 0.0; // the value pointed to by Det->data is assigned to be zero
	omxMatrix* smallC = ose->smallC;
	omxMatrix* smallD = ose->smallD;
	omxMatrix* smallr = ose->smallr;
	omxMatrix* R = ose->R;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(R, "....State Space: R on entry"); }
	omxMatrix* smallR = ose->smallR;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallR, "....State Space: small R on entry"); }
	omxMatrix* smallK = ose->smallK;
	omxMatrix* smallS = ose->smallS;
	omxMatrix* smallY = ose->smallY;
	int ny = y->rows;
	int nx = x->rows;
	Eigen::VectorXi toRemoveSS(ny);
	int numRemovesSS = 0;
	Eigen::VectorXi toRemoveNoneLat(nx);
	toRemoveNoneLat.setZero();
	Eigen::VectorXi toRemoveNoneOne(1);
	toRemoveNoneOne.setZero();

	omxMatrix* covInfo = ose->covInfo;
	int info = 0; // Used for computing inverse for Kalman gain

	omxCopyMatrix(smallS, ose->S);

	/* Reset/Resample aliased matrices */
	omxCopyMatrix(smallC, ose->C);
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallC, "....State Space: C (Reset)"); }
	omxCopyMatrix(smallD, ose->D);
	omxCopyMatrix(smallR, ose->R);
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallR, "....State Space: small R (Reset)"); }
	omxCopyMatrix(smallr, ose->r);
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallr, "....State Space: r (Reset)"); }
	omxCopyMatrix(smallK, ose->K);
	omxCopyMatrix(smallS, ose->S);
	omxCopyMatrix(smallY, ose->Y);

	/* r = r - C x - D u */
	/* Alternatively, create just the expected value for the data row, x. */
	omxDGEMV(FALSE, 1.0, smallC, x, 0.0, s); // s = C x
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(s, "....State Space: s = C x"); }
	omxDGEMV(FALSE, 1.0, D, u, 1.0, s); // s = D u + s THAT IS s = C x + D u
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(s, "....State Space: s = C x + D u"); }
	omxCopyMatrix(Means, s); // Means = s THAT IS Means = C x + D u
	//if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Means, "....State Space: Means"); }
	omxTransposeMatrix(Means);
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Means, "....State Space: Means"); }

	//If entire data vector, y, is missing, then set residual, r, to zero.
	//otherwise, compute residual.
	toRemoveSS.setZero();
	for(int j = 0; j < y->rows; j++) {
		double dataValue = omxMatrixElement(y, j, 0);
		int dataValuefpclass = std::fpclassify(dataValue);
		if(dataValuefpclass == FP_NAN || dataValuefpclass == FP_INFINITE) {
			numRemovesSS++;
			toRemoveSS[j] = 1;
			omxSetMatrixElement(r, j, 0, 0.0);
		} else {
			omxSetMatrixElement(r, j, 0, (dataValue -  omxMatrixElement(s, j, 0)));
		}
	}
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(r, "....State Space: Residual (Loop)"); }
	/* Now compute the residual */
	//omxCopyMatrix(r, y); // r = y
	//omxDAXPY(-1.0, s, r); // r = r - s THAT IS r = y - (C x + D u)
	omxCopyMatrix(smallr, ose->r);

	/* Filter S Here */
	// N.B. if y is completely missing or completely present, leave S alone.
	// Otherwise, filter S.
	if(numRemovesSS < ny && numRemovesSS > 0) {
		//if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: S"); }
		if(OMX_DEBUG) { mxLog("Filtering S, R, C, r, K, and Y."); }
		omxRemoveRowsAndColumns(smallS, numRemovesSS, numRemovesSS, toRemoveSS.data(), toRemoveSS.data());
		//if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: S (Filtered)"); }

		omxRemoveRowsAndColumns(smallR, numRemovesSS, numRemovesSS, toRemoveSS.data(), toRemoveSS.data());

		if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallC, "....State Space: C"); }
		omxRemoveRowsAndColumns(smallC, numRemovesSS, 0, toRemoveSS.data(), toRemoveNoneLat.data());
		if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallC, "....State Space: C (Filtered)"); }

		if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(r, "....State Space: r"); }
		omxRemoveRowsAndColumns(smallr, numRemovesSS, 0, toRemoveSS.data(), toRemoveNoneOne.data());
		if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallr, "....State Space: r (Filtered)"); }

		//if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallK, "....State Space: K"); }
		omxRemoveRowsAndColumns(smallK, numRemovesSS, 0, toRemoveSS.data(), toRemoveNoneLat.data());
		//if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallK, "....State Space: K (Filtered)"); }

		omxRemoveRowsAndColumns(smallY, numRemovesSS, 0, toRemoveSS.data(), toRemoveNoneLat.data());
	}
	if(numRemovesSS == ny) {
		if(OMX_DEBUG_ALGEBRA) { mxLog("Completely missing row of data found."); }
		if(OMX_DEBUG_ALGEBRA) { mxLog("Skipping much of Kalman Update."); }
		return ;
	}

	/* S = C P C^T + R */
	omxDSYMM(FALSE, 1.0, P, smallC, 0.0, smallY); // Y = C P
	//omxCopyMatrix(S, smallR); // S = R
	memcpy(smallS->data, smallR->data, smallR->rows * smallR->cols * sizeof(double)); // Less safe omxCopyMatrix that keeps smallS aliased to S.
	omxDGEMM(FALSE, TRUE, 1.0, smallY, smallC, 1.0, smallS); // S = Y C^T + S THAT IS C P C^T + R
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: S = C P C^T + R"); }

	/* Now compute the Kalman Gain and update the error covariance matrix */
	/* S = S^-1 */
	omxDPOTRF(smallS, &info); // S replaced by the lower triangular matrix of the Cholesky factorization
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: Cholesky of S"); }
	covInfo->data[0] = (double) info;
	for(int i = 0; i < smallS->cols; i++) {
		*Det->data += log(fabs(omxMatrixElement(smallS, i, i)));
	}
	//det *= 2.0; //sum( log( abs( diag( chol(S) ) ) ) )*2
	omxDPOTRI(smallS, &info); // S = S^-1 via Cholesky factorization
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: Inverse of S"); }
	// If Cholesky of exp cov failed (i.e. non-positive def), Populate 1,1 element of smallS (inverse of exp cov) with NA_REAL
	if(covInfo->data[0] > 0) {
		omxSetMatrixElement(smallS, 0, 0, NA_REAL);
	}

	/* K = P C^T S^-1 */
	/* Computed as K^T = S^-1 C P */
	omxDSYMM(TRUE, 1.0, smallS, smallY, 0.0, smallK); // K = Y^T S THAT IS K = P C^T S^-1
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallK, "....State Space: K^T = S^-1 C P"); }

	/* x = x + K r */
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallr, "....State Space Check Residual: r"); }
	omxDGEMV(TRUE, 1.0, smallK, smallr, 1.0, x); // x = K r + x
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x = K r + x"); }

	/* P = (I - K C) P */
	/* P = P - K C P */
	omxDGEMM(TRUE, FALSE, -1.0, smallK, smallY, 1.0, P); // P = -K Y + P THAT IS P = P - K C P
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P = P - K C P"); }

	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: Inverse of S"); }

	/*m2ll = y^T S y */ // n.b. y originally is the data row but becomes the data residual!
	//omxDSYMV(1.0, S, y, 0.0, s); // s = S y
	//m2ll = omxDDOT(y, s); // m2ll = y s THAT IS y^T S y
	//m2ll += det; // m2ll = m2ll + det THAT IS m2ll = log(det(S)) + y^T S y
	// Note: this leaves off the S->cols * log(2*pi) THAT IS k*log(2*pi)
}

void omxKalmanBucyPredict(omxStateSpaceExpectation* ose) {
	if(OMX_DEBUG) { mxLog("Kalman Bucy Predict Called."); }
	/* Creat local copies of State Space Matrices */
	omxMatrix* A = ose->A;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(A, "....State Space: A"); }
	omxMatrix* B = ose->B;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(B, "....State Space: B"); }
	omxMatrix* Q = ose->Q;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Q, "....State Space: Q"); }
	omxMatrix* u = ose->u;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(u, "....State Space: u"); }
	omxMatrix* x = ose->x;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x"); }
	omxMatrix* P = ose->P;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P"); }
	omxMatrix* Z = ose->Z;
	omxMatrix* t = ose->t;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(t, "....State Space: t"); }
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space oldT on entrance:\n" << ose->oldT << std::endl; }
	ose->deltaT = omxMatrixElement(t, 0, 0) - ose->oldT;
	ose->oldT = omxMatrixElement(t, 0, 0);
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space oldT on exit:\n" << ose->oldT << std::endl; }
	double deltaT = ose->deltaT;
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space deltaT:\n" << deltaT << std::endl; }

	//EigenMatrixAdaptor eigenA(A);
	// intializes eigenA as an instance of EigenMatrixAdaptor class, initialized to omxMatrix A
	// EigenVectorAdaptor
	// eigenB = eigenA.exp(); // matrix exponential
	// for scalar multiplication
	// A * omxElement(B,1,4); A eigen matrix, B omxMatrix
	// Subtract from diagonal 1.0
	// A.diagonal() -= 1

	/* Eigen Matrix reference setting */
	Eigen::MatrixXd &eigenExpA = ose->eigenExpA;
	Eigen::MatrixXd &eigenIA = ose->eigenIA;
	Eigen::MatrixXd &PSI = ose->PSI;
	Eigen::MatrixXd &IP = ose->IP;
	Eigen::MatrixXd &I = ose->I;

	/*R code for the next few lines
		ldim <- nrow(x$A)
		I <- diag(1, nrow=ldim)
		expA <- as.matrix(expm(x$A * x$deltaT))
		intA <- solve(x$A) %*% (expA - I)
		x$x <- expA %*% x$x + intA %*% x$B %*% x$u
	*/

	/*  Z = A
		eigenA = Z
		eigenExpA = eigenA*deltaT  THAT IS eigenExpA = A*deltaT
	*/
	omxCopyMatrix(Z, A);
	EigenMatrixAdaptor eigenA(Z);
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space eigenA:\n" << eigenA << std::endl; }
	eigenExpA = eigenA * deltaT;
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space eigenA:\n" << eigenA << std::endl; }
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space deltaT:\n" << deltaT << std::endl; }
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space eigenExpA:\n" << eigenExpA << std::endl; }

	/* eigenExpA = expm(eigenExpA)  THAT IS eigenExpA = expm(A*deltaT) */
	eigenExpA = eigenExpA.exp();
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space eigenExpA:\n" << eigenExpA << std::endl; }

	/* eigenIA = eigenExpA - I*/
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space I:\n" << I << std::endl; }
	eigenIA = eigenExpA - I;
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space expA - I:\n" << eigenIA << std::endl; }
	eigenIA = eigenA.lu().solve(eigenIA);
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space A^-1 (expA - I):\n" << eigenIA << std::endl; }
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space eigenA:\n" << eigenA << std::endl; }
	/* eigenIA = A^-1 IA  THAT IS eigenIA = A^-1 (expm(A*deltaT) - I) */

	/* x = expm(A*deltaT) * x + IA * B * u */
	EigenMatrixAdaptor eigenx(x); //or vector
	EigenMatrixAdaptor eigenu(u); //or vector
	EigenMatrixAdaptor eigenB(B);
	eigenx.derived() = eigenExpA * eigenx + eigenIA * eigenB * eigenu;
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space XPred:\n" << eigenx << std::endl; }

	/* SUMMARY */
	// EA = expm(A*deltaT)
	// IA = A^-1 (EA - I)
	// x = EA x + IA B u

	/*R code for the next few lines
		ldim <- nrow(x$A)
		psi <- cbind(rbind(x$A, matrix(0, nrow=ldim, ncol=ldim)), rbind(x$Q, -x$A))
		epsi <- as.matrix(expm(psi * x$deltaT)) %*% rbind(x$P, I)
		x$P <- epsi[1:ldim, ] %*% solve(epsi[(ldim+1):(2*ldim), ])
	*/

	/* PSI = block matrix
	-A^T  0
	 Q    A
	*/
	EigenMatrixAdaptor eigenQ(Q);
	PSI << -1.0*eigenA.transpose(), Eigen::MatrixXd::Zero(A->rows, A->rows), eigenQ, eigenA;
	PSI = PSI * deltaT;
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space PSI:\n" << PSI << std::endl; }
	/* PSI = PSI * deltaT */
	/* PSI = expm(PSI) */
	PSI = PSI.exp();
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space expPSI*deltaT:\n" << PSI << std::endl; }

	/* IP = block matrix
	I
	P
	*/
	EigenMatrixAdaptor eigenP(P);
	IP << I, eigenP;
	//IP << eigenP, Eigen::MatrixXd::Identity(rows, rows)
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space IP:\n" << IP << std::endl; }

	/* IP = PSI IP */
	IP = PSI * IP;
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space Blocks:\n" << IP << std::endl; }

	/* eigenIA = block 1 of IP; eigenExpA = block 2 of IP */
	eigenP.derived() = IP.block(0, 0, A->rows, A->cols);
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space Block1:\n" << eigenP << std::endl; }
	eigenExpA = IP.block(A->rows, 0, A->rows, A->cols);
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space Block2:\n" << eigenExpA << std::endl; }
	eigenP.derived() = eigenP.transpose().lu().solve(eigenExpA.transpose());
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space eigenP:\n" << eigenP << std::endl; }
	/* P = B1 B2^-1  THAT IS  solve B2^T P = B1^T for P */

	/* SUMMARY */
	// PSI = 	-A^T  0
	//			 Q    A
	// PSI = expm(PSI * deltaT)
	// IP = 	I
	// 			P
	// IP = PSI IP
	// P = IP[1] IP[2]^-1

	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space eigenX" << eigenx << std::endl; }
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x"); }
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space eigenP:\n" << eigenP << std::endl; }
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P"); }
}

void omxRauchTungStriebelSmooth(omxStateSpaceExpectation* ose) {
	if(OMX_DEBUG) { mxLog("Rauch Tung Striebel Smooth Called."); }
	/* Creat local copies of State Space Matrices */
	omxMatrix* A = ose->A;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(A, "....State Space: A"); }
	omxMatrix* x = ose->x;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x smoothed"); }
	omxMatrix* z = ose->z;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(z, "....State Space: x updated"); }
	omxMatrix* P = ose->P;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P smoothed"); }
	omxMatrix* Z = ose->Z;
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Z, "....State Space: P updated"); }

	/* Eigen Matrix reference setting */
	Eigen::MatrixXd &eigenPreX = ose->eigenPreX;
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space x predicted:\n" << eigenPreX << std::endl; }
	Eigen::MatrixXd &eigenExpA = ose->eigenExpA;
	if(OMX_DEBUG_ALGEBRA) {std::cout << "... State Space P predicted:\n" << eigenExpA << std::endl; }
	Eigen::MatrixXd &eigenIA = ose->eigenIA; // Storage for Sg (RTS smoother gain matrix)

	/* Eigen Adaptor copies (not really copies, more like wrappers) */
	EigenMatrixAdaptor eigenA(A);
	EigenMatrixAdaptor eigenx(x);
	EigenMatrixAdaptor eigenz(z);
	EigenMatrixAdaptor eigenP(P);
	EigenMatrixAdaptor eigenZ(Z);

	/* Create the RTS Gain matrix*/
	// Sg = Pui * A * Ppi+1 ^-1
	// eigenIA = Z * A * eigenExpA^-1
	// Possible typo above, A should be A^T
	eigenIA = eigenExpA.lu().solve( eigenA * eigenZ ).transpose();
	// try also
	//eigenIA = eigenExpA.ldlt().solve( eigenA.transpose() * eigenZ ).transpose();
	// with #include <Eigen/Cholesky>

	/* Smooth the latent state */
	// xsi = xui + Sg * (xsi+1 - xpi+1)
	// x = z + eigenIA * (x - eigenPreX)
	eigenx.derived() = eigenz + eigenIA * (eigenx - eigenPreX);

	/* Smooth the latent covariance */
	// Psi = Pui + Sg * (Psi+1 - Ppi+1) * Sg^T
	// P = Z + eigenIA * (P - eigenExpA) * eigenIA^T
	eigenP.derived() = eigenZ + eigenIA * (eigenP - eigenExpA) * eigenIA.transpose();

	// TODO add eigenPreX to struct and initialize
}

void omxInitStateSpaceExpectation(omxExpectation* ox) {

	SEXP rObj = ox->rObj;
	if(OMX_DEBUG) { mxLog("Initializing State Space Expectation."); }

	int nx, ny, nu;

	//SEXP slotValue;   //Used by PPML

	/* Create and fill expectation */
	//omxStateSpaceExpectation *SSMexp = (omxStateSpaceExpectation*) R_alloc(1, sizeof(omxStateSpaceExpectation));
	omxStateSpaceExpectation *SSMexp = new omxStateSpaceExpectation;

	omxState* currentState = ox->currentState;

	/* Set Expectation Calls and Structures */
	ox->computeFun = omxCallStateSpaceExpectation;
	ox->destructFun = omxDestroyStateSpaceExpectation;
	ox->componentFun = omxGetStateSpaceExpectationComponent;
	ox->mutateFun = omxSetStateSpaceExpectationComponent;
	ox->populateAttrFun = omxPopulateSSMAttributes;
	ox->argStruct = (void*) SSMexp;
	ox->canDuplicate = false;

	/* Set up expectation structures */
	if(OMX_DEBUG) { mxLog("Initializing State Space Meta Data for expectation."); }

	if(OMX_DEBUG) { mxLog("Processing A."); }
	SSMexp->A = omxNewMatrixFromSlot(rObj, currentState, "A");

	if(OMX_DEBUG) { mxLog("Processing B."); }
	SSMexp->B = omxNewMatrixFromSlot(rObj, currentState, "B");

	if(OMX_DEBUG) { mxLog("Processing C."); }
	SSMexp->C = omxNewMatrixFromSlot(rObj, currentState, "C");

	if(OMX_DEBUG) { mxLog("Processing D."); }
	SSMexp->D = omxNewMatrixFromSlot(rObj, currentState, "D");

	if(OMX_DEBUG) { mxLog("Processing Q."); }
	SSMexp->Q = omxNewMatrixFromSlot(rObj, currentState, "Q");

	if(OMX_DEBUG) { mxLog("Processing R."); }
	SSMexp->R = omxNewMatrixFromSlot(rObj, currentState, "R");

	if(OMX_DEBUG) { mxLog("Processing initial x."); }
	SSMexp->x0 = omxNewMatrixFromSlot(rObj, currentState, "x0");

	if(OMX_DEBUG) { mxLog("Processing initial P."); }
	SSMexp->P0 = omxNewMatrixFromSlot(rObj, currentState, "P0");

	if(OMX_DEBUG) { mxLog("Processing u."); }
	SSMexp->u = omxNewMatrixFromSlot(rObj, currentState, "u");

	if(OMX_DEBUG) { mxLog("Processing t."); }
	SSMexp->t = omxNewMatrixFromSlot(rObj, currentState, "t");

	/* Initialize the place holder matrices used in calculations */
	nx = SSMexp->C->cols;
	ny = SSMexp->C->rows;
	nu = SSMexp->D->cols;

	if(OMX_DEBUG) { mxLog("Processing first data row for y."); }
	SSMexp->y = omxInitMatrix(ny, 1, TRUE, currentState);
	for(int i = 0; i < ny; i++) {
		omxSetMatrixElement(SSMexp->y, i, 0, omxDoubleDataElement(ox->data, 0, i));
	}
	if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(SSMexp->y, "....State Space: y"); }

	// TODO Make x0 and P0 static (if possible) to save memory
	// TODO Look into omxMatrix.c/h for a possible new matrix from omxMatrix function
	if(OMX_DEBUG) { mxLog("Generating static internals for resetting initial values."); }
	SSMexp->x = 	omxInitMatrix(nx, 1, TRUE, currentState);
	SSMexp->P = 	omxInitMatrix(nx, nx, TRUE, currentState);
	omxCopyMatrix(SSMexp->x, SSMexp->x0);
	omxCopyMatrix(SSMexp->P, SSMexp->P0);

	if(OMX_DEBUG) { mxLog("Generating internals for computation."); }

	SSMexp->covInfo = 	omxInitMatrix(1, 1, TRUE, currentState);
	SSMexp->det = 	omxInitMatrix(1, 1, TRUE, currentState);
	SSMexp->r = 	omxInitMatrix(ny, 1, TRUE, currentState);
	SSMexp->s = 	omxInitMatrix(ny, 1, TRUE, currentState);
	SSMexp->z = 	omxInitMatrix(nx, 1, TRUE, currentState);
	SSMexp->K = 	omxInitMatrix(ny, nx, TRUE, currentState); // Actually the tranpose of the Kalman gain
	SSMexp->S = 	omxInitMatrix(ny, ny, TRUE, currentState);
	SSMexp->Y = 	omxInitMatrix(ny, nx, TRUE, currentState);
	SSMexp->Z = 	omxInitMatrix(nx, nx, TRUE, currentState);

	SSMexp->cov = 		omxInitMatrix(ny, ny, TRUE, currentState);
	SSMexp->means = 	omxInitMatrix(1, ny, TRUE, currentState);

	/* Create alias matrices for missing data filtering */
	SSMexp->smallC = 	omxInitMatrix(ny, nx, TRUE, currentState);
	SSMexp->smallD = 	omxInitMatrix(ny, nu, TRUE, currentState);
	SSMexp->smallR = 	omxInitMatrix(ny, ny, TRUE, currentState);
	SSMexp->smallr = 	omxInitMatrix(ny, 1, TRUE, currentState);
	SSMexp->smallK = 	omxInitMatrix(ny, nx, TRUE, currentState);
	SSMexp->smallS = 	omxInitMatrix(ny, ny, TRUE, currentState);
	SSMexp->smallY = 	omxInitMatrix(ny, nx, TRUE, currentState);

	//SSMexp->deltaT = 	omxInitMatrix(1, 1, TRUE, currentState);
	SSMexp->oldT = 0.0;
	SSMexp->deltaT = 0.0;

	/* Eigen Matrix initialization */
	SSMexp->eigenExpA.resize(nx, nx);
	SSMexp->I.resize(nx, nx);
	SSMexp->I = Eigen::MatrixXd::Identity(nx, nx);
	SSMexp->eigenIA.resize(nx, nx);
	SSMexp->PSI.resize(2*nx, 2*nx);
	SSMexp->IP.resize(2*nx, nx);
	SSMexp->eigenPreX.resize(nx, 1);

	/* Population of Kalman scores*/
	if(OMX_DEBUG) {
		mxLog("Accessing Kalman score population option.");
	}
	SSMexp->returnScores = Rf_asInteger(R_do_slot(rObj, Rf_install("scores")));

	omxCopyMatrix(SSMexp->smallC, SSMexp->C);
	omxCopyMatrix(SSMexp->smallD, SSMexp->D);
	omxCopyMatrix(SSMexp->smallR, SSMexp->R);
	omxCopyMatrix(SSMexp->smallr, SSMexp->r);
	omxCopyMatrix(SSMexp->smallK, SSMexp->K);
	omxCopyMatrix(SSMexp->smallS, SSMexp->S);
	omxCopyMatrix(SSMexp->smallY, SSMexp->Y);

}

omxMatrix* omxGetStateSpaceExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component) {
	omxStateSpaceExpectation* ose = (omxStateSpaceExpectation*)(ox->argStruct);
	omxMatrix* retval = NULL;

	if(strEQ("cov", component)) {
		retval = ose->cov;
	} else if(strEQ("means", component)) {
		retval = ose->means;
	} else if(strEQ("pvec", component)) {
		// Once implemented, change compute function and return pvec
	} else if(strEQ("inverse", component)) {
		retval = ose->smallS;
	} else if(strEQ("determinant", component)) {
		retval = ose->det;
	} else if(strEQ("r", component)) {
		retval = ose->r;
	} else if(strEQ("covInfo", component)) {
		retval = ose->covInfo;
	}

	return retval;
}

void omxSetStateSpaceExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component, omxMatrix* om) {
	omxStateSpaceExpectation* ose = (omxStateSpaceExpectation*)(ox->argStruct);

	if(!strcmp("y", component)) {
		for(int i = 0; i < ose->y->rows; i++) {
			omxSetMatrixElement(ose->y, i, 0, omxVectorElement(om, i));
		}
		//ose->y = om;
	}
	if(!strcmp("Reset", component)) {
		omxRecompute(ose->x0, NULL);
		omxRecompute(ose->P0, NULL);
		omxCopyMatrix(ose->x, ose->x0);
		omxCopyMatrix(ose->P, ose->P0);
		if(ose->t != NULL){
			ose->oldT = 0.0;
		}
	}
}