/* Noah Himed
 *
 * Implement the Board type.
 *
 * Licensed under MIT License. Terms and conditions enclosed in "LICENSE.txt".
 */

#include "board.h"

#include <boost/multiprecision/cpp_int.hpp>
#include <cctype>
#include <chrono>
#include <cstdint>
#include <random>
#include <stdexcept>
#include <string>
#include <unordered_map>

#include "bad_move.h"
#include "move.h"

namespace omegazero {

using std::invalid_argument;
using std::string;

using std::cout;
using std::endl;

typedef boost::multiprecision::uint128_t U128;

Board::Board(const string& init_pos) {
  for (S8 piece_type = kPawn; piece_type <= kKing; ++piece_type) {
    pieces_[piece_type] = 0ULL;
  }
  for (S8 player = kWhite; player <= kBlack; ++player) {
    player_pieces_[player] = 0ULL;
    // Initialize all castling rights to false before parsing the FEN string to
    // set the board, which may reset some castling rights to true.
    for (S8 board_side = kQueenSide; board_side <= kKingSide; ++board_side) {
      castling_rights_[player][board_side] = false;
    }
  }
  ep_target_sq_ = kNA;

  // Set the piece positions, castling rights, and player to move.
  InitBoardPos(init_pos);
  InitBoardHash();
}

auto Board::GetAttackMap(S8 attacking_player, S8 sq, S8 attacking_piece) const
    -> Bitboard {
  if (!SqOnBoard(sq)) {
    throw invalid_argument("sq in Board::GetAttackMap()");
  }

  Bitboard attack_map = 0X0;
  S8 attacked_player = GetOtherPlayer(attacking_player);
  switch (attacking_piece) {
    case kPawn: {
      Bitboard capture_attacks;
      Bitboard push_attacks;
      if (attacking_player == kWhite) {
        capture_attacks = kNonSliderAttackMaps[kWhitePawnCapture][sq];
        push_attacks = kNonSliderAttackMaps[kWhitePawnPush][sq];
      } else {
        capture_attacks = kNonSliderAttackMaps[kBlackPawnCapture][sq];
        push_attacks = kNonSliderAttackMaps[kBlackPawnPush][sq];
      }
      // Include captures that attack occupied squares and push moves that move
      // onto empty squares only. Note that the resulting attack map may include
      // squares in front of the pawn occupied by other pieces. This is
      // necessary due to how the function is used by other parts of the engine.
      attack_map =
          (capture_attacks & player_pieces_[attacked_player]) | push_attacks;
      break;
    }
    case kKnight:
      attack_map = kNonSliderAttackMaps[kKnightAttack][sq];
      break;
    // Use the magic bitboard method to get possible moves for bishops and
    // rooks. The Boost library's 128 bit unsigned int data type "U128"
    // is used here to avoid integer overflow.
    case kBishop: {
      Bitboard all_pieces = player_pieces_[kWhite] | player_pieces_[kBlack];
      Bitboard blockers = kSliderPieceMaps[kBishopMoves][sq] & all_pieces;
      if (blockers == 0X0) {
        attack_map = kUnblockedSliderAttackMaps[kBishopMoves][sq];
      } else {
        U128 magic = kMagics[kBishopMoves][sq];
        U128 index = (blockers * magic) >> (kNumSq - kBishopMagicLengths[sq]);
        U64 index_U64 = static_cast<U64>(index);
        attack_map = kMagicIndexToAttackMap.at(index_U64);
      }
      break;
    }
    case kRook: {
      Bitboard all_pieces = player_pieces_[kWhite] | player_pieces_[kBlack];
      Bitboard blockers = kSliderPieceMaps[kRookMoves][sq] & all_pieces;
      if (blockers == 0X0) {
        attack_map = kUnblockedSliderAttackMaps[kRookMoves][sq];
      } else {
        U128 magic = kMagics[kRookMoves][sq];
        U128 index = (blockers * magic) >> (kNumSq - kRookMagicLengths[sq]);
        U64 index_U64 = static_cast<U64>(index);
        attack_map = kMagicIndexToAttackMap.at(index_U64);
      }
      break;
    }
    // Combine the attack maps of a rook and bishop to get a queen's attack.
    case kQueen: {
      Bitboard bishop_attack = GetAttackMap(attacking_player, sq, kBishop);
      Bitboard rook_attack = GetAttackMap(attacking_player, sq, kRook);
      return bishop_attack | rook_attack;
    }
    case kKing:
      attack_map = kNonSliderAttackMaps[kKingAttack][sq];
      break;
    default:
      throw invalid_argument("attacking_piece in Board::GetAttackMap()");
  }

  return attack_map;
}

auto Board::GetPiecesByType(S8 piece_type, S8 player) const -> Bitboard {
  if (piece_type == kNA) {
    if (player == kWhite || player == kBlack) {
      return player_pieces_[player];
    }
    throw invalid_argument("player in Board::GetPiecesByType()");
  }
  if (player == kNA) {
    if (piece_type >= kPawn && piece_type <= kKing) {
      return pieces_[piece_type];
    }
    throw invalid_argument("piece_type in Board::GetPiecesByType()");
  }
  if ((piece_type >= kPawn && piece_type <= kKing) &&
      (player == kWhite || player == kBlack)) {
    return pieces_[piece_type] & player_pieces_[player];
  }

  throw invalid_argument("player, piece_type in Board::GetPiecesByType");
}

auto Board::CastlingLegal(S8 board_side) const -> bool {
  // For castling moves, check that the following hold:
  //   * Neither the king nor the chosen rook has previously moved.
  //   * There are no pieces between the king and the chosen rook.
  //   * The king is not currently in check.
  //   * The king does not pass through a square that is attacked by an enemy
  //     piece.
  if (board_side == kQueenSide) {
    return castling_rights_[player_to_move_][kQueenSide] && !KingInCheck() &&
           ((player_to_move_ == kWhite && piece_layout_[kSqB1] == kNA &&
             piece_layout_[kSqC1] == kNA && piece_layout_[kSqD1] == kNA &&
             GetAttackersToSq(kSqD1, kWhite) == 0X0) ||
            (player_to_move_ == kBlack && GetPieceOnSq(kSqB8) == kNA &&
             piece_layout_[kSqC8] == kNA && piece_layout_[kSqD8] == kNA &&
             GetAttackersToSq(kSqD8, kBlack) == 0X0));
  }
  if (board_side == kKingSide) {
    return castling_rights_[player_to_move_][kKingSide] && !KingInCheck() &&
           ((player_to_move_ == kWhite && piece_layout_[kSqF1] == kNA &&
             GetAttackersToSq(kSqF1, kWhite) == 0X0 &&
             piece_layout_[kSqG1] == kNA) ||
            (player_to_move_ == kBlack && GetPieceOnSq(kSqF8) == kNA &&
             GetAttackersToSq(kSqF8, kBlack) == 0X0 &&
             piece_layout_[kSqG8] == kNA));
  }

  throw invalid_argument("board_side in Board::CastlingLegal()");
}

auto Board::DoublePawnPushLegal(S8 file) const -> bool {
  if (!FileOnBoard(file)) {
    throw invalid_argument("file in Board::DoublePawnPushLegal()");
  }

  if (player_to_move_ == kWhite) {
    S8 rank3_double_pawn_push_sq = GetSqFromRankFile(kRank3, file);
    S8 rank2_double_pawn_push_sq = GetSqFromRankFile(kRank2, file);
    return piece_layout_[rank3_double_pawn_push_sq] == kNA &&
           piece_layout_[rank2_double_pawn_push_sq] == kPawn &&
           player_layout_[rank2_double_pawn_push_sq] == kWhite;
  }
  // Handle the case of evaluating if a double pawn push from black is legal.
  S8 rank6_double_pawn_push_sq = GetSqFromRankFile(kRank6, file);
  S8 rank7_double_pawn_push_sq = GetSqFromRankFile(kRank7, file);
  return piece_layout_[rank6_double_pawn_push_sq] == kNA &&
         piece_layout_[rank7_double_pawn_push_sq] == kPawn &&
         player_layout_[rank7_double_pawn_push_sq] == kBlack;
}

auto Board::Evaluate() const -> int {
  Bitboard white_pieces;
  Bitboard black_pieces;
  int board_score = 0;
  for (S8 piece_type = kPawn; piece_type <= kKing; ++piece_type) {
    white_pieces = pieces_[piece_type] & player_pieces_[kWhite];
    black_pieces = pieces_[piece_type] & player_pieces_[kBlack];
    board_score += kPieceVals[piece_type] *
                   (GetNumSetSq(white_pieces) - GetNumSetSq(black_pieces));
  }
  S8 moving_side = (player_to_move_ == kWhite) ? 1 : -1;
  return board_score * moving_side;
}

auto Board::MakeMove(const Move& move) -> void {
  if (move.castling_type == kNA) {
    MakeNonCastlingMove(move);
  } else if (move.castling_type == kQueenSide) {
    // Make queenside castling moves.
    if (player_to_move_ == kWhite) {
      MovePiece(kRook, kSqA1, kSqD1);
      MovePiece(kKing, kSqE1, kSqC1);
    } else if (player_to_move_ == kBlack) {
      MovePiece(kRook, kSqA8, kSqD8);
      MovePiece(kKing, kSqE8, kSqC8);
    }
  } else if (move.castling_type == kKingSide) {
    // Make kingside castling moves.
    if (player_to_move_ == kWhite) {
      MovePiece(kRook, kSqH1, kSqF1);
      MovePiece(kKing, kSqE1, kSqG1);
    } else if (player_to_move_ == kBlack) {
      MovePiece(kRook, kSqH8, kSqF8);
      MovePiece(kKing, kSqE8, kSqG8);
    }
  }

  // Update the en passent target square and the board hash to reflect a change
  // in the file of the en passent target square.
  ep_target_sq_history_.push(ep_target_sq_);
  S8 prev_ep_target_file =
      (ep_target_sq_ == kNA) ? kNA : GetFileFromSq(ep_target_sq_);
  ep_target_sq_ = move.new_ep_target_sq;
  S8 curr_ep_target_file =
      (ep_target_sq_ == kNA) ? kNA : GetFileFromSq(ep_target_sq_);
  if (prev_ep_target_file != curr_ep_target_file) {
    if (prev_ep_target_file != kNA) {
      board_hash_ ^= ep_file_rand_nums_[prev_ep_target_file];
    }
    if (curr_ep_target_file != kNA) {
      board_hash_ ^= ep_file_rand_nums_[curr_ep_target_file];
    }
  }

  // Reset the halfmove clock if a pawn was moved or if a move resulted in a
  // capture.
  halfmove_clock_history_.push(halfmove_clock_);
  if (move.captured_piece != kNA || move.moving_piece == kPawn) {
    halfmove_clock_ = 0;
  } else {
    ++halfmove_clock_;
  }

  UpdateCastlingRights(move);

  // Undo the move if it puts the king in check.
  if (KingInCheck()) {
    SwitchPlayer();
    UnmakeMove(move);
    throw BadMove("move leaves king in check");
  }

  SwitchPlayer();
  // Update the board hash to reflect player turnover.
  board_hash_ ^= black_to_move_rand_num_;
}

// Assume the passed move has been made use MakeMove(). Calling UnmakeMove()
// on a move that wasn't already made will result in undefined behavior.
auto Board::UnmakeMove(const Move& move) -> void {
  // Revert back to the previous player.
  SwitchPlayer();
  // Update the board hash to reflect player turnover.
  board_hash_ ^= black_to_move_rand_num_;

  if (move.castling_type == kNA) {
    // Undo all non-castling moves.
    UnmakeNonCastlingMove(move);
  } else if (move.castling_type == kQueenSide) {
    // Undo queenside castling moves.
    if (player_to_move_ == kWhite) {
      MovePiece(kRook, kSqD1, kSqA1);
      MovePiece(kKing, kSqC1, kSqE1);
    } else if (player_to_move_ == kBlack) {
      MovePiece(kRook, kSqD8, kSqA8);
      MovePiece(kKing, kSqC8, kSqE8);
    }
  } else if (move.castling_type == kKingSide) {
    // Undo kingside castling moves.
    if (player_to_move_ == kWhite) {
      MovePiece(kRook, kSqF1, kSqH1);
      MovePiece(kKing, kSqG1, kSqE1);
    } else if (player_to_move_ == kBlack) {
      MovePiece(kRook, kSqF8, kSqH8);
      MovePiece(kKing, kSqG8, kSqE8);
    }
  }

  // Revert the halfmove clock.
  halfmove_clock_ = halfmove_clock_history_.top();
  halfmove_clock_history_.pop();

  // Revert the en passent target square and update the board hash.
  if (ep_target_sq_ != kNA) {
    S8 ep_file = GetFileFromSq(ep_target_sq_);
    board_hash_ ^= ep_file_rand_nums_[ep_file];
  }
  ep_target_sq_ = ep_target_sq_history_.top();
  ep_target_sq_history_.pop();
  if (ep_target_sq_ != kNA) {
    S8 ep_file = GetFileFromSq(ep_target_sq_);
    board_hash_ ^= ep_file_rand_nums_[ep_file];
  }

  // Revert all castling rights and update the board hash.
  if (castling_rights_[kWhite][kQueenSide] !=
      white_queenside_castling_rights_history_.top()) {
    board_hash_ ^= castling_rights_rand_nums_[kWhite][kQueenSide];
    castling_rights_[kWhite][kQueenSide] =
        white_queenside_castling_rights_history_.top();
  }
  white_queenside_castling_rights_history_.pop();
  if (castling_rights_[kWhite][kKingSide] !=
      white_kingside_castling_rights_history_.top()) {
    board_hash_ ^= castling_rights_rand_nums_[kWhite][kKingSide];
    castling_rights_[kWhite][kKingSide] =
        white_kingside_castling_rights_history_.top();
  }
  white_kingside_castling_rights_history_.pop();
  if (castling_rights_[kBlack][kQueenSide] !=
      black_queenside_castling_rights_history_.top()) {
    board_hash_ ^= castling_rights_rand_nums_[kBlack][kQueenSide];
    castling_rights_[kBlack][kQueenSide] =
        black_queenside_castling_rights_history_.top();
  }
  black_queenside_castling_rights_history_.pop();
  if (castling_rights_[kBlack][kKingSide] !=
      black_kingside_castling_rights_history_.top()) {
    board_hash_ ^= castling_rights_rand_nums_[kBlack][kKingSide];
    castling_rights_[kBlack][kKingSide] =
        black_kingside_castling_rights_history_.top();
  }
  black_kingside_castling_rights_history_.pop();
}

// Implemement private member functions.

auto Board::GetAttackersToSq(S8 sq, S8 attacked_player) const -> Bitboard {
  if (!SqOnBoard(sq)) {
    throw invalid_argument("sq in Board::GetAttackersToSq()");
  }

  Bitboard potential_pawn_attacks;
  S8 attacking_player = GetOtherPlayer(attacked_player);
  // Capture only diagonal squares to sq in the direction of movement.
  if (attacked_player == kWhite) {
    potential_pawn_attacks = kNonSliderAttackMaps[kWhitePawnCapture][sq];
  } else {
    potential_pawn_attacks = kNonSliderAttackMaps[kBlackPawnCapture][sq];
  }
  // Compute the union (bitwise OR) of all pieces of each type that could
  // capture the square "sq"; each of these bitboards are computed by
  // finding the intersection (bitwise AND) between all spots a piece of a
  // given type could move to from sq, and all the positions that pieces of
  // this type from the opposing player are located.
  return (potential_pawn_attacks & GetPiecesByType(kPawn, attacking_player)) |
         (GetAttackMap(attacked_player, sq, kKnight) &
          GetPiecesByType(kKnight, attacking_player)) |
         (GetAttackMap(attacked_player, sq, kBishop) &
          GetPiecesByType(kBishop, attacking_player)) |
         (GetAttackMap(attacked_player, sq, kRook) &
          GetPiecesByType(kRook, attacking_player)) |
         (GetAttackMap(attacked_player, sq, kQueen) &
          GetPiecesByType(kQueen, attacking_player)) |
         (GetAttackMap(attacked_player, sq, kKing) &
          GetPiecesByType(kKing, attacking_player));
}

auto Board::AddPiece(S8 piece_type, S8 player, S8 sq) -> void {
  if (!SqOnBoard(sq)) {
    throw invalid_argument("sq in Board::AddPiece()");
  }

  if (piece_type == kNA && player == kNA) {
    piece_layout_[sq] = kNA;
    player_layout_[sq] = kNA;
  } else if (piece_type >= kPawn && piece_type <= kKing &&
             (player == kWhite || player == kBlack)) {
    Bitboard piece_mask = 1ULL << sq;
    pieces_[piece_type] |= piece_mask;
    player_pieces_[player] |= piece_mask;
    piece_layout_[sq] = piece_type;
    player_layout_[sq] = player;
  } else {
    throw invalid_argument("piece_type, player in Board::AddPiece()");
  }
}

auto Board::InitBoardHash() -> void {
  board_hash_ = 0X0;
  // Initialize the Mersenne Twister 64 bit pseudo-random number generator.
  U64 seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::mt19937_64 rand_num_gen(seed);
  // Generate a set of random numbers for Zobrist Hashing.
  for (S8 player = kWhite; player < kNumPlayers; ++player) {
    for (S8 board_side = kQueenSide; board_side <= kKingSide; ++board_side) {
      castling_rights_rand_nums_[player][board_side] = rand_num_gen();
      if (castling_rights_[player][board_side]) {
        board_hash_ ^= castling_rights_rand_nums_[player][board_side];
      }
    }
  }
  for (S8 file = kFileA; file <= kFileH; ++file) {
    ep_file_rand_nums_[file] = rand_num_gen();
  }
  // Update the hash using the current en passent target square.
  if (ep_target_sq_ != kNA) {
    S8 ep_target_file = GetFileFromSq(ep_target_sq_);
    board_hash_ ^= ep_file_rand_nums_[ep_target_file];
  }
  S8 piece_type;
  for (S8 piece = kPawn; piece <= kKing; ++piece) {
    for (S8 sq = kSqA1; sq <= kSqH8; ++sq) {
      piece_rand_nums_[piece][sq] = rand_num_gen();
      // Update the hash using the current piece placement.
      piece_type = piece_layout_[sq];
      if (piece_type != kNA) {
        board_hash_ ^= piece_rand_nums_[piece_type][sq];
      }
    }
  }
  black_to_move_rand_num_ = rand_num_gen();
  // Update the hash using the side to move.
  if (player_to_move_ == kBlack) {
    board_hash_ ^= black_to_move_rand_num_;
  }
}

auto Board::InitBoardPos(const std::string& init_pos) -> void {
  S8 FEN_field = 0;
  S8 current_sq = kSqA8;
  for (char ch : init_pos) {
    // Keep track of which of the six fields is currently being parsed.
    if (ch == ' ') {
      ++FEN_field;
      continue;
    }

    if (FEN_field == 0) {
      // Add pieces to the board.
      if (isalpha(ch)) {
        switch (ch) {
          // Check for white pieces.
          case 'P':
            AddPiece(kPawn, kWhite, current_sq);
            break;
          case 'N':
            AddPiece(kKnight, kWhite, current_sq);
            break;
          case 'B':
            AddPiece(kBishop, kWhite, current_sq);
            break;
          case 'R':
            AddPiece(kRook, kWhite, current_sq);
            break;
          case 'Q':
            AddPiece(kQueen, kWhite, current_sq);
            break;
          case 'K':
            AddPiece(kKing, kWhite, current_sq);
            break;
          // Check for black pieces.
          case 'p':
            AddPiece(kPawn, kBlack, current_sq);
            break;
          case 'n':
            AddPiece(kKnight, kBlack, current_sq);
            break;
          case 'b':
            AddPiece(kBishop, kBlack, current_sq);
            break;
          case 'r':
            AddPiece(kRook, kBlack, current_sq);
            break;
          case 'q':
            AddPiece(kQueen, kBlack, current_sq);
            break;
          case 'k':
            AddPiece(kKing, kBlack, current_sq);
            break;
          default:
            throw invalid_argument("init_pos in Board::InitBoardPos()");
        }
        ++current_sq;
      } else if (ch - '0' >= 0 && ch - '0' <= 8) {
        S8 empty_sq_count = static_cast<S8>(ch - '0');
        S8 empty_sq = current_sq;
        for (; empty_sq < current_sq + empty_sq_count; ++empty_sq) {
          AddPiece(kNA, kNA, empty_sq);
        }
        current_sq = empty_sq;
      } else if (ch == '/') {
        // Set the current square to the rank below the current position.
        current_sq = static_cast<S8>(current_sq - 2 * kNumFiles);
      } else {
        throw invalid_argument("init_pos in Board::InitBoardPos()");
      }
    } else if (FEN_field == 1) {
      // Record the player to move.
      if (ch == 'w') {
        player_to_move_ = kWhite;
      } else if (ch == 'b') {
        player_to_move_ = kBlack;
      } else {
        throw invalid_argument("init_pos in Board::InitBoardPos()");
      }
    } else if (FEN_field == 2) {
      // Assign castling rights for each player and board side.
      switch (ch) {
        case 'Q':
          castling_rights_[kWhite][kQueenSide] = true;
          break;
        case 'K':
          castling_rights_[kWhite][kKingSide] = true;
          break;
        case 'q':
          castling_rights_[kBlack][kQueenSide] = true;
          break;
        case 'k':
          castling_rights_[kBlack][kKingSide] = true;
          break;
        case '-':
          break;
        default:
          throw invalid_argument("init_pos in Board::InitBoardPos()");
      }
    } else if (FEN_field == 3) {
      // Assign the en passent target square.
      if (ep_target_sq_ == kNA) {
        S8 ep_target_sq_file = static_cast<S8>(ch - 'a');
        if (FileOnBoard(ep_target_sq_file)) {
          ep_target_sq_ = ep_target_sq_file;
        } else if (ch != '-') {
          throw invalid_argument("init_pos in Board::InitBoardPos()");
        }
      } else {
        // Assume ep_target_sq_ is initialized to the target
        // square's file.
        S8 ep_target_sq_rank = static_cast<S8>(ch - '1');
        if (RankOnBoard(ep_target_sq_rank)) {
          ep_target_sq_ = GetSqFromRankFile(ep_target_sq_rank, ep_target_sq_);
        } else {
          throw invalid_argument("init_pos in Board::InitBoardPos()");
        }
      }
    } else if (FEN_field == 4) {
      // Initialize the halfmove clock.
      if (isdigit(ch)) {
        S8 next_digit = static_cast<S8>(ch - '0');
        if (halfmove_clock_ == kNA) {
          halfmove_clock_ = next_digit;
        } else {
          halfmove_clock_ = static_cast<S8>(10 * halfmove_clock_ + next_digit);
        }
      } else {
        throw invalid_argument("init_pos in Board::InitBoardPos()");
      }
    } else if (FEN_field == 5) {
      // Ignore the fullmove counter.
      ;
    } else {
      throw invalid_argument("init_pos in Board::InitBoardPos()");
    }
  }
}

auto Board::MakeNonCastlingMove(const Move& move) -> void {
  // Remove a captured piece from the board.
  if (move.captured_piece != kNA) {
    S8 other_player = GetOtherPlayer(player_to_move_);
    if (move.is_ep) {
      S8 ep_capture_sq = 0X0;
      S8 target_file = GetFileFromSq(move.target_sq);
      // Compute the position for a captured pawn in an en passent,
      // and then remove it from the board.
      if (player_to_move_ == kWhite) {
        ep_capture_sq = GetSqFromRankFile(kRank5, target_file);
        piece_layout_[ep_capture_sq] = kNA;
        player_layout_[ep_capture_sq] = kNA;
      } else if (player_to_move_ == kBlack) {
        ep_capture_sq = GetSqFromRankFile(kRank4, target_file);
        piece_layout_[ep_capture_sq] = kNA;
        player_layout_[ep_capture_sq] = kNA;
      }
      Bitboard ep_capture_mask = ~(1ULL << ep_capture_sq);
      pieces_[kPawn] &= ep_capture_mask;
      player_pieces_[other_player] &= ep_capture_mask;
      // Update the board hash to reflect piece removal.
      board_hash_ ^= piece_rand_nums_[kPawn][ep_capture_sq];
    } else {
      // Remove the captured piece from the board.
      Bitboard piece_capture_mask = ~(1ULL << move.target_sq);
      pieces_[move.captured_piece] &= piece_capture_mask;
      player_pieces_[other_player] &= piece_capture_mask;
      // Update the board hash to reflect piece removal.
      board_hash_ ^= piece_rand_nums_[move.captured_piece][move.target_sq];
    }
  }

  MovePiece(move.moving_piece, move.start_sq, move.target_sq,
            move.promoted_to_piece);
}

auto Board::MovePiece(S8 piece, S8 start_sq, S8 target_sq, S8 promoted_to_piece)
    -> void {
  if (piece < kPawn || piece > kKing) {
    throw invalid_argument("piece in Board::MovePiece()");
  } else if (!SqOnBoard(start_sq)) {
    throw invalid_argument("start_sq in Board::MovePiece()");
  } else if (!SqOnBoard(target_sq)) {
    throw invalid_argument("target_sq in Board::MovePiece()");
  } else if (promoted_to_piece != kNA &&
             (promoted_to_piece <= kPawn || promoted_to_piece >= kKing)) {
    throw invalid_argument("promoted_to_piece in Board::MovePiece()");
  }

  // Remove the selected piece from its start position on the board.
  piece_layout_[start_sq] = kNA;
  player_layout_[start_sq] = kNA;
  Bitboard rm_piece_mask = ~(1ULL << start_sq);
  pieces_[piece] &= rm_piece_mask;
  player_pieces_[player_to_move_] &= rm_piece_mask;
  // Update the board hash to reflect piece removal.
  board_hash_ ^= piece_rand_nums_[piece][start_sq];

  // Add the selected piece back at its target position on the board and update
  // the board hash to reflect piece addition.
  Bitboard new_piece_pos_mask = 1ULL << target_sq;
  if (promoted_to_piece == kNA) {
    pieces_[piece] |= new_piece_pos_mask;
    piece_layout_[target_sq] = piece;
    board_hash_ ^= piece_rand_nums_[piece][target_sq];
  } else {
    // Add a piece back as the type it promotes to if move is a pawn promotion.
    pieces_[promoted_to_piece] |= new_piece_pos_mask;
    piece_layout_[target_sq] = promoted_to_piece;
    board_hash_ ^= piece_rand_nums_[promoted_to_piece][target_sq];
  }

  player_layout_[target_sq] = player_to_move_;
  player_pieces_[player_to_move_] |= new_piece_pos_mask;
}

auto Board::UnmakeNonCastlingMove(const Move& move) -> void {
  // Move the moving piece back to its original position and undo
  // any pawn promotion.
  if (move.promoted_to_piece == kNA) {
    MovePiece(move.moving_piece, move.target_sq, move.start_sq);
  } else {
    // Remove the promoted-to piece from the board.
    Bitboard piece_promotion_rm_mask = ~(1ULL << move.target_sq);
    pieces_[move.promoted_to_piece] &= piece_promotion_rm_mask;
    player_pieces_[player_to_move_] &= piece_promotion_rm_mask;
    piece_layout_[move.target_sq] = kNA;
    player_layout_[move.target_sq] = kNA;
    // Update the board hash to reflect piece removal.
    board_hash_ ^= piece_rand_nums_[move.promoted_to_piece][move.target_sq];

    // Add the original pawn back to its start position.
    Bitboard og_piece_pos_mask = 1ULL << move.start_sq;
    pieces_[kPawn] |= og_piece_pos_mask;
    player_pieces_[player_to_move_] |= og_piece_pos_mask;
    piece_layout_[move.start_sq] = kPawn;
    player_layout_[move.start_sq] = player_to_move_;
    // Update the board hash to reflect piece addition.
    board_hash_ ^= piece_rand_nums_[kPawn][move.start_sq];
  }

  // Place a captured piece back onto the board.
  if (move.captured_piece != kNA) {
    S8 other_player = GetOtherPlayer(player_to_move_);
    if (move.is_ep) {
      S8 ep_capture_sq = 0X0;
      S8 target_file = GetFileFromSq(move.target_sq);
      // Place a captured pawn back onto the board after an en passent.
      if (player_to_move_ == kWhite) {
        ep_capture_sq = GetSqFromRankFile(kRank5, target_file);
        piece_layout_[ep_capture_sq] = kPawn;
        player_layout_[ep_capture_sq] = kBlack;
      } else if (player_to_move_ == kBlack) {
        ep_capture_sq = GetSqFromRankFile(kRank4, target_file);
        piece_layout_[ep_capture_sq] = kPawn;
        player_layout_[ep_capture_sq] = kWhite;
      }
      Bitboard undo_ep_capture_mask = 1ULL << ep_capture_sq;
      pieces_[kPawn] |= undo_ep_capture_mask;
      player_pieces_[other_player] |= undo_ep_capture_mask;
      // Update the board hash to reflect piece addition.
      board_hash_ ^= piece_rand_nums_[kPawn][ep_capture_sq];
    } else {
      Bitboard undo_capture_mask = 1ULL << move.target_sq;
      // Add the captured piece back to its original position.
      pieces_[move.captured_piece] |= undo_capture_mask;
      player_pieces_[other_player] |= undo_capture_mask;
      piece_layout_[move.target_sq] = move.captured_piece;
      player_layout_[move.target_sq] = other_player;
      // Update the board hash to reflect piece addition.
      board_hash_ ^= piece_rand_nums_[move.captured_piece][move.target_sq];
    }
  }
}

auto Board::UpdateCastlingRights(const Move& move) -> void {
  // Record the current castling rights before updating them.
  white_queenside_castling_rights_history_.push(
      castling_rights_[kWhite][kQueenSide]);
  white_kingside_castling_rights_history_.push(
      castling_rights_[kWhite][kKingSide]);
  black_queenside_castling_rights_history_.push(
      castling_rights_[kBlack][kQueenSide]);
  black_kingside_castling_rights_history_.push(
      castling_rights_[kBlack][kKingSide]);

  if (move.castling_type != kNA || move.moving_piece == kKing) {
    // Revoke all castling rights for a player after moving the king.
    if (castling_rights_[player_to_move_][kQueenSide]) {
      castling_rights_[player_to_move_][kQueenSide] = false;
      board_hash_ ^= castling_rights_rand_nums_[player_to_move_][kQueenSide];
    }
    if (castling_rights_[player_to_move_][kKingSide]) {
      castling_rights_[player_to_move_][kKingSide] = false;
      board_hash_ ^= castling_rights_rand_nums_[player_to_move_][kKingSide];
    }
  } else if (move.moving_piece == kRook) {
    S8 start_rank = GetRankFromSq(move.start_sq);
    S8 start_file = GetFileFromSq(move.start_sq);
    // Check that a rook is moving from its original starting position
    // and that the player still has castling rights on that side before
    // revoking castling rights.
    if ((player_to_move_ == kWhite && start_rank == kRank1) ||
        (player_to_move_ == kBlack && start_rank == kRank8)) {
      if (start_file == kFileA &&
          castling_rights_[player_to_move_][kQueenSide]) {
        castling_rights_[player_to_move_][kQueenSide] = false;
        board_hash_ ^= castling_rights_rand_nums_[player_to_move_][kQueenSide];
      } else if (start_file == kFileH &&
                 castling_rights_[player_to_move_][kKingSide]) {
        castling_rights_[player_to_move_][kKingSide] = false;
        board_hash_ ^= castling_rights_rand_nums_[player_to_move_][kKingSide];
      }
    }
  }

  if (move.captured_piece == kRook) {
    // Revoke the other player's castling rights if a player's rook is captured.
    if (player_to_move_ == kWhite) {
      if (move.target_sq == kSqA8 && castling_rights_[kBlack][kQueenSide]) {
        castling_rights_[kBlack][kQueenSide] = false;
        board_hash_ ^= castling_rights_rand_nums_[kBlack][kQueenSide];
      } else if (move.target_sq == kSqH8 &&
                 castling_rights_[kBlack][kKingSide]) {
        castling_rights_[kBlack][kKingSide] = false;
        board_hash_ ^= castling_rights_rand_nums_[kBlack][kKingSide];
      }
    } else if (player_to_move_ == kBlack) {
      if (move.target_sq == kSqA1 && castling_rights_[kWhite][kQueenSide]) {
        castling_rights_[kWhite][kQueenSide] = false;
        board_hash_ ^= castling_rights_rand_nums_[kWhite][kQueenSide];
      } else if (move.target_sq == kSqH1 &&
                 castling_rights_[kWhite][kKingSide]) {
        castling_rights_[kWhite][kKingSide] = false;
        board_hash_ ^= castling_rights_rand_nums_[kWhite][kKingSide];
      }
    }
  }
}

}  // namespace omegazero


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                         (C) 2007 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include <sstream>
#include <boost/python.hpp>
#include "mccomponents/kernels/sample/phonon/IncoherentInelastic.h"
#include "mccomponents/boostpython_binding/wrap_kernel.h"
#include "mccomponents/kernels/sample/phonon/AbstractDOS.h"
#include "mccomponents/kernels/sample/phonon/AbstractDebyeWallerFactor.h"

namespace wrap_mccomponents {

  void wrap_Phonon_IncoherentInelastic_kernel()
  {
    using namespace boost::python;
    using namespace mccomponents::boostpython_binding;

    typedef mccomponents::kernels::phonon::IncoherentInelastic w_t;

    kernel_wrapper<w_t>::wrap
      ( "Phonon_IncoherentInelastic_kernel",
	init<
	const w_t::atoms_t &, // atoms
	w_t::float_t, // unitcell_vol. AA**3
	w_t::dos_t &, // DOS
	w_t::dwcalculator_t &, // Debye Waller calculator
	w_t::float_t, // temperature. K
	w_t::float_t, // average mass. atomic weight. default 0
	w_t::float_t, // total scattering xs. barn. default 0
	w_t::float_t  // total absorption xs. barn. default 0
	> ()
	[with_custodian_and_ward<1,2,
	 with_custodian_and_ward<1,4,
	 with_custodian_and_ward<1,5 > > > () ]
	)
      ;

  }

}

// version
// $Id: wrap_Phonon_IncoherentInelastic_kernel.cc 603 2010-10-04 15:58:16Z linjiao $

// End of file


/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#ifdef BOOST_TEST_MAIN
#  undef BOOST_TEST_MAIN
#endif
#include <mc_control/api.h>
#include <mc_control/mc_controller.h>
#include <mc_rtc/logging.h>

#include <boost/test/unit_test.hpp>

namespace mc_control
{

struct MC_CONTROL_DLLAPI TestPostureController : public MCController
{
public:
  TestPostureController(std::shared_ptr<mc_rbdyn::RobotModule> rm, double dt) : MCController(rm, dt)
  {
    // Check that the default constructor loads the robot + ground environment
    BOOST_CHECK_EQUAL(robots().robots().size(), 2);
    // Check that JVRC-1 was loaded
    BOOST_CHECK_EQUAL(robot().name(), "jvrc1");
    qpsolver->addConstraintSet(contactConstraint);
    qpsolver->addConstraintSet(kinematicsConstraint);
    postureTask->stiffness(20);
    qpsolver->addTask(postureTask.get());
    qpsolver->setContacts({});
    BOOST_CHECK(robot().hasJoint("NECK_P"));
    BOOST_CHECK_NO_THROW(head_joint_index = robot().jointIndexByName("NECK_P"));
    head_joint_target = std::min(std::abs(robot().ql()[head_joint_index][0]), robot().qu()[head_joint_index][0]) - 0.1;
    LOG_SUCCESS("Created TestPostureController")
  }

  virtual bool run() override
  {
    bool ret = MCController::run();
    BOOST_CHECK(ret);
    if(nrIter++ > 250)
    {
      BOOST_CHECK_SMALL(robot().mbc().q[head_joint_index][0] - head_joint_target, 0.05);
    }
    return ret;
  }

  virtual void reset(const ControllerResetData & reset_data) override
  {
    MCController::reset(reset_data);
    postureTask->target({{"NECK_P", {head_joint_target}}});
  }

private:
  unsigned int nrIter = 0;
  unsigned int head_joint_index;
  double head_joint_target;
};

} // namespace mc_control

SIMPLE_CONTROLLER_CONSTRUCTOR("TestPostureController", mc_control::TestPostureController)


// (C) Copyright 2016 by Autodesk, Inc.

#ifdef TEST_ON

#include "ChecksumFile.hh"
#include "Base/Utils/OStringStream.hh"

#include <boost/functional/hash.hpp>
#include <boost/filesystem.hpp>

#include <fstream>

namespace Test {
namespace Checksum {

void File::record(const char* _flnm)
{
  // Reads the file, makes a has number form its data and finally record
  // hash_number and filename.
  std::ifstream fstr(_flnm);
  std::array<char, 1000> buf;
  size_t file_hash = 0;
  size_t file_size = 0;
  while(fstr)
  {
    fstr.read(buf.data(), buf.size());
    auto buf_hash = boost::hash_range(buf.data(), buf.data() + fstr.gcount());
    file_size += fstr.gcount();
    boost::hash_combine(file_hash, buf_hash);
  }
  Base::OStringStream strm;
  strm << "\"" << _flnm << '\"' << " (Size: " << file_size << ")" <<
    "(Hash: " << Base::format_hex(file_hash) << ")";

  add(Result::OK, strm.str);
}

}//namespace Checksum
}//namespace Test

#endif//TEST_ON


#include "my_capture_d400.h"

#include <iostream>
#include <boost/thread.hpp>
#include <boost/bind.hpp>
#include <librealsense2/rsutil.h>

MyCaptureD400::MyCaptureD400(StreamSetting ss)
{
	bagrec = false;

	startStreams(ss);

	num_camera = cameras.size();

	std::cout << "number of cameras: " << num_camera << std::endl;

	pc_filter_setting.color_filt_on = false;
	pc_filter_setting.outlier_removal_on = false;
}

void MyCaptureD400::startStreams(StreamSetting ss)
{
	rs2::context rs_ctx;
	const std::string platform_camera_name = "Platform Camera";

	cameras.clear();

	int fps, c_w, c_h, d_w, d_h;

	if (ss.fps == 2) fps = 90;
	else if (ss.fps == 1) fps = 60;
	else fps = 30;

	if (ss.depth_res == 2) { d_w = 1280; d_h = 720; }
	else if (ss.depth_res == 1) { d_w = 848; d_h = 480; }
	else { d_w = 424; d_h = 240; }

	if (ss.color_res == 2) { c_w = 1280; c_h = 720; }
	else if (ss.color_res == 1) { c_w = 848; c_h = 480; }
	else { c_w = 424; c_h = 240; }

	smode = ss.smode;

	for (auto&& dev : rs_ctx.query_devices())
	{
		if (dev.get_info(RS2_CAMERA_INFO_NAME) == platform_camera_name) continue;

		rs2::pipeline pipe;
		rs2::config cfg;

		cfg.enable_device(dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));

		if (smode == SMODE_DEPTH_COLOR)
		{
			cfg.enable_stream(RS2_STREAM_COLOR, c_w, c_h, RS2_FORMAT_BGR8, fps);
			cfg.enable_stream(RS2_STREAM_DEPTH, d_w, d_h, RS2_FORMAT_Z16, fps);
		}
		else if (smode == SMODE_DEPTH_IR)
		{
			cfg.enable_stream(RS2_STREAM_INFRARED, 1, d_w, d_h, RS2_FORMAT_Y8, fps);
			cfg.enable_stream(RS2_STREAM_DEPTH, d_w, d_h, RS2_FORMAT_Z16, fps);
		}

		if (bagrec)
		{
			cfg.enable_record_to_file(bagrec_path + bagrec_sessionname + ".rosbag." + std::to_string(cameras.size() + 1) + ".bag");
		}

		rs2::pipeline_profile prof = pipe.start(cfg);

		Camera cam;

		cam.pipe = pipe;
		cam.pc.clear();
		cam.ci.depth_intrin = prof.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>().get_intrinsics();

		if (smode == SMODE_DEPTH_COLOR)
		{
			cam.ci.color_intrin = prof.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>().get_intrinsics();
			cam.ci.depth_to_color = prof.get_stream(RS2_STREAM_DEPTH).get_extrinsics_to(prof.get_stream(RS2_STREAM_COLOR));
			cam.ci.scale = prof.get_device().first<rs2::depth_sensor>().get_depth_scale();
		}
		else if (smode == SMODE_DEPTH_IR)
		{
			cam.ci.color_intrin = prof.get_stream(RS2_STREAM_INFRARED, 1).as<rs2::video_stream_profile>().get_intrinsics();
			cam.ci.depth_to_color = prof.get_stream(RS2_STREAM_DEPTH).get_extrinsics_to(prof.get_stream(RS2_STREAM_INFRARED, 1));
			cam.ci.scale = prof.get_device().first<rs2::depth_sensor>().get_depth_scale();
		}

		cameras.push_back(cam);
	}
}

void MyCaptureD400::stopStreams()
{
	for (auto & c : cameras)
	{
		c.pipe.stop();
	}
}

void MyCaptureD400::getNextFrames()
{
	boost::thread_group thr_grp;

	for (auto & c : cameras)
	{
		thr_grp.create_thread(boost::bind(&MyCaptureD400::updateFrame, this, boost::ref(c)));
	}

	thr_grp.join_all();
}

void MyCaptureD400::updateFrame(Camera & c)
{
	rs2::frameset fset = c.pipe.wait_for_frames();

	rs2::frame color_frame, depth_frame;

	if (smode == SMODE_DEPTH_COLOR)
	{
		color_frame = fset.get_color_frame();
		c.color_frame = cv::Mat(cv::Size(c.ci.color_intrin.width, c.ci.color_intrin.height), CV_8UC3, (void*)color_frame.get_data(), cv::Mat::AUTO_STEP);
	}
	else if (smode == SMODE_DEPTH_IR)
	{
		color_frame = fset.get_infrared_frame();
		cv::Mat ir = cv::Mat(cv::Size(c.ci.color_intrin.width, c.ci.color_intrin.height), CV_8UC1, (void*)color_frame.get_data(), cv::Mat::AUTO_STEP);
		cv::cvtColor(ir, c.color_frame, CV_GRAY2BGR);
	}
	depth_frame = fset.get_depth_frame();
	c.depth_frame = cv::Mat(cv::Size(c.ci.depth_intrin.width, c.ci.depth_intrin.height), CV_16UC1, (void*)depth_frame.get_data(), cv::Mat::AUTO_STEP);

	c.timestamp = fset.get_timestamp();
}

void MyCaptureD400::getFrameData(cv::Mat & frame, int camera_id, std::string frame_type)
{
	if (frame_type == "DEPTH")
	{
		frame = cameras[camera_id].depth_frame.clone();
	}
	else if (frame_type == "COLOR")
	{
		frame = cameras[camera_id].color_frame.clone();
	}
}

void MyCaptureD400::getPointClouds()
{
	boost::thread_group thr_grp;

	for (auto & c : cameras)
	{
		thr_grp.create_thread(boost::bind(&MyCaptureD400::updatePointCloud, this, boost::ref(c)));
	}

	thr_grp.join_all();
}

void MyCaptureD400::updatePointCloud(Camera & c)
{
	frame2PointCloud(c.color_frame, c.depth_frame, c.pc, c.ci, pc_filter_setting);
}

void MyCaptureD400::frame2PointCloud(const cv::Mat & color_frame, const cv::Mat & depth_frame, pcl::PointCloud<pcl::PointXYZRGB>& pc, const RsCameraIntrinsics2 & intrinsics, const PointCloudFilterSetting & filter_setting)
{
	cv::Mat hsv_frame, detection_frame;
	if (filter_setting.color_filt_on) // pre processing for color filtering; finding pixels within the range in HSV color space
	{
		cv::cvtColor(color_frame, hsv_frame, cv::COLOR_BGR2HSV);
		cv::inRange(hsv_frame, filter_setting.color_filt_hsv_min, filter_setting.color_filt_hsv_max, detection_frame);
	}

	pc.clear();

	for (int dy = 0; dy < intrinsics.depth_intrin.height; ++dy)
	{
		for (int dx = 0; dx < intrinsics.depth_intrin.width; ++dx)
		{
			// Retrieve the 16-bit depth value and map it into a depth in meters
			uint16_t depth_value = depth_frame.at<unsigned __int16>(dy, dx);

			// Skip over pixels with a depth value of zero, which is used to indicate no data
			if (depth_value == 0) continue;

			float depth_in_meters = (float)depth_value * intrinsics.scale;

			// Map from pixel coordinates in the depth image to pixel coordinates in the color image
			float depth_pixel[2] = { (float)dx - 0.5f, (float)dy - 0.5f };
			float depth_point[3], color_point[3], color_pixel[2];
			rs2_deproject_pixel_to_point(depth_point, &intrinsics.depth_intrin, depth_pixel, depth_in_meters);
			rs2_transform_point_to_point(color_point, &intrinsics.depth_to_color, depth_point);
			rs2_project_point_to_pixel(color_pixel, &intrinsics.color_intrin, color_point);

			const int cx = static_cast<int>(color_pixel[0] + 0.5f), cy = static_cast<int>(color_pixel[1] + 0.5f);

			if (cx < 0 || cy < 0 || cx >= intrinsics.color_intrin.width || cy >= intrinsics.color_intrin.height) continue;

			if (filter_setting.color_filt_on && detection_frame.at<unsigned char>(cy, cx) == 0) continue;

			pcl::PointXYZRGB p;

			// Use the color from the nearest color pixel
			auto clr = color_frame.at<cv::Vec3b>(cy, cx);
			p.r = clr[2]; p.g = clr[1]; p.b = clr[0];

			// Emit a vertex at the 3D location of this depth pixel
			p.x = -depth_point[0];
			p.y = -depth_point[1];
			p.z = depth_point[2];

			pc.push_back(p);
		}
	}

	if (filter_setting.outlier_removal_on)
	{
		removeNoiseFromThresholdedPc(pc, filter_setting.outlier_filt_meanK, filter_setting.outlier_filt_thresh);
	}
}

void MyCaptureD400::startBagRecording(const std::string & path_data_dir, const std::string & name_session, int res_idx, int fps)
{
	StreamSetting ss = { smode, res_idx, res_idx, fps };
	bagrec = true;
	bagrec_path = path_data_dir;
	bagrec_sessionname = name_session;

	restartStreams(ss);

	/*
	for (int i = 0; i < num_camera; i++)
	{
		std::string s_num = cameras[i].pipe.get_active_profile().get_device().get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);

		cameras[i].pipe.stop();

		rs2::config cfg;

		cfg.enable_device(s_num);
		cfg.enable_stream(RS2_STREAM_COLOR, w, h, RS2_FORMAT_BGR8, frate);
		cfg.enable_stream(RS2_STREAM_DEPTH, w, h, RS2_FORMAT_Z16, frate);

		cfg.enable_record_to_file(path_data_dir + name_session + ".rosbag." + std::to_string(i+1) + ".bag");

		cameras[i].pipe.start(cfg);

		//update intrinsics
		auto prof = cameras[i].pipe.get_active_profile();
		cameras[i].ci.color_intrin = prof.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>().get_intrinsics();

			cameras[i].ci.depth_intrin = prof.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>().get_intrinsics();
			cameras[i].ci.depth_to_color = prof.get_stream(RS2_STREAM_DEPTH).get_extrinsics_to(prof.get_stream(RS2_STREAM_COLOR));
			cameras[i].ci.scale = prof.get_device().first<rs2::depth_sensor>().get_depth_scale();
	}
	*/
}

void MyCaptureD400::stopBagRecording()
{
	StreamSetting ss = { smode, 0, 0, 30 };
	bagrec = false;

	restartStreams(ss);

	/*
	for (int i = 0; i < num_camera; i++)
	{
		std::string s_num = cameras[i].pipe.get_active_profile().get_device().get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);

		cameras[i].pipe.stop();

		rs2::config cfg;

		cfg.enable_device(s_num);
		//!! resolution should be original value !!
		cfg.enable_stream(RS2_STREAM_COLOR, 848, 480, RS2_FORMAT_BGR8, 30);
		if (!depth_off) cfg.enable_stream(RS2_STREAM_DEPTH, 424, 240, RS2_FORMAT_Z16, 30);

		cameras[i].pipe.start(cfg);

		//update intrinsics
		auto prof = cameras[i].pipe.get_active_profile();
		cameras[i].ci.color_intrin = prof.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>().get_intrinsics();
		if (!depth_off)
		{
			cameras[i].ci.depth_intrin = prof.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>().get_intrinsics();
			cameras[i].ci.depth_to_color = prof.get_stream(RS2_STREAM_DEPTH).get_extrinsics_to(prof.get_stream(RS2_STREAM_COLOR));
			cameras[i].ci.scale = prof.get_device().first<rs2::depth_sensor>().get_depth_scale();
		}
	}
	*/
}

void MyCaptureD400::getPointCloudData(pcl::PointCloud<pcl::PointXYZRGB>& pc, int camera_id)
{
	pc = cameras[camera_id].pc;
}

void MyCaptureD400::getCameraIntrinsics(RsCameraIntrinsics2 & ci, int camera_id)
{
	ci = cameras[camera_id].ci;
}

void MyCaptureD400::getColorCameraSettings(MyColorCameraSettings & cs, int camera_id)
{
	/*
	Camera & c = cameras[camera_id];

	c.dev->get_option_range(rs::option::color_gain, cs.gain.min, cs.gain.max, cs.gain.step);
	c.dev->get_option_range(rs::option::color_exposure, cs.exposure.min, cs.exposure.max, cs.exposure.step);
	c.dev->get_option_range(rs::option::color_contrast, cs.contrast.min, cs.contrast.max, cs.contrast.step);
	c.dev->get_option_range(rs::option::color_brightness, cs.brightness.min, cs.brightness.max, cs.brightness.step);
	c.dev->get_option_range(rs::option::color_gamma, cs.gamma.min, cs.gamma.max, cs.gamma.step);

	cs.gain.value = c.dev->get_option(rs::option::color_gain);
	cs.exposure.value = c.dev->get_option(rs::option::color_exposure);
	cs.contrast.value = c.dev->get_option(rs::option::color_contrast);
	cs.brightness.value = c.dev->get_option(rs::option::color_brightness);
	cs.gamma.value = c.dev->get_option(rs::option::color_gamma);

	cs.auto_exposure = static_cast<bool>(c.dev->get_option(rs::option::color_enable_auto_exposure));
	*/
}

void MyCaptureD400::setColorCameraSettings(MyColorCameraSettings & cs, int camera_id)
{
	/*
	Camera & c = cameras[camera_id];

	bool flag_update_exposure = (cs.exposure.value != c.dev->get_option(rs::option::color_exposure));

	c.dev->set_option(rs::option::color_gain, cs.gain.value);
	c.dev->set_option(rs::option::color_contrast, cs.contrast.value);
	c.dev->set_option(rs::option::color_brightness, cs.brightness.value);
	c.dev->set_option(rs::option::color_gamma, cs.gamma.value);

	if (flag_update_exposure) {
		c.dev->set_option(rs::option::color_exposure, cs.exposure.value);
		cs.auto_exposure = false;
	}

	bool flag_update_auto_exposure = (cs.auto_exposure != static_cast<bool>(c.dev->get_option(rs::option::color_enable_auto_exposure)));
	if (flag_update_auto_exposure)
	{
		c.dev->set_option(rs::option::color_enable_auto_exposure, static_cast<bool>(cs.auto_exposure));
	}
	//reload color camera setting after setting (some parameters are rounded by R200 after setting)
	getColorCameraSettings(cs, camera_id);
	*/
}

bool MyCaptureD400::getInfraredEmitter(int camera_id)
{
	return cameras[camera_id].pipe.get_active_profile().get_device().first<rs2::depth_sensor>().get_option(RS2_OPTION_EMITTER_ENABLED) > 0;
}

void MyCaptureD400::setInfraredEmitter(bool emitter_on, int camera_id)
{
	if (emitter_on)
	{
		cameras[camera_id].pipe.get_active_profile().get_device().first<rs2::depth_sensor>().set_option(RS2_OPTION_EMITTER_ENABLED, 1.0f);
	}
	else
	{
		cameras[camera_id].pipe.get_active_profile().get_device().first<rs2::depth_sensor>().set_option(RS2_OPTION_EMITTER_ENABLED, 0.0f);
	}
}

void MyCaptureD400::setInfraredCamGain(double gain_value)
{
	for (auto c : cameras) c.pipe.get_active_profile().get_device().first<rs2::depth_sensor>().set_option(RS2_OPTION_LASER_POWER, gain_value);
}

void MyCaptureD400::setInfraredCamExposure(double exp_value)
{

	for (auto c : cameras)
	{
		auto dev = c.pipe.get_active_profile().get_device().first<rs2::depth_sensor>();

		if (exp_value < 0.0)
		{
			dev.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1);
		}
		else
		{
			auto range = dev.get_option_range(RS2_OPTION_EXPOSURE);
			dev.set_option(RS2_OPTION_EXPOSURE, exp_value*(range.max - range.min) + range.min);
		}
	}
}

void MyCaptureD400::setColorFilter(PointCloudFilterSetting pcfs)
{
	pc_filter_setting = pcfs;
}

MyCaptureD400::~MyCaptureD400()
{
}


#include "MBTB_TimeStepping.hpp"
#include "MBTB_PYTHON_API.hpp"
#include "MBTB_internalTool.hpp"
#include <boost/math/quaternion.hpp>
//#define TS_DEBUG
MBTB_TimeStepping::MBTB_TimeStepping(SP::TimeDiscretisation td,
                                     SP::OneStepIntegrator osi,
                                     SP::OneStepNSProblem osnspb_velo):TimeStepping(td,osi,osnspb_velo)
{
}

void MBTB_TimeStepping::updateWorldFromDS()
{
#ifdef TS_DEBUG
  printf("MBTB_TimeStepping::updateWordFromDS \n");
#endif
  MBTB_updateDSFromSiconos();
  _MBTB_updateContactFromDS();
}


/*
 * Throttling.actor.cpp
 *
 * This source file is part of the FoundationDB open source project
 *
 * Copyright 2013-2018 Apple Inc. and the FoundationDB project authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <boost/lexical_cast.hpp>

#include "fdbclient/ManagementAPI.actor.h"
#include "fdbclient/ReadYourWrites.h"
#include "fdbclient/Schemas.h"
#include "fdbserver/workloads/workloads.actor.h"
#include "flow/actorcompiler.h" // This must be the last include

struct TokenBucket {
	static constexpr const double addTokensInterval = 0.1;
	static constexpr const double maxSleepTime = 60.0;

	double transactionRate;
	double maxBurst;
	double bucketSize;
	Future<Void> tokenAdderActor;

	ACTOR static Future<Void> tokenAdder(TokenBucket* self) {
		loop {
			self->bucketSize = std::min(self->bucketSize + self->transactionRate * addTokensInterval, self->maxBurst);
			if (deterministicRandom()->randomInt(0, 100) == 0)
				TraceEvent("AddingTokensx100")
				    .detail("BucketSize", self->bucketSize)
				    .detail("TransactionRate", self->transactionRate);
			wait(delay(addTokensInterval));
		}
	}

	TokenBucket(double maxBurst = 1000) : transactionRate(0), maxBurst(maxBurst), bucketSize(maxBurst) {
		tokenAdderActor = tokenAdder(this);
	}

	ACTOR static Future<Void> startTransaction(TokenBucket* self) {
		state double sleepTime = addTokensInterval;
		loop {
			if (self->bucketSize >= 1.0) {
				--self->bucketSize;
				return Void();
			}
			if (deterministicRandom()->randomInt(0, 100) == 0)
				TraceEvent("ThrottlingTransactionx100").detail("SleepTime", sleepTime);
			wait(delay(sleepTime));
			sleepTime = std::min(sleepTime * 2, maxSleepTime);
		}
	}
};

// workload description:
// The Throttling workload runs a simple random read-write workload while throttling using a token bucket algorithm
// using the TPS limit obtained from the health metrics namespace. It periodically reads health metrics from the special
// key and tests whether or not the received health metrics are reasonable.
struct ThrottlingWorkload : KVWorkload {

	double testDuration;
	int actorsPerClient;
	int writesPerTransaction;
	int readsPerTransaction;
	double throttlingMultiplier;
	int transactionsCommitted;
	TokenBucket tokenBucket;
	bool correctSpecialKeys = true;

	static constexpr const char* NAME = "Throttling";

	ThrottlingWorkload(WorkloadContext const& wcx)
	  : KVWorkload(wcx), transactionsCommitted(0) {
		testDuration = getOption(options, LiteralStringRef("testDuration"), 60.0);
		actorsPerClient = getOption(options, LiteralStringRef("actorsPerClient"), 10);
		writesPerTransaction = getOption(options, LiteralStringRef("writesPerTransaction"), 10);
		readsPerTransaction = getOption(options, LiteralStringRef("readsPerTransaction"), 10);
		throttlingMultiplier = getOption(options, LiteralStringRef("throttlingMultiplier"), 0.5);
		int maxBurst = getOption(options, LiteralStringRef("maxBurst"), 1000);
		tokenBucket.maxBurst = maxBurst;
	}

	static Value getRandomValue() { return Standalone<StringRef>(format("Value/%d", deterministicRandom()->randomInt(0, 10e6))); }

	ACTOR static Future<Void> clientActor(Database cx, ThrottlingWorkload* self) {
		state ReadYourWritesTransaction tr(cx);

		loop {
			wait(TokenBucket::startTransaction(&self->tokenBucket));
			tr.reset();
			try {
				state int i;
				for (i = 0; i < self->readsPerTransaction; ++i) {
					state Optional<Value> value = wait(tr.get(self->getRandomKey()));
				}
				for (i = 0; i < self->writesPerTransaction; ++i) {
					tr.set(self->getRandomKey(), getRandomValue());
				}
				wait(tr.commit());
				if (deterministicRandom()->randomInt(0, 1000) == 0) TraceEvent("TransactionCommittedx1000");
				++self->transactionsCommitted;
			} catch (Error& e) {
				if (e.code() == error_code_actor_cancelled) throw;
				// ignore failing transactions
			}
		}
	}

	ACTOR static Future<Void> specialKeysActor(Database cx, ThrottlingWorkload* self) {
		state ReadYourWritesTransaction tr(cx);
		state json_spirit::mValue aggregateSchema =
		    readJSONStrictly(JSONSchemas::aggregateHealthSchema.toString()).get_obj();
		state json_spirit::mValue storageSchema =
		    readJSONStrictly(JSONSchemas::storageHealthSchema.toString()).get_obj();
		state json_spirit::mValue logSchema = readJSONStrictly(JSONSchemas::logHealthSchema.toString()).get_obj();
		loop {
			try {
				Standalone<RangeResultRef> result = wait(
				    tr.getRange(prefixRange(LiteralStringRef("\xff\xff/metrics/health/")), CLIENT_KNOBS->TOO_MANY));
				ASSERT(!result.more);
				for (const auto& [k, v] : result) {
					ASSERT(k.startsWith(LiteralStringRef("\xff\xff/metrics/health/")));
					auto valueObj = readJSONStrictly(v.toString()).get_obj();
					if (k.removePrefix(LiteralStringRef("\xff\xff/metrics/health/")) == LiteralStringRef("aggregate")) {
						TEST(true); // Test aggregate health metrics schema
						std::string errorStr;
						if (!schemaMatch(aggregateSchema, valueObj, errorStr, SevError, true)) {
							TraceEvent(SevError, "AggregateHealthSchemaValidationFailed")
							    .detail("ErrorStr", errorStr.c_str())
							    .detail("JSON", json_spirit::write_string(json_spirit::mValue(v.toString())));
							self->correctSpecialKeys = false;
						}
						auto tpsLimit = valueObj.at("tps_limit").get_real();
						self->tokenBucket.transactionRate = tpsLimit * self->throttlingMultiplier / self->clientCount;
					} else if (k.removePrefix(LiteralStringRef("\xff\xff/metrics/health/"))
					               .startsWith(LiteralStringRef("storage/"))) {
						TEST(true); // Test storage health metrics schema
						UID::fromString(k.removePrefix(LiteralStringRef("\xff\xff/metrics/health/storage/"))
						                    .toString()); // Will throw if it's not a valid uid
						std::string errorStr;
						if (!schemaMatch(storageSchema, valueObj, errorStr, SevError, true)) {
							TraceEvent(SevError, "StorageHealthSchemaValidationFailed")
							    .detail("ErrorStr", errorStr.c_str())
							    .detail("JSON", json_spirit::write_string(json_spirit::mValue(v.toString())));
							self->correctSpecialKeys = false;
						}
					} else if (k.removePrefix(LiteralStringRef("\xff\xff/metrics/health/"))
					               .startsWith(LiteralStringRef("log/"))) {
						TEST(true); // Test log health metrics schema
						UID::fromString(k.removePrefix(LiteralStringRef("\xff\xff/metrics/health/log/"))
						                    .toString()); // Will throw if it's not a valid uid
						std::string errorStr;
						if (!schemaMatch(logSchema, valueObj, errorStr, SevError, true)) {
							TraceEvent(SevError, "LogHealthSchemaValidationFailed")
							    .detail("ErrorStr", errorStr.c_str())
							    .detail("JSON", json_spirit::write_string(json_spirit::mValue(v.toString())));
							self->correctSpecialKeys = false;
						}
					} else {
						ASSERT(false); // Unrecognized key
					}
				}
				wait(delayJittered(5));
			} catch (Error& e) {
				wait(tr.onError(e));
			}
		}
	}

	ACTOR static Future<Void> _start(Database cx, ThrottlingWorkload* self) {
		state vector<Future<Void>> clientActors;
		state int actorId;
		for (actorId = 0; actorId < self->actorsPerClient; ++actorId) {
			clientActors.push_back(timeout(clientActor(cx, self), self->testDuration, Void()));
		}
		clientActors.push_back(timeout(specialKeysActor(cx, self), self->testDuration, Void()));
		clientActors.push_back(timeout(self->tokenBucket.tokenAdderActor, self->testDuration, Void()));
		wait(delay(self->testDuration));
		return Void();
	}

	std::string description() const override { return ThrottlingWorkload::NAME; }
	Future<Void> start(Database const& cx) override { return _start(cx, this); }
	Future<bool> check(Database const& cx) override { return correctSpecialKeys; }

	void getMetrics(vector<PerfMetric>& m) override {
		m.push_back(PerfMetric("TransactionsCommitted", transactionsCommitted, false));
	}
};

WorkloadFactory<ThrottlingWorkload> ThrottlingWorkloadFactory(ThrottlingWorkload::NAME);


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2014-2018.
// Modifications copyright (c) 2014-2018 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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_GEOMETRY_STRATEGIES_STRATEGIES_HPP
#define BOOST_GEOMETRY_STRATEGIES_STRATEGIES_HPP

#include <boost/geometry/strategies/tags.hpp>

#include <boost/geometry/strategies/area.hpp>
#include <boost/geometry/strategies/azimuth.hpp>
#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/strategies/centroid.hpp>
#include <boost/geometry/strategies/compare.hpp>
#include <boost/geometry/strategies/convex_hull.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/densify.hpp>
#include <boost/geometry/strategies/disjoint.hpp>
#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/envelope.hpp>
#include <boost/geometry/strategies/intersection.hpp>
#include <boost/geometry/strategies/intersection_strategies.hpp> // for backward compatibility
#include <boost/geometry/strategies/relate.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/strategies/transform.hpp>
#include <boost/geometry/strategies/within.hpp>

#include <boost/geometry/strategies/cartesian/area.hpp>
#include <boost/geometry/strategies/cartesian/azimuth.hpp>
#include <boost/geometry/strategies/cartesian/box_in_box.hpp>
#include <boost/geometry/strategies/cartesian/buffer_end_flat.hpp>
#include <boost/geometry/strategies/cartesian/buffer_end_round.hpp>
#include <boost/geometry/strategies/cartesian/buffer_join_miter.hpp>
#include <boost/geometry/strategies/cartesian/buffer_join_round.hpp>
#include <boost/geometry/strategies/cartesian/buffer_join_round_by_divide.hpp>
#include <boost/geometry/strategies/cartesian/buffer_point_circle.hpp>
#include <boost/geometry/strategies/cartesian/buffer_point_square.hpp>
#include <boost/geometry/strategies/cartesian/buffer_side_straight.hpp>
#include <boost/geometry/strategies/cartesian/centroid_average.hpp>
#include <boost/geometry/strategies/cartesian/centroid_bashein_detmer.hpp>
#include <boost/geometry/strategies/cartesian/centroid_weighted_length.hpp>
#include <boost/geometry/strategies/cartesian/densify.hpp>
#include <boost/geometry/strategies/cartesian/disjoint_segment_box.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras_point_box.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras_box_box.hpp>
#include <boost/geometry/strategies/cartesian/distance_projected_point.hpp>
#include <boost/geometry/strategies/cartesian/distance_projected_point_ax.hpp>
#include <boost/geometry/strategies/cartesian/distance_segment_box.hpp>
#include <boost/geometry/strategies/cartesian/envelope_segment.hpp>
#include <boost/geometry/strategies/cartesian/intersection.hpp>
#include <boost/geometry/strategies/cartesian/point_in_box.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_franklin.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_crossings_multiply.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/cartesian/line_interpolate.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>

#include <boost/geometry/strategies/spherical/area.hpp>
#include <boost/geometry/strategies/spherical/azimuth.hpp>
#include <boost/geometry/strategies/spherical/densify.hpp>
#include <boost/geometry/strategies/spherical/disjoint_segment_box.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track_box_box.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track_point_box.hpp>
#include <boost/geometry/strategies/spherical/distance_segment_box.hpp>
#include <boost/geometry/strategies/spherical/compare.hpp>
#include <boost/geometry/strategies/spherical/envelope_segment.hpp>
#include <boost/geometry/strategies/spherical/intersection.hpp>
#include <boost/geometry/strategies/spherical/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/spherical/line_interpolate.hpp>
#include <boost/geometry/strategies/spherical/ssf.hpp>

#include <boost/geometry/strategies/geographic/area.hpp>
#include <boost/geometry/strategies/geographic/azimuth.hpp>
#include <boost/geometry/strategies/geographic/buffer_point_circle.hpp>
#include <boost/geometry/strategies/geographic/densify.hpp>
#include <boost/geometry/strategies/geographic/disjoint_segment_box.hpp>
#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/distance_andoyer.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track_box_box.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track_point_box.hpp>
#include <boost/geometry/strategies/geographic/distance_segment_box.hpp>
#include <boost/geometry/strategies/geographic/distance_thomas.hpp>
#include <boost/geometry/strategies/geographic/distance_vincenty.hpp>
#include <boost/geometry/strategies/geographic/envelope_segment.hpp>
#include <boost/geometry/strategies/geographic/intersection.hpp>
//#include <boost/geometry/strategies/geographic/intersection_elliptic.hpp>
#include <boost/geometry/strategies/geographic/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/geographic/line_interpolate.hpp>
#include <boost/geometry/strategies/geographic/side.hpp>
#include <boost/geometry/strategies/geographic/side_andoyer.hpp>
#include <boost/geometry/strategies/geographic/side_thomas.hpp>
#include <boost/geometry/strategies/geographic/side_vincenty.hpp>

#include <boost/geometry/strategies/agnostic/buffer_distance_symmetric.hpp>
#include <boost/geometry/strategies/agnostic/buffer_distance_asymmetric.hpp>
#include <boost/geometry/strategies/agnostic/hull_graham_andrew.hpp>
#include <boost/geometry/strategies/agnostic/point_in_box_by_side.hpp>
#include <boost/geometry/strategies/agnostic/point_in_point.hpp>
#include <boost/geometry/strategies/agnostic/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/agnostic/simplify_douglas_peucker.hpp>

#include <boost/geometry/strategies/strategy_transform.hpp>

#include <boost/geometry/strategies/transform/matrix_transformers.hpp>
#include <boost/geometry/strategies/transform/map_transformer.hpp>
#include <boost/geometry/strategies/transform/inverse_transformer.hpp>

#endif // BOOST_GEOMETRY_STRATEGIES_STRATEGIES_HPP