// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "laplace_3d_calderon_projector.hpp"

#include "../common/shared_ptr.hpp"

#include "laplace_3d_single_layer_boundary_operator.hpp"
#include "laplace_3d_double_layer_boundary_operator.hpp"
#include "laplace_3d_hypersingular_boundary_operator.hpp"
#include "identity_operator.hpp"
#include "blocked_operator_structure.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../assembly/context.hpp"

#include <boost/make_shared.hpp>

namespace Bempp {

template <typename BasisFunctionType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
laplace3dExteriorCalderonProjector(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    const std::string &label) {

  typedef BoundaryOperator<BasisFunctionType, ResultType> BdOp;

  shared_ptr<const Space<BasisFunctionType>> internalHplusSpace =
      hplusSpace->discontinuousSpace(hplusSpace);

  BdOp internalSlp = laplace3dSingleLayerBoundaryOperator(
      context, internalHplusSpace, internalHplusSpace, internalHplusSpace,
      label + "_slp", SYMMETRIC);

  BdOp dlp = laplace3dDoubleLayerBoundaryOperator(context, hplusSpace,
                                                  hplusSpace, hminusSpace,
                                                  label + "_dlp", NO_SYMMETRY);

  BdOp adjDlp = adjoint(dlp);

  BdOp hyp = laplace3dHypersingularBoundaryOperator(
      context, hplusSpace, hminusSpace, hplusSpace, label + "_hyp", SYMMETRIC,
      internalSlp);

  BdOp idSpaceTransformation1 = identityOperator(
      context, hminusSpace, internalHplusSpace, internalHplusSpace);
  BdOp idSpaceTransformation2 =
      identityOperator(context, internalHplusSpace, hplusSpace, hminusSpace);
  BdOp idDouble =
      identityOperator(context, hplusSpace, hplusSpace, hminusSpace);
  BdOp idAdjDouble =
      identityOperator(context, hminusSpace, hminusSpace, hplusSpace);

  // Now Assemble the entries of the Calderon Projector

  BlockedOperatorStructure<BasisFunctionType, ResultType> structure;

  structure.setBlock(0, 0, .5 * idDouble + dlp);
  structure.setBlock(0, 1, -1. * idSpaceTransformation2 * internalSlp *
                               idSpaceTransformation1);
  structure.setBlock(1, 0, -1. * hyp);
  structure.setBlock(1, 1, .5 * idAdjDouble - adjDlp);

  return BlockedBoundaryOperator<BasisFunctionType, ResultType>(structure);
}

template <typename BasisFunctionType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
laplace3dInteriorCalderonProjector(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    const std::string &label) {

  typedef BoundaryOperator<BasisFunctionType, ResultType> BdOp;

  shared_ptr<const Space<BasisFunctionType>> internalHplusSpace =
      hplusSpace->discontinuousSpace(hplusSpace);

  BdOp internalSlp = laplace3dSingleLayerBoundaryOperator(
      context, internalHplusSpace, internalHplusSpace, internalHplusSpace,
      label + "_slp", SYMMETRIC);

  BdOp dlp = laplace3dDoubleLayerBoundaryOperator(context, hplusSpace,
                                                  hplusSpace, hminusSpace,
                                                  label + "_dlp", NO_SYMMETRY);

  BdOp adjDlp = adjoint(dlp);

  BdOp hyp = laplace3dHypersingularBoundaryOperator(
      context, hplusSpace, hminusSpace, hplusSpace, label + "_hyp", SYMMETRIC,
      internalSlp);

  BdOp idSpaceTransformation1 = identityOperator(
      context, hminusSpace, internalHplusSpace, internalHplusSpace);
  BdOp idSpaceTransformation2 =
      identityOperator(context, internalHplusSpace, hplusSpace, hminusSpace);
  BdOp idDouble =
      identityOperator(context, hplusSpace, hplusSpace, hminusSpace);
  BdOp idAdjDouble =
      identityOperator(context, hminusSpace, hminusSpace, hplusSpace);

  // Now Assemble the entries of the Calderon Projector

  BlockedOperatorStructure<BasisFunctionType, ResultType> structure;

  structure.setBlock(0, 0, .5 * idDouble - dlp);
  structure.setBlock(0, 1, idSpaceTransformation2 * internalSlp *
                               idSpaceTransformation1);
  structure.setBlock(1, 0, hyp);
  structure.setBlock(1, 1, .5 * idAdjDouble + adjDlp);

  return BlockedBoundaryOperator<BasisFunctionType, ResultType>(structure);
}

template <typename BasisFunctionType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
laplace3dExteriorCalderonProjector(
    const ParameterList &parameterList,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    const std::string &label) {

  shared_ptr<const Context<BasisFunctionType, ResultType>> context(
      new Context<BasisFunctionType, ResultType>(parameterList));
  return laplace3dExteriorCalderonProjector<BasisFunctionType, ResultType>(
      context, hminusSpace, hplusSpace, label);
}

template <typename BasisFunctionType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
laplace3dInteriorCalderonProjector(
    const ParameterList &parameterList,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    const std::string &label) {

  shared_ptr<const Context<BasisFunctionType, ResultType>> context(
      new Context<BasisFunctionType, ResultType>(parameterList));
  return laplace3dInteriorCalderonProjector<BasisFunctionType, ResultType>(
      context, hminusSpace, hplusSpace, label);
}

#define INSTANTIATE_NONMEMBER_CONSTRUCTOR(BASIS, RESULT)                       \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  laplace3dExteriorCalderonProjector(                                          \
      const ParameterList &, const shared_ptr<const Space<BASIS>> &,           \
      const shared_ptr<const Space<BASIS>> &, const std::string &);            \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  laplace3dInteriorCalderonProjector(                                          \
      const ParameterList &, const shared_ptr<const Space<BASIS>> &,           \
      const shared_ptr<const Space<BASIS>> &, const std::string &);            \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  laplace3dExteriorCalderonProjector(                                          \
      const shared_ptr<const Context<BASIS, RESULT>> &,                        \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, const std::string &);            \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  laplace3dInteriorCalderonProjector(                                          \
      const shared_ptr<const Context<BASIS, RESULT>> &,                        \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, const std::string &)

FIBER_ITERATE_OVER_BASIS_AND_RESULT_TYPES(INSTANTIATE_NONMEMBER_CONSTRUCTOR);

} // Bempp


#include <iostream>
#include <bits/stdc++.h>
#include <vector>
#include <string>
#include <climits>
#include <boost/algorithm/string.hpp>
#include <cmath>

using namespace std;

// valid_move_list(board, vector<pair<int,int>>)
vector<pair<int,int>> dirs = {make_pair(0,-2),make_pair(-1,1),make_pair(-1,-1),make_pair(0,2),make_pair(1,1),make_pair(1,-1)};
pair<int,int> sel_ring = make_pair(-1,-1);
pair<int,int> remove_row_start = make_pair(-1,-1);
pair<int,int> remove_row_end = make_pair(-1,-1);

vector<vector<int>> setMarker(vector<vector<int>> board, int x, int y, int marker){
  board[x][y] = marker;
  return board;
}

bool isValidPoint(vector<vector<int>> board,int col, int row){
  if(row > 19 || row < 0 || col < 0 || col > 10 || board[col][row] == -100)
        return false;
    return true;
}

vector<vector<int>> removeMarker(vector<vector<int>> board, int col, int row){
  board[col][row] = 0;
  return board;
}

vector<vector<int>> flipMarkers(vector<vector<int>> board, int x_init, int y_init, int x_fin, int y_fin){
  int ind = 0;
  for(int i = 0; i < dirs.size(); i++){
    if(dirs[i].first == 0){
      if( x_init == x_fin  && (y_fin - y_init) % dirs[i].second == 0 ){
                ind = i;
                break;
      }
    }
    else if( (x_fin - x_init) / dirs[i].first == (y_fin - y_init) / dirs[i].second){
            ind = i;
            break;
    }
  }
  int temp_x = x_init + dirs[ind].first;
  int temp_y = y_init + dirs[ind].second;
  int k = (y_fin - y_init) / dirs[ind].second;
  for(int l = 1 ; l < k; l++){
    if(board[temp_x][temp_y] == -1)
      board[temp_x][temp_y] = 1;
    else if(board[temp_x][temp_y] == 1)
      board[temp_x][temp_y] = -1;
    temp_x = temp_x + dirs[ind].first;
    temp_y = temp_y + dirs[ind].second;
  }
  return board;
}

vector<vector<int>> moveRing(vector<vector<int>> board,int x_init, int y_init, int x_fin, int y_fin){
  int ring = board[x_init][y_init];
  int marker = (ring == 2) ? 1 : -1;
  board[x_init][y_init] = marker;
  board = flipMarkers(board,x_init,y_init,x_fin,y_fin);
  board[x_fin][y_fin] = ring;
  return board;
}

vector<vector<int>> placeRingAt(vector<vector<int>> board, int x, int y, int id){
  if(id == 0){
    return setMarker(board, x, y, 2);
  }
  else{
    return setMarker(board,x,y,-2);
  }
}

vector<vector<int>> removeRing(vector<vector<int>> board, int x, int y, int id){
  return removeMarker(board,x,y);
}

vector<vector<int>> selectRing(vector<vector<int>> board, int x, int y){
  sel_ring.first = x;
  sel_ring.second = y;
  return board;
}

vector<vector<int>> removeRow(vector<vector<int>> board, int x_init, int y_init, int x_fin, int y_fin){
  int ind = 0;
  for(int i = 0; i < 6; i++){
    if(dirs[i].first == 0){
      if( x_init == x_fin  && (y_fin - y_init) % dirs[i].second == 0 ){
                ind = i;
                break;
      }
    }
    else if((x_fin - x_init) / dirs[i].first == (y_fin - y_init) / dirs[i].second){
    ind = i;
    break;
    }
  }
  int temp_x = x_init;
  int temp_y = y_init;
  int k = (y_fin - y_init) / dirs[ind].second;
  for(int k = 1; k < 4; k++){
    board[temp_x][temp_y] = 0;

    temp_x = temp_x + dirs[ind].first;
    temp_y = temp_y + dirs[ind].second;
  }
  return board;
}

vector<vector<int>> selectRowStart(vector<vector<int>> board, int x, int y){
  remove_row_start.first = x;
  remove_row_start.second = y;
  return board;
}

vector<vector<int>> selectRowEnd(vector<vector<int>> board, int x, int y){
  remove_row_end.first = x;
  remove_row_end.second = y;
  return removeRow(board,remove_row_start.first,remove_row_start.second,remove_row_end.first, remove_row_end.second);
}

vector<vector<int>> execute(vector<vector<int>> board, string cmd,int id);

vector<vector<int>> execute_sequence(vector<vector<int>> board,vector<string> moves,int id){
  vector<string> moves_list = {};
  //cout << "lol" << endl;
  string move;
  for(int i = 0; i < moves.size(); i++){
    move = moves[i];
  //  cout << "move " << move << endl;
    if( i % 3 == 2){
      moves_list.push_back(move);
      string lol;
      for (auto const& s : moves_list) { lol = lol + s + " "; }
    // cout <<"lol :  " << lol << endl;
      board = execute(board,lol.substr(0,lol.length()-1), id);
      moves_list = {};
    }
    else{
      moves_list.push_back(move);
    }
  }
  return board;
}

vector<vector<int>> execute(vector<vector<int>> board, string cmd,int id){
  //vector<vector<int>> temp = board;
  vector<string> moves;
  boost::split(moves, cmd, [](char c){return (c == ' ');});
  if(moves.size() > 3){
    //cout << moves.size() << "moves[0]: " << moves[0] << endl;
    return execute_sequence(board,moves,id);
  }
  string move_type = moves[0];
  int x = stoi(moves[1]);
  int y = stoi(moves[2]);
  //cout << "Error in next funs" << endl;
  if(move_type == "P")
        return placeRingAt(board, x, y, id);
  else if (move_type == "S")
        return selectRing(board, x ,y);
  else if (move_type == "M")
        return moveRing(board,sel_ring.first,sel_ring.second,x,y);
  else if (move_type == "RS")
        return selectRowStart(board, x, y);
  else if (move_type == "RE")
        return selectRowEnd(board, x, y);
  else if (move_type == "X")
        return removeMarker(board, x, y);
  else
    return board;
}

vector<float> CalculateScore(int wRings,int bRings,int wMarkers,int bMarkers, char Error_state){
    vector<int> scores = {0,0};
    vector<float> score = {0.0,0.0};
    if(Error_state == '1'){
      wRings = 0;
      bRings = 3;
    }
    else if(Error_state == '2'){
      bRings = 0;
      wRings = 3;
    }
    if(wRings == 3){
      scores[0] = 10 - bRings;
      scores[1] = bRings;
    }
    else if(bRings == 3){
      scores[0] = wRings;
      scores[1] = 10 - wRings;
    }
    else if(wRings == bRings){
      scores[0] = 5;
      scores[1] = 5;
    }
    else if(wRings - bRings == 2){
      scores[0] = 7;
      scores[1] = 3;
    }
    else if(bRings - wRings == 2){
      scores[0] = 3;
      scores[1] = 7;
    }
    else if(wRings > bRings){
      scores[0] = 6;
      scores[1] = 4;
    }
    else if(bRings > wRings){
      scores[0] = 4;
      scores[1] = 6;
    }
    else{
      scores[0] = -1;
      scores[1] = -1;
    }
    score[0] = scores[0] + ((float) wMarkers)/1000;
    score[1] = scores[1] + ((float) bMarkers)/1000;
    return score;
}

float get_score(vector<vector<int>> board, int p_id, char Error_state = '0'){
  int w_rings = 0;
  int b_rings = 0;
  int w_markers = 0;
  int b_markers = 0;
  for(int i = 0; i < 11; i++){
    for(int j = 0; j < 19; j++){
      int s = board[i][j];
      if(s == 0 || s == -100){
        continue;
      }
      else if(s == 2){
        w_rings++;
      }
      else if(s == -2){
        b_rings++;
      }
      else if(s == 1){
        w_markers++;
      }
      else if(s == -1){
        b_markers++;
      }
    }
  }
  return CalculateScore(5-w_rings,5-b_rings,w_markers,b_markers,Error_state)[p_id];
}

int lenRowsFormed(vector<vector<int>> board, pair<int,int> dir, int i, int j){
  int row = j + dir.second;
  int col = i + dir.first;
  int marker = 1;
  int len = 1;
  while(row > -1 && row < 19 && col > -1 && col < 19){
    if(board[col][row] == marker){
      len++;
    }
    else{
      break;
    }
  }
  return len;
}

float fun1(vector<vector<int>> board){
  float score = 0.0f;
  int len = 0;
  for(int i = 0; i < 11; i++){
    for(int j = 0; j < 19; j++){
      if(board[i][j] == 1){
        for(int k = 0 ; k < 6; k++){
          len = lenRowsFormed(board,dirs[k], i, j);
          score += pow(2.0f,len);
        }
      }
    }
  }
  //cout << "Working fine fun1" << endl;
  return score;
}

float fun2(vector<vector<int>> board){
  float score = 0.0f;
  float weight = 0.0f;
  for(int i = 0; i < 11; i++){
    for(int j = 0; j < 19; j++){
      if(board[i][j] == 2){
        if(i == 5 && j == 9){
          weight = 1000.0f;
        }
        else{
          int l = (i-5 > 0) ? i-5:5-i;
          int m = (j-9 > 0) ? j-9:9-j;
          weight = (100.0f/sqrt(pow((float)l,2) + pow((float)m,2)));
        }
        score += weight;
      }
    }
  }
  return score;
}

int flippedUtil(vector<vector<int>> board, int marker, int ring, pair<int,int> dir, int i, int j){
  int row = i + dir.second;
  int col = j + dir.first;
  int marker_count = 0;
  while (row < 19 && row > -1 && col < 11 && col > -1 && board[col][row] == 0){
        row += dir.second;
        col += dir.first;
      }
  while (row < 19 && row > -1 && col < 11 && col > -1 && board[col][row] != -100){
      if(board[col][row] == marker)
            marker_count += 1;
      else if (board[col][row] == 0)
            return marker_count;
      else if (board[col][row] == -2 || board[col][row] == 2 ){
            return 0;
      }
      row += dir.second;
      col += dir.first;
      }
    return 0;
}

float flippedScore(vector<vector<int>> board, int marker, int ring){
  float flipped_score = 0.0;
  for(int i = 0; i < 11; i++){
    for(int j = 0; j < 19; j++){
      if(board[i][j] == ring){
        for(int k = 0; j < 6; k++){
          flipped_score += 1.0 * flippedUtil(board, marker, ring, dirs[k], i ,j);
        }
      }
    }
  }
  return flipped_score;
}

int countMarkers(vector<vector<int>> board, int marker){
  int marker_count = 0;
  for(int i = 0; i < 11; i++){
    for(int j = 0; j < 19; j++){
      if(board[i][j] == marker){
        marker_count++;
      }
    }
  }
  return marker_count;
}

int mobilityUtil(vector<vector<int>> board, int ring, pair<int,int> dir, int i, int j){
  int row = j + dir.second;
  int col = i + dir.first;
  int space_count = 0;
  while (row < 19 && row > -1 && col < 11 && col > -1 && board[col][row] == 0){
        space_count++;
        row += dir.second;
        col += dir.first;
      }
  while (row < 19 && row > -1 && col < 11 && col > -1 && board[col][row] != -100){
      if(board[col][row] == 0)
            return space_count+1;
      else if (board[col][row] == -2 || board[col][row] == 2 )
            return space_count;
        row += dir.second;
        col += dir.first;
      }
  return space_count;
}

float mobilityScore(vector<vector<int>> board, int ring){
  float mobility_score = 0.0;
  for(int i = 0; i < 11; i++){
    for(int j = 0; j < 19; j++){
      if(board[i][j] == ring){
        for(int k = 0; j < 6; k++){
          mobility_score += 1.0 * mobilityUtil(board,ring, dirs[k], i ,j);
        }
      }
    }
  }
  return mobility_score;
}

float evaluate(vector<vector<int>> board, int current_player){
  //cout << "Before countMarkers white" << endl;
  int num_w_marker = 0;//countMarkers(board,1);
  //cout << "Before countMarkers black" << endl;
  int num_b_marker = 0;//countMarkers(board,-1);

  //  black_row = markerScore(board,-1,-2)
  //  white_row = markerScore(board,1,2)
  //cout << "Before countMarkers " << endl;
  int flip_ring = (current_player == 0) ? 2 : -2;
  //cout << "Before flippedscore white" << endl;
  int flip_white_marker = 0;//flippedScore(board,-1,flip_ring);
  //cout << "Before flippedscore blacks" << endl;
  int flip_black_marker = 0;//flippedScore(board,1,flip_ring);
  //cout << "Before mobilityscore white" << endl;
  int mobility_black_ring = 0;//mobilityScore(board,-2);
  //cout << "Before mobilityscore black" << endl;
  int mobility_white_ring = 0;//mobilityScore(board,2);
  //cout << "Before get_score white" << endl;
  int val1 = fun1(board);
  int val2 = fun2(board);
  int white_score = get_score(board, 0);
  //cout << "Before get_score blacak" << endl;
  int black_score = get_score(board, 1);
  //cout << "after all this time? always" << endl;

  return (num_w_marker +flip_white_marker + mobility_white_ring + white_score) * (0.5 * (1 + white_score)) + (num_b_marker + flip_black_marker + mobility_black_ring + black_score) * (0.5 * (1 + black_score)) + val1 + val2;

}

//valid_moves_list()
// execute()
// rows_formed()
// evaluate()
vector<pair<int,int>> validPointsInARow(vector<vector<int>> board, pair<int,int> point, pair<int,int> dir){
    vector<pair<int,int>> lis;
    int col = point.first + dir.first;
    int row = point.second + dir.second;
    bool no_jump = true;
    bool first_jump = false;
    while(row > -1 && row < 19 && col > -1 && col < 11){
      if(board[col][row] == 2 || board[col][row] == -2){
        return lis;
      }
      else if(board[col][row] == 1 || board[col][row] == -1){
        first_jump = true;
        no_jump = false;
      }
      else if(board[col][row] == 0){
        if(no_jump){
          lis.push_back(make_pair(col,row));
        }
        if(first_jump){
          lis.push_back(make_pair(col,row));
          return lis;
        }
      }
      col += dir.first;
      row += dir.second;
    }
    return lis;
}

vector<pair<int,int>> addList(vector<pair<int,int>> lis1, vector<pair<int,int>> lis2){
  for(auto it = lis2.begin(); it != lis2.end();it++){
    lis1.push_back((*it));
  }
  return lis1;
}

vector<pair<int,int>> valid_moves_list(vector<vector<int>> board, pair<int, int> point){
  vector<pair<int,int>> lis;
  for(int i = 0; i < 6; i++){
    lis = addList(lis,validPointsInARow(board,point,dirs[i]));
  }
  return lis;
}

vector<pair<pair<int,int>,pair<int,int>>> rows_formed(vector<vector<int>> board, int pid){
  vector<vector<int>> temp = board;
  int marker = (pid == 0) ? 1 : -1;
  vector<pair<pair<int,int>,pair<int,int>>> lis;
  for(int i = 0; i < 11; i++){
    for(int j = 0; j < 11; j++){
      if(temp[i][j] != marker){
        continue;
      }
      bool isrow = true;
      for(int k = 1; k < 5; k++){
        if(temp[i][j+k*2] != marker){
          isrow = false;
          break;
        }
      }
      if(!isrow){
        continue;
      }
      lis.push_back(make_pair(make_pair(i,j),make_pair(i,j+8)));
    }
  }
  for(int i = 0; i < 7; i++){
    for(int j = 4; j < 19; j++){
      if(temp[i][j] != marker){
        continue;
      }
      bool isrow = true;
      for(int k = 1; k < 5; k++){
        if(temp[i+k][j-k] != marker){
          isrow = false;
          break;
        }
      }
      if(!isrow){
        continue;
      }
      lis.push_back(make_pair(make_pair(i,j),make_pair(i+4,j-4)));
    }
  }
  for(int i = 0; i < 7; i++){
    for(int j = 0; j < 15; j++){
      if(temp[i][j] != marker){
        continue;
      }
      bool isrow = true;
      for(int k = 1; k < 5; k++){
        if(temp[i][j+k*2] != marker){
          isrow = false;
          break;
        }
      }
      if(!isrow){
        continue;
      }
      lis.push_back(make_pair(make_pair(i,j),make_pair(i+4,j+4)));
    }
  }
  return lis;
}

class Node{
public:
  vector<vector<int>> board;
  string move;
  vector<Node> list_children;
  int won;
  int num_ring;
  int utility;

  Node(vector<vector<int>> b, string m, vector<Node> lis,int rings_num, int w){
    this->board = b;
    this->move = m;
    this->num_ring = rings_num;
    this->list_children = lis;
    this->utility = 0;
    this->won = w;
  }

  void append_children(int num, int player){
    int ring,marker;
    if(num == 0){
      for(int i = 0; i < 11; i++){
        for(int j = 0; j < 19; j++){
          if(board[i][j] == 0){
            string temp_move = "P " + to_string(i) + " " + to_string(j);
            vector<vector<int>> b = this->board;
            vector<vector<int>> temp_board = execute(b,temp_move,player);
            Node a(temp_board,temp_move, {} ,0,0);
            (this->list_children).push_back(a);
          }
        }
      }
    }
    if(num == 1){
      if(player == 0){
        ring = 2;
        marker = 1;
      }
      else{
        ring = -2;
        marker = -1;
      }
      if(won == 0){
        while(rows_formed(board,0).size() != 0){
          vector<pair<pair<int,int>,pair<int,int>>> temp = rows_formed(board,0);
          vector<Node> lol;
          for(int j = 0; j < temp.size(); j++){
            string temp_move = "RS " + to_string(temp[j].first.first) + " " + to_string(temp[j].first.second) + " RE " + to_string(temp[j].second.first) + " " + to_string(temp[j].second.second);
            lol.push_back(Node(execute(board,temp_move,player),temp_move, {}, num_ring, won));
          }
          vector<Node> lol1;
          for(int i = 0; i < lol.size(); i++){
            for(int j = 0; j < 11; j++){
              for(int k = 0; k < 19; k++){
                if(lol1[i].board[j][k] == ring){
                  string temp_move = lol1[i].move + " X " + to_string(j) + " " + to_string(k);
                  int ring_num = num_ring + 1;
                  if(ring_num > 2){
                    lol1.push_back(Node(execute(lol1[i].board,temp_move,player),temp_move, lol1[i].list_children, ring_num, 1));
                  }
                  else{
                    lol1.push_back(Node(execute(lol1[i].board,temp_move,player),temp_move, lol1[i].list_children, ring_num, 0));
                  }
                }
              }
            }
          }
          int max_score = INT_MIN;
          int index = -1;
          Node max_node({},"",{},0,0);

          for(int i = 0; i < lol1.size(); i++){
            int score = evaluate(lol1[i].board, 0);
            if(score > max_score){
              max_score = score;
              max_node = lol1[i];
              index = i;
            }
          }
          this->board = lol1[index].board;
          this->move = lol1[index].move;
          this->won = lol1[index].won;
          this->num_ring = lol1[index].num_ring;
          this->list_children = lol1[index].list_children;
        }
        if(rows_formed(board,0).size() == 0){
          for(int i = 0; i < 11; i++){
            for(int j = 0; j < 19; j++){
              if(board[i][j] == ring){
                vector<pair<int, int>> moves_list = valid_moves_list(board,make_pair(i,j));
                for(int k = 0; k < moves_list.size(); k++){
                  string temp_move = "S " + to_string(i) + " " + to_string(j) + " M " + to_string(moves_list[k].first) + " " + to_string(moves_list[k].second);
                   list_children.push_back(Node(execute(board,temp_move,player), temp_move, {}, num_ring, won));
                }
              }
            }
          }
          vector<Node>::iterator it;
          for(it = list_children.begin(); it != list_children.end(); it++){
            if(rows_formed((*it).board,0).size() != 0 || rows_formed((*it).board,1).size() != 0){
              if(won == 0){
                it = list_children.erase(it);
                --it;
                vector<pair<pair<int,int>,pair<int,int>>> temp_rows = rows_formed(board,0);
                vector<Node> temp;
                for(int j = 0; j < temp_rows.size();j++){
                  string temp_move = (*it).move + " RS " + to_string(temp_rows[j].first.first) + " " + to_string(temp_rows[j].first.second) + " RE " + to_string(temp_rows[j].second.first) + " "  + to_string(temp_rows[j].second.second);
                  temp.push_back(Node(execute((*it).board, temp_move, player), temp_move, (*it).list_children, (*it).num_ring, (*it).won));
                }
                vector<Node> temp1;
                for(int j = 0; j < temp.size(); j++){
                  for(int k = 0; k < 11; k++){
                    for(int l = 0; l < 19; l++){
                      if(temp[j].board[k][l] == ring){
                        string temp_move = temp[j].move + " X " +to_string(k) + " " + to_string(l);
                        int rings_num = temp[j].num_ring + 1;
                        if(rings_num > 2){
                          temp1.push_back(Node(execute(temp[j].board, temp_move, player), temp_move, temp[j].list_children, rings_num, 1));
                        }
                        else{
                          temp1.push_back(Node(execute(temp[j].board, temp_move, player), temp_move, temp[j].list_children, rings_num, 0));
                        }
                      }
                    }
                  }
                }
                vector<Node>:: iterator it1;
                for(it1 = temp1.begin(); it1 != temp1.end(); it1++){
                  list_children.push_back((*it1));
                }
              }
            }
          }
        }
      }
    }
  }
};

Node get_tree(int num,vector<vector<int>> b, int num_moves){
  Node root = Node(b, " ", {}, 0, 0);
  if(num == 0){
    root.append_children(num, 0);
    for(auto it = root.list_children.begin(); it != root.list_children.end(); it++){
      (*it).append_children(num,1);
    //  for(auto jt = (*it).list_children.begin(); jt != (*it).list_children.end(); jt++){
    //    (*jt).append_children(num,0);
    //  }
    }
    return root;
  }
  if(num == 1){
    if(num_moves < 30){
      root.append_children(num,0);
      for(auto it = root.list_children.begin(); it != root.list_children.end(); it++){
        (*it).append_children(num,1);
      // for(auto jt = (*it).list_children.begin(); jt != (*it).list_children.end(); jt++){
      //    (*jt).append_children(num,0);
        //}
      }
    }
    else{
      root.append_children(num,0);
      for(auto it = root.list_children.begin(); it != root.list_children.end(); it++){
        (*it).append_children(num,1);
        for(auto jt = (*it).list_children.begin(); jt != (*it).list_children.end(); jt++){
          (*jt).append_children(num,0);
          //for(auto kt = (*jt).list_children.begin(); kt != (*jt).list_children.end(); kt++){
            //(*kt).append_children(num,1);
            //for(auto lt = (*kt).list_children.begin(); lt != (*kt).list_children.end(); lt++){
            //  (*lt).append_children(num,0);
            //}
          //}
        }
      }
    }
    return root;
  }
  return root;
}
int max_val(Node node, int alpha, int beta, int player);

int min_val(Node node, int alpha, int beta, int player){
//  cout << "In min : size of children " << node.list_children.size() << endl;
  if(node.list_children.size() == 0){
  //  cout << "before evaluate " << endl;
    node.utility = evaluate(node.board, player);
  //  cout << "after evaluate " << endl;
    return node.utility;
  }

  int value = INT_MAX;

  vector<Node> children = node.list_children;

  for(auto it = children.begin(); it != children.end(); it++){
    //cout << "move " << (*it).move << endl;
    Node child = *it;
    value = min(value, max_val(child,alpha,beta, 1-player));
  //  cout << "value " << value << "alpha "<< alpha <<endl;
    if(value <= alpha){
      return value;
    }
    beta = min(beta, value);
  }
  return value;
}

int max_val(Node node, int alpha, int beta, int player){
//  cout << "In max " << node.move << endl;
  if(node.list_children.size() == 0){
    node.utility = evaluate(node.board, player);
    return node.utility;
  }

  int value = INT_MIN;

  vector<Node> children = node.list_children;

  for(auto it = children.begin(); it != children.end(); it++){
    Node child = *it;
    value = max(value, min_val(child,alpha,beta, 1-player));
  //  cout << "value " << value << "beta "<< beta <<endl;
    if(value >= beta){
      return value;
    }
    alpha = max(alpha, value);
  }
  return value;
}

Node main_minimax(Node node){
  int best_val = INT_MIN;
  int beta = INT_MAX;

  vector<Node> children = node.list_children;
  Node best_child({},"",{},0,0);
  for(auto it = children.begin(); it != children.end(); it++){
    //cout <<"size of children  "<< children.size() << endl;
    //cout << "size of list_children " << node.list_children.size() << endl;
    //cout << "In main minimax "<<(*child).move << endl;
    Node child = *it;
    int value = max_val(child,best_val,beta, 0);
  //  cout << "after max_val in main minmax" << endl;
    if(value > best_val){
      best_val = value;
      best_child.board = child.board;
      best_child.move = child.move;
      best_child.list_children = child.list_children;
      best_child.won = child.won;
    }
  }
  //cout << best_child.move << endl;
  return best_child;
}

string minimax(int num_moves, vector<vector<int>> b){
  //cout << "In minimax" << endl;
  Node tree({},"",{},0,0);
  if(num_moves < 5){
    tree = get_tree(0,b,num_moves);
    //cout << "get_tree working" << endl;
  }
  else{
    tree = get_tree(1,b,num_moves);
  }
  //for(auto it = tree.list_children.begin(); it != tree.list_children.end();it++){
  //  cout << (*it).move << endl;
  //}
  Node child = main_minimax(tree);
  return child.move;
}

pair<int,int> null = make_pair(-10,-10);
vector<vector<pair<int,int>>> b_to_h = {{null, null, null, null, null, null, make_pair(5,24), null, make_pair(5,23), null, make_pair(5,22), null, make_pair(5,21), null, null, null, null, null, null}, {null, null, null, make_pair(5,26), null, make_pair(4,20), null, make_pair(4,19), null, make_pair(4,18), null, make_pair(4,17), null, make_pair(4,16), null, make_pair(5,19), null, null, null}, {null, null, make_pair(5,27), null, make_pair(4,21), null, make_pair(3,15), null, make_pair(3,14), null, make_pair(3,13), null, make_pair(3,12), null, make_pair(4,15), null, make_pair(5,18), null, null}, {null, make_pair(5,28), null, make_pair(4,22), null, make_pair(3,16), null, make_pair(2,10), null, make_pair(2,9), null, make_pair(2,8), null, make_pair(3,11), null, make_pair(4,14), null, make_pair(5,17), null}, {make_pair(5,29), null, make_pair(4,23), null, make_pair(3,17), null, make_pair(2,11), null, make_pair(1,5), null, make_pair(1,4), null, make_pair(2,7), null, make_pair(3,10), null, make_pair(4,13), null, make_pair(5,16)}, {null, make_pair(4,0), null, make_pair(3,0), null, make_pair(2,0), null, make_pair(1,0), null, make_pair(0,0), null, make_pair(1,3), null, make_pair(2,6), null, make_pair(3,9), null, make_pair(4,12), null}, {make_pair(5,1), null, make_pair(4,1), null, make_pair(3,1), null, make_pair(2,1), null, make_pair(1,1), null, make_pair(1,2), null, make_pair(2,5), null, make_pair(3,8), null, make_pair(4,11), null, make_pair(5,14)}, {null, make_pair(5,2), null, make_pair(4,2), null, make_pair(3,2), null, make_pair(2,2), null, make_pair(2,3), null, make_pair(2,4), null, make_pair(3,7), null, make_pair(4,10), null, make_pair(5,13), null}, {null, null, make_pair(5,3), null, make_pair(4,3), null, make_pair(3,3), null, make_pair(3,4), null, make_pair(3,5), null, make_pair(3,6), null, make_pair(4,9), null, make_pair(5,12), null, null}, {null, null, null, make_pair(5,4), null, make_pair(4,4), null, make_pair(4,5), null, make_pair(4,6), null, make_pair(4,7), null, make_pair(4,8), null, make_pair(5,11), null, null, null}, {null, null, null, null, null, null, make_pair(5,6), null, make_pair(5,7), null, make_pair(5,8), null, make_pair(5,9), null, null, null, null, null, null}};

vector<vector<pair<int,int>>> h_to_b = {{make_pair(5,9)},{make_pair(5,7),make_pair(6,8),make_pair(6,10),make_pair(5,11),make_pair(4,10),make_pair(4,8)},{make_pair(5,5),make_pair(6,6),make_pair(7,7),make_pair(7,9),make_pair(7,11),make_pair(6,12),make_pair(5,13),make_pair(4,12),make_pair(3,11),make_pair(3,9),make_pair(3,7),make_pair(4,6)},{make_pair(5,3),make_pair(6,4),make_pair(7,5),make_pair(8,6),make_pair(8,8),make_pair(8,10),make_pair(8,12),make_pair(7,13),make_pair(6,14),make_pair(5,15),make_pair(4,14),make_pair(3,13),make_pair(2,12),make_pair(2,10),make_pair(2,8),make_pair(2,6),make_pair(3,5),make_pair(4,4)},{make_pair(5,1),make_pair(6,2),make_pair(7,3),make_pair(8,4),make_pair(9,5),make_pair(9,7),make_pair(9,9),make_pair(9,11),make_pair(9,13),make_pair(8,14),make_pair(7,15),make_pair(6,16),make_pair(5,17),make_pair(4,16),make_pair(3,15),make_pair(2,14),make_pair(1,13),make_pair(1,11),make_pair(1,9),make_pair(1,7),make_pair(1,5),make_pair(2,4),make_pair(3,3),make_pair(4,2)},{null,make_pair(6,0),make_pair(7,1),make_pair(8,2),make_pair(9,3),null,make_pair(10,6),make_pair(10,8),make_pair(10,10),make_pair(10,12),null,make_pair(9,15),make_pair(8,16),make_pair(7,17),make_pair(6,18),null,make_pair(4,18),make_pair(3,17),make_pair(2,16),make_pair(1,15),null,make_pair(0,12),make_pair(0,10),make_pair(0,8),make_pair(0,6),null,make_pair(1,3),make_pair(2,2),make_pair(3,1),make_pair(4,0)}};

string get_board_move(string move){
  vector<string> results = {};
  boost::split(results, move, [](char c){return (c == ' ');});
  vector<string> finals = {};
  int turn = results.size();
  int i = 0;
  while(turn > 0){
    if(i % 3 == 0){
      finals.push_back(results[i]);
      i++;
      turn--;
    }
    else{
      pair<int,int> points = h_to_b[stoi(results[i])][stoi(results[i+1])];
      finals.push_back(to_string(points.first));
      finals.push_back(to_string(points.second));
      i += 2;
      turn -= 2;
    }
  }
//  string result = "";
//  for(int i = 0; i < finals.size(); i++){
    //cout << "results[i] " << results[i] << endl;
//    result = result + finals[i] + " ";
//  }
  string result;
  for (auto const& s : finals) { result = result + s + " "; };
  return result.substr(0, result.length() - 1);
}

string get_hex_move(string move){
  vector<string> results = {};
  boost::split(results, move, [](char c){return (c == ' ');});
  vector<string> finals = {};
  int turn = results.size();
  int i = 0;
  while(turn > 0){
    if(i % 3 == 0){
      finals.push_back(results[i]);
      i++;
      turn--;
    }
    else{
      pair<int,int> points = b_to_h[stoi(results[i])][stoi(results[i+1])];
      finals.push_back(to_string(points.first));
      finals.push_back(to_string(points.second));
      i += 2;
      turn -= 2;
    }
  }
  string result;
  for (auto const& s : finals) { result = result + s + " "; }
  return result.substr(0,result.length() -1);
}

void print(vector<vector<int>> board){
  for(int i = 0; i < 19; i++){
    for(int j = 0; j < 11; j++){
      if(board[j][i] == -100){
          cout <<"*" <<"\t";
      }
      else{
        cout << board[j][i] << "\t";
      }
    }
    cout << "\n";
  }
  cout <<"\n--------------------------------------------\n";
}

int main(){
  vector<vector<int>> board;
  vector<int> column;
  for(int i = 0; i < 11; i++){
    column = {};
    for(int j = 0; j < 19; j++){
      column.push_back(-100);
    }
    board.push_back(column);
  }

  for(auto i = h_to_b.begin(); i != h_to_b.end(); i++){
    for(auto j = (*i).begin(); j != (*i).end(); j++){
      if((*j) != null){
        board[(*j).first][(*j).second] = 0;
      }
    }
  }
//  print(board);
  int num_moves = 0;
  int flag = 0;
  int current_player, board_size, time_limit;
  string data;
  cin >> current_player >> board_size >> time_limit;
  cin.ignore();
    //boost::split(res, move, [](char c){return (c == ' ');});

  int player = current_player -1;
  string op_move;
  while(true){
    if((flag == 0) && (player == 0)){
      string my_move = "P 5 9\n" ;
      board = execute(board,my_move, 0);
  //    print(board);
      num_moves++;
      flag = 1;
      cout << "P 0 0" << endl;
    }
    else{
      getline(cin,op_move);
      //cout << "op_move " << op_move << endl;
      op_move = get_board_move(op_move);
      //cout << "op_move " << op_move << endl;
    // cout << "\n";
      board = execute(board,op_move,1);
    //  cout << "opponent" << endl;
    //  print(board);
      string my_move = minimax(num_moves,board);
    //  cout << "my" << endl;
      board = execute(board,my_move, 0);
      my_move = get_hex_move(my_move);
    //  print(board);
      num_moves += 1;
      cout << my_move << endl;
    }
  }
  return 0;
}


#include <iostream>
#include <vector>

#include <dlib/dnn.h>
#include <sampml/data.hpp>

#include "common.hpp"
#include "dnn.hpp"

void train() {
    std::cout << "TRAINING:\n";

    sampml::data::reader<sample_type> data_positive_train(positive_train_data);
    sampml::data::reader<sample_type> data_negative_train(negative_train_data);

    std::vector<sample_type> training_data;
    std::vector<unsigned long> training_labels;
    for(auto& v : data_positive_train) {
        training_data.push_back(v);
        training_labels.push_back(1);
    }

    for(auto& v : data_negative_train) {
        training_data.push_back(v);
        training_labels.push_back(0);
    }

    dlib::randomize_samples(training_data, training_labels);

    train_network_type net;
    dlib::dnn_trainer<train_network_type> trainer(net);
    trainer.set_learning_rate(0.01);
    trainer.set_min_learning_rate(0.00001);
    trainer.set_mini_batch_size(32);
    trainer.set_max_num_epochs(25000);
    //trainer.be_verbose();
    trainer.train(training_data, training_labels);
    std::cout << "average loss: " << trainer.get_average_loss() << "\n\n";
    net.clean();

    std::cout << net.subnet() << std::endl;
    std::cout << "params 1:\n" << dlib::mat(dlib::layer<1>(net).layer_details().get_layer_params())<< '\n';
    std::cout << "params 2:\n" << dlib::mat(dlib::layer<2>(net).layer_details().get_layer_params())<< '\n';
    std::cout << "params 3:\n" << dlib::mat(dlib::layer<3>(net).layer_details().get_layer_params())<< '\n';
    std::cout << "params 4:\n" << dlib::mat(dlib::layer<4>(net).layer_details().get_layer_params())<< '\n';
    std::cout << "params 5:\n" << dlib::mat(dlib::layer<5>(net).layer_details().get_layer_params())<< '\n';
    dlib::serialize(std::string(dnn_model.begin(), dnn_model.end())) << net.subnet();
}

void test() {
    const float cutoff = 0.5;

    std::cout << "TESTING:\n";

    sampml::data::reader<sample_type> data_positive_test(positive_test_data);
    sampml::data::reader<sample_type> data_negative_test(negative_test_data);

    test_network_type test_net;
    dlib::deserialize(std::string(dnn_model.begin(), dnn_model.end())) >> test_net.subnet();

    int true_positives = 0,
        false_positives = 0,
        true_negatives = 0,
        false_negatives = 0;
    dlib::running_stats<double> values;
    for(const auto& v : data_positive_test) {
        double prob = dlib::mat(test_net(v))(1);
        if(prob > cutoff)
            true_positives++;
        else
            false_negatives++;
        values.add(prob);
    }
    std::cout << "positive test set statistics: " << '\n';
    std::cout << "average: " << values.mean() << ", stddev: " << values.stddev() << '\n'
              << "min: " << values.min() << ", max: " << values.max()  << '\n'
              << "skewness: " << values.skewness() << ", excess kurtosis: " << values.ex_kurtosis() << '\n';
    values.clear();

    for(const auto& v : data_negative_test) {
        double prob = dlib::mat(test_net(v))(1);
        if(prob < cutoff)
            true_negatives++;
        else
            false_positives++;
        values.add(prob);
    }
    std::cout << "negative test set statistics: " << '\n';
    std::cout << "average: " << values.mean() << ", stddev: " << values.stddev() << '\n'
              << "min: " << values.min() << ", max: " << values.max()  << '\n'
              << "skewness: " << values.skewness() << ", excess kurtosis: " << values.ex_kurtosis() << '\n';

    int num_correct = true_positives + true_negatives,
        num_wrong = false_positives + false_negatives;
    std::cout << "\n\n";
    std::cout << "true positives: " << true_positives << ", false positives: " << false_positives << '\n';
    std::cout << "true negatives: " << true_negatives << ", false negatives: " << false_negatives << '\n';
    std::cout << "number of samples classified corretly: " << num_correct << '\n';
    std::cout << "number of samples classified incorrectly: " << num_wrong << '\n';
    std::cout << "accuracy: " << num_correct/float(num_correct + num_wrong) << '\n';
}

int main()
{
    train();
    test();
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "tudat/astro/basic_astro/unitConversions.h"
#include "tudat/basics/testMacros.h"

#include "tudat/astro/basic_astro/timeConversions.h"
#include "tudat/astro/ephemerides/simpleRotationalEphemeris.h"
//#include "tudat/interface/spice/spiceInterface.h"
//#include "tudat/io/basicInputOutput.h"
#include "tudat/astro/reference_frames/referenceFrameTransformations.h"

namespace tudat
{
namespace unit_tests
{

using unit_conversions::convertDegreesToRadians;
using ephemerides::SimpleRotationalEphemeris;
using basic_astrodynamics::JULIAN_DAY_ON_J2000;
using basic_astrodynamics::JULIAN_DAY_AT_0_MJD;
using reference_frames::getInertialToPlanetocentricFrameTransformationQuaternion;

BOOST_AUTO_TEST_SUITE( test_simple_rotational_ephemeris )

// Test simple rotational ephemeris class by compariing results to Venus-fixed frame with Spice.
BOOST_AUTO_TEST_CASE( testSimpleRotationalEphemeris )
{
    // Data from pck00010.tpc spice kernel.
    const double venusPoleRightAscension = convertDegreesToRadians( 272.76 );
    const double venusPoleDeclination = convertDegreesToRadians( 67.16 );
    const double venusPrimeMeridianAtJ2000 = convertDegreesToRadians( 160.20 );
    const double venusRotationRate = convertDegreesToRadians( -1.4813688 ) /
            physical_constants::JULIAN_DAY;

    // Define names of frames.
    const std::string baseFrame = "J2000";
    const std::string targetFrame = "IAU_VENUS";

    // Calculate initial rotation quaternion to frame.
    const Eigen::Quaterniond initialRotationToTargetFrame =
            getInertialToPlanetocentricFrameTransformationQuaternion(
                venusPoleDeclination, venusPoleRightAscension, venusPrimeMeridianAtJ2000 );

    // Create rotational ephemeris objects from (one for each type of constructor)
    SimpleRotationalEphemeris venusRotationalEphemerisFromAngles(
                venusPoleRightAscension, venusPoleDeclination, venusPrimeMeridianAtJ2000,
                venusRotationRate, 0.0, baseFrame, targetFrame );
    tudat::ephemerides::SimpleRotationalEphemeris venusRotationalEphemerisFromInitialState(
                initialRotationToTargetFrame,
                venusRotationRate, 0.0, baseFrame, targetFrame );

    // Test initial rotation matrix, both from the direct constructed object, and the one from
    // constituent angles.
    {
        // The following code block can be used to retrieve the benchmark data from Spice.
        /*
        loadSpiceKernelInTudat( getTudatRootPath( ) +
              "astro/ephemerides/pck00010.tpc" );
        Eigen::Matrix3d spiceInitialRotationToTargetFrameMatrix;
        Eigen::Quaterniond spiceInitialRotationToTargetFrame =
                computeRotationQuaternionBetweenFrames( baseFrame, targetFrame, 0.0 );
       */

        // Set initial rotation, as calculated with Spice (see above commented lines)
        Eigen::Matrix3d spiceInitialRotationToTargetFrameMatrix;
        spiceInitialRotationToTargetFrameMatrix
                << -0.9548214974296336, 0.2665104385944917, 0.1314841974018291,
                -0.296591573568662, -0.882413772579987, -0.3652114078848295,
                0.01869081416890202, -0.3877088083617989, 0.9215923900425705;

        // Check calculated with spice initial state.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( spiceInitialRotationToTargetFrameMatrix,
                                           Eigen::Matrix3d( initialRotationToTargetFrame ),
                                           1.0E-15 );

        // Check ephemeris calculations with Spice initial state.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    Eigen::Matrix3d(
                        venusRotationalEphemerisFromAngles.getRotationToTargetFrame( 0.0 ) ),
                    Eigen::Matrix3d( initialRotationToTargetFrame ),
                    1.0E-15 );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    Eigen::Matrix3d(
                        venusRotationalEphemerisFromInitialState.getRotationToTargetFrame( 0.0 ) ),
                    Eigen::Matrix3d( initialRotationToTargetFrame ),
                    1.0E-15 );
    }

    // Set time at which rotational ephemeris it to be called for subsequent tests.
    const double secondsSinceJ2000 = 1.0E6;

    // Test rotation to target frame at specified time.
    {
        /*
        spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "pck00010.tpc" );
        // The following code block can be used to retrieve the benchmark data from Spice.

        Eigen::Quaterniond spiceRotationMatrix =
               computeRotationQuaternionBetweenFrames( baseFrame, targetFrame, secondsSinceJ2000 );\
        spice_interface::computeRotationMatrixDerivativeBetweenFrames( baseFrame, targetFrame, secondsSinceJ2000 );

        */
        Eigen::Matrix3d spiceRotationMatrixDerivative;
        spiceRotationMatrixDerivative << 1.690407961416589e-07, 2.288121921543265e-07, 9.283170431475241e-08,
                -2.468632444964533e-07, 1.540516111965609e-07, 6.981529179974795e-08,
                0.0,           0.0,          0.0;

        // Set rotation at given time, as calculated with Spice (see above commented lines)
        Eigen::Matrix3d spiceRotationMatrix;
        spiceRotationMatrix << -0.8249537745726603, 0.5148010526833556, 0.2333048348715243,
                -0.5648910720519699, -0.7646317780963481, -0.3102197940834743,
                0.01869081416890206, -0.3877088083617987, 0.9215923900425707;

        // Calculate rotation to frame, and its time derivative, at certain time;
        Eigen::Quaterniond ephemerisRotation =
                venusRotationalEphemerisFromAngles.getRotationToTargetFrame(
                    secondsSinceJ2000 );
        Eigen::Matrix3d ephemerisRotationDerivative =
                venusRotationalEphemerisFromAngles.getDerivativeOfRotationToTargetFrame(
                    secondsSinceJ2000 );;

        // Check Spice result with ephemerides results.
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL(
                            Eigen::Matrix3d( ephemerisRotation )( i, j ) -
                            spiceRotationMatrix( i, j ), 2.0E-15 );
                BOOST_CHECK_SMALL(
                            spiceRotationMatrixDerivative( i, j ) -
                            ephemerisRotationDerivative( i, j ), 2.0E-22 );
            }

        }

        // Compare inverse rotation and its time derivative.
        Eigen::Quaterniond inverseEphemerisRotation =
                venusRotationalEphemerisFromInitialState.getRotationToBaseFrame(
                    secondsSinceJ2000 );
        Eigen::Matrix3d inverseEphemerisRotationDerivative =
                venusRotationalEphemerisFromAngles.getDerivativeOfRotationToBaseFrame(
                    secondsSinceJ2000 );

        Eigen::Matrix3d inverseSpiceRotationMatrix = spiceRotationMatrix.inverse( );
        Eigen::Matrix3d inverseSpiceRotationMatrixDerivative = spiceRotationMatrixDerivative.transpose( );

        // Check Spice result with ephemerides results.
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL(
                            Eigen::Matrix3d( inverseEphemerisRotation )( i, j ) -
                            inverseSpiceRotationMatrix( i, j ), 2.0E-15 );
                BOOST_CHECK_SMALL(
                            inverseEphemerisRotationDerivative( i, j ) -
                            inverseSpiceRotationMatrixDerivative( i, j ), 2.0E-22 );
            }

        }
    }

    // Test rotation matrix derivative by means of finite differences
    {
        double timeStep = 10.0;

        // Calculate rotations to frame at two times.
        Eigen::Quaterniond ephemerisRotation1 =
                venusRotationalEphemerisFromAngles.getRotationToTargetFrame(
                    secondsSinceJ2000 );
        Eigen::Quaterniond ephemerisRotation2 =
                venusRotationalEphemerisFromAngles.getRotationToTargetFrame(
                    secondsSinceJ2000 + timeStep );

        // Numerically calculate matrix derivative.
        Eigen::Matrix3d numericalEphemerisRotationDerivative =
                ( Eigen::Matrix3d( ephemerisRotation2 ) - Eigen::Matrix3d( ephemerisRotation1 ) ) /
                timeStep;

        // Calculate matrix derivative directly and compare.
        Eigen::Matrix3d ephemerisRotationDerivative =
                venusRotationalEphemerisFromAngles.getDerivativeOfRotationToTargetFrame(
                    secondsSinceJ2000 + timeStep / 2.0 );
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL(
                            numericalEphemerisRotationDerivative( i, j ) -
                            ephemerisRotationDerivative( i, j ), 2.0E-16 );
            }

        }

        // Calculate rotations from frame at two times;
        ephemerisRotation1 =
                venusRotationalEphemerisFromAngles.getRotationToBaseFrame(
                    secondsSinceJ2000 );
        ephemerisRotation2 =
                venusRotationalEphemerisFromAngles.getRotationToBaseFrame(
                    secondsSinceJ2000 + timeStep );

        // Numerically calculate matrix derivative.
        ephemerisRotationDerivative =
                venusRotationalEphemerisFromAngles.getDerivativeOfRotationToBaseFrame(
                    secondsSinceJ2000 + timeStep / 2.0 );

        // Calculate matrix derivative directly and compare.
        numericalEphemerisRotationDerivative =
                ( Eigen::Matrix3d( ephemerisRotation2 ) - Eigen::Matrix3d( ephemerisRotation1 ) ) /
                timeStep;
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL(
                            numericalEphemerisRotationDerivative( i, j ) -
                            ephemerisRotationDerivative( i, j ), 2.0E-16 );
            }

        }
    }

    // Test computation of rotational velocity vector
    {
        Eigen::Vector3d spiceRotationalVelocityVectorInBaseFrame;
        spiceRotationalVelocityVectorInBaseFrame << -5.593131603532092e-09, 1.160198999048488e-07, -2.75781861386115e-07;

        // Set rotation at given time, as calculated with Spice (see above commented lines)
        Eigen::Matrix3d spiceRotationMatrixToTargetFrame;
        spiceRotationMatrixToTargetFrame << -0.8249537745726603, 0.5148010526833556, 0.2333048348715243,
                -0.5648910720519699, -0.7646317780963481, -0.3102197940834743,
                0.01869081416890206, -0.3877088083617987, 0.9215923900425707;

        Eigen::Vector3d spiceRotationalVelocityVectorInTargetFrame =
                spiceRotationMatrixToTargetFrame * spiceRotationalVelocityVectorInBaseFrame;

        Eigen::Vector3d rotationalVelocityVectorInBaseFrame =
                venusRotationalEphemerisFromAngles.getRotationalVelocityVectorInBaseFrame( secondsSinceJ2000 );
        Eigen::Vector3d rotationalVelocityVectorInBaseFrame2 =
                venusRotationalEphemerisFromInitialState.getRotationalVelocityVectorInBaseFrame( secondsSinceJ2000 );

        Eigen::Vector3d rotationalVelocityVectorInTargetFrame =
                venusRotationalEphemerisFromAngles.getRotationalVelocityVectorInTargetFrame( secondsSinceJ2000 );
        Eigen::Vector3d rotationalVelocityVectorInTargetFrame2 =
                venusRotationalEphemerisFromInitialState.getRotationalVelocityVectorInTargetFrame( secondsSinceJ2000 );

        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL(
                        std::fabs( rotationalVelocityVectorInBaseFrame( i ) -
                                   spiceRotationalVelocityVectorInBaseFrame( i ) ), 1.0E-21 );
            BOOST_CHECK_SMALL(
                        std::fabs( rotationalVelocityVectorInBaseFrame2( i ) -
                                   spiceRotationalVelocityVectorInBaseFrame( i ) ), 1.0E-21 );

            BOOST_CHECK_SMALL(
                        std::fabs( rotationalVelocityVectorInTargetFrame( i ) -
                                   spiceRotationalVelocityVectorInTargetFrame( i ) ), 1.0E-21 );
            BOOST_CHECK_SMALL(
                        std::fabs( rotationalVelocityVectorInTargetFrame2( i ) -
                                   spiceRotationalVelocityVectorInTargetFrame( i ) ), 1.0E-21 );
        }
    }

    // Test rotation from target frame at specified time (is checked by checking if it is inverse
    // of opposite rotation).
    {
        // Test orthonormality of matrices from object created from initial angles.
        Eigen::Matrix3d productOfOppositeRotations =
                Eigen::Matrix3d( venusRotationalEphemerisFromAngles.getRotationToTargetFrame(
                                     secondsSinceJ2000 ) ) *
                Eigen::Matrix3d( venusRotationalEphemerisFromAngles.getRotationToBaseFrame(
                                     secondsSinceJ2000 ) );

        for ( int i = 0; i < 3; i++ )
        {
            for ( int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( productOfOppositeRotations( i, j ) -
                                   Eigen::Matrix3d::Identity( )( i, j ), 1.0E-15 );
            }
        }

        // Test orthonormality of matrices from object created from initial state.
        productOfOppositeRotations =
                Eigen::Matrix3d( venusRotationalEphemerisFromInitialState.getRotationToTargetFrame(
                                     secondsSinceJ2000 ) ) *
                Eigen::Matrix3d( venusRotationalEphemerisFromInitialState.getRotationToBaseFrame(
                                     secondsSinceJ2000 ) );

        for( int i = 0; i < 3; i++ )
        {
            for( int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( productOfOppositeRotations( i, j ) -
                                   Eigen::Matrix3d::Identity( )( i, j ), 1.0E-15 );
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#ifndef CORRELATION_HPP_INCLUDED
#define CORRELATION_HPP_INCLUDED

#include <functional>
#include <string>
#include <vector>
#include "vector.hpp"
#include <boost/numeric/ublas/matrix.hpp>

typedef std::function<double(const gen_vect&)> correlation_fn;
typedef boost::numeric::ublas::c_matrix<double, 3, 3> trafo_matrix_t;

correlation_fn makeGaussianCorrelation( double corrlength );
correlation_fn makeAnisotropicGaussianCorrelation( double corrlength, gen_vect ani );
correlation_fn makeSechCorrelation( double corrlength );
correlation_fn makePowerCorrelation( double corrlength, double alpha );
correlation_fn makeLuaCorrelation( double corrlength, std::string scriptfile, const std::vector<std::string>& vars );
correlation_fn makeTransformedCorrelation( correlation_fn original, trafo_matrix_t matrix );

/// creates a correlation function according to the specification in \p specs
correlation_fn makeCorrelation( const std::vector<std::string>& specs, double length, std::string trafo );

#endif // CORRELATION_HPP_INCLUDED


#ifndef BOOST_PREPROCESSOR_COMPARISON_NOT_EQUAL_HPP
#define BOOST_PREPROCESSOR_COMPARISON_NOT_EQUAL_HPP

/* Copyright (C) 2001
 * Housemarque Oy
 * http://www.housemarque.com
 *
 * Permission to copy, use, modify, sell and distribute this software is
 * granted provided this copyright notice appears in all copies. This
 * software is provided "as is" without express or implied warranty, and
 * with no claim as to its suitability for any purpose.
 *
 * See http://www.boost.org for most recent version.
 */

#include <boost/preprocessor/arithmetic/add.hpp>
#include <boost/preprocessor/arithmetic/sub.hpp>
#include <boost/preprocessor/logical/bool.hpp>

/**
Expands to 1 if X != Y and 0
otherwise.

Both X and Y must expand to integer literals
in the range [0, BOOST_PP_LIMIT_MAG].

For example, BOOST_PP_NOT_EQUAL(4,4) expands to 0.

Uses

BOOST_PP_WHILE()

Test

arithmetic_test.cpp

*/
#define BOOST_PP_NOT_EQUAL(X,Y) BOOST_PP_NOT_EQUAL_D(0,X,Y)

/**
Can be used inside BOOST_PP_WHILE().
 */
#define BOOST_PP_NOT_EQUAL_D(D,X,Y) BOOST_PP_BOOL(BOOST_PP_ADD_D(D,BOOST_PP_SUB_D(D,X,Y),BOOST_PP_SUB_D(D,Y,X)))

/*
Obsolete. Use BOOST_PP_NOT_EQUAL().
 */
#define BOOST_PREPROCESSOR_NOT_EQUAL(X,Y) BOOST_PP_NOT_EQUAL(X,Y)
#endif


#include "../include/cte_bits/bunch.hpp"
#include "../include/cte_bits/distributions.hpp"
#include "../include/cte_bits/physics.hpp"
#include "../include/cte_bits/radiation.hpp"
#include "../include/cte_bits/random.hpp"
#include "../include/cte_bits/synch.hpp"
#include "../include/cte_bits/utils.hpp"
#include <algorithm>
#include <boost/math/tools/roots.hpp>
#include <fstream>
#include <ibs>
#include <iterator>
#include <map>
#include <random>
#include <sstream>
#include <string>
#include <vector>

namespace cte_long {
/*
 *****************************************************************************
 *****************************************************************************
 * FUNCTOR TO RETURN VOLTAGE * CHARGE - U0 AND DERIVATIVE AS TUPLE
 * THIS IS USED AS INPUT FOR BOOST NEWTON RAPHSON ROOT SEARCH.
 *****************************************************************************
 * Authors:
 *  - Tom Mertens
 *
 * History:
 *  - 10/08/2021 : updated version from original ste code
 *****************************************************************************
 * Arguments:
 * ----------
 *  - T const &target
 *      Target value for the voltage (RF compensate for U0 so U0 in eV)
 *  - std::vector<double> &voltages
 *      Voltages of RF systems IMPORTANT: first element is for main RF
 *  - std::vector<double> &harmonicNumbers
 *      Harmonic numbers  IMPORTANT: first element is for main RF
 *  - double charge
 *      Particle charge
 *  - T const &phi
 *      phase in rad
 ******************************************************************************
 ******************************************************************************
 */
template <class T> struct synchronousPhaseFunctor {
  synchronousPhaseFunctor(T const &target, std::vector<double> &voltages,
                          std::vector<double> &harmonicNumbers, double charge)
      : U0(target), volts(voltages), hs(harmonicNumbers), ch(charge) {}
  std::tuple<double, double> operator()(T const &phi) {

    // init
    T vrf = ch * volts[0] * sin(phi);
    T dvrf = ch * volts[0] * cos(phi);

    // add the rest taking harmonic numbers into account
    for (int i = 1; i < hs.size(); i++) {
      vrf += ch * volts[i] * sin((hs[i] / hs[0]) * phi);
      dvrf += ch * volts[i] * (hs[i] / hs[0]) * cos((hs[i] / hs[0]) * phi);
    }

    std::tuple<double, double> out = {vrf - U0, dvrf};
    return out;
  }

private:
  T U0;
  std::vector<double> volts;
  std::vector<double> hs;
  double ch;
};

/*
================================================================================
================================================================================
BOOST NEWTON RAPHSON ROOT SEARCH FOR SYNCHRONOUS PHASE.
================================================================================

================================================================================
Arguments:

//phi is in rad
================================================================================
================================================================================
*/
template <class T>
T synchronousPhaseFunctorRoot(T x, std::vector<double> &voltages,
                              std::vector<double> &harmnumbers, double charge,
                              T guess, T min, T max) {
  // return cube root of x using 1st derivative and Newton_Raphson.
  using namespace boost::math::tools;

  const int digits =
      std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy
                                      // for type T.
  int get_digits = static_cast<int>(
      digits * 0.6); // Accuracy doubles with each step, so stop when we have
                     // just over half the digits correct.
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = newton_raphson_iterate(
      synchronousPhaseFunctor<T>(x, voltages, harmnumbers, charge), guess, min,
      max, get_digits, it);
  return result;
};

/********************************************************************************
 ********************************************************************************
 * CALCULATE TOTAL RF VOLTAGE FOR GIVEN PHASE (IN RAD)
 *
 ********************************************************************************
 */
double VoltageRf(double phi, std::vector<double> &volts,
                 std::vector<double> &hs) {
  double vrf = volts[0] * sin(phi);

  for (int i = 1; i < hs.size(); i++) {
    vrf += volts[i] * sin((hs[i] / hs[0]) * phi);
  }

  return vrf;
};

double VoltageRfPrime(double phi, double charge, std::vector<double> &volts,
                      std::vector<double> &hs) {
  // init
  double vrf = volts[0] * cos(phi);

  // add other rfs
  for (int i = 1; i < volts.size(); i++) {
    vrf += volts[i] * (hs[i] / hs[0]) * cos((hs[i] / hs[0]) * phi);
  }

  // V -> eV
  vrf *= charge;
  return vrf;
}

double SynchrotronTune(double omega0, double U0, double charge,
                       std::vector<double> &volts, std::vector<double> &hs,
                       double phis, double eta, double pc) {
  return sqrt(hs[0] * eta *
              fabs(charge * VoltageRfPrime(phis, charge, volts, hs)) /
              (2 * pi * pc * 1e9));
}

} // namespace cte_long

Bunch::Bunch(std::map<std::string, double> &twissheader,
             std::map<std::string, std::vector<double>> &twiss,
             std::map<std::string, double> &bparam, std::vector<double> &h,
             std::vector<double> &v) {
  twheader = twissheader;
  tw = twiss;
  bunchParam = bparam;

  setBasic();
  setLongitudinalParameters(h, v);
  setRadiationParameters();
  setDistribution(h, v);
};

void Bunch::setBasic() {
  // basic
  double gamma = twheader["GAMMA"];
  double gammatr = twheader["GAMMATR"];
  double p0 = twheader["PC"];
  double len = twheader["LENGTH"];
  double mass = twheader["MASS"];
  double charge = twheader["CHARGE"];
  // set Atomic Number in twiss header
  twheader["aatom"] = bunchParam["atomNumber"];
  twheader["sigs"] = bunchParam["sigs"];
  twheader["betar"] = BetaRelativisticFromGamma(gamma);
  twheader["trev"] = len / (twheader["betar"] * clight);
  twheader["frev"] = 1.0 / twheader["trev"];
  twheader["omega"] = 2.0 * pi * twheader["frev"];
  twheader["eta"] = eta(gamma, gammatr);
  twheader["timeratio"] = bunchParam["timeRatio"];
}

void Bunch::setLongitudinalParameters(std::vector<double> &h,
                                      std::vector<double> &v) {
  double gamma = twheader["GAMMA"];
  double gammatr = twheader["GAMMATR"];
  double angularf = twheader["trev"] * h[0] * twheader["omega"];

  // search synch phase parameters
  double search1 = angularf / (8.0 * *std::max_element(h.begin(), h.end()));
  double search2 = search1 + angularf / *std::min_element(h.begin(), h.end());
  double searchWidth = angularf / (2.0 * *std::max_element(h.begin(), h.end()));

  // bucket number determines phis
  search1 += bunchParam["bucket"] * 2.0 * pi;
  search2 += bunchParam["bucket"] * 2.0 * pi;

  // energy loss per turn
  double U0 = cte_radiation::RadiationLossesPerTurn(twheader);

  // synchronuous phases
  double phis = cte_long::synchronousPhaseFunctorRoot(
      U0, v, h, bunchParam["charge"], search1, search1 - searchWidth,
      search1 + searchWidth);
  double phis1 = cte_long::synchronousPhaseFunctorRoot(
      U0, v, h, bunchParam["charge"], search2, search2 - searchWidth,
      search2 + searchWidth);

  double phisNext = cte_long::synchronousPhaseFunctorRoot(
      U0, v, h, bunchParam["charge"], search1 + pi,
      search1 + pi - searchWidth / 2.0, search1 + pi + searchWidth / 2.0);
  double phis1Next = cte_long::synchronousPhaseFunctorRoot(
      U0, v, h, bunchParam["charge"], search2 + pi,
      search2 + pi - searchWidth / 2.0, search2 + pi + searchWidth / 2.0);

  // save
  longitudinalParameters["sigs"] = bunchParam["sigs"];
  longitudinalParameters["phis"] = phis;
  longitudinalParameters["phis1"] = phis1;
  longitudinalParameters["phisNext"] = phisNext;
  longitudinalParameters["phisNext1"] = phis1Next;

  longitudinalParameters["qs"] =
      CTESYNCH::SynchrotronTune(twheader, longitudinalParameters, v, h);
  longitudinalParameters["tauhat"] =
      abs((phisNext - phis) / (h[0] * twheader["omega"]));

  // add to twheader for easy passing around of the values (pre-existing code)
  // twheader["phis"] = phis;
  // twheader["phisNext"] = phisNext;
  // twheader["qs"] = longitudinalParameters["qs"];
  twheader["tauhat"] = longitudinalParameters["tauhat"];
  twheader["betxavg"] = twheader["LENGTH"] / (twheader["Q1"] * 2.0 * pi);
  twheader["betyavg"] = twheader["LENGTH"] / (twheader["Q2"] * 2.0 * pi);
  longitudinalParameters["sige"] =
      sigefromsigs(twheader["omega"], bunchParam["sigs"],
                   longitudinalParameters["qs"], gamma, gammatr);
  longitudinalParameters["delta"] =
      dee_to_dpp(twheader["sige"], twheader["betar"]);
}

void Bunch::setRadiationParameters() {
  // add energy loss per turn
  double U0 = cte_radiation::RadiationLossesPerTurn(twheader);
  // add the equilibrium values and radiation damping times
  radiationParameters =
      cte_radiation::radiationEquilib(twheader, longitudinalParameters);
  radiationParameters["U0"] = U0;
  cte_radiation::CalcRadDecayExcitation(twheader, radiationParameters);
};

void Bunch::setDistribution(std::vector<double> &h, std::vector<double> &v) {
  distribution = cte_distributions::GenerateDistributionMatched(
      bunchParam["nMacro"], twheader["betxavg"], bunchParam["ex"],
      twheader["betyavg"], bunchParam["ey"], h, v, twheader,
      longitudinalParameters, bunchParam["seed"]);
}

void Bunch::getEmittance() {
  std::vector<double> out, avg;
  std::vector<double> res;
  for (int i = 0; i < distribution[0].size(); i++) {
    out.push_back(0.0);
    avg.push_back(0.0);
  }

  // calculate averages
  std::for_each(distribution.begin(), distribution.end(),
                [&](std::vector<double> v) {
                  for (int i = 0; i < distribution[0].size(); i++) {
                    avg[i] += v[i] / distribution.size();
                  }
                });

  // subtract avg, square and sum
  std::for_each(
      distribution.begin(), distribution.end(), [&](std::vector<double> v) {
        for (int i = 0; i < distribution[0].size(); i++) {
          out[i] += ((v[i] - avg[i]) * (v[i] - avg[i])) / distribution.size();
        }
      });

  out[0] /= twheader["betxavg"];
  out[2] /= twheader["betyavg"];
  out[4] = sqrt(out[4]) * clight;
  out[5] = sqrt(out[5]);

  res.push_back(out[0]);
  res.push_back(out[2]);
  res.push_back(out[4]);
  res.push_back(out[5]);

  emittances.push_back(res);
}

void Bunch::getIBSGrowthRates(int model) {

  double pnumber = bunchParam["nReal"];
  std::vector<double> emit = emittances.back();
  double ex = emit[0];
  double ey = emit[1];
  double sigs = emit[2];
  double sige = emit[3];
  double aatom = twheader["aatom"];
  double r0 = ParticleRadius(twheader["CHARGE"], aatom);
  double *ibs;
  /*
    std::printf("%-30s %12.8e\n", "aatom", aatom);
    std::printf("%-30s %12.8e\n", "ex", ex);
    std::printf("%-30s %12.8e\n", "ey", ey);
    std::printf("%-30s %12.8e\n", "ro", r0);
    std::printf("%-30s %12.8e\n", "pnumber", pnumber);
  */
  // ibs growth rates update
  switch (model) {
  case 1:
    ibs = PiwinskiSmooth(pnumber, ex, ey, sigs, sige, twheader, r0);
    break;
  case 2:
    ibs = PiwinskiLattice(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 3:
    ibs =
        PiwinskiLatticeModified(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 4:
    ibs = Nagaitsev(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 5:
    ibs =
        Nagaitsevtailcut(pnumber, ex, ey, sigs, sige, twheader, tw, r0, aatom);
    break;
  case 6:
    ibs = ibsmadx(pnumber, ex, ey, sigs, sige, twheader, tw, r0, false);
    break;
  case 7:
    ibs = ibsmadxtailcut(pnumber, ex, ey, sigs, sige, twheader, tw, r0, aatom);
    break;
  case 8:
    ibs = BjorkenMtingwa2(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 9:
    ibs = BjorkenMtingwa(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 10:
    ibs = BjorkenMtingwatailcut(pnumber, ex, ey, sigs, sige, twheader, tw, r0,
                                aatom);
    break;
  case 11:
    ibs = ConteMartini(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  case 12:
    ibs = ConteMartinitailcut(pnumber, ex, ey, sigs, sige, twheader, tw, r0,
                              aatom);
    break;
  case 13:
    ibs = MadxIBS(pnumber, ex, ey, sigs, sige, twheader, tw, r0);
    break;
  }

  std::vector<double> ibsgr;
  for (int i = 0; i < 3; i++) {
    // std::printf("%-30s %12.8e\n", "ibs", ibs[i]);
    ibsgr.push_back(ibs[i] * 2.0 * bunchParam["fracibstot"] *
                    bunchParam["nMacro"] * bunchParam["timeRatio"] /
                    bunchParam["nReal"]);
  }
  ibsGrowthRates.push_back(ibsgr);
}

void Bunch::getIBSCoefficients() {
  double coeffs, coeffx, coeffy;
  double alphaAverage;
  double coeffMulT;

  std::vector<double> ibsgr = ibsGrowthRates.back();
  double alfax = ibsgr[1];
  double alfay = ibsgr[2];
  double alfap = ibsgr[0];
  /*
    std::printf("%-30s %12.8e\n", "alfax", alfax);
    std::printf("%-30s %12.8e\n", "alfay", alfay);
    std::printf("%-30s %12.8e\n", "alfap", alfap);
  */
  double dtsamp2 = 2.0 * longitudinalParameters["tauhat"] / bunchParam["nbins"];
  double rmsdelta = CalcRMS(distribution)[5];
  std::vector<double> emit = emittances.back();
  double sigs = emit[3];

  double rmsx = sqrt(emit[0] * twheader["betxavg"]);
  double rmsy = sqrt(emit[1] * twheader["betyavg"]);

  // debugging
  // cout << "alfap " << alfap << endl << endl;
  if (alfap > 0.0)
    coeffs = sqrt(6.0 * alfap * twheader["trev"]) * rmsdelta;
  else
    coeffs = 0.00;

  // coupling
  if (bunchParam["ibsCoupling"] == 0.0) {
    if (alfay > 0.0)
      coeffy = sqrt(6.0 * alfay * twheader["trev"]) * rmsy;
    else
      coeffy = 0.0;

    if (alfax > 0.0)
      coeffx = sqrt(6.0 * alfax * twheader["trev"]) * rmsx;
    else
      coeffx = 0.0;
  } else {
    // alphaAverage
    alphaAverage = 0.5 * (alfax + alfay);
    if (alphaAverage > 0.0) {
      coeffx = sqrt(6.0 * alphaAverage * twheader["trev"]) * rmsx;
      coeffy = sqrt(6.0 * alphaAverage * twheader["trev"]) * rmsy;
    } else {
      coeffx = 0.0;
      coeffy = 0.0;
    }
    // end if alphaAverage
  }
  // end if ibs coupling
  coeffMulT = sigs * 2.0 * sqrt(pi) / (bunchParam["nMacro"] * dtsamp2 * clight);

  std::vector<double> ibscoeffs;
  ibscoeffs.push_back(coeffx);
  ibscoeffs.push_back(coeffy);
  ibscoeffs.push_back(coeffs);
  ibscoeffs.push_back(coeffMulT);

  ibsCoeff.push_back(ibscoeffs);
}

void Bunch::updateIBS(std::vector<double> &h) {
  histogramTime = ParticlesTimesToHistogram(
      distribution, bunchParam["nbins"], longitudinalParameters["tauhat"] / 2.,
      longitudinalParameters["phisNext"] / (h[0] * twheader["omega"]));
  /*
    for (std::vector<int>::const_iterator i = histogramTime.begin();
         i != histogramTime.end(); ++i) {
      std::printf("%3i\n", *i);
    }
  */
  getIBSGrowthRates(bunchParam["model"]);
  getIBSCoefficients();
  /*
  std::for_each(ibsCoeff.begin(), ibsCoeff.end(),
                [](std::vector<double> &particle) {
                  std::printf("%12.8e %12.8e %12.8e \n", particle[0],
                              particle[1], particle[2]);
                });
*/
  sqrthistogram = HistogramToSQRTofCumul(histogramTime, ibsCoeff.back()[3]);
  // for (std::vector<double>::const_iterator i = sqrthistogram.begin();
  //   i != sqrthistogram.end(); ++i)
  //  std::printf("%4.2f ", *i);
  // std::printf("\n");
}
/*
************************************************************************************************************************
************************************************************************************************************************
*/
void Bunch::printBunchParameters() {
  std::printf("%-30s\n", "Bunch Parameters");
  std::printf("%-30s\n", "================");
  for (auto const &pair : bunchParam) {
    std::printf("%-30s %16.8e\n", pair.first.c_str(), pair.second);
  }
  std::printf("\n");
}

void Bunch::printTwissHeader() {
  std::printf("%-30s\n", "Twiss Header");
  std::printf("%-30s\n", "============");
  for (auto const &pair : twheader) {
    std::printf("%-30s %16.8e\n", pair.first.c_str(), pair.second);
  }
  std::printf("\n");
}

void Bunch::printLongParam() {
  std::printf("%-30s\n", "Longitudinal Parameters");
  std::printf("%-30s\n", "=======================");
  for (auto const &pair : longitudinalParameters) {
    std::printf("%-30s %16.8e\n", pair.first.c_str(), pair.second);
  }
  std::printf("\n");
}
void Bunch::printRadiationParameters() {
  std::printf("%-30s\n", "Radiation Parameters");
  std::printf("%-30s\n", "===================");
  for (auto const &pair : Bunch::radiationParameters) {
    std::printf("%-30s %16.8e\n", pair.first.c_str(), pair.second);
  }
  std::printf("\n");
}

void Bunch::printDistribution() {
  std::for_each(distribution.begin(), distribution.end(),
                [](std::vector<double> &particle) {
                  std::printf("%12.8e %12.8e %12.8e %12.8e %12.8e %12.8e\n",
                              particle[0], particle[1], particle[2],
                              particle[3], particle[4], particle[5]);
                });

  std::printf("\n");
};

void Bunch::printEmittance() {
  std::for_each(emittances.begin(), emittances.end(),
                [](std::vector<double> &particle) {
                  std::printf("%12.8e %12.8e %12.8e %12.8e\n", particle[0],
                              particle[1], particle[2], particle[3]);
                });

  std::printf("\n");
};

void Bunch::printIBSGrowthRates() {
  std::for_each(ibsGrowthRates.begin(), ibsGrowthRates.end(),
                [](std::vector<double> &particle) {
                  std::printf("%12.8e %12.8e %12.8e\n", particle[0],
                              particle[1], particle[2]);
                });

  std::printf("\n");
};

void Bunch::printDebunchLosses() {
  for (int i = 0; i < debunchLosses.size(); i++) {
    std::printf("%12i\n", debunchLosses[i]);
  };
}
#define MAX_DATE 12

std::string Bunch::get_date() {

  /* function returning current date as string for creating timestamped output
   * files */

  time_t now;

  // struct tm * timeinfo;
  char the_date[MAX_DATE];
  // char buffer [80];

  // time_t rawtime;
  // timeinfo = localtime (&rawtime);
  // strftime (buffer,80,"Now it's %I:%M%p.",timeinfo);

  // std::stringstream ss;
  // ss << fn << "_" << "%d_%m_%Y" << "_" << buffer << ".dat";
  // std::string s = ss.str();

  the_date[0] = '\0';

  now = time(NULL);

  if (now != -1) {
    strftime(the_date, MAX_DATE, "%d_%m_%Y", gmtime(&now));
  }

  return std::string(the_date);
}

template <typename T>
std::ostream &operator<<(std::ostream &os, const std::vector<T> &v) {
  for (int i = 0; i < v.size(); i++) {
    os << std::scientific << std::setw(15) << v[i];
  }
  // os << std::endl;
  // std::copy(v.begin(), v.end(), ostream_iterator<T>(os, "\t"));
  return os;
}

template <typename T>
std::ostream &operator<<(std::ostream &os,
                         const std::vector<std::vector<T>> &v) {
  using namespace std;

  // NOTE: for some reason std::copy doesn't work here, so I use manual loop
  // copy(v.begin(), v.end(), ostream_iterator<std::vector<T>>(os, "\n"));

  for (size_t i = 0; i < v.size(); ++i)
    os << std::setw(6) << i << v[i] << "\n";
  return os;
}

void Bunch::writeDistribution(int turn) {

  std::stringstream ss;
  ss << "Distribution_bucket_" << bunchParam["bucket"] << "_"
     << get_date()
     // << "_turn_" << std::setw(10) << std::setfill('0') << turn
     << ".dat";

  std::string s;
  s = ss.str();

  if (turn == 0) {
    if (remove(s.c_str()) != 0)
      perror("Error deleting file");
    else
      puts("File successfully deleted");
  }
  std::ofstream ofile;
  ofile.open(s.c_str(), std::ios_base::app);
  ofile << distribution;
  // std::copy(distribution.begin(), distribution.end(),
  //        std::ostream_iterator<std::vector<double>>(ofile));

  // std::cout << "writing vector size = " << distribution.size() << std::endl;
  ofile.close();
}

void Bunch::writeEmittances() {

  std::stringstream ss;
  ss << "CTE_Emittances_" << bunchParam["bucket"] << "_" << get_date()
     << ".dat";

  std::string s;
  s = ss.str();

  if (remove(s.c_str()) != 0)
    perror("Error deleting file");
  else
    puts("File successfully deleted");

  std::ofstream ofile(s.c_str());
  ofile << emittances;

  ofile.close();
}


#ifndef PYTHONIC_INCLUDE_NUMPY_COS_HPP
#define PYTHONIC_INCLUDE_NUMPY_COS_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/cos.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    double cos(long const &v);
    template <class T>
    auto cos(T const &v) -> decltype(boost::simd::cos(v));
  }
#define NUMPY_NARY_FUNC_NAME cos
#define NUMPY_NARY_FUNC_SYM wrapper::cos
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


/** $Id: CORSIKAGenerationProbability.cxx 165001 2018-08-27 09:22:30Z jvansanten $
 * @file
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 * $Revision: 165001 $
 * $Date: 2018-08-27 03:22:30 -0600 (Mon, 27 Aug 2018) $
 */

#include <MuonGun/I3MuonGun.h>
#include <MuonGun/CORSIKAGenerationProbability.h>

#include <dataclasses/physics/I3Particle.h>
#include <MuonGun/Flux.h>
#include <MuonGun/RadialDistribution.h>
#include <MuonGun/EnergyDistribution.h>

#include <boost/foreach.hpp>

namespace I3MuonGun {

CORSIKAGenerationProbability::CORSIKAGenerationProbability(SamplingSurfacePtr s, FluxPtr f, RadialDistributionPtr r, EnergyDistributionPtr e)
    : surface_(s), flux_(f), radialDistribution_(r), energyDistribution_(e)
{
	if (!surface_)
		log_fatal("No sampling surface defined!");
	if (!flux_)
		log_fatal("No flux defined!");
	if (!radialDistribution_)
		log_fatal("No radial distribution defined!");
	if (!energyDistribution_)
		log_fatal("No energy distribution defined!");
}

GenerationProbabilityPtr
CORSIKAGenerationProbability::Clone() const
{
	return boost::make_shared<CORSIKAGenerationProbability>(*this);
}

bool
CORSIKAGenerationProbability::IsCompatible(GenerationProbabilityConstPtr o) const
{
	boost::shared_ptr<const CORSIKAGenerationProbability> other
	    = boost::dynamic_pointer_cast<const CORSIKAGenerationProbability>(o);
	if (!other)
		return false;
	return (*surface_ == *(other->surface_) && *flux_ == *(other->flux_)
	    && *radialDistribution_ == *(other->radialDistribution_)
	    && *energyDistribution_ == *(other->energyDistribution_));
}

double
CORSIKAGenerationProbability::GetLogGenerationProbability(const I3Particle &axis, const BundleConfiguration &bundlespec) const
{
	std::pair<double, double> steps = surface_->GetIntersection(axis.GetPos(), axis.GetDir());
	// This shower axis doesn't intersect the sampling surface. Bail.
	if (!std::isfinite(steps.first))
		return 0.;

	double h = GetDepth(axis.GetPos().GetZ() + steps.first*axis.GetDir().GetZ());
	double coszen = cos(axis.GetDir().GetZenith());
	unsigned m = static_cast<unsigned>(bundlespec.size());
	double logprob = flux_->GetLog(h, coszen, m);
	BOOST_FOREACH(const BundleConfiguration::value_type &track, bundlespec)
		logprob += energyDistribution_->GetLog(h, coszen, m, track.radius,
		    EnergyDistribution::log_value(std::log(track.energy)));

	return logprob;
}

}


//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_BLOCK_CYPHERS_DETAIL_BASIC_FUNCTIONS_HPP
#define BOOST_HASH_BLOCK_CYPHERS_DETAIL_BASIC_FUNCTIONS_HPP

#include <boost/integer.hpp>
#include <boost/static_assert.hpp>

namespace boost {
namespace hashes {
namespace block_cyphers {
namespace detail {

template <unsigned word_bits_N>
struct basic_functions {
    static unsigned const word_bits = word_bits_N;
    typedef typename uint_t<word_bits>::exact word_type;

    static word_type SHR(word_type x, unsigned n) {
        return x >> n;
    }
    template <unsigned n>
    static word_type SHR(word_type x) {
        BOOST_STATIC_ASSERT(n < word_bits);
        return ((x) >> (n));
    }
    static word_type SHL(word_type x, unsigned n) {
        return x << n;
    }
    template <unsigned n>
    static word_type SHL(word_type x) {
        BOOST_STATIC_ASSERT(n < word_bits);
        return ((x) << (n));
    }

    static word_type ROTR(word_type x, unsigned n) {
        return SHR(x, n) | SHL(x, word_bits-n);
    }
    template <unsigned n>
    static word_type ROTR(word_type x) {
        return SHR<n>(x) | SHL<word_bits-n>(x);
    }
    static word_type ROTL(word_type x, unsigned n) {
        return SHL(x, n) | SHR(x, word_bits-n);
    }
    template <unsigned n>
    static word_type ROTL(word_type x) {
        return SHL<n>(x) | SHR<word_bits-n>(x);
    }
};

} // namespace detail
} // namespace block_cyphers
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_BLOCK_CYPHERS_DETAIL_BASIC_FUNCTIONS_HPP


#include <Eigen/Dense>
//#include <dart/collision/bullet/BulletCollisionDetector.hpp>
#include <dart/dynamics/Skeleton.hpp>
#include <dart/dynamics/DegreeOfFreedom.hpp>
#include <dart/dynamics/FreeJoint.hpp>
#include <dart/dynamics/WeldJoint.hpp>
#include <dart/dynamics/SphereShape.hpp>
#include <dart/simulation/World.hpp>
#include <dart/utils/urdf/DartLoader.hpp>
#include <dart/constraint/ConstraintSolver.hpp>
#include <dart/gui/osg/osg.hpp>
#include <dart/gui/osg/TrackballManipulator.hpp>

#include <Nomad/MessageTypes/sim_data_t.hpp>
#include <Nomad/MessageTypes/joint_control_cmd_t.hpp>
#include <condition_variable>
#include <vector>
#include "../include/NomadRobot.h"

// Third Party Includes
#include <zcm/zcm-cpp.hpp>

using namespace dart::dynamics;
using namespace dart::simulation;

WorldPtr g_world;
std::shared_ptr<NomadRobot> g_nomad;
Eigen::VectorXd joint_torques_cmd = Eigen::VectorXd::Zero(18);

std::mutex mtx;
std::condition_variable cv;

bool b_first = true;

class NomadSimWorldNode : public dart::gui::osg::RealTimeWorldNode
{

public:
  explicit NomadSimWorldNode(WorldPtr world, const std::shared_ptr<NomadRobot> nomad)
      : dart::gui::osg::RealTimeWorldNode(world), nomad_(nomad), world_(world)
  {
    // World gets saved as -> mWorld if ever needed.  Also ->getWorld()
    Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();

    // TODO: Pass IP Address as a parameter
    context_ = std::make_unique<zcm::ZCM>("udpm://239.255.76.67:7667?ttl=0");

    std::cout << "Nomad DART Sim connecting." << std::endl;

    context_->subscribe("nomad.sim.joint_cmd", &NomadSimWorldNode::OnJointControlMsg, this);

    // TODO: Publish state back
    pub_context_ = std::make_unique<zcm::ZCM>("udpm://239.255.76.67:7667?ttl=0");

    sequence_num_ = 0;

    step_iter = 0;
  }

  void ReceiveCommand()
  {
    context_->handle();
  }

  void customPreStep()
  {
    if(step_iter < 10)
    {
      return;
    }
    if(b_first)
    {
      PublishState();
      b_first = false;
    }

    ReceiveCommand();

   nomad_->Skeleton()->setForces(joint_torques_cmd);
  }

  void customPostStep()
  {
    PublishState();

    //joint_torques = Eigen::VectorXd::Zero(18);
    step_iter++;
  }

  void OnJointControlMsg(const zcm::ReceiveBuffer *rbuf, const std::string &chan, const joint_control_cmd_t *msg)
  {
    // Read Input

    Eigen::VectorXd tau_cmd(12);

    tau_cmd[0] = msg->tau_ff[0];
    tau_cmd[1] = msg->tau_ff[1];
    tau_cmd[2] = msg->tau_ff[2];
    tau_cmd[3] = msg->tau_ff[3];
    tau_cmd[4] = msg->tau_ff[4];
    tau_cmd[5] = msg->tau_ff[5];
    tau_cmd[6] = msg->tau_ff[6];
    tau_cmd[7] = msg->tau_ff[7];
    tau_cmd[8] = msg->tau_ff[8];
    tau_cmd[9] = msg->tau_ff[9];
    tau_cmd[10] = msg->tau_ff[10];
    tau_cmd[11] = msg->tau_ff[11];

    joint_torques_cmd.tail(12) = tau_cmd;
  }

  void PublishState()
  {
    sim_data_t sim_data;

    uint64_t time_now = sequence_num_;

    // Get Relevant State Data
    Eigen::Quaterniond body_orientation = nomad_->GetBaseOrientation();
    Eigen::Vector3d body_pos = nomad_->GetBasePosition(); // World
    Eigen::Vector3d accel_body = nomad_->GetBaseLinearAcceleration(); // Body
    Eigen::Vector3d angular_body = nomad_->GetBaseAngularVelocity(); // Body
    Eigen::Vector3d vel_body = nomad_->GetBaseLinearVelocity(); // Body

    // Cheater Orientation
    sim_data.timestamp = time_now;
    sim_data.sequence_num = sequence_num_;

    // Copy Body World Position
    Eigen::Map<Eigen::VectorXd>(sim_data.com_pos, 3) = body_pos;

    // Copy Body World Velocity
    Eigen::Map<Eigen::VectorXd>(sim_data.com_vel, 3) = body_orientation.toRotationMatrix() * vel_body;

    // Copy Body Orientation
    sim_data.com_orientation[0] = body_orientation.x();
    sim_data.com_orientation[1] = body_orientation.y();
    sim_data.com_orientation[2] = body_orientation.z();
    sim_data.com_orientation[3] = body_orientation.w();

    // Copy Body Angular Velocity (World)
    Eigen::Map<Eigen::VectorXd>(sim_data.com_omega, 3) = body_orientation.toRotationMatrix() * angular_body;

    // IMU Orientation
    // Copy Body Orientation
    sim_data.imu_orientation[0] = body_orientation.x();
    sim_data.imu_orientation[1] = body_orientation.y();
    sim_data.imu_orientation[2] = body_orientation.z();
    sim_data.imu_orientation[3] = body_orientation.w();

    // IMU Linear Acceleration
    // Accelerometer (Local)
    Eigen::Map<Eigen::VectorXd>(sim_data.imu_accel, 3) = accel_body;

    // Gyro
    // Copy Body Angular Velocity (Local)
    Eigen::Map<Eigen::VectorXd>(sim_data.imu_omega, 3) = angular_body;

    // Copy Joint State "Estimates"
    Eigen::VectorXd joint_pos = nomad_->Skeleton()->getPositions();
    Eigen::VectorXd joint_vel = nomad_->Skeleton()->getVelocities();
    Eigen::VectorXd joint_tau = nomad_->Skeleton()->getForces();

    sim_data.q[0] = joint_pos[6];
    sim_data.q[1] = joint_pos[7];
    sim_data.q[2] = joint_pos[8];
    sim_data.q[3] = joint_pos[9];
    sim_data.q[4] = joint_pos[10];
    sim_data.q[5] = joint_pos[11];
    sim_data.q[6] = joint_pos[12];
    sim_data.q[7] = joint_pos[13];
    sim_data.q[8] = joint_pos[14];
    sim_data.q[9] = joint_pos[15];
    sim_data.q[10] = joint_pos[16];
    sim_data.q[11] = joint_pos[17];

    sim_data.q_dot[0] = joint_vel[6];
    sim_data.q_dot[1] = joint_vel[7];
    sim_data.q_dot[2] = joint_vel[8];
    sim_data.q_dot[3] = joint_vel[9];
    sim_data.q_dot[4] = joint_vel[10];
    sim_data.q_dot[5] = joint_vel[11];
    sim_data.q_dot[6] = joint_vel[12];
    sim_data.q_dot[7] = joint_vel[13];
    sim_data.q_dot[8] = joint_vel[14];
    sim_data.q_dot[9] = joint_vel[15];
    sim_data.q_dot[10] = joint_vel[16];
    sim_data.q_dot[11] = joint_vel[17];

    sim_data.tau[0] = joint_tau[6];
    sim_data.tau[1] = joint_tau[7];
    sim_data.tau[2] = joint_tau[8];
    sim_data.tau[3] = joint_tau[9];
    sim_data.tau[4] = joint_tau[10];
    sim_data.tau[5] = joint_tau[11];
    sim_data.tau[6] = joint_tau[12];
    sim_data.tau[7] = joint_tau[13];
    sim_data.tau[8] = joint_tau[14];
    sim_data.tau[9] = joint_tau[15];
    sim_data.tau[10] = joint_tau[16];
    sim_data.tau[11] = joint_tau[17];

    sequence_num_++;

    // Publish
    int rc = pub_context_->publish("nomad.sim.data", &sim_data);
    //pub_context_->flush();
  }

protected:
  std::shared_ptr<NomadRobot> nomad_;
  WorldPtr world_;

  std::unique_ptr<zcm::ZCM> context_;

  std::unique_ptr<zcm::ZCM> pub_context_;

  int step_iter;

  uint64_t sequence_num_;

};

SkeletonPtr CreateGround()
{
  SkeletonPtr ground = Skeleton::create("ground");

  // Give the ground a body
  BodyNodePtr body = ground->createJointAndBodyNodePair<WeldJoint>(nullptr).second;
  double thickness = 0.1;

  ShapePtr groundShape = std::make_shared<BoxShape>(Eigen::Vector3d(4, 4, thickness));
  auto shapeNode = body->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(groundShape);

  shapeNode->getVisualAspect()->setColor(dart::Color::LightGray());

  Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
  tf.translation() = Eigen::Vector3d(0, 0, -thickness / 2.0);
  body->getParentJoint()->setTransformFromParentBodyNode(tf);

  return ground;
}

int main(int argc, char *argv[])
{

  // Create dart simulation world
  g_world = World::create();

  // Update Gravity vector to Z down
  Eigen::Vector3d gravity(0.0, 0.0, -9.81);
  //Eigen::Vector3d gravity(0.0, 0.0, 0.0);
  g_world->setGravity(gravity);

  // Create and add ground to world
  SkeletonPtr ground = CreateGround();
  g_world->addSkeleton(ground);

  // Update Collision
  //g_world->getConstraintSolver()->setCollisionDetector(dart::collision::BulletCollisionDetector::create());

  g_nomad = std::make_shared<NomadRobot>(g_world);

  std::string urdf = std::getenv("NOMAD_RESOURCE_PATH");
  urdf.append("/Robot_V2/NOMAD.urdf");

  g_nomad->LoadFromURDF(urdf);
  g_nomad->SetInitialPose();

  // Create osg world node
  ::osg::ref_ptr<NomadSimWorldNode> node = new NomadSimWorldNode(g_world, g_nomad);

  // Create osg Viewer
  dart::gui::osg::Viewer viewer = dart::gui::osg::Viewer();
  viewer.addWorldNode(node);

  // Default with Headlights
  viewer.switchHeadlights(false);

  //Add Grid Attachment
  ::osg::ref_ptr<dart::gui::osg::GridVisual> grid = new dart::gui::osg::GridVisual();

  viewer.addAttachment(grid);

  viewer.addAttachment(
      new dart::gui::osg::SupportPolygonVisual(g_nomad->Skeleton(), 0.025));

  // Print out instructions for the viewer
  std::cout << viewer.getInstructions() << std::endl;

  viewer.setUpViewInWindow(0, 0, 1280, 1024);
  viewer.getCameraManipulator()->setHomePosition(::osg::Vec3(0.0f, -5.0f, 0.0f),
                                                 ::osg::Vec3(0.0f, 0.0f, 0.0f),
                                                 ::osg::Vec3(0.0f, 0.707f, 0.707f), false);

  viewer.setCameraManipulator(viewer.getCameraManipulator());
  viewer.simulate(true);
  viewer.run();

  return 0;
}