/*
 *  Orca-Components: Components for robotics.
 *  
 *  Copyright (C) 2004
 *  
 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or (at your option) any later version.
 *  
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *  
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

#include <fstream>
#include <unistd.h>

#include "clsfslam.h"
//#include "mcsCorrespondenceGraph.h"
//#include "mcsMaximumCliqueRand.h"

void *CLSFFusionThread(void *pArg)
{
  CLSFSlam *pCLSFSlam = (CLSFSlam *)pArg;

  nice(5);
  
  std::cout << "Fusion thread started..." << std::endl;
  pCLSFSlam->CLSFFuseLocalMap();

  //  sleep(1000);

  pthread_exit(NULL);
}

/*!
  Specify number of vehicle for multi-vehicle SLAM
*/
CLSFSlam::CLSFSlam() : Slam(),
    xLocalFrames_(__NUM_VEHICLE_STATES),
    XLocalFrames_(__NUM_VEHICLE_STATES, __NUM_VEHICLE_STATES)
{
  xLocalFrames_.clear();
  XLocalFrames_.clear();
  fusingLocalMap_ = false;
}

CLSFSlam::~CLSFSlam(void)
{
}

//! overload base class
bool CLSFSlam::prediction(const uVector &u, const uSymMatrix &U, const uSymMatrix &Q, double d_t)
{
  return localMap_.prediction(u, U, Q, d_t);
}

bool CLSFSlam::predictiondPose(const uVector &u, const uSymMatrix &U, const uSymMatrix &Q, double d_t)
{
  return localMap_.predictiondPose(u, U, Q, d_t);
}

//! overload base
bool CLSFSlam::observation(const uVector &z, const uSymMatrix &Z)
{
  // take the observation in the local map
  localMap_.observation(z, Z);
  static int numObservations = 0;

  // if the local map is 'big enough' or we've moved 'far enough' constrain it.
  // Note that other conditions may be more appropriate during this step.
  //    if (localMap_.x().size() > 19 ||
  //            pow(localMap_.x(STATE_X), 2.0) + pow(localMap_.x(STATE_Y), 2.0) > 9.0)
  if (numObservations++ > 50 && localMap_.x().size() > 7 && !fusingLocalMap_)
  {
    std::cout << "Local map Fusion Thread will be launched..." << std::endl;
    numObservations = 0;

    // prepare to launch a background thread to perform the constraint step with the local map.  Copy
    // the localMap_ so we can carry on in the foreground while the fusion is happening in the background
    // thread.
    currentLocalMap_ = localMap_;
    //    std::cout << currentLocalMap_.x() << localMap_.x() << std::endl;
    //    std::cout << currentLocalMap_.X() << localMap_.X() << std::endl;

    uVector xVeh(numVehicleStates_);
    uSymMatrix XVeh(numVehicleStates_, numVehicleStates_);

    // update the local frame estimate to be used for reporting vehicle position
    CLSFVehicleEstimate(xVeh, XVeh);

    xLocalFrames_[STATE_X] = xVeh[STATE_X];
    xLocalFrames_[STATE_Y] = xVeh[STATE_Y];
    xLocalFrames_[STATE_PSI] = xVeh[STATE_PSI];

    sub_matrix(XLocalFrames_, STATE_X, STATE_PSI + 1, STATE_X, STATE_PSI + 1)
    = sub_matrix(XVeh, STATE_X, STATE_PSI + 1, STATE_X, STATE_PSI + 1);

    fusingLocalMap_ = true;

    // launch the thread

    pthread_t id_copy;
    int err = pthread_create(&id_copy, 0, CLSFFusionThread, static_cast<void *>(this));
    if ( err != 0 )
    {
      std::cerr << "*** Error starting Fusion thread *** : Error code " << err << std::endl;
      fusingLocalMap_ = false;
    } else {
      // detach the thread as we a are not going to wait for it complete
      pthread_detach(id_copy);
      
      // reinitialise the local map
      std::cout << "Thread has been launched, carrying on..." << std::endl;
      localMap_.initialise(0.0, 0.0, 0.0);
    }
  }
  return true;
}

void CLSFSlam::CLSFFuseLocalMap()
{
  // apply the constraints to the local map
  CLSFConstrainLocalMap(currentLocalMap_);
  
  //  std::cout << "Finished up with the constraint application..." << std::endl;
  xLocalFrames_[STATE_X] = x_[STATE_X];
  xLocalFrames_[STATE_Y] = x_[STATE_Y];
  xLocalFrames_[STATE_PSI] = x_[STATE_PSI];

  sub_matrix(XLocalFrames_, STATE_X, STATE_PSI + 1, STATE_X, STATE_PSI + 1)
  = sub_matrix(X_, STATE_X, STATE_PSI + 1, STATE_X, STATE_PSI + 1);


  // re-enable fusion
  fusingLocalMap_ = false;
}

bool CLSFSlam::CLSFConstrainLocalMap(Slam &localMap)
{
  int numVehicleStatesLocal = localMap.NumVehicleStates();
  //int numLocalFeatures = (localMap.x().size()-numVehicleStatesLocal)/2;
  size_t numFeatures = (x_.size()-numVehicleStates_)/2;
  size_t numStatesLocal = localMap.x().size();
  size_t numStatesGlobal = x_.size();

  std::vector<int> Assoc;//(localMap.x().size());

  //std::cout << "For " << numVehicleStatesLocal << "local v states " << " and " << numStatesLocal << " matching against " << numFeatures << " global features" << std::endl;
  
  //std::cout << "x prior " << x_ << std::endl;
  //std::cout << "X prior " << X_ << std::endl;
  
  //std::cout << "x local " << localMap.x() << std::endl;
  //std::cout << "X local " << localMap.X() << std::endl;
  
  CLSFDataAssociationJC(localMap, Assoc);
  CLSFAddLocalEstimates(localMap);
  CLSFApplyConstraints(Assoc, numVehicleStatesLocal, numFeatures, numStatesLocal, numStatesGlobal);
  
  //std::cout << "x posterior " << x_ << std::endl;
  //std::cout << "X posterior " << X_ << std::endl;
  
  return true;
}

//bool CLSFSlam::CLSFApplyConstraints(Slam &localMap, std::vector<int> &Assoc)
bool CLSFSlam::CLSFApplyConstraints(std::vector<int> &Assoc, int numVehicleStatesLocal, int numFeatures, int numStatesLocal, int numStatesGlobal)
{
  size_t localXndx = numVehicleStatesLocal + STATE_X;
  size_t localYndx = numVehicleStatesLocal + STATE_Y;
  size_t localPSIndx = numVehicleStatesLocal + STATE_PSI;

  std::vector<int> compatibleLocalNdx;
  std::vector<int> compatibleGlobalNdx;
  std::vector<int> newLocalNdx;

  for (std::vector<int>::size_type j = 0; j < Assoc.size(); j++)
  {
    // the state vector now contains both the local and global states so
    // reflect this in the state indexes
    size_t localNdx = 2*j + numVehicleStatesLocal + numStatesGlobal;

    std::cout << Assoc[j] << " ";
    // find the associate and new feature states and compile the appropriate lists
    if (Assoc[j] != -1)
    {
      // the state vector now contains both the local and global states so
      // reflect this in the state indexes
      size_t globalNdx = Assoc[j] + numVehicleStatesLocal;

      compatibleLocalNdx.push_back(localNdx);
      compatibleGlobalNdx.push_back(globalNdx);
    }
    else
    {
      // this is a new state to be added to the global state vector
      newLocalNdx.push_back(localNdx);
    }
  }

  std::cout << std::endl;

  uMatrix C(2*compatibleLocalNdx.size(), sizeNewStateVector_);
  uMatrix dC(2*compatibleLocalNdx.size(), sizeNewStateVector_);

  C.clear();
  dC.clear();

  // Now apply constraints to the map estimates
  for ( int i = 0; i < compatibleLocalNdx.size(); i++)
  {
    int BxL = compatibleLocalNdx[i]; int ByL = compatibleLocalNdx[i] + 1;
    int BxG = compatibleGlobalNdx[i]; int ByG = compatibleGlobalNdx[i] + 1;

    // X constraints
    C(2*i, BxL) = cos(xNew_[localPSIndx]);
    C(2*i, ByL) = -sin(xNew_[localPSIndx]);
    C(2*i, localXndx) = 1;
    C(2*i, BxG) = -1;
    // Y constraints
    C(2*i + 1, BxL) = sin(xNew_[localPSIndx]);
    C(2*i + 1, ByL) = cos(xNew_[localPSIndx]);
    C(2*i + 1, localYndx) = 1;
    C(2*i + 1, ByG) = -1;

    // X jacobians
    dC(2*i, BxL) = cos(xNew_[localPSIndx]);
    dC(2*i, ByL) = -sin(xNew_[localPSIndx]);
    dC(2*i, localPSIndx) = -xNew_[BxL]*sin(xNew_[localPSIndx]) - xNew_[ByL]*cos(xNew_[localPSIndx]);
    dC(2*i, localXndx) = 1;
    dC(2*i, BxG) = -1;
    // Y jacobians
    dC(2*i + 1, BxL) = sin(xNew_[localPSIndx]);
    dC(2*i + 1, ByL) = cos(xNew_[localPSIndx]);
    dC(2*i + 1, localPSIndx) = xNew_[BxL]*cos(xNew_[localPSIndx]) - xNew_[ByL]*sin(xNew_[localPSIndx]);
    dC(2*i + 1, localYndx) = 1;
    dC(2*i + 1, ByG) = -1;
  }

  //std::cout << "X pre constraint " << X_ << std::endl;
  if (compatibleLocalNdx.size() > 0)
  {
    int NumConstraints = 2*compatibleLocalNdx.size();
    uSymMatrix Sc(NumConstraints, NumConstraints);
    //uSymMatrix iSc(NumConstraints, NumConstraints);
    uMatrix Wc(sizeNewStateVector_, NumConstraints);

    // apply the constraints
    //Sc = FM::mult_SPD(dC, X_);
    Sc = prod_SPD(dC, XNew_);
    uSymMatrix iSc(Sc);
    //FM::copy(Sc,iSc);
    //std::cout << FM::UdUinversePD(iSc);
    //SBW    iSc = FM::inverseLU(Sc);
    invert_SPD(iSc);
    uVector innc(C.size1());
    innc = prod(C, xNew_);
    Wc = prod(prod(XNew_, trans(dC)), iSc);

    xNew_ = xNew_ - prod(Wc, innc);  // note : - sign is from application of constraint with perfect information
    XNew_ = XNew_ - prod_SPD(Wc, Sc);//mult_SPD(Wc, Sc);
  }
  //std::cout << "X post constraint " << X_ << std::endl;

  uMatrix G(sizeNewStateVector_, sizeNewStateVector_);
  uMatrix dG(sizeNewStateVector_, sizeNewStateVector_);

  G.clear();
  dG.clear();

  identity(G);
  identity(dG);

  // now convert the vehicle estimate using the constrained frame estimate
  G(STATE_X, STATE_X) = cos(xNew_[localPSIndx]);
  G(STATE_X, STATE_Y) = -sin(xNew_[localPSIndx]);
  G(STATE_X, localXndx) = 1;
  G(STATE_Y, STATE_X) = sin(xNew_[localPSIndx]);
  G(STATE_Y, STATE_Y) = cos(xNew_[localPSIndx]);
  G(STATE_Y, localYndx) = 1;
  G(STATE_PSI, localPSIndx) = 1;

  // compute the relevant Jacobian
  dG(STATE_X, STATE_X) = cos(xNew_[localPSIndx]);
  dG(STATE_X, STATE_Y) = -sin(xNew_[localPSIndx]);
  dG(STATE_X, localXndx) = 1;
  dG(STATE_X, localPSIndx) = -xNew_[STATE_X]*sin(xNew_[localPSIndx]) - xNew_[STATE_Y]*cos(xNew_[localPSIndx]);
  dG(STATE_Y, STATE_X) = sin(xNew_[localPSIndx]);
  dG(STATE_Y, STATE_Y) = cos(xNew_[localPSIndx]);
  dG(STATE_Y, localYndx) = 1;
  dG(STATE_Y, localPSIndx) = xNew_[STATE_X]*cos(xNew_[localPSIndx]) - xNew_[STATE_Y]*sin(xNew_[localPSIndx]);
  dG(STATE_PSI, localPSIndx) = 1;

  // do the new local states here to use the updated estimate of the local
  // frame of reference
  for ( size_t i = 0; i < newLocalNdx.size(); i++)
  {
    // this is a new local state, add this fact to the transformation matrix G
    int Bx = newLocalNdx[i]; int By = newLocalNdx[i] + 1;
    G(Bx, Bx) = cos(xNew_[localPSIndx]);
    G(Bx, By) = -sin(xNew_[localPSIndx]);
    G(Bx, localXndx) = 1;
    G(By, Bx) = sin(xNew_[localPSIndx]);
    G(By, By) = cos(xNew_[localPSIndx]);
    G(By, localYndx) = 1;

    // compute the relevant Jacobian
    dG(Bx, Bx) = cos(xNew_[localPSIndx]);
    dG(Bx, By) = -sin(xNew_[localPSIndx]);
    dG(Bx, localXndx) = 1;
    dG(Bx, localPSIndx) = -xNew_[Bx]*sin(xNew_[localPSIndx]) - xNew_[By]*cos(xNew_[localPSIndx]);
    dG(By, Bx) = sin(xNew_[localPSIndx]);
    dG(By, By) = cos(xNew_[localPSIndx]);
    dG(By, localYndx) = 1;
    dG(By, localPSIndx) = xNew_[Bx]*cos(xNew_[localPSIndx]) - xNew_[By]*sin(xNew_[localPSIndx]);
  }

  //std::cout << "dG " << dG << std::endl;
  //std::cout << "X pre transform " << X_ << std::endl;
  
  // apply the transformation to the global frame to the remaining local states
  xNew_ = prod(G, xNew_);
  XNew_ = prod_SPD(dG, XNew_);//FM::mult_SPD(dG, X_);

  //std::cout << "X post transform " << X_ << std::endl;
  
  int NumStates = xNew_.size();

  std::vector<size_t> fndx;
  // the estimate of the local frame will be removed from the global state estimate
  // store the new vehicle estimate as the new frame estimate
  for ( size_t i = numVehicleStatesLocal; i<NumStates; i++)
  {
    // remove the local frame states and replace them with the updated vehicle estimate
    if (i == localXndx)
      fndx.push_back(STATE_X);
    else if (i == localYndx)
      fndx.push_back(STATE_Y);
    else if (i == localPSIndx)
      fndx.push_back(STATE_PSI);
    // ignore any duplicate feature estimates
    else if (std::find(compatibleLocalNdx.begin(), compatibleLocalNdx.end(), i) == compatibleLocalNdx.end()
             && std::find(compatibleLocalNdx.begin(), compatibleLocalNdx.end(), i-1) == compatibleLocalNdx.end())
      fndx.push_back(i);
  }

  // resize the state vector by extracting the relevant states
  pthread_mutex_lock(&state_mt);
  x_.resize(fndx.size());
  x_ = sub(xNew_, fndx);

  // resize the covariance matrix by extracting the relevant states
  X_.resize(fndx.size(), fndx.size());
  X_ = sub(XNew_, fndx, fndx);
  
  //std::cout << "X post reduction " << X_ << std::endl;

  // reflect the changes in the size of the state vector
  sizeStateVector_ = x_.size();
  //numVehicleStates_ -= numVehicleStatesLocal;
  numObservedTargets_ = (x_.size() - numVehicleStates_)/2;
  pthread_mutex_unlock(&state_mt);

  return true;
}

bool CLSFSlam::CLSFDataAssociationJC(Slam &localMap, std::vector<int> &Assoc)
{
  int i, j, k;
  int numVehicleStatesLocal = localMap.NumVehicleStates();
  int numLocalFeatures = (localMap.x().size()-numVehicleStatesLocal)/2;
  int numFeatures = (x_.size()-numVehicleStates_)/2;
  Assoc.resize(numLocalFeatures);

  // initialise the association vector
  std::fill (Assoc.begin (), Assoc.end (), -1);

  if (numLocalFeatures == 0 || numFeatures == 0)
    return false;

  double Dij;//[NumObs][NumFeatures];

  int numPairings = 1;
  std::vector<int> *Pairings = NULL;

  uVector xySingle(2);
  uSymMatrix XYSingle(2,2);
  uSymMatrix XLocal(localMap.X());

  for (i = 0; i < numLocalFeatures; i++)
  {

    std::vector<int> compatibleIJ;
    int localndx = 2*i + numVehicleStatesLocal;
    xySingle[0] = localMap.x(localndx);
    xySingle[1] = localMap.x(localndx + 1);
    XYSingle = sub_matrix(XLocal, localndx,localndx+2, localndx,localndx+2);

    //std::cout << "XYSingle: " << XYSingle << std::endl;
    
    for (j = 0; j < numFeatures; j++)
    {
      std::vector<int> Fndx;
      Fndx.push_back(2*j+numVehicleStates_);

      // check the individual compatibility between the feature and local map
      // states
      //std::cout << "Checking individual compatibility for local feature " << i << " and global " << j << std::endl;
      InnovationXY(xySingle, XYSingle, Fndx);
      //std::cout << "Inn Cov S: " << S_;// << std::endl;
      //FM::copy(S_,iS);
      //std::cout << FM::UdUinversePD(iS);
      //SBW                     iS = FM::inverseLU(S_);
      uSymMatrix iS(S_);
      //std::cout << " iS: " << iS << std::endl;
      invert_SPD(iS);
      Dij = fabs(prod_SPDT(inn_, iS));

      // if the features are compatible, add them to the list of compatible
      // features
      if (Dij < 12)
        //                      if (Dij < xySingle.size()-0.7) // this approximates the 50th percentile of the Chi^2 distribution
      // if (Dij < 6)
      {
        compatibleIJ.push_back(j);
      }
    }
    // if no states are compatible, push a -1 onto the vector
    if (compatibleIJ.size() == 0)
    {
      compatibleIJ.push_back(-1);
    }
    int numNewPairings = compatibleIJ.size();

    // expand the compatibility tree.  At this step there should be a possible NULL
    // pairing added for each state.  However this introduces problems with compairing
    // pairings of different sizes and makes the size of the tree to search significantly
    // larger.  For now, consider pairings that have been matched individually.  This may
    // cause some issues but these can be considered later...
    std::vector<int> *NewPairings = new std::vector<int>[numPairings*numNewPairings];
    for (j = 0; j < numPairings; j++)
    {
      for (k = 0; k < numNewPairings; k++)
      {
        int pairingNdx = j*numNewPairings + k;
        if (Pairings != NULL)
          NewPairings[pairingNdx] = Pairings[j];
        NewPairings[pairingNdx].push_back(compatibleIJ[k]);
      }
    }
    if (Pairings != NULL)
      delete [] Pairings;

    Pairings = NewPairings;
    numPairings = numPairings*numNewPairings;
  }

  double DijJoint;
  double minDijJoint = -1;
  int minDijNdx = -1;

  if (numPairings > 5)
    std::cout << "CLSF Found " << numPairings << " pairings " << std::endl;

  // now evaluate the joint compatibility of the pairings
  for (i = 0; i < numPairings; i++)
  {
    std::vector<size_t> compatiblelocalNdx;
    std::vector<int> BState;

    for (j = 0; j < numLocalFeatures; j++)
    {
      if (Pairings[i][j] != -1)
      {
        int localNdx = 2*j + numVehicleStatesLocal;
        compatiblelocalNdx.push_back(localNdx);
        compatiblelocalNdx.push_back(localNdx + 1);
        BState.push_back(numVehicleStates_ + 2*Pairings[i][j]);
      }
    }

    uVector xyCombined(compatiblelocalNdx.size());
    uSymMatrix XYCombined(compatiblelocalNdx.size(), compatiblelocalNdx.size());//zeros(length(find(Pairings(:,i)), length(find(Pairings(:,i)));

    xyCombined = sub(localMap.x(), compatiblelocalNdx);
    XYCombined = sub(localMap.X(), compatiblelocalNdx, compatiblelocalNdx);

    if (xyCombined.size() > 0)
    {
      // evaluate the joint innovation
      InnovationXY(xyCombined, XYCombined, BState);

      uSymMatrix iS(S_);
      //FM::copy(S_,iS);
      //std::cout << FM::UdUinversePD(iS);
      //SBW                     iS = FM::inverseLU(S_);
      invert_SPD(iS);
      DijJoint = prod_SPDT(inn_, iS);
      // we're interested in the closest match
      if (minDijJoint < 0 || DijJoint < minDijJoint)
      {
        minDijJoint = DijJoint;
        minDijNdx = i;
      }
    }
  }

  // if a compatible pairing was found, fill in the values in the Assciation vector
  if (minDijNdx != -1)
  {
    int AssocNdx = 0;
    for (std::vector<int>::iterator pNdx = Pairings[minDijNdx].begin(); pNdx < Pairings[minDijNdx].end(); pNdx++, AssocNdx++)
    {
      if (Pairings[minDijNdx][AssocNdx] != -1)
      {
        Assoc[AssocNdx] = 2*Pairings[minDijNdx][AssocNdx] + numVehicleStates_;
      }
    }
  }
  else
  {
    if (Pairings != NULL)
      delete [] Pairings;
    return false;
  }

  if (Pairings != NULL)
    delete [] Pairings;

  return true;
}

bool CLSFSlam::CLSFAddLocalEstimates(Slam &localMap)
{
  int numStatesLocal = localMap.x().size();
  int numVehicleStatesLocal = localMap.NumVehicleStates();
  int numStatesGlobal = x_.size();
  int newNumStates = numStatesGlobal + numStatesLocal;

  // copy the local state elements into the combined state vector
  // covariance structure is:
  // L^xv
  // G^x
  // L^xm
  //std::cout << "Populating new state vector" << std::endl;
  uVector xLocal(localMap.x());
  //uVector xNew(newNumStates);
  xNew_.resize(newNumStates);
  sub_vector(xNew_, 0, numVehicleStatesLocal) = sub_vector(xLocal, 0, numVehicleStatesLocal);
  sub_vector(xNew_, numVehicleStatesLocal, numVehicleStatesLocal + numStatesGlobal) = x_;
  sub_vector(xNew_, numVehicleStatesLocal + numStatesGlobal, newNumStates) =  sub_vector(xLocal, numVehicleStatesLocal, numStatesLocal);
  //x_.resize(newNumStates);
  //x_ = xNew;
  //std::cout << " x_ " << x_ << std::endl;
  
  // copy the local covariance elements into the combined covariance matrix
  // covariance strcuture is:
  // L^Xvv      0               L^Xvm
  // 0          G^X             0
  // LXvm               0               L^Xmm
  uSymMatrix XLocal(localMap.X());
  XNew_.resize(newNumStates);
  XNew_.clear();

  //FM::Matrix XLocal = localMap.X();

  //std::cout << "Populating new state matrix from" << std::endl;
  //std::cout << " XLocal " << XLocal << std::endl;
  //std::cout << " X_ " << X_ << std::endl;
  // copy in the Local Map elements
  // top left
  sub_matrix(XNew_, 0, numVehicleStatesLocal, 0, numVehicleStatesLocal) = sub_matrix(XLocal, 0, numVehicleStatesLocal, 0, numVehicleStatesLocal);
  // bottom right
  sub_matrix(XNew_, numVehicleStatesLocal + numStatesGlobal, newNumStates,numVehicleStatesLocal + numStatesGlobal, newNumStates) = sub_matrix(XLocal, numVehicleStatesLocal, numStatesLocal, numVehicleStatesLocal, numStatesLocal);
  // top right
  sub_matrix(XNew_, numVehicleStatesLocal + numStatesGlobal, newNumStates, 0,numVehicleStatesLocal) = trans(sub_matrix(XLocal, 0,numVehicleStatesLocal,numVehicleStatesLocal, numStatesLocal));
  // bottom left
  sub_matrix(XNew_, 0,numVehicleStatesLocal,numVehicleStatesLocal + numStatesGlobal, newNumStates) = sub_matrix(XLocal, 0, numVehicleStatesLocal, numVehicleStatesLocal, numStatesLocal);

  //std::cout << XNew << std::endl;
  
  // now copy in the Global Map elements - middle
  sub_matrix(XNew_, numVehicleStatesLocal, numVehicleStatesLocal + numStatesGlobal, numVehicleStatesLocal, numVehicleStatesLocal + numStatesGlobal) = X_;

  //std::cout << XNew << std::endl;
  
  //std::cout << "Copying new state matrix" << std::endl;
  //X_.resize(newNumStates, newNumStates); 
  //X_ = XNew;

  //std::cout << "Done with copying new state matrix" << std::endl;
  // reflect the changes in the size of the state vector
  sizeNewStateVector_ = newNumStates;
  //numObservedTargets_ = (sizeStateVector_ - numVehicleStatesLocal - numVehicleStates_)/2;
  //numVehicleStates_ += numVehicleStatesLocal;

  return true;
}

bool CLSFSlam::InnovationXY(const uVector &xLocal, const uSymMatrix &XLocal, std::vector<int> &BStates)
{
  // resize the H, inn and S variables if necessary
  if (inn_.size() != xLocal.size())
  {
    inn_.resize(xLocal.size());
    S_.resize(xLocal.size(), xLocal.size());
  }

  uMatrix HGlobal(xLocal.size(), x_.size());
  uMatrix dHGlobal(xLocal.size(), x_.size());
  uMatrix HLocal(xLocal.size(), xLocal.size());
  uMatrix dHLocal(xLocal.size(), xLocal.size());

  HGlobal.clear();
  dHGlobal.clear();
  HLocal.clear();
  dHLocal.clear();

  // for each local feature, compute the observation jacobians
  for (std::vector<int>::size_type i = 0; i < xLocal.size(); i += 2)
  {
    int BStateGlobal = BStates[(int)ceil(i/2)];
    int BStateLocal = i;

    // global H
    HGlobal(i, BStateGlobal) = 1;
    HGlobal(i + 1, BStateGlobal + 1) = 1;
    HGlobal(i, STATE_X) = -1;
    HGlobal(i + 1, STATE_Y) = -1;
    // global dH
    dHGlobal(i, BStateGlobal) = 1;
    dHGlobal(i + 1, BStateGlobal + 1) = 1;
    dHGlobal(i, STATE_X) = -1;
    dHGlobal(i + 1, STATE_Y) = -1;
    dHGlobal(i, STATE_PSI) = xLocal[i]*sin(x_[STATE_PSI]) + xLocal[i + 1]*cos(x_[STATE_PSI]);
    dHGlobal(i + 1, STATE_PSI) = -xLocal[i]*cos(x_[STATE_PSI]) + xLocal[i + 1]*sin(x_[STATE_PSI]);

    // local H
    HLocal(i, BStateLocal) = -cos(x_[STATE_PSI]);
    HLocal(i, BStateLocal + 1) = sin(x_[STATE_PSI]);
    HLocal(i + 1, BStateLocal) = -sin(x_[STATE_PSI]);
    HLocal(i + 1, BStateLocal + 1) = -cos(x_[STATE_PSI]);
    // local dH
    dHLocal(i, BStateLocal) = -cos(x_[STATE_PSI]);
    dHLocal(i, BStateLocal + 1) = sin(x_[STATE_PSI]);
    dHLocal(i + 1, BStateLocal) = -sin(x_[STATE_PSI]);
    dHLocal(i + 1, BStateLocal + 1) = -cos(x_[STATE_PSI]);
  }
  
  // compute the innovation and innovation covariance
  inn_ = prod(HGlobal, x_) + prod(HLocal, xLocal);
  S_ = prod_SPD(dHGlobal,X_) + prod_SPD(dHLocal,XLocal);

  return true;
}

/*!
    alex m: should this be in Slam class??? or is CLSFSlam different?
*/
void
CLSFSlam::vehicle(orca::Pose2DGauss &poseEstimate) //const
{
  uVector xVeh(numVehicleStates_);
  uSymMatrix XVeh(numVehicleStates_, numVehicleStates_);
  CLSFVehicleEstimate(xVeh, XVeh);

  poseEstimate.setPose2DGauss(xVeh[STATE_X], xVeh[STATE_Y],
                   xVeh[STATE_PSI],
                   XVeh(STATE_X, STATE_X),
                   XVeh(STATE_X, STATE_Y),
                   XVeh(STATE_Y, STATE_Y),
                   XVeh(STATE_PSI, STATE_PSI),
                   XVeh(STATE_X, STATE_PSI),
                   XVeh(STATE_Y, STATE_PSI)
      );
}

bool CLSFSlam::CLSFVehicleEstimate(uVector &xVeh, uSymMatrix &XVeh) //const
{
  // transform the current local vehicle estimate using the estimate of the global
  // frame of reference
  xVeh[STATE_X] = xLocalFrames_[STATE_X] + localMap_.x(STATE_X)*cos(xLocalFrames_[STATE_PSI]) - localMap_.x(STATE_Y)*sin(xLocalFrames_[STATE_PSI]);
  xVeh[STATE_Y] = xLocalFrames_[STATE_Y] + localMap_.x(STATE_X)*sin(xLocalFrames_[STATE_PSI]) + localMap_.x(STATE_Y)*cos(xLocalFrames_[STATE_PSI]);
  xVeh[STATE_PSI] = xLocalFrames_[STATE_PSI] + localMap_.x(STATE_PSI);

  // compute the jacobians for the global and local states
  uMatrix HGlobal(__NUM_VEHICLE_STATES, __NUM_VEHICLE_STATES);
  identity(HGlobal);
  HGlobal(STATE_X, STATE_PSI) = -localMap_.x(STATE_X)*sin(xLocalFrames_[STATE_PSI]) - localMap_.x(STATE_Y)*cos(xLocalFrames_[STATE_PSI]);
  HGlobal(STATE_Y, STATE_PSI) = localMap_.x(STATE_X)*cos(xLocalFrames_[STATE_PSI]) - localMap_.x(STATE_Y)*sin(xLocalFrames_[STATE_PSI]);

  uMatrix HLocal(__NUM_VEHICLE_STATES, __NUM_VEHICLE_STATES);
  HLocal.clear();
  HLocal(STATE_X, STATE_X) = cos(xLocalFrames_[STATE_PSI]);
  HLocal(STATE_X, STATE_Y) = -sin(xLocalFrames_[STATE_PSI]);
  HLocal(STATE_Y, STATE_X) = sin(xLocalFrames_[STATE_PSI]);
  HLocal(STATE_Y, STATE_Y) = cos(xLocalFrames_[STATE_PSI]);
  HLocal(STATE_PSI, STATE_PSI) = 1;

  // project the vehicle uncertainty into the global frame of reference
  uSymMatrix XLocal(localMap_.X());
  sub_matrix(XVeh, STATE_X, STATE_PSI + 1, STATE_X, STATE_PSI + 1) =
    prod_SPD(HGlobal, sub_matrix(XLocalFrames_, STATE_X, STATE_PSI + 1, STATE_X, STATE_PSI + 1))
    + prod_SPD(HLocal, sub_matrix(XLocal, 0, __NUM_VEHICLE_STATES, 0, __NUM_VEHICLE_STATES));

  return true;
}