/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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.
*/

#include "BlockMatrixIterators.hpp"
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "SimpleMatrix.hpp"
#include "SiconosAlgebra.hpp"

using namespace Siconos;

void add(const SiconosMatrix & A, const SiconosMatrix& B, SiconosMatrix& C)
{
  // To compute C = A + B in an "optimized" way (in comparison with operator +)

  if ((A.size(0) != B.size(0)) || (A.size(1) != B.size(1)))
    SiconosMatrixException::selfThrow("Matrix addition: inconsistent sizes");
  if ((A.size(0) != C.size(0)) || (A.size(1) != C.size(1)))
    SiconosMatrixException::selfThrow("Matrix addition: inconsistent sizes");

  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();

  // === if C is zero or identity => read-only ===
  if (numC == 6 || numC == 7)
    SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C (read-only: zero or identity).");

  // === common memory between A, B, C ===
  if (&A == &C) // A and C have common memory
  {
    C += B;
  }
  else if (&B == &C)  // B and C have common memory
  {
    C += A;
  }
  else // No common memory between C and A or B.
  {
    if (numA == 6) // A = 0
      C = B ;
    else if (numB == 6) // B = 0
      C = A;
    else // A and B different from 0
    {
      if (numC == 0) // if C is Block
      {
        if (numA != 0) // A simple, whatever is B
        {
          C = A;
          C += B;
        }
        else  // A Block
        {
          C = B;
          C += A;
        }
      }
      else // if C is a SimpleMatrix
      {
        if (numA == numB && numA != 0) // A and B are of the same type and NOT block
        {
          if (numC == numA)
          {
            if (numA == 1)
              noalias(*C.dense()) = *A.dense() + *B.dense();
            else if (numA == 2)
              noalias(*C.triang()) = *A.triang() + *B.triang();
            else if (numA == 3)
              noalias(*C.sym()) = *A.sym() + *B.sym();
            else if (numA == 4)
              noalias(*C.sparse()) = *A.sparse() + *B.sparse();
            else //if(numA==5)
              noalias(*C.banded()) = *A.banded() + *B.banded();
          }
          else // C and A of different types.
          {
            if (numC != 1)
              SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C.");
            // Only dense matrices are allowed for output.

            if (numA == 1)
              noalias(*C.dense()) = *A.dense() + *B.dense();
            else if (numA == 2)
              noalias(*C.dense()) = *A.triang() + *B.triang();
            else if (numA == 3)
              noalias(*C.dense()) = *A.sym() + *B.sym();
            else if (numA == 4)
              noalias(*C.dense()) = *A.sparse() + *B.sparse();
            else //if(numA==5)
              noalias(*C.dense()) = *A.banded() + *B.banded();
          }
          C.resetLU();
        }
        else if (numA != 0 && numB != 0 && numA != numB) // A and B of different types and none is block
        {
          if (numC != 1)
            SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C.");
          // Only dense matrices are allowed for output.

          if (numA == 1)
            switch (numB)
            {
            case 2:
              noalias(*C.dense()) = *A.dense() + *B.triang();
              break;
            case 3:
              noalias(*C.dense()) = *A.dense() + *B.sym();
              break;
            case 4:
              noalias(*C.dense()) = *A.dense() + *B.sparse();
              break;
            case 5:
              noalias(*C.dense()) = *A.dense() + *B.banded();
              break;
            case 7:
              noalias(*C.dense()) = *A.dense() + *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 2)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.triang() + *B.dense();
              break;
            case 3:
              noalias(*C.dense()) = *A.triang() + *B.sym();
              break;
            case 4:
              noalias(*C.dense()) = *A.triang() + *B.sparse();
              break;
            case 5:
              noalias(*C.dense()) = *A.triang() + *B.banded();
              break;
            case 7:
              noalias(*C.dense()) = *A.triang() + *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 3)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.sym() + *B.dense();
              break;
            case 2:
              noalias(*C.dense()) = *A.sym() + *B.triang();
              break;
            case 4:
              noalias(*C.dense()) = *A.sym() + *B.sparse();
              break;
            case 5:
              noalias(*C.dense()) = *A.sym() + *B.banded();
              break;
            case 7:
              noalias(*C.dense()) = *A.sym() + *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 4)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.sparse() + *B.dense();
              break;
            case 2:
              noalias(*C.dense()) = *A.sparse() + *B.triang();
              break;
            case 3:
              noalias(*C.dense()) = *A.sparse() + *B.sym();
              break;
            case 5:
              noalias(*C.dense()) = *A.sparse() + *B.banded();
              break;
            case 7:
              noalias(*C.dense()) = *A.sparse() + *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 5)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.banded() + *B.dense();
              break;
            case 2:
              noalias(*C.dense()) = *A.banded() + *B.triang();
              break;
            case 3:
              noalias(*C.dense()) = *A.banded() + *B.sym();
              break;
            case 4:
              noalias(*C.dense()) = *A.banded() + *B.sparse();
              break;
            case 7:
              noalias(*C.dense()) = *A.banded() + *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 7)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.identity() + *B.dense();
              break;
            case 2:
              noalias(*C.dense()) = *A.identity() + *B.triang();
              break;
            case 3:
              noalias(*C.dense()) = *A.identity() + *B.sym();
              break;
            case 4:
              noalias(*C.dense()) = *A.identity() + *B.sparse();
              break;
            case 5:
              noalias(*C.dense()) = *A.identity() + *B.banded();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else
            SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
          C.resetLU();
        }
        else // A and/or B is Block
        {
          if (numA != 0) // A Simple, whatever is B
          {
            C = A;
            C += B;
          }
          else // A Block
          {
            C = B;
            C += A;
          }
        }
      }
    }
  }

}

void sub(const SiconosMatrix & A, const SiconosMatrix& B, SiconosMatrix& C)
{
  // To compute C = A - B in an "optimized" way (in comparison with operator +)

  if ((A.size(0) != B.size(0)) || (A.size(1) != B.size(1)))
    SiconosMatrixException::selfThrow("Matrix addition: inconsistent sizes");
  if ((A.size(0) != C.size(0)) || (A.size(1) != C.size(1)))
    SiconosMatrixException::selfThrow("Matrix addition: inconsistent sizes");

  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();

  // === if C is zero or identity => read-only ===
  if (numC == 6 || numC == 7)
    SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C (read-only: zero or identity).");

  // === common memory between A, B, C ===
  if (&A == &C) // A and C have common memory
  {
    C -= B;
  }
  else if (&B == &C)  // B and C have common memory
  {
    if (numB == 0 || numA == 0) // if A or B(C) is Block
    {
      C *= -1.0;
      C += A;
    }
    else
    {
      if (numC == 0) // if C is Block
      {
        C = A;
        C -= B;
      }
      else // if C is a SimpleMatrix
      {
        if (numA == numB && numA != 0) // A and B are of the same type and NOT block
        {
          if (numA == 1)
            *C.dense() = *A.dense() - *B.dense();
          else if (numA == 2)
            *C.triang() = *A.triang() - *B.triang();
          else if (numA == 3)
            *C.sym() = *A.sym() - *B.sym();
          else if (numA == 4)
            *C.sparse() = *A.sparse() - *B.sparse();
          else //if(numA==5)
            *C.banded() = *A.banded() - *B.banded();
        }
        else if (numA != 0 && numB != 0 && numA != numB) // A and B of different types and none is block
        {
          if (numC != 1)  // => numB == 1
            SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C.");
          // Only dense matrices are allowed for output.

          if (numA == 1)
            *C.dense() = *A.dense() - *B.dense();
          else if (numA == 2)
            *C.dense() = *A.triang() - *B.dense();
          else if (numA == 3)
            *C.dense() = *A.sym() - *B.dense();
          else if (numA == 4)
            *C.dense() = *A.sparse() - *B.dense();
          else if (numA == 5)
            *C.dense() = *A.banded() - *B.dense();
          else if (numA == 6)
            *C.dense() = *A.zero_mat() - *B.dense();
          else //if(numA==7)
            *C.dense() = *A.identity() - *B.dense();
        }
        else // A and/or B is Block
        {
          C = A;
          C -= B;
        }
        C.resetLU();
      }
    }
  }
  else // No common memory between C and A or B.
  {
    if (numB == 6) // B = 0
      C = A;
    else // B different from 0
    {
      if (numC == 0) // if C is Block
      {
        C = A;
        C -= B;
      }
      else // if C is a SimpleMatrix
      {
        if (numA == numB && numA != 0) // A and B are of the same type and NOT block
        {
          if (numC == numA)
          {
            if (numA == 1)
              noalias(*C.dense()) = *A.dense() - *B.dense();
            else if (numA == 2)
              noalias(*C.triang()) = *A.triang() - *B.triang();
            else if (numA == 3)
              noalias(*C.sym()) = *A.sym() - *B.sym();
            else if (numA == 4)
              noalias(*C.sparse()) = *A.sparse() - *B.sparse();
            else //if(numA==5)
              noalias(*C.banded()) = *A.banded() - *B.banded();
          }
          else // C and A of different types.
          {
            if (numC != 1)
              SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C.");
            // Only dense matrices are allowed for output.

            if (numA == 1)
              noalias(*C.dense()) = *A.dense() - *B.dense();
            else if (numA == 2)
              noalias(*C.dense()) = *A.triang() - *B.triang();
            else if (numA == 3)
              noalias(*C.dense()) = *A.sym() - *B.sym();
            else if (numA == 4)
              noalias(*C.dense()) = *A.sparse() - *B.sparse();
            else //if(numA==5)
              noalias(*C.dense()) = *A.banded() - *B.banded();
          }
          C.resetLU();
        }
        else if (numA != 0 && numB != 0 && numA != numB) // A and B of different types and none is block
        {
          if (numC != 1)
            SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C.");
          // Only dense matrices are allowed for output.

          if (numA == 1)
            switch (numB)
            {
            case 2:
              noalias(*C.dense()) = *A.dense() - *B.triang();
              break;
            case 3:
              noalias(*C.dense()) = *A.dense() - *B.sym();
              break;
            case 4:
              noalias(*C.dense()) = *A.dense() - *B.sparse();
              break;
            case 5:
              noalias(*C.dense()) = *A.dense() - *B.banded();
              break;
            case 7:
              noalias(*C.dense()) = *A.dense() - *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 2)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.triang() - *B.dense();
              break;
            case 3:
              noalias(*C.dense()) = *A.triang() - *B.sym();
              break;
            case 4:
              noalias(*C.dense()) = *A.triang() - *B.sparse();
              break;
            case 5:
              noalias(*C.dense()) = *A.triang() - *B.banded();
              break;
            case 7:
              noalias(*C.dense()) = *A.triang() - *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 3)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.sym() - *B.dense();
              break;
            case 2:
              noalias(*C.dense()) = *A.sym() - *B.triang();
              break;
            case 4:
              noalias(*C.dense()) = *A.sym() - *B.sparse();
              break;
            case 5:
              noalias(*C.dense()) = *A.sym() - *B.banded();
              break;
            case 7:
              noalias(*C.dense()) = *A.sym() - *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 4)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.sparse() - *B.dense();
              break;
            case 2:
              noalias(*C.dense()) = *A.sparse() - *B.triang();
              break;
            case 3:
              noalias(*C.dense()) = *A.sparse() - *B.sym();
              break;
            case 5:
              noalias(*C.dense()) = *A.sparse() - *B.banded();
              break;
            case 7:
              noalias(*C.dense()) = *A.sparse() - *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 5)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.banded() - *B.dense();
              break;
            case 2:
              noalias(*C.dense()) = *A.banded() - *B.triang();
              break;
            case 3:
              noalias(*C.dense()) = *A.banded() - *B.sym();
              break;
            case 4:
              noalias(*C.dense()) = *A.banded() - *B.sparse();
              break;
            case 7:
              noalias(*C.dense()) = *A.banded() - *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 6)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.zero_mat() - *B.dense();
              break;
            case 2:
              noalias(*C.dense()) = *A.zero_mat() - *B.triang();
              break;
            case 3:
              noalias(*C.dense()) = *A.zero_mat() - *B.sym();
              break;
            case 4:
              noalias(*C.dense()) = *A.zero_mat() - *B.sparse();
              break;
            case 7:
              noalias(*C.dense()) = *A.zero_mat() - *B.identity();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else if (numA == 7)
            switch (numB)
            {
            case 1:
              noalias(*C.dense()) = *A.identity() - *B.dense();
              break;
            case 2:
              noalias(*C.dense()) = *A.identity() - *B.triang();
              break;
            case 3:
              noalias(*C.dense()) = *A.identity() - *B.sym();
              break;
            case 4:
              noalias(*C.dense()) = *A.identity() - *B.sparse();
              break;
            case 5:
              noalias(*C.dense()) = *A.identity() - *B.banded();
              break;
            default:
              SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
            }
          else
            SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
          C.resetLU();
        }
        else // A and/or B is Block
        {
          C = A;
          C -= B;
        }
      }
    }
  }
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110519    F.M. Engelen      File created.
 *      110628    K. Kumar          Minor comment and layout modifications.
 *      110701    K. Kumar          Updated unit tests to check relative error;
 *                                  Updated file path.
 *      110718    F.M. Engelen      Took out falacy in test (only checking the norm) and
 *                                  added ItoE and EtoI transformation.
 *      110726    K. Kumar          Minor modifications; updated relative error
 *                                  wrt to norm.
 *      110808    F.M. Engelen      Updated with better tests, changed test for vertical frame.
 *      120529    E.A.G. Heeren     Boostified unit tests.
 *      120614    P. Musegaas       Removed unneccessary using statements and normalizations.
 *      130312    D. Dirkx          Added unit test for planet-fixed <-> inertial without equal
 *                                  equatorial frame.
 *      130312    A. Ronse          Added tests for V-T, TA-AA and AA-B transformations.
 *
 *    References
 *      Mooij, E. The Motion of a Vehicle in a Planetary Atmosphere, TU Delft, 1997.
 *
 *    Notes
 *      The reference frame definitions/abbreviations can be found in the file
 *      referenceFrameTransformations.h.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"

#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h"

namespace tudat
{
namespace unit_tests
{

using namespace unit_conversions;

BOOST_AUTO_TEST_SUITE( test_reference_frame_transformations )

// Summary of tests.
// Test 1: Test inertial to rotating planetocentric frame transformation.
// Test 2: Check whether the transformed matrix of Test 1 is also correct.
// Test 3: Same test as Test 1 for the transformation quaternion.
// Test 4: Same test as Test 2 for the transformation quaternion.
// Test 5: Test airspeed-based aerodynamic to body frame transformation.
// Test 6: Test planetocentric to local vertical frame transformation quaternion.
// Test 7: Check whether the transformed matrix of Test 6 is also correct.
// Test 8: Test transformations between trajectory and local vertical frame.
// Test 9: Test transformations between trajectory and aerodynamic frame.
// Test 10: Test transformations between body and aerodynamic reference frame.

// Test inertial to rotating planetocentric frame transformations.
BOOST_AUTO_TEST_CASE( testRotatingPlanetocentricFrameTransformations )
{
    // Using declarations.
    using std::atan2;
    using std::cos;
    using std::sin;
    using std::pow;
    using std::sqrt;

    // Test 1: Test Inertial to rotating planetocentric frame transformation.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 4.0;
        startLocation( 1 ) = 1.0;
        startLocation( 2 ) = 5.0;

        double horizontalStartLocationSize = sqrt( pow( startLocation( 0 ), 2.0 )
                                                   + pow( startLocation( 1 ), 2.0 ) );

        // Declare and initialize angle between vector and XR-axis.
        double startAngle = atan2( startLocation( 1 ), startLocation( 0 ) );

        // Rotate by 10 degrees around the positive Z-axis
        double angleInTime = convertDegreesToRadians( 10.0 );

        // Declare and initialize the angle between the XI- and XR-axis.
        double endAngle = startAngle - angleInTime;

        // Declare the expected location of the point in the planetocentric reference frame.
        Eigen::Vector3d expectedLocation;
        expectedLocation( 0 ) = horizontalStartLocationSize * cos( endAngle );
        expectedLocation( 1 ) = horizontalStartLocationSize * sin( endAngle );
        expectedLocation( 2 ) = startLocation( 2 );

        // Compute location of the point in the rotating frame subject to the transformation
        // matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getInertialToPlanetocentricFrameTransformationMatrix( angleInTime )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, expectedLocation,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: Check whether the transformed matrix of Test 1 is also correct.
    // Compute the error in the calculation.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 4.0;
        startLocation( 1 ) = 1.0;
        startLocation( 2 ) = 5.0;

        // Rotate by 10 degrees around the positive Z-axis
        double angleInTime = convertDegreesToRadians( 10.0 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getRotatingPlanetocentricToInertialFrameTransformationMatrix( angleInTime ) *
                reference_frames::
                getInertialToPlanetocentricFrameTransformationMatrix( angleInTime )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, startLocation,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_CASE( testRotatingPlanetocentricFrameTransformationQuaternion )
{
    // Using declarations.
    using std::atan2;
    using std::cos;
    using std::sin;
    using std::pow;
    using std::sqrt;

    // Test 3: Same test as Test 1 for the transformation quaternion.
    // Compute location of the point in the Rotating frame subject to the transformation matrix.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 4.0;
        startLocation( 1 ) = 1.0;
        startLocation( 2 ) = 5.0;

        double horizontalStartLocationSize = sqrt( pow( startLocation( 0 ), 2.0 )
                                                   + pow( startLocation( 1 ), 2.0 ) );

        // Declare and initialize angle between vector and XR-axis.
        double startAngle = atan2( startLocation( 1 ), startLocation( 0 ) );

        // Rotate by 10 degrees around the positive Z-axis
        double angleInTime = convertDegreesToRadians( 10.0 );

        // Declare and initialize the angle between the XI- and XR-axis.
        double endAngle = startAngle - angleInTime;

        // Declare the expected location of the point in the planetocentric reference frame.
        Eigen::Vector3d expectedLocation;
        expectedLocation( 0 ) = horizontalStartLocationSize * cos( endAngle );
        expectedLocation( 1 ) = horizontalStartLocationSize * sin( endAngle );
        expectedLocation( 2 ) = startLocation( 2 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getInertialToPlanetocentricFrameTransformationQuaternion( angleInTime )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, expectedLocation,
                                           std::numeric_limits< double >::epsilon( ) );
    }

// Test 4: Same test as Test 2 for the transformation quaternion.
// Compute the error in the calculation.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 4.0;
        startLocation( 1 ) = 1.0;
        startLocation( 2 ) = 5.0;

        // Rotate by 10 degrees around the positive Z-axis
        double angleInTime = convertDegreesToRadians( 10.0 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getRotatingPlanetocentricToInertialFrameTransformationQuaternion( angleInTime ) *
                reference_frames::
                getInertialToPlanetocentricFrameTransformationQuaternion( angleInTime )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, startLocation,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_CASE( testAirspeedBasedAerodynamicToBodyFrameTransformation )
{
    // Using declarations.
    using std::atan2;
    using std::cos;

    // Test 5: Test airspeed-Based Aerodynamic to body frame transformation.
    // Declare and initialize the start location and angles.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = -10.0;
        startLocation( 1 ) = 0.0;
        startLocation( 2 ) = 0.0;

        double angleOfAttack = convertDegreesToRadians( 45.0 );
        double angleOfSideslip = convertDegreesToRadians( 60.0 );;

        // Compute expected location.
        // As there is only an angle of attack, the following simplified equations can be used.
        Eigen::Vector3d expectedLocation;
        expectedLocation( 0 ) = startLocation( 0 ) * cos( angleOfSideslip ) * cos( angleOfAttack );
        expectedLocation( 1 ) = startLocation( 0 ) * sin( angleOfSideslip );
        expectedLocation( 2 ) = startLocation( 0 ) * cos( angleOfSideslip ) * sin( angleOfAttack );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getAirspeedBasedAerodynamicToBodyFrameTransformationMatrix(
                        angleOfAttack, angleOfSideslip ) * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, expectedLocation, 1.0e-14 );
    }
}

BOOST_AUTO_TEST_CASE( testRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion )
{
    // Using declarations.
    using std::atan2;
    using std::cos;
    using std::sin;

    // Test 6: Test Rotating planetocentric to local vertical frame transformation quaternion.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angle for planet and set to 90 degrees.
        double longitude = convertDegreesToRadians( 60.0 );

        // Declare latitude and set to 45 degrees.
        double latitude = convertDegreesToRadians( 20.0 );

        // Declare the expected location of the point in the planet reference frame.
        Eigen::Vector3d expectedLocation;
        expectedLocation( 0 ) = -cos( longitude ) * sin( latitude ) * startLocation( 0 ) -
                sin( longitude ) * sin( latitude ) * startLocation( 1 )  +
                cos( latitude ) * startLocation( 2 );
        expectedLocation( 1 ) = -sin( longitude ) * startLocation( 0 ) +
                cos( longitude ) * startLocation( 1 ) +
                0.0;
        expectedLocation( 2 ) = -cos( longitude ) * cos( latitude ) * startLocation( 0 ) -
                sin( longitude ) * cos( latitude ) * startLocation( 1 )  -
                sin( latitude ) * startLocation( 2 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                        longitude, latitude ) * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, expectedLocation, 1.0e-14 );
    }

    // Test 7: Check whether the transformed matrix of Test 6 is also correct.
    // Compute the error in the calculation.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angle for planet and set to 90 degrees.
        double longitude = convertDegreesToRadians( 60.0 );

        // Declare latitude and set to 45 degrees.
        double latitude = convertDegreesToRadians( 20.0 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getLocalVerticalToRotatingPlanetocentricFrameTransformationQuaternion( longitude,
                                                                                latitude )
                * reference_frames::
                getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion( longitude,
                                                                                latitude )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, startLocation, 1.0e-14 );
    }
}

// Test inertial to rotating planetocentric frame transformations, without assumption of equal x-y
// planes.
BOOST_AUTO_TEST_CASE( testRotatingPlanetocentricWithEquatorChangeFrameTransformations )
{
    using namespace reference_frames;

    // Data from pck00010.tpc Spice kernel.
    const double venusPoleRightAscension = convertDegreesToRadians( 272.76 );
    const double venusPoleDeclination = convertDegreesToRadians( 67.16 );
    const double venusPrimeMeridianAtJ2000 = convertDegreesToRadians( 160.20 );

    // Set rotation, as calculated with Spice
    // (see Astrodynamics/Ephemerides/unitTestSimpleRotationalEphemeris.cpp).
    Eigen::Matrix3d spiceInitialRotationToTargetFrameMatrix;
    spiceInitialRotationToTargetFrameMatrix
            << -0.9548214974296336, 0.2665104385944917, 0.1314841974018291,
            -0.296591573568662, -0.882413772579987, -0.3652114078848295,
            0.01869081416890202, -0.3877088083617989, 0.9215923900425705;

    // Calculate given rotation to Venus-fixed frame.
    Eigen::Matrix3d calculatedTransformationMatrix = Eigen::Matrix3d(
                getInertialToPlanetocentricFrameTransformationQuaternion(
                    venusPoleDeclination, venusPoleRightAscension, venusPrimeMeridianAtJ2000 ) );

    // Compare calculated with Spice result.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                spiceInitialRotationToTargetFrameMatrix, calculatedTransformationMatrix, 1.0E-15 );

    // Calculate inverse rotation.
    calculatedTransformationMatrix = Eigen::Matrix3d(
                getRotatingPlanetocentricToInertialFrameTransformationQuaternion(
                    venusPoleDeclination, venusPoleRightAscension,
                    venusPrimeMeridianAtJ2000 ) );

    // Multiply inverse rotation with Spice rotation to Venus-fixed frame. The two should be
    // orthonormal, so result should be the identity matrix.
    Eigen::Matrix3d matrixToCheck =
            spiceInitialRotationToTargetFrameMatrix * calculatedTransformationMatrix;

    // Check whether result is indeed (close to) identity matrix.
    for ( unsigned int i = 0; i < 3; i++ )
    {
        for ( unsigned int j = 0; j < 3; j++ )
        {
            if ( i == j )
            {
                BOOST_CHECK_CLOSE_FRACTION( matrixToCheck( i, j ), 1.0, 1.0E-15 );
            }
            else
            {
                BOOST_CHECK_SMALL( matrixToCheck( i, j ), 1.0E-15 );
            }
        }
    }
}

BOOST_AUTO_TEST_CASE( testTrajectoryToLocalVerticalFrameTransformations )
{
    // Using declarations.
    using std::cos;
    using std::sin;

    // Test 8: Test trajectory to local vertical frame and inverse transformations.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angles.
        for ( double headingAngle = convertDegreesToRadians( 0.0 );
              headingAngle < convertDegreesToRadians( 360.0 );
              headingAngle += convertDegreesToRadians( 30.0 ) )
        {
            for ( double flightPathAngle = convertDegreesToRadians( -90.0 );
                  flightPathAngle < convertDegreesToRadians( 90.0 );
                  flightPathAngle += convertDegreesToRadians( 30.0 ) )
            {
                // Declare the expected location of the point in the V-frame (per Mooij 1997).
                Eigen::Vector3d expectedLocation;
                Eigen::Matrix3d rotation;
                rotation << cos( headingAngle ) * cos( flightPathAngle ),
                            -sin( headingAngle ),
                            cos( headingAngle ) * sin( flightPathAngle ),
                            sin( headingAngle ) * cos( flightPathAngle ),
                            cos( headingAngle ),
                            sin( headingAngle ) * sin( flightPathAngle ),
                            -sin( flightPathAngle ),
                            0.0,
                            cos( flightPathAngle );
                            expectedLocation = rotation * startLocation;

                // Compute location of the point in the V-frame using the tested function.
                Eigen::Vector3d transformedLocationQuat =
                        reference_frames::
                        getTrajectoryToLocalVerticalFrameTransformationQuaternion(
                                flightPathAngle, headingAngle ) * startLocation;
                Eigen::Vector3d transformedLocationMat =
                        reference_frames::getTrajectoryToLocalVerticalFrameTransformationMatrix(
                                flightPathAngle, headingAngle ) * startLocation;

                // Compute product of inverse rotations.
                Eigen::Vector3d inverseRotationProductMat =
                        reference_frames::getTrajectoryToLocalVerticalFrameTransformationMatrix(
                                flightPathAngle, headingAngle ) *
                        reference_frames::getLocalVerticalFrameToTrajectoryTransformationMatrix(
                                flightPathAngle, headingAngle ) * startLocation;
                Eigen::Vector3d inverseRotationProductQuat =
                        reference_frames::
                        getTrajectoryToLocalVerticalFrameTransformationQuaternion(
                                flightPathAngle, headingAngle ) *
                        reference_frames::
                        getLocalVerticalFrameToTrajectoryTransformationQuaternion(
                                flightPathAngle, headingAngle ) * startLocation;

                // Check whether transformed vectors match within tolerances.
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        transformedLocationQuat, expectedLocation, 1.0e-14 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        transformedLocationMat, expectedLocation, 2.0e-14 );

                // Check whether product of inverse rotations equals unity.
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        inverseRotationProductMat, startLocation, 1.0e-14 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        inverseRotationProductQuat, startLocation, 1.0e-14 );
            }
        }
    }
}

BOOST_AUTO_TEST_CASE( testTrajectoryToAerodynamicFrameTransformations )
{
    // Using declarations.
    using std::cos;
    using std::sin;

    // Test 9: Test trajectory to aerodynamic and inverse transformations.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angles.
        for ( double bankAngle = convertDegreesToRadians( 0.0 );
              bankAngle < convertDegreesToRadians( 360.0 );
              bankAngle += convertDegreesToRadians( 30.0 ) )
        {
            // Declare the expected location of the point in the aerodynamic reference frame
            // (per Mooij 1997).
            Eigen::Vector3d expectedLocation;
            Eigen::Matrix3d rotation;
            rotation << 1.0, 0.0, 0.0,
                        0.0, cos( bankAngle ), -sin( bankAngle ),
                        0.0, sin( bankAngle ), cos( bankAngle );
            expectedLocation = rotation * startLocation;

            // Compute location of the point in the rotating frame subject to the
            // transformation matrix.
            Eigen::Vector3d transformedLocationQuat =
                    reference_frames::getTrajectoryToAerodynamicFrameTransformationQuaternion(
                            bankAngle ) * startLocation;
            Eigen::Vector3d transformedLocationMat =
                    reference_frames::getTrajectoryToAerodynamicFrameTransformationMatrix(
                            bankAngle ) * startLocation;

            // Compute product of inverse rotations.
            Eigen::Vector3d inverseRotationProductMat =
                    reference_frames::getAerodynamicToTrajectoryFrameTransformationMatrix(
                            bankAngle ) *
                    reference_frames::getTrajectoryToAerodynamicFrameTransformationMatrix(
                            bankAngle ) * startLocation;
            Eigen::Vector3d inverseRotationProductQuat =
                    reference_frames::getAerodynamicToTrajectoryFrameTransformationQuaternion(
                            bankAngle ) *
                    reference_frames::getTrajectoryToAerodynamicFrameTransformationQuaternion(
                            bankAngle ) * startLocation;

            // Check whether transformed vectors match within tolerances.
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    transformedLocationQuat, expectedLocation, 1.0e-14 );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocationMat, expectedLocation, 1.0e-14 );

            // Check whether product of inverse rotations equals unity.
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION( inverseRotationProductMat, startLocation, 1.0e-14 );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    inverseRotationProductQuat, startLocation, 1.0e-14 );
        }
    }
}

BOOST_AUTO_TEST_CASE( testAerodynamicToBodyFrameTransformations )
{
    // Using declarations.
    using std::cos;
    using std::sin;

    // Test 10: Test body to airspeed-based aerodynamic frame and inverse transformations.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angles.
        for ( double angleOfAttack = convertDegreesToRadians( -180.0 );
              angleOfAttack < convertDegreesToRadians( 180.0 );
              angleOfAttack += convertDegreesToRadians( 30.0 ) )
        {
            for ( double angleOfSideslip = convertDegreesToRadians( -90.0 );
                  angleOfSideslip < convertDegreesToRadians( 90.0 );
                  angleOfSideslip += convertDegreesToRadians( 30.0 ) )
            {
                // Declare the expected location of the point in the AA-frame.
                // (per Mooij 1997).
                Eigen::Vector3d expectedLocation;
                Eigen::Matrix3d rotation;
                rotation << cos( angleOfAttack ) * cos( angleOfSideslip ),
                            sin( angleOfSideslip ),
                            sin( angleOfAttack ) * cos( angleOfSideslip ),
                            -cos( angleOfAttack ) * sin( angleOfSideslip ),
                            cos( angleOfSideslip ),
                            -sin( angleOfAttack ) * sin( angleOfSideslip ),
                            -sin( angleOfAttack ),
                            0.0,
                            cos( angleOfAttack );
                expectedLocation = rotation * startLocation;

                // Compute location of the point in the AA-frame using the tested function.
                Eigen::Vector3d transformedLocationQuat =
                        reference_frames::
                        getBodyToAirspeedBasedAerodynamicFrameTransformationQuaternion(
                                angleOfAttack, angleOfSideslip ) * startLocation;
                Eigen::Vector3d transformedLocationMat =
                        reference_frames::
                        getBodyToAirspeedBasedAerodynamicFrameTransformationMatrix(
                                angleOfAttack, angleOfSideslip ) * startLocation;

                // Compute product of inverse rotations
                Eigen::Vector3d inverseRotationProductMat =
                        reference_frames::
                        getAirspeedBasedAerodynamicToBodyFrameTransformationMatrix(
                                angleOfAttack, angleOfSideslip ) *
                        reference_frames::
                        getBodyToAirspeedBasedAerodynamicFrameTransformationMatrix(
                                angleOfAttack, angleOfSideslip ) * startLocation;
                Eigen::Vector3d inverseRotationProductQuat =
                        reference_frames::
                        getAirspeedBasedAerodynamicToBodyFrameTransformationQuaternion(
                                angleOfAttack, angleOfSideslip ) *
                        reference_frames::
                        getBodyToAirspeedBasedAerodynamicFrameTransformationQuaternion(
                                angleOfAttack, angleOfSideslip ) * startLocation;

                // Check whether transformed vectors match within tolerances.
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        transformedLocationQuat, expectedLocation, 1.0e-14 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        transformedLocationMat, expectedLocation, 1.0e-14 );

                // Check whether product of inverse rotations equals unity.
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        inverseRotationProductMat, startLocation, 1.0e-14 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        inverseRotationProductQuat, startLocation, 1.0e-14 );
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#ifndef SM_HOMOGENEOUS_POINT_HPP
#define SM_HOMOGENEOUS_POINT_HPP

#include <Eigen/Core>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <sm/assert_macros.hpp>
#include <sm/eigen/serialization.hpp>

namespace sm {
namespace kinematics {
class UncertainHomogeneousPoint;

class HomogeneousPoint {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    SM_DEFINE_EXCEPTION(Exception, std::runtime_error);

    typedef Eigen::Matrix3d euclidean_jacobian_t;
    typedef Eigen::Matrix<double, 4, 3> homogeneous_jacobian_t;

    /// \brief Initialize the point to zero
    HomogeneousPoint();
    /// \brief Initialize the point from a Euclidean point
    HomogeneousPoint(const Eigen::Vector3d& p);
    /// \brief Initialize the point from a Homogeneous point
    HomogeneousPoint(const Eigen::Vector4d& p);

    virtual ~HomogeneousPoint();

    /// \brief get the Euclidean representation
    Eigen::Vector3d toEuclidean() const;

    /// \brief get the Euclidean representation and the jacobian of the oplus operator with respect to this point. The
    /// Jacobian should be 3x3.
    Eigen::Vector3d toEuclideanAndJacobian(euclidean_jacobian_t& J) const;

    /// \brief get the homogeneous representation
    const Eigen::Vector4d& toHomogeneous() const;

    /// \brief get the homogeneous representation and the Jacobian of the oplus operator with respect to this point. The
    /// Jacobian should be 3x3.
    const Eigen::Vector4d& toHomogeneousAndJacobian(homogeneous_jacobian_t& J) const;

    /// \brief Add two homogeneous points.
    /// The result of this operation is the same as adding
    /// the equivelent vectors in R^3
    HomogeneousPoint operator+(const HomogeneousPoint& rhs) const;

    /// \brief Add two homogeneous points.
    /// The result of this operation is the same as adding
    /// the equivelent vectors in R^3
    /// This version adds uncertainty.
    virtual UncertainHomogeneousPoint operator+(const UncertainHomogeneousPoint& rhs) const;

    /// \brief Subtract one homogeneous point from another.
    /// The result of this operation is the same as subtracting
    /// the equivelent vectors in R^3
    HomogeneousPoint operator-(const HomogeneousPoint& rhs) const;

    /// \brief Subtract one homogeneous point from another.
    /// The result of this operation is the same as subtracting
    /// the equivelent vectors in R^3
    virtual UncertainHomogeneousPoint operator-(const UncertainHomogeneousPoint& rhs) const;

    /// \brief Set this point from a 4x1 column
    HomogeneousPoint& operator=(const Eigen::Vector4d& rhs);

    /// \brief Set this point from a 3x1 column
    HomogeneousPoint& operator=(const Eigen::Vector3d& rhs);

    template <typename DERIVED>
    void set(const Eigen::MatrixBase<DERIVED>& p);

    /// \brief Implements the "oplus" operator for updating the point from a minimal perturbation
    ///
    /// This oplus operator maps to the \rho(dp)^+ p from quaternion algebra.
    void oplus(const Eigen::Vector3d& dp);

    /// \brief converts the homogenous point to a pointing vector by setting the homogeneous coordinate to zero
    void convertToVector();

    /// \brief converts a pointing vector to a normalized homogeneous point with unit length
    void convertToPoint();

    /// \brief scale the point by a constant factor
    void scale(double scaleFactor);

    /// \brief set to a random point.
    void setRandom();

    /// \brief set to the Euclidean point equaling zero
    void setZero();

    /// \brief checks if homogeneous point is at infinity (if fourth coordinate is zero)
    bool atInfinity() const;

    /// \brief checks if homogeneous point is a vector, i.e. it has infinite length (if fourth coordinate is zero)
    bool isVector() const;

    double* pptr() { return &_ph[0]; }

    /// \brief Normalize the point so that it is unit length
    virtual void normalize();

    /// \brief boost::serialization support
    enum { CLASS_SERIALIZATION_VERSION = 0 };
    BOOST_SERIALIZATION_SPLIT_MEMBER()

    template <class Archive>
    void save(Archive& ar, const unsigned int /* version */) const {
        ar << ::boost::serialization::make_nvp("_ph", _ph);
    }

    template <class Archive>
    void load(Archive& ar, const unsigned int version) {
        SM_ASSERT_LE(std::runtime_error, version, (unsigned int)CLASS_SERIALIZATION_VERSION,
                     "Unsupported serialization version");
        ar >> ::boost::serialization::make_nvp("_ph", _ph);
    }

    bool isBinaryEqual(const HomogeneousPoint& rhs) const;

  protected:
    Eigen::Vector4d _ph;
};

template <typename DERIVED>
void HomogeneousPoint::set(const Eigen::MatrixBase<DERIVED>& p) {
    SM_ASSERT_EQ_DBG(Exception, p.cols(), 1, "Trying to set a homogeneous point from a matrix");

    if (p.rows() == 3) {
        _ph[0] = p[0];
        _ph[1] = p[1];
        _ph[2] = p[2];
        _ph[3] = 1.0;
    } else if (p.rows() == 4) {
        _ph[0] = p[0];
        _ph[1] = p[1];
        _ph[2] = p[2];
        _ph[3] = p[3];
    } else {
        SM_THROW(Exception, "Trying to initialize a homogeneous point with a "
                                << p.rows() << "x" << p.cols() << " matrix. Only 3x1 and 4x1 are supported");
    }
}

}  // namespace kinematics
}  // namespace sm

BOOST_CLASS_VERSION(sm::kinematics::HomogeneousPoint, sm::kinematics::HomogeneousPoint::CLASS_SERIALIZATION_VERSION);

#endif /* SM_HOMOGENEOUS_POINT_HPP */


/*
 * Copyright (c) 2016, Niklas Gürtler
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 * following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 *    disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *    following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <boost/variant/apply_visitor.hpp>

#include "ctl.hh"

namespace CTL {

std::unique_ptr<S_Formula> SatVisitor::operator () (const Formula::E_Literal& exp) const {
	return nullary (exp, [&] () { return exp.value ? SatSet { tranSys.statesSet } : SatSet {}; });
}

std::unique_ptr<S_Formula> SatVisitor::operator () (const Formula::E_Negation& exp) const {
	return unary (exp, [&] (S_Formula& child) {
		return tranSys.statesSet - child.sat;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator () (const Formula::E_And& exp) const {
	return binary (exp, [&] (S_Formula& left, S_Formula& right) {
		return left.sat & right.sat;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator () (const Formula::E_Or& exp) const {
	return binary (exp, [&] (S_Formula& left, S_Formula& right) {
		return left.sat | right.sat;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator() (const Formula::E_Implication& exp) const {
	return binary (exp, [&] (S_Formula& left, S_Formula& right) {
		return (tranSys.statesSet - left.sat) | right.sat;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator () (const Formula::E_ExistNext& exp) const {
	return unary (exp, [&] (S_Formula& child) {
		std::set<TS::State*> predecessors;
		for (auto& s : child.sat) {
			predecessors |= s->predecessors;
		}

		return predecessors;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator () (const Formula::E_ExistUntil& exp) const {
	return binary (exp, [&] (S_Formula& left, S_Formula& right) {
		std::set<TS::State*> T = right.sat;
		bool modified;
		do {
			modified = false;
			for (TS::State* s : T) {
				for (TS::State* p : s->predecessors) {
					if (left.sat.find (p) != left.sat.end () && T.find (p) != T.end ()) {
						T.insert (p);
						modified = true;
					}
				}
			}
		} while (modified);

		return T;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator () (const Formula::E_ExistAlways& exp) const {
	return unary (exp, [&] (S_Formula& child) {
		std::set<TS::State*> T = child.sat;
		bool modified;
		do {
			modified = false;
			using I = std::set<TS::State*>::const_iterator;
			for (I iter = T.cbegin (); iter != T.cend (); ) {
				I next = iter;
				++next;
				if (!((*iter)->successors && T)) {
					T.erase (iter);
					modified = true;
				}
				iter = next;
			}
		} while (modified);

		return T;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator() (const Formula::E_AllNext& exp) const {
	return unary (exp, [&] (S_Formula& child) {
		std::set<TS::State*> exclusivePredecessors;
		for (auto& s : child.sat) {
			for (auto& p : s->predecessors) {
				if (p->successors <= child.sat)
					exclusivePredecessors.insert (p);
			}
		}

		return exclusivePredecessors;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator() (const Formula::E_AllUntil& exp) const {
	return binary (exp, [&] (S_Formula& left, S_Formula& right) {
		std::set<TS::State*> T = right.sat;
		bool modified;
		do {
			modified = false;
			for (TS::State* s : T) {
				for (TS::State* p : s->predecessors) {

					if (left.sat.find (p) != left.sat.end ()
					&&	s->successors <= T
					&&	T.find (p) != T.end ()) {
						T.insert (p);
						modified = true;
					}
				}
			}
		} while (modified);

		return T;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator() (const Formula::E_AllAlways& exp) const {
	return unary (exp, [&] (S_Formula& child) {
		std::set<TS::State*> T = child.sat;
		bool modified;
		do {
			modified = false;
			for (TS::State* s : T) {
				if (!(s->successors <= T)) {
					T.erase (s);
					modified = true;
				}
			}
		} while (modified);

		return T;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator () (const Formula::E_Label& exp) const {
	return nullary (exp, [&] () {
		auto res = tranSys.labels.find (exp.name);
		if (res == tranSys.labels.end ())
			throw std::runtime_error ("Unknown label \"" + exp.name + "\" in formula");

		std::set<TS::State*> sat;
		for (const auto s : tranSys.statesSet) {
			if (s->atomicPropositions.find (&res->second) != s->atomicPropositions.end ())
				sat.insert (s);
		}

		return sat;
	});
}

std::unique_ptr<S_Formula> SatVisitor::operator() (const Formula::E_Next&) const {
	throw std::runtime_error ("Illegal node in AST");
}

std::unique_ptr<S_Formula> SatVisitor::operator() (const Formula::E_Until&) const {
	throw std::runtime_error ("Illegal node in AST");
}

std::pair<bool, std::unique_ptr<S_Formula>> computeSat (const Formula::Expression& formula, TS::TranSys& ts) {
	std::unique_ptr<S_Formula> res = boost::apply_visitor (SatVisitor { ts }, formula);
	bool s = ts.init <= res->sat;
	return std::make_pair (s, std::move (res));
}

void S_Formula::print (std::ostream& os, size_t depth) {
	for (size_t i = 0; i < depth; ++i) os << '\t';
	os << "Sat (";
	std::copy (strBegin, strEnd, std::ostreambuf_iterator<char> (os));
	os << ") = ";
}

void S_Nullary::print (std::ostream& os, size_t depth) {
	S_Formula::print (os, depth);
	os << sat << std::endl;
}

void S_Unary::print (std::ostream& os, size_t depth) {
	S_Formula::print (os, depth);
	os << sat << std::endl;
	child->print (os, depth+1);
}

void S_Binary::print (std::ostream& os, size_t depth) {
	S_Formula::print (os, depth);
	os << sat << std::endl;
	left->print (os, depth+1);
	right->print (os, depth+1);
}

std::ostream& operator << (std::ostream& os, const SatSet& sat) {
	os << '{';
	bool next = false;
	for (SatSet::const_iterator i = sat.cbegin (); i != sat.cend (); ++i) {
		if (next) {
			os << ", ";
		} else {
			next = true;
		}
		os << (*i)->name;
	}
	os << '}';
	return os;
}

}


#ifndef PERSONAL
#define PERSONAL

#include <chrono>
#include <functional>
#include <future>
#include <iostream>
#include <iterator>
#include <queue>
#include <sstream>
#include <string>
#include <vector>

#include <boost/container/small_vector.hpp>

namespace my
{

int get_opt_template(int argc, char **argv);

void print_usage(
    std::ostream &out,
    int exit_code,
    const std::string &program_name);

//------------------------------------------------------------------------------
template <typename T>
std::vector<T> split(
    const std::string &buffer,
    const char delimiter)
{
    std::stringstream ss(buffer);
    std::string word;
    std::vector<T> output;

    while (std::getline(ss, word, delimiter))
    {
        std::stringstream ws(word);
        T chunk;
        ws >> chunk;
        output.push_back(chunk);
    }

    return output;
}

template <>
std::vector<int> split<int>(
    const std::string &buffer,
    const char delimiter);

template <>
std::vector<float> split<float>(
    const std::string &buffer,
    const char delimiter);

//------------------------------------------------------------------------------
template <typename Sequence>
void print_seq(Sequence const &v)
{
    std::copy(v.cbegin(), v.cend(),
              std::ostream_iterator<typename Sequence::value_type>(std::cout, " "));
    std::cout << "\n";
}

//------------------------------------------------------------------------------
template <typename T>
using Compare = std::function<bool(T const &, T const &)>;

template <typename T>
using ItList = boost::container::small_vector<std::pair<T, T>, 2>;

template <typename RandomAccessIterator>
ItList<RandomAccessIterator>
quick_sort(RandomAccessIterator start, RandomAccessIterator end)
{
    ItList<RandomAccessIterator> next_iteration;
    // Bad quick_sort implementation
    auto pivot = start;
    auto i = start + 1;
    auto j = end;
    while (i != j)
    {
        if (*i >= *pivot)
        {
            std::swap(*i, *(j - 1));
            --j;
        }
        else
        {
            std::swap(*i, *pivot);
            ++i;
        }
    }

    if (start < pivot - 1)
    {
        next_iteration.push_back(
            make_pair(start, pivot));
    }
    if (pivot + 1 < end - 1)
    {
        next_iteration.push_back(
            make_pair(pivot + 1, end));
    }
    return next_iteration;
}

template <typename Sequence>
void sort(Sequence &s, Compare<typename Sequence::value_type> const &compare_func = std::less())
{
    using Seq_It = typename Sequence::iterator;
    using SortQueue = std::queue<std::future<ItList<Seq_It>>>;

    SortQueue sort_queue;
    sort_queue.push(
        std::async(
            quick_sort<Seq_It>,
            s.begin(),
            s.end()));

    while (!sort_queue.empty())
    {
        auto current = sort_queue.front().get();
        sort_queue.pop();
        for (auto &val : current)
        {
            sort_queue.push(
                std::async(
                    quick_sort<Seq_It>,
                    val.first,
                    val.second));
        }
    }
}
//------------------------------------------------------------------------------
template <typename Function, typename... Args>
void bench_func(Function const &f, Args &&... args)
{
    auto start = std::chrono::high_resolution_clock::now();
    f(std::forward<Args>(args)...);
    auto end = std::chrono::high_resolution_clock::now();
    std::cout << std::chrono::duration<double, std::milli>(end - start).count() << " ms\n";
}

} // namespace my

#endif


#include <big_types.h>
#include <big_return_codes.h>

#include <min_map/big_compute_index_reordering.h>
#include <min_map/big_partition_vector.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(big_partition_vector);

BOOST_AUTO_TEST_CASE(test_case1)
{
  typedef ublas::vector<size_t>   idx_vector_type;

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(2) = big::IN_ACTIVE;
  bitmask(4) = big::IN_ACTIVE;
  bitmask(6) = big::IN_ACTIVE;
  bitmask(9) = big::IN_ACTIVE;

  bitmask(0) = big::IN_NON_ACTIVE;
  bitmask(1) = big::IN_NON_ACTIVE;
  bitmask(3) = big::IN_NON_ACTIVE;
  bitmask(5) = big::IN_NON_ACTIVE;
  bitmask(7) = big::IN_NON_ACTIVE;
  bitmask(8) = big::IN_NON_ACTIVE;

  idx_vector_type old2new;
  idx_vector_type new2old;

  big::compute_index_reordering( bitmask, old2new, new2old );

  ublas::vector<double> rhs;
  rhs.resize(10,false);

  rhs(0) = 1.0;
  rhs(1) = 2.0;
  rhs(2) = 3.0;
  rhs(3) = 4.0;
  rhs(4) = 5.0;
  rhs(5) = 6.0;
  rhs(6) = 7.0;
  rhs(7) = 8.0;
  rhs(8) = 9.0;
  rhs(9) = 10.0;

  ublas::vector<double> rhs_a;
  ublas::vector<double> rhs_b;
  big::partition_vector( rhs, bitmask, old2new, 4, 6, rhs_a, rhs_b );

  BOOST_CHECK( rhs_a.size() == 4);
  BOOST_CHECK( rhs_b.size() == 6);

  double tol =0.01;
  BOOST_CHECK_CLOSE( double( rhs_a( 0 ) ), double( rhs( new2old(0) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 1 ) ), double( rhs( new2old(1) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 2 ) ), double( rhs( new2old(2) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 3 ) ), double( rhs( new2old(3) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 0 ) ), double( rhs( new2old(0+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 1 ) ), double( rhs( new2old(1+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 2 ) ), double( rhs( new2old(2+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 3 ) ), double( rhs( new2old(3+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 4 ) ), double( rhs( new2old(4+4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 5 ) ), double( rhs( new2old(5+4) ) ), tol );
}

BOOST_AUTO_TEST_CASE(test_case2)
{
  typedef ublas::vector<size_t>   idx_vector_type;

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(2) = big::IN_ACTIVE;
  bitmask(4) = big::IN_ACTIVE;
  bitmask(6) = big::IN_ACTIVE;
  bitmask(9) = big::IN_ACTIVE;
  bitmask(0) = big::IN_ACTIVE;
  bitmask(1) = big::IN_ACTIVE;
  bitmask(3) = big::IN_ACTIVE;
  bitmask(5) = big::IN_ACTIVE;
  bitmask(7) = big::IN_ACTIVE;
  bitmask(8) = big::IN_ACTIVE;

  idx_vector_type old2new;
  idx_vector_type new2old;

  big::compute_index_reordering( bitmask, old2new, new2old );

  ublas::vector<double> rhs;
  rhs.resize(10,false);

  rhs(0) = 1.0;
  rhs(1) = 2.0;
  rhs(2) = 3.0;
  rhs(3) = 4.0;
  rhs(4) = 5.0;
  rhs(5) = 6.0;
  rhs(6) = 7.0;
  rhs(7) = 8.0;
  rhs(8) = 9.0;
  rhs(9) = 10.0;

  ublas::vector<double> rhs_a;
  ublas::vector<double> rhs_b;
  big::partition_vector( rhs, bitmask, old2new, 10, 0, rhs_a, rhs_b );

  BOOST_CHECK( rhs_a.size() == 10);
  BOOST_CHECK( rhs_b.size() == 0);

  double tol =0.01;
  BOOST_CHECK_CLOSE( double( rhs_a( 0 ) ), double( rhs( new2old(0) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 1 ) ), double( rhs( new2old(1) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 2 ) ), double( rhs( new2old(2) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 3 ) ), double( rhs( new2old(3) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 4 ) ), double( rhs( new2old(4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 5 ) ), double( rhs( new2old(5) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 6 ) ), double( rhs( new2old(6) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 7 ) ), double( rhs( new2old(7) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 8 ) ), double( rhs( new2old(8) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_a( 9 ) ), double( rhs( new2old(9) ) ), tol );
}

BOOST_AUTO_TEST_CASE(test_case3)
{
  typedef ublas::vector<size_t>   idx_vector_type;

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(2) = big::IN_NON_ACTIVE;
  bitmask(4) = big::IN_NON_ACTIVE;
  bitmask(6) = big::IN_NON_ACTIVE;
  bitmask(9) = big::IN_NON_ACTIVE;
  bitmask(0) = big::IN_NON_ACTIVE;
  bitmask(1) = big::IN_NON_ACTIVE;
  bitmask(3) = big::IN_NON_ACTIVE;
  bitmask(5) = big::IN_NON_ACTIVE;
  bitmask(7) = big::IN_NON_ACTIVE;
  bitmask(8) = big::IN_NON_ACTIVE;

  idx_vector_type old2new;
  idx_vector_type new2old;

  big::compute_index_reordering( bitmask, old2new, new2old );

  ublas::vector<double> rhs;
  rhs.resize(10,false);

  rhs(0) = 1.0;
  rhs(1) = 2.0;
  rhs(2) = 3.0;
  rhs(3) = 4.0;
  rhs(4) = 5.0;
  rhs(5) = 6.0;
  rhs(6) = 7.0;
  rhs(7) = 8.0;
  rhs(8) = 9.0;
  rhs(9) = 10.0;

  ublas::vector<double> rhs_a;
  ublas::vector<double> rhs_b;
  big::partition_vector( rhs, bitmask, old2new, 0, 10, rhs_a, rhs_b );

  BOOST_CHECK( rhs_a.size() == 0);
  BOOST_CHECK( rhs_b.size() == 10);

  double tol =0.01;
  BOOST_CHECK_CLOSE( double( rhs_b( 0 ) ), double( rhs( new2old(0) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 1 ) ), double( rhs( new2old(1) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 2 ) ), double( rhs( new2old(2) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 3 ) ), double( rhs( new2old(3) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 4 ) ), double( rhs( new2old(4) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 5 ) ), double( rhs( new2old(5) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 6 ) ), double( rhs( new2old(6) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 7 ) ), double( rhs( new2old(7) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 8 ) ), double( rhs( new2old(8) ) ), tol );
  BOOST_CHECK_CLOSE( double( rhs_b( 9 ) ), double( rhs( new2old(9) ) ), tol );
}

BOOST_AUTO_TEST_SUITE_END();


#include "myslam/dataset.h"

#include <boost/format.hpp>
#include <fstream>
#include <opencv2/opencv.hpp>

#include "myslam/frame.h"
using namespace std;

namespace myslam {

Dataset::Dataset(const std::string& dataset_path)
    : dataset_path_(dataset_path) {}

bool Dataset::Init() {
  // read camera intrinsics and extrinsics
  ifstream fin(dataset_path_ + "/calib.txt");
  if (!fin) {
    LOG(ERROR) << "cannot find " << dataset_path_ << "/calib.txt!";
    return false;
  }

  for (int i = 0; i < 4; ++i) {
    char camera_name[3];
    for (int k = 0; k < 3; ++k) {
      fin >> camera_name[k];
    }
    double projection_data[12];
    for (int k = 0; k < 12; ++k) {
      fin >> projection_data[k];
    }
    Mat33 K;
    K << projection_data[0], projection_data[1], projection_data[2],
        projection_data[4], projection_data[5], projection_data[6],
        projection_data[8], projection_data[9], projection_data[10];
    Vec3 t;
    t << projection_data[3], projection_data[7], projection_data[11];
    t = K.inverse() * t;
    K = K * 0.5;
    Camera::Ptr new_camera(new Camera(K(0, 0), K(1, 1), K(0, 2), K(1, 2),
                                      t.norm(), SE3(SO3(), t)));
    cameras_.push_back(new_camera);
    LOG(INFO) << "Camera " << i << " extrinsics: " << t.transpose();
  }
  fin.close();
  current_image_index_ = 0;
  return true;
}

Frame::Ptr Dataset::NextFrame() {
  boost::format fmt("%s/image_%d/%06d.png");
  cv::Mat image_left, image_right;
  // read images
  image_left =
      cv::imread((fmt % dataset_path_ % 0 % current_image_index_).str(),
                 cv::IMREAD_GRAYSCALE);
  image_right =
      cv::imread((fmt % dataset_path_ % 1 % current_image_index_).str(),
                 cv::IMREAD_GRAYSCALE);

  if (image_left.data == nullptr || image_right.data == nullptr) {
    LOG(WARNING) << "cannot find images at index " << current_image_index_;
    return nullptr;
  }

  cv::Mat image_left_resized, image_right_resized;
  cv::resize(image_left, image_left_resized, cv::Size(), 0.5, 0.5,
             cv::INTER_NEAREST);
  cv::resize(image_right, image_right_resized, cv::Size(), 0.5, 0.5,
             cv::INTER_NEAREST);

  auto new_frame = Frame::CreateFrame();
  new_frame->left_img_ = image_left_resized;
  new_frame->right_img_ = image_right_resized;
  current_image_index_++;
  return new_frame;
}

}  // namespace myslam


#ifndef HOMOTOPY_BASE_HH
#define HOMOTOPY_BASE_HH

#include <vector>
#include <memory> // unique_ptr
#include <cmath>
#include <fstream>

#include "dune/grid/config.h"

#include "abstract_interface.hh"
#include "algorithm_base.hh"
#include "newton_damped.hh"
#include "modelfunction.hh"
#include <boost/timer/timer.hpp>

namespace Kaskade
{

class NewtonsMethod;

  /** Abstract algorithm of interior point method.
  */
class InteriorPointSqrtModel;
template<class IPF, class DomainVector> class InteriorPointTangentialPredictor;
class InteriorPointSlopeEstimate;

class InteriorPointParameters : IterationParameters
{
public:
  InteriorPointParameters(double mu_, double desiredAccuracy_, double desiredContraction_, int maxSteps_, double muFinal_, double relDist, double accfinal_,
                          double reductionStart_,double reductionFactorBest_, double redWorst_=1-1e-4)
    : IterationParameters(desiredAccuracy_, maxSteps_),
      desiredContraction(desiredContraction_),
      muFinal(muFinal_),
      reductionFactorWorst(redWorst_),
      firstStepFailureFactor(2.0),
      reductionStart(reductionStart_),
      relDistanceToPath(relDist),
      accfinal(accfinal_),
      reductionFactorBest(reductionFactorBest_),
      maxTrialSteps(10)
  {
    reset();
    mu=mu_;
  }

//Parameters with values that must be supplied by client
  double desiredContraction, muFinal, reductionFactorWorst, firstStepFailureFactor, reductionStart, relDistanceToPath, accfinal, reductionFactorBest;
  int maxTrialSteps;

  virtual ~InteriorPointParameters() {}

  int termination;

  void reset()
  {
    IterationParameters::reset();
    stepSuccessful=true;
    correctorTermination=-1;
  }

  LoggedQuantity<double> mu, sigma;

protected:

  bool stepSuccessful;

  int correctorTermination;

  LoggedQuantity<double> deltaMu, j, jp, muTrial;

  friend class HomotopyBase;
  friend class InteriorPointSimple;
  friend class InteriorPointSqrtModel;
  template<class A, class B> friend class InteriorPointTangentialPredictor;
  friend class InteriorPointSlopeEstimate;

  virtual void doForAll(LQAction::ToDo td)
  {
    IterationParameters::doForAll(td);
    mu.doAction(td,"mu");
    muTrial.doAction(td,"muTrial");
    j.doAction(td,"jp");
    jp.doAction(td,"j");
    sigma.doAction(td,"sigma");
    deltaMu.doAction(td,"deltaMu");
  }

};

/// Base class for homotopy methods. Here, the main algorithm is programmed
class HomotopyBase : public Algorithm
  {
  public:
    HomotopyBase(NewtonsMethod& n_, InteriorPointParameters& p_) :
      corrector(n_), p(p_)
    {}
    virtual ~HomotopyBase()
    { }

    void solve(AbstractParameterFunctional* f,AbstractFunctionSpaceElement& x);
  protected:

/// Optional overload by derived class
    virtual void initialize() {};

/// Optional overload by derived class
    virtual void finalize() {};

    virtual void logQuantities();

/// Default: classical predictor
    virtual void computePredictor(int step);

/// New mu if corrector was successful
    virtual double muOnSuccess(int step) = 0;

/// New mu if corrector failed
    virtual double muOnFailure() = 0;

/// mu for successful termination
    virtual double muFinal() = 0;

/// update of path parameters, e.g. eta and omega
    virtual void updateModelOfHomotopy(int step) = 0;

/// what should be done before corrector
    virtual void initializeCorrector() = 0;

/// what should be done after corrector
    virtual void finalizeCorrector() = 0;

/// make trial iterate to current iterate (on successful corrector)
    virtual void updateIterate() = 0;

/// make old iterate to current iterate (on failed corrector)
    virtual void recoverIterate() = 0;

/// estimate of length of homopoty path
    virtual double lengthOfPath() = 0;

/// convergence test. returns true if method converged and solution is found.
    virtual int convergenceTest();

/// to be performed, if convergence of path has occured
    virtual void finalizeHomotopy(){};

/// output of a termination message
    virtual void terminationMessage(int errorFlag);

    NewtonsMethod& corrector;
    InteriorPointParameters& p;
    AbstractParameterFunctional* functional;

    std::unique_ptr<AbstractFunctionSpaceElement> trialIterate;
    int step;

  private:

    bool stepSuccessful();

    void computeCorrector();
    void computeGapParameter();
    int runAlgorithm();
  };

/// Very simple implementation of homotopy method: fixed stepsize
class InteriorPointSimple : public HomotopyBase
{
public:
  InteriorPointSimple(NewtonsMethod& n_, InteriorPointParameters& p_) :
    HomotopyBase(n_, p_) {}

  virtual double muOnSuccess(int step) { return p.mu*p.reductionStart;};
  virtual double muOnFailure() { return p.mu*p.reductionStart;};
  virtual double muFinal() { return 1e-7;};

  virtual void updateModelOfHomotopy(int step) {};

  virtual void initializeCorrector() {};
  virtual void finalizeCorrector() {};

  virtual void updateIterate() {};
  virtual void recoverIterate() {};

  virtual double lengthOfPath() { return std::sqrt(p.mu); };
};

class InteriorPointParametersSqrt : IterationParameters
{
public:
  InteriorPointParametersSqrt()
    : IterationParameters(0.0,0)
  {
    reset();
  }
  friend class InteriorPointSqrtModel;
  template<class A, class B> friend class InteriorPointTangentialPredictor;
  friend class InteriorPointSlopeEstimate;

  virtual ~InteriorPointParametersSqrt() {}

protected:

  LoggedQuantity<double> omega;
  LoggedQuantity<double> eta;
  LoggedQuantity<double> slope;
  LoggedQuantity<double> curvature;
  LoggedQuantity<double> jpl;
  LoggedQuantity<double> accuracyCorrector;
  LoggedQuantity<int> newtonSum;
  LoggedQuantity<double> timemeasured;

  virtual void doForAll(LQAction::ToDo td)
  {
    omega.doAction(td,"omega");
    eta.doAction(td,"eta");
    slope.doAction(td,"slope");
    curvature.doAction(td,"curvature");
    jpl.doAction(td,"jpl");
    accuracyCorrector.doAction(td,"accuracyCorrector");
    newtonSum.doAction(td,"newtonSum");
    timemeasured.doAction(td,"timemeasured");
  }
};

/// InteriorPointMethod with a sqrt-model of the path: eta ~ mu^{-1/2}, omega ~mu^{-1/2}
class InteriorPointSqrtModel : public HomotopyBase
{
public:
  InteriorPointSqrtModel(NewtonsMethod& n_, AbstractNorm const& norm_,
                         InteriorPointParameters& p_, AbstractNewtonDirection& solver_,
                         AbstractNorm const* normPlain_=0) :
    HomotopyBase(n_, p_),
    norm(norm_),
    solver(solver_),
    normPlain(normPlain_)
  {
    if(!normPlain) normPlain = &norm;
  }

  virtual void initialize() { iterate=trialIterate->clone(); nNewton=0;};
  virtual double muOnSuccess(int step);
  virtual double muOnFailure();
  virtual double muFinal() { return p.muFinal;};
  virtual void updateModelOfHomotopy(int step);
  virtual void initializeCorrector();
  virtual void finalizeCorrector() { nNewton += corrector.stepsPerformed(); pp.newtonSum=nNewton;};
  virtual void updateIterate();
  virtual void recoverIterate() { *trialIterate=*iterate; }
  virtual double lengthOfPath() {if(pp.eta.isValid()) return 2*p.mu*pp.slope; else return 1e300;};
  void printDiagnosis();

  virtual void logQuantities()
  {
    HomotopyBase::logQuantities();
    pp.logStep();
    printDiagnosis();
  }

private:
  std::unique_ptr<AbstractFunctionSpaceElement> iterate;
  AbstractNorm const& norm;
  AbstractNewtonDirection& solver;
  InteriorPointParametersSqrt pp;
  JModel jModel;
  JModelLin jModelL;
  AbstractNorm const* normPlain;
  int nNewton;
  boost::timer::cpu_timer overalltime;
};

/// Performs bisection algorithm to find a zero of a
/**************************************************
 * Assumptions
 *
 *  a < b
 *
 *  Equation f with has double operator(double ) monotonically INCREASING
 *
 ************************************************************/

template<class Equation>
double bisection(double a, double b, Equation const& f, double accuracy, int& iterations)
{
  iterations = 0;
  double u;
  do {
    ++iterations;
    u=(a+b)/2;
    if(u == a) return b;
    if(f(u) > 0) b=u; else a=u;
  }
  while(b-a >= accuracy && (a+b)/2 != a && (a+b)/2 != b && iterations<50);
  if(iterations > 48) std::cout << "Warning: bisection algorithm performed > 48 iterations" << std::endl;
  return u;
}

class InteriorPointSlopeEstimate : public HomotopyBase
{

public:
  InteriorPointSlopeEstimate(NewtonsMethod& n_, AbstractNorm const& norm_, InteriorPointParameters& p_,
                          AbstractNewtonDirection& solver_, AbstractNorm const* normPlain_=0,
                          NewtonsMethod* finalsolver_=0) :
    HomotopyBase(n_, p_),
    norm(norm_),
    solver(solver_),
    stp(0),
    normPlain(normPlain_),
    finalsolver(finalsolver_)
  {
    if(!normPlain_) normPlain=&norm;
  }

  virtual void initialize() {
    iterate=trialIterate->clone();
    tangent=trialIterate->clone();
    nNewton=0;
  };
  virtual void finalize() {};

  virtual double muOnSuccess(int step);

  virtual double muOnFailure() {
    p.sigma=0.5+p.sigma/2.0;
    return p.mu*p.sigma;
  };

  virtual double muFinal() { return p.muFinal;};

  virtual void updateModelOfHomotopy(int step);

  virtual void computePredictor(int step){}

  virtual void initializeCorrector();
  virtual void finalizeCorrector() {    nNewton += corrector.stepsPerformed(); pp.newtonSum=nNewton;};

  virtual void finalizeHomotopy();

  virtual void updateIterate();

  virtual void recoverIterate() { *trialIterate=*iterate; }

  virtual double lengthOfPath()
  {
    if(pp.eta.isValid())
      return 2*p.mu*pp.slope;
    else
      return 1e300;
  };

  virtual void logQuantities()
  {
    HomotopyBase::logQuantities();
    pp.logStep();
    printDiagnosis();
  }

  void printDiagnosis();

private:
  std::unique_ptr<AbstractFunctionSpaceElement> iterate, tangent;

  AbstractNorm const& norm;
  AbstractNewtonDirection& solver;
  InteriorPointParametersSqrt pp;
  JModelLin jModelL;
  int stp;
  AbstractNorm const* normPlain;
  NewtonsMethod* finalsolver;
  int nNewton;
  boost::timer::cpu_timer overalltime;
};

}  // namespace Kaskade

#endif


//
// Copyright (c) 2020 Kenshi Abe
//

#include "Trainer.hpp"

#include <fstream>
#include <iostream>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/filesystem.hpp>
#include <boost/serialization/unordered_map.hpp>
#include "Node.hpp"

namespace Trainer {

/// @param mode variant of CFR algorithm
/// @param seed random seed
/// @param strategyPaths paths to the binary files that represent fixed strategies
template <typename T>
Trainer<T>::Trainer(const std::string &mode, const uint32_t seed, const std::vector<std::string> &strategyPaths) : mEngine(seed), mNodeTouchedCnt(0), mModeStr(mode) {
    mGame = new T(mEngine);
    mFolderPath = "../strategies/" + mGame->name();
    boost::filesystem::create_directories(mFolderPath);
    mFixedStrategies = new std::unordered_map<std::string, Node *>[mGame->playerNum()];
    mUpdate = new bool[mGame->playerNum()];
    for (int i = 0; i < mGame->playerNum(); ++i) {
        if (strategyPaths.size() >= i + 1 && !strategyPaths[i].empty()) {
            std::cout << "load strategy \"" << strategyPaths[i] << "\" as static player " << i << std::endl;
            std::ifstream ifs(strategyPaths[i]);
            boost::archive::binary_iarchive ia(ifs);
            ia >> mFixedStrategies[i];
            ifs.close();
            mUpdate[i] = false;
        } else {
            mUpdate[i] = true;
        }
    }
}

template <typename T>
Trainer<T>::~Trainer() {
    for (auto &itr : mNodeMap) {
        delete itr.second;
    }
    for (int i = 0; i < mGame->playerNum(); ++i) {
        if (mUpdate[i]) {
            continue;
        }
        for (auto &itr : mFixedStrategies[i]) {
            delete itr.second;
        }
    }
    delete[] mFixedStrategies;
    delete[] mUpdate;
    delete mGame;
}

/// @brief Calculate the expected payoff of each player
/// @param game game
/// @param strategies list of strategies for each player
/// @return list of expected payoffs
template <typename T>
std::vector<float> Trainer<T>::CalculatePayoff(const T &game, const std::vector<std::function<const float *(const T &)>> &strategies) {
    // return payoff for terminal states
    if (game.done()) {
        std::vector<float> payoffs(game.playerNum());
        for (int i = 0; i < game.playerNum(); ++i) {
            payoffs[i] = game.payoff(i);
        }
        return payoffs;
    }

    // chance node turn
    const int actionNum = game.actionNum();
    if (game.isChanceNode()) {
        std::vector<float> nodeUtils(game.playerNum());
        for (int i = 0; i < game.playerNum(); ++i) {
            nodeUtils[i] = 0.0f;
        }
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            const float chanceProbability = game_cp.chanceProbability();
            std::vector<float> utils = CalculatePayoff(game_cp, strategies);
            for (int i = 0; i < game.playerNum(); ++i) {
                nodeUtils[i] += chanceProbability * utils[i];
            }
        }
        return nodeUtils;
    }

    // for each action, recursively calculate payoff with additional history and probability
    const int player = game.currentPlayer();
    std::vector<float> nodeUtils(game.playerNum());
    for (int i = 0; i < game.playerNum(); ++i) {
        nodeUtils[i] = 0.0f;
    }
    for (int a = 0; a < actionNum; ++a) {
        auto game_cp(game);
        game_cp.step(a);
        std::vector<float> utils = CalculatePayoff(game_cp, strategies);
        for (int i = 0; i < game.playerNum(); ++i) {
            nodeUtils[i] += strategies[player](game)[a] * utils[i];
        }
    }
    return nodeUtils;
}

/// @brief Execute the CFR algorithm to compute an approximate Nash equilibrium
/// @param iterations number of iterations of CFR
template <typename T>
void Trainer<T>::train(const int iterations) {
    float utils[mGame->playerNum()];

    for (int i = 0; i < iterations; ++i) {
        for (int p = 0; p < mGame->playerNum(); ++p) {
            if (!mUpdate[p]) {
                continue;
            }
            if (mModeStr == "vanilla") {
                mGame->reset(false);
                utils[p] = CFR(*mGame, p, 1.0f, 1.0f);
            } else {
                mGame->reset();
                if (mModeStr == "chance") {
                    utils[p] = chanceSamplingCFR(*mGame, p, 1.0f, 1.0f);
                } else if (mModeStr == "external") {
                    utils[p] = externalSamplingCFR(*mGame, p);
                } else if (mModeStr == "outcome") {
                    utils[p] = std::get<0>(outcomeSamplingCFR(*mGame, p, i, 1.0f, 1.0f, 1.0f));
                } else {
                    assert(false);
                }
            }
        }
        if (i % 1000 == 0) {
            std::cout << "iteration:" << i << ", cumulative nodes touched: " << mNodeTouchedCnt << ", infosets num: " << mNodeMap.size() << ", expected payoffs: (";
            for (int p = 0; p < mGame->playerNum(); ++p) {
                std::cout << utils[p] << ",";
            }
            std::cout << ")" << std::endl;
        }
        if (i != 0 && i % 10000000 == 0) {
            writeStrategyToBin(i);
        }
    }

    writeStrategyToBin();
}

/// @brief Main procedure of vanilla CFR
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @param pi the probability of reaching the current game node if all players other than the acting player always choose actions leading to the current game node
/// @param po the probability of reaching the current game node if the acting player always chooses actions leading to the current game node
/// @return expected payoff of the specified player at the current game node
template <typename T>
float Trainer<T>::CFR(const T &game, const int playerIndex, const float pi, const float po) {
    ++mNodeTouchedCnt;

    // return payoff for terminal states
    if (game.done()) {
        return game.payoff(playerIndex);
    }

    // chance node turn
    const int actionNum = game.actionNum();
    if (game.isChanceNode()) {
        float nodeUtil = 0.0f;
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            const float chanceProbability = game_cp.chanceProbability();
            nodeUtil += chanceProbability * CFR(game_cp, playerIndex, pi, po * chanceProbability);
        }
        return nodeUtil;
    }

    // get information set string representation
    std::string infoSet = game.infoSetStr();

    // treat static player as chance node
    const int player = game.currentPlayer();
    if (!mUpdate[player]) {
        float nodeUtil = 0.0f;
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            const float chanceProbability = float(mFixedStrategies[player].at(infoSet)->averageStrategy()[a]);
            nodeUtil += chanceProbability * CFR(game_cp, playerIndex, pi, po * chanceProbability);
        }
        return nodeUtil;
    }

    // get information set node or create it if nonexistant
    Node *node = mNodeMap[infoSet];
    if (node == nullptr) {
        node = new Node(actionNum);
        mNodeMap[infoSet] = node;
    }

    // get current strategy through regret-matching
    const float *strategy = node->strategy();

    // for each action, recursively call CFR with additional history and probability
    float utils[actionNum];
    float nodeUtil = 0;
    for (int a = 0; a < actionNum; ++a) {
        auto game_cp(game);
        game_cp.step(a);
        if (player == playerIndex) {
            utils[a] = CFR(game_cp, playerIndex, pi * strategy[a], po);
        } else {
            utils[a] = CFR(game_cp, playerIndex, pi, po * strategy[a]);
        }
        nodeUtil += strategy[a] * utils[a];
    }

    if (player == playerIndex) {
        // for each action, compute and accumulate counterfactual regret
        for (int a = 0; a < actionNum; ++a) {
            const float regret = utils[a] - nodeUtil;
            const float regretSum = node->regretSum(a) + po * regret;
            node->regretSum(a, regretSum);
        }
        // update average strategy across all training iterations
        node->strategySum(strategy, pi);
    }

    return nodeUtil;
}

/// @brief Main procedure of chance-sampling MCCFR
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @param pi the probability of reaching the current game node if all players other than the acting player always choose actions leading to the current game node
/// @param po the probability of reaching the current game node if the acting player and the chance player always choose actions leading to the current game node
/// @return estimated expected payoff of the specified player at the current game node
template <typename T>
float Trainer<T>::chanceSamplingCFR(const T &game, const int playerIndex, const float pi, const float po) {
    ++mNodeTouchedCnt;

    // return payoff for terminal states
    if (game.done()) {
        return game.payoff(playerIndex);
    }

    // get information set string representation
    std::string infoSet = game.infoSetStr();

    // treat static player as chance node
    const int actionNum = game.actionNum();
    const int player = game.currentPlayer();
    if (!mUpdate[player]) {
        auto game_cp(game);
        auto strategy = mFixedStrategies[player].at(infoSet)->averageStrategy();
        std::discrete_distribution<int> dist(strategy, strategy + actionNum);
        game_cp.step(dist(mEngine));
        return chanceSamplingCFR(game_cp, playerIndex, pi, po);
    }

    // get information set node or create it if nonexistant
    Node *node = mNodeMap[infoSet];
    if (node == nullptr) {
        node = new Node(actionNum);
        mNodeMap[infoSet] = node;
    }

    // get current strategy through regret-matching
    const float *strategy = node->strategy();

    // for each action, recursively call cfr with additional history and probability
    float utils[actionNum];
    float nodeUtil = 0;
    for (int a = 0; a < actionNum; ++a) {
        auto game_cp(game);
        game_cp.step(a);
        if (player == playerIndex) {
            utils[a] = chanceSamplingCFR(game_cp, playerIndex, pi * strategy[a], po);
        } else {
            utils[a] = chanceSamplingCFR(game_cp, playerIndex, pi, po * strategy[a]);
        }
        nodeUtil += strategy[a] * utils[a];
    }

    if (player == playerIndex) {
        // for each action, compute and accumulate counterfactual regret
        for (int a = 0; a < actionNum; ++a) {
            const float regret = utils[a] - nodeUtil;
            const float regretSum = node->regretSum(a) + po * regret;
            node->regretSum(a, regretSum);
        }
        // update average strategy across all training iterations
        node->strategySum(strategy, pi);
    }

    return nodeUtil;
}

/// @brief Main procedure of external-sampling MCCFR
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @return estimated expected payoff of the specified player at the current game node
template <typename T>
float Trainer<T>::externalSamplingCFR(const T &game, const int playerIndex) {
    ++mNodeTouchedCnt;

    // return payoff for terminal states
    if (game.done()) {
        return game.payoff(playerIndex);
    }

    // get information set string representation
    std::string infoSet = game.infoSetStr();

    // external sampling with stochastically-weighted averaging cannot treat static player
    const int actionNum = game.actionNum();
    const int player = game.currentPlayer();
    assert(mUpdate[player] && "External sampling with stochastically-weighted averaging cannot treat static player.");

    // get information set node or create it if nonexistant
    Node *node = mNodeMap[infoSet];
    if (node == nullptr) {
        node = new Node(actionNum);
        mNodeMap[infoSet] = node;
    }

    // get current strategy through regret-matching
    const float *strategy = node->strategy();

    // if current player is not the target player, sample a single action and recursively call cfr
    if (player != playerIndex) {
        auto game_cp(game);
        std::discrete_distribution<int> dist(strategy, strategy + actionNum);
        game_cp.step(dist(mEngine));
        const float util = externalSamplingCFR(game_cp, playerIndex);
        // update average strategy
        node->strategySum(strategy, 1.0f);
        return util;
    }

    // for each action, recursively call cfr with additional history and probability
    float utils[actionNum];
    float nodeUtil = 0;
    for (int a = 0; a < actionNum; ++a) {
        auto game_cp(game);
        game_cp.step(a);
        utils[a] = externalSamplingCFR(game_cp, playerIndex);
        nodeUtil += strategy[a] * utils[a];
    }

    // for each action, compute and accumulate counterfactual regret
    for (int a = 0; a < actionNum; ++a) {
        const float regret = utils[a] - nodeUtil;
        const float regretSum = node->regretSum(a) + regret;
        node->regretSum(a, regretSum);
    }

    return nodeUtil;
}

/// @brief Main procedure of outcome-sampling MCCFR
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @param pi the probability of reaching the current game node if all players other than the acting player always choose actions leading to the current game node
/// @param po the probability of reaching the current game node if the acting player and the chance player always choose actions leading to the current game node
/// @param s the probability of reaching the current game node if the chance player always chooses actions leading to the current game node and the other players act according to the sample profile
/// @return estimated expected payoff of the specified player at the current game node, and the probability of reaching the terminal game node if the chance player always chooses actions leading to the terminal game node
template <typename T>
std::tuple<float, float> Trainer<T>::outcomeSamplingCFR(const T &game, const int playerIndex, const int iteration , const float pi, const float po, const float s) {
    ++mNodeTouchedCnt;

    // return payoff for terminal states
    if (game.done()) {
        return std::make_tuple(game.payoff(playerIndex) / s, 1.0f);
    }

    // get information set string representation
    std::string infoSet = game.infoSetStr();

    // outcome sampling with stochastically-weighted averaging cannot treat static player
    const int actionNum = game.actionNum();
    const int player = game.currentPlayer();
    assert(mUpdate[player] && "Outcome sampling with stochastically-weighted averaging cannot treat static player.");

    // get information set node or create it if nonexistant
    Node *node = mNodeMap[infoSet];
    if (node == nullptr) {
        node = new Node(actionNum);
        mNodeMap[infoSet] = node;
    }

    // get current strategy through regret-matching
    const float *strategy = node->strategy();

    // if current player is the target player, sample a single action according to epsilon-on-policy
    // otherwise, sample a single action according to the player's strategy
    const float epsilon = 0.6;
    float probability[actionNum];
    if (player == playerIndex) {
        for (int a = 0; a < actionNum; ++a) {
            probability[a] = (epsilon / (float) actionNum) + (1.0f - epsilon) * strategy[a];
        }
    } else {
        for (int a = 0; a < actionNum; ++a) {
            probability[a] = strategy[a];
        }
    }
    std::discrete_distribution<int> dist(probability, probability + actionNum);
    const int action = dist(mEngine);

    // for sampled action, recursively call cfr with additional history and probability
    float util, pTail;
    auto game_cp(game);
    game_cp.step(action);
    const float newPi = pi * (player == playerIndex ? strategy[action] : 1.0f);
    const float newPo = po * (player == playerIndex ? 1.0f : strategy[action]);
    std::tuple<float, float> ret = outcomeSamplingCFR(game_cp, playerIndex, iteration, newPi, newPo, s * probability[action]);
    util = std::get<0>(ret);
    pTail = std::get<1>(ret);
    if (player == playerIndex) {
        // for each action, compute and accumulate counterfactual regret
        const float W = util * po;
        for (int a = 0; a < actionNum; ++a) {
            const float regret = a == action ? W * (1.0f - strategy[action]) * pTail : -W * pTail * strategy[action];
            const float regretSum = node->regretSum(a) + regret;
            node->regretSum(a, regretSum);
        }
    } else {
        // update average strategy
        node->strategySum(strategy, po / s);
    }
    return std::make_tuple(util, pTail * strategy[action]);
}

/// @brief Save the current average strategy as a binary file
/// @param iteration current iteration
template <typename T>
void Trainer<T>::writeStrategyToBin(const int iteration) const {
    for (auto &itr : mNodeMap) {
//        std::cout << itr.first << ":";
        for (int i = 0; i < itr.first.size(); ++i) {
            std::cout << int(itr.first[i]);
        }
        std::cout << ":";
        for (int i = 0; i < itr.second->actionNum(); ++i) {
            std::cout << itr.second->averageStrategy()[i] << ",";
        }
        std::cout << std::endl;
    }
    std::string path = iteration > 0 ? "strategy_" + std::to_string(iteration)
                                     : "strategy";
    path += "_" + mModeStr + ".bin";
    std::ofstream ofs(mFolderPath + "/" + path);
    boost::archive::binary_oarchive oa(ofs);
    oa << mNodeMap;
    ofs.close();
}

} // namespace