#include <goliath/controller/commands/wunderhorn_command.h>

#include <vector>
#include <chrono>
#include <thread>
#include <boost/log/trivial.hpp>

using namespace goliath::handles;
using namespace goliath;

const std::vector<size_t> leftMotors{HANDLE_LEFT_FRONT_MOTOR,
                                     HANDLE_LEFT_BACK_MOTOR};
const std::vector<size_t> rightMotors{HANDLE_RIGHT_FRONT_MOTOR,
                                      HANDLE_RIGHT_BACK_MOTOR};

commands::WunderhornCommand::WunderhornCommand(const size_t &id)
        : BasicCommand(id, {HANDLE_CAM, HANDLE_I2C_BUS, HANDLE_MOTOR_CONTROLLER,
                            HANDLE_LEFT_FRONT_MOTOR, HANDLE_LEFT_BACK_MOTOR,
                            HANDLE_RIGHT_FRONT_MOTOR, HANDLE_RIGHT_BACK_MOTOR}),
          lineRoi(cv::Mat(480, 640, CV_8U), 0, 240, 639, 239), areaRoi(cv::Mat(480, 640, CV_8U), 220, 390, 200, 80) {
}

void commands::WunderhornCommand::execute(HandleMap &handles, const proto::CommandMessage &message) {
    handleMap = handles;
    vision::Webcam webcam = std::static_pointer_cast<WebcamHandle>(handles[HANDLE_CAM])->getDevice();
    vision::FollowLineDetector followLineDetector(webcam.getFrame(), 4, 40, 10, 20, 10, 10000);

    i2c::I2cSlave controllerSlave(*handles.get<handles::I2cBusHandle>(HANDLE_I2C_BUS),
                                  *handles.get<handles::I2cSlaveHandle>(HANDLE_MOTOR_CONTROLLER));
    motor_controller::MotorController motorController(controllerSlave);

    BOOST_LOG_TRIVIAL(trace) << "following line...";
    follow_line(followLineDetector, webcam, motorController);
    BOOST_LOG_TRIVIAL(trace) << "stopped following line";

    vision::ColorRegionDetector colorRegionDetector(webcam.getRoiFrame(areaRoi), 330, 30, 40, 100);

    BOOST_LOG_TRIVIAL(trace) << "driving into red zone";
    move(0, 50, motorController);

    int count = 0;
    while (count < 20) {
        if (isInterrupted()) {
            move(0, 0, motorController);
            return;
        }

        int detected = static_cast<int>(colorRegionDetector.detect()[0][0]);

        if (detected == 1) {
            BOOST_LOG_TRIVIAL(trace) << "red zone detected, stopping in 500ms";
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
            move(0, 0, motorController);
            BOOST_LOG_TRIVIAL(trace) << "stopped";
            break;
        }

        cv::Mat new_frame = webcam.getFrame();
        followLineDetector.update(new_frame);
        count++;
    }

    BOOST_LOG_TRIVIAL(trace) << "waiting 30 seconds";
    std::this_thread::sleep_for(std::chrono::seconds(30));

    BOOST_LOG_TRIVIAL(trace) << "following line...";
    follow_line(followLineDetector, webcam, motorController);
    BOOST_LOG_TRIVIAL(trace) << "stopped following line";
}

void commands::WunderhornCommand::follow_line(vision::FollowLineDetector &followLineDetector, vision::Webcam &camera,
                                              motor_controller::MotorController &motorController) {
    std::vector<cv::Vec4d> lines = followLineDetector.detect();
    int noLinesCount = 0;
    double lastDirection = 0;

    while (noLinesCount < 10) {
        if (isInterrupted()) {
            move(0, 0, motorController);
            return;
        }

        if (lines[0][0] == vision::FollowLineDirection::NO_LINE) {
            noLinesCount++;
            BOOST_LOG_TRIVIAL(trace) << "no line detected";
        }

        double direction = 0;
        if (lines[0][0] == vision::FollowLineDirection::LEFT) {
            direction = -lines[0][1] * 4;
        } else if (lines[0][0] == vision::FollowLineDirection::RIGHT) {
            direction = lines[0][1] * 4;
        }
        BOOST_LOG_TRIVIAL(trace) << "direction set to " << direction;

        if (direction != lastDirection) {
            move(direction, 50, motorController);
            lastDirection = direction;
        }

        cv::Mat new_frame = camera.getFrame();
        followLineDetector.update(new_frame);
        lines = followLineDetector.detect();
    }

    BOOST_LOG_TRIVIAL(trace) << "setting speed to 0";
    move(0, 0, motorController);
}

void
commands::WunderhornCommand::move(double direction, int speed, motor_controller::MotorController &motorController) {
    std::vector<motor_controller::MotorStatus> commands{};

    double leftSpeed = speed;
    double rightSpeed = speed;

    if (std::abs(direction) > 1) {
        direction = direction / std::abs(direction);
    }

    if (direction < 0) {
        leftSpeed -= leftSpeed * -direction;
        rightSpeed += rightSpeed * -direction;
    } else if (direction > 0) {
        leftSpeed += leftSpeed * direction;
        rightSpeed -= rightSpeed * direction;
    }

    BOOST_LOG_TRIVIAL(trace) << "speed left: " << leftSpeed << "\t right: " << rightSpeed;

    motor_controller::MotorDirection gear = motor_controller::MotorDirection::BACKWARDS;
    if (speed == 0) {
        gear = motor_controller::MotorDirection::LOCKED;
    }

    for (size_t motor : leftMotors) {
        auto handle = handleMap.get<handles::MotorHandle>(motor);
        motor_controller::MotorStatus motorCommand{
                .id = handle->getMotorId(),
                .direction = gear,
                .speed = static_cast<motor_controller::MotorSpeed>(leftSpeed)
        };

        commands.emplace_back(motorCommand);
    }

    for (size_t motor : rightMotors) {
        auto handle = handleMap.get<handles::MotorHandle>(motor);
        motor_controller::MotorStatus motorCommand{
                .id = handle->getMotorId(),
                .direction = gear,
                .speed = static_cast<motor_controller::MotorSpeed>(rightSpeed)
        };

        commands.emplace_back(motorCommand);
    }

    if (!commands.empty()) {
        motorController.sendCommands(commands.begin(), commands.end());
    }
}


/*
* Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
*/

#include <fstream>

/*
* \author ?
*/

#ifndef GEOREFERENCE_CPP
#define GEOREFERENCE_CPP

#include <Eigen/Dense>
#include "../DatagramGeoreferencer.hpp"
#include "../datagrams/DatagramParserFactory.hpp"
#include <iostream>
#include <string>
#include "../utils/Exception.hpp"
#include "../math/Boresight.hpp"

using namespace std;

/**Writes the usage information about the program*/
void printUsage(){
	std::cerr << "\n\
	NAME\n\n\
	georeference - Produit un nuage de points d'un fichier de datagrammes multifaisceaux\n\n\
	SYNOPSIS\n \
	georeference [-x lever_arm_x] [-y lever_arm_y] [-z lever_arm_z] [-r roll_angle] [-p pitch_angle] [-h heading_angle] [-s svp_file] file\n\n\
	DESCRIPTION\n \
	-L Use a local geographic frame (NED)\n \
	-T Use a terrestrial geographic frame (WGS84 ECEF)\n\n \
	Copyright 2017-2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés" << std::endl;
	exit(1);
}

/**
* Declares the parser depending on argument received
*
* @param argc number of argument
* @param argv value of the arguments
*/
int main (int argc , char ** argv){

	#ifdef __GNU__
	setenv("TZ", "UTC", 1);
	#endif
	#ifdef _WIN32
	putenv("TZ");
#endif
if(argc < 2)
{
    printUsage();
}
else
{
    std::string fileName(argv[argc-1]);

    //Lever arm
    double leverArmX = 0.0;
    double leverArmY = 0.0;
    double leverArmZ = 0.0;

    //Boresight
    double roll     = 0.0;
    double pitch    = 0.0;
    double heading  = 0.0;

    //Georeference method
    Georeferencing * georef;

    //SoundVelocityProfile
    SoundVelocityProfile * svp = NULL;
    std::string svpFile = "";
    char svpChar;
    int index;

    while((index=getopt(argc,argv,"x:y:z:r:p:h:s:LT"))!=-1)
    {
        switch(index)
        {
            case 'x':
                if(sscanf(optarg,"%lf", &leverArmX) != 1)
                {
                    std::cerr << "Invalid lever arm X offset (-x)" << std::endl;
                    printUsage();
                }
           break;

            case 'y':
                if (sscanf(optarg,"%lf", &leverArmY) != 1)
                {
                    std::cerr << "Invalid lever arm Y offset (-y)" << std::endl;
                    printUsage();
                }
            break;

            case 'z':
                if (sscanf(optarg,"%lf", &leverArmZ) != 1)
                {
                    std::cerr << "Invalid lever arm Z offset (-z)" << std::endl;
                    printUsage();
                }
            break;

            case 'r':
                if (sscanf(optarg,"%lf", &roll) != 1)
                {
                    std::cerr << "Invalid roll angle offset (-r)" << std::endl;
                    printUsage();
                }
            break;

            case 'h':
                if (sscanf(optarg,"%lf", &heading) != 1)
                {
                    std::cerr << "Invalid heading angle offset (-h)" << std::endl;
                    printUsage();
                }
            break;

            case 'p':
                if (sscanf(optarg,"%lf", &pitch) != 1)
                {
                    std::cerr << "Invalid pitch angle offset (-p)" << std::endl;
                    printUsage();
                }
            break;

            case 's':
                if (sscanf(optarg,"%s",&svpChar) != 1)
                {
                    std::cerr << "Invalid file name offset (-s)" << std::endl;
                    printUsage();
                }
                else
                {
                    svpFile =svpChar;
                }
            break;

            case 'L':
		            georef = new GeoreferencingLGF();
            break;

            case 'T':
		            georef = new GeoreferencingTRF();
            break;
        }
    }

    if(georef == NULL)
    {
	    std::cerr << "No georeferencing method defined (-L or -T)" << std::endl;
	    printUsage();
    }

    try
    {
	    DatagramParser * parser = NULL;
	    DatagramGeoreferencer  printer(*georef);

	    std::cerr << "Decoding " << fileName << std::endl;
      std::ifstream inFile;
      inFile.open(fileName);
      if (inFile)
      {
		    parser = DatagramParserFactory::build(fileName,printer);
      }
      else
      {
        throw new Exception("File not found");
      }
      parser->parse(fileName);
	    std::cout << std::setprecision(6);
	    std::cout << std::fixed;

	//Lever arm
	    Eigen::Vector3d leverArm;
	    leverArm << leverArmX,leverArmY,leverArmZ;

	//Boresight
	    Attitude boresightAngles(0,roll,pitch,heading);
	    Eigen::Matrix3d boresight;
	    Boresight::buildMatrix(boresight,boresightAngles);

         //Sound Velocity Profile
            if(svpFile != "")
            {
               if(svp->read(svpFile) == false)
                {
                   throw new Exception("Svp file not valid");
                }
            }

	//Do the georeference dance
      printer.georeference(leverArm,boresight,svp);

	    delete parser;
    }
    catch(Exception * error)
    {
	    std::cerr << "Error while parsing " << fileName << ": " << error->getMessage() << std::endl;
    }
  }
}

#endif


#ifndef BOOST_MPL_ADVANCE_HPP_INCLUDED
#define BOOST_MPL_ADVANCE_HPP_INCLUDED

// Copyright Aleksey Gurtovoy 2000-2004
//
// 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)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: advance.hpp,v 1.2 2009/02/16 01:51:05 wdong-pku Exp $
// $Date: 2009/02/16 01:51:05 $
// $Revision: 1.2 $

#include <boost/mpl/advance_fwd.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/negate.hpp>
#include <boost/mpl/long.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/tag.hpp>
#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/aux_/advance_forward.hpp>
#include <boost/mpl/aux_/advance_backward.hpp>
#include <boost/mpl/aux_/value_wknd.hpp>
#include <boost/mpl/aux_/na_spec.hpp>
#include <boost/mpl/aux_/nttp_decl.hpp>

namespace boost { namespace mpl {

// default implementation for forward/bidirectional iterators
template< typename Tag >
struct advance_impl
{
    template< typename Iterator, typename N > struct apply
    {
        typedef typename less< N,long_<0> >::type backward_;
        typedef typename if_< backward_, negate<N>, N >::type offset_;

        typedef typename if_<
              backward_
            , aux::advance_backward< BOOST_MPL_AUX_VALUE_WKND(offset_)::value >
            , aux::advance_forward< BOOST_MPL_AUX_VALUE_WKND(offset_)::value >
            >::type f_;

        typedef typename apply_wrap1<f_,Iterator>::type type;
    };
};

template<
      typename BOOST_MPL_AUX_NA_PARAM(Iterator)
    , typename BOOST_MPL_AUX_NA_PARAM(N)
    >
struct advance
    : advance_impl< typename tag<Iterator>::type >
        ::template apply<Iterator,N>
{
};

template<
      typename Iterator
    , BOOST_MPL_AUX_NTTP_DECL(long, N)
    >
struct advance_c
    : advance_impl< typename tag<Iterator>::type >
        ::template apply<Iterator,long_<N> >
{
};

BOOST_MPL_AUX_NA_SPEC(2, advance)

}}

#endif // BOOST_MPL_ADVANCE_HPP_INCLUDED


#pragma once

#include "modules/GenericModule.hpp"
#include "MicroPackets.hpp"

#include "motion-control/DribblerController.hpp"
#include "motion-control/RobotController.hpp"
#include "motion-control/RobotEstimator.hpp"

#include <Eigen/Dense>
#include "LockedStruct.hpp"

/**
 * Module handling robot state estimation and motion control for motors
 */
class MotionControlModule : public GenericModule {
public:
    /**
     * Number of times per second (frequency) that MotionControlModule should run (Hz)
     */
    static constexpr float kFrequency = 200.0f;

    /**
     * Number of seconds elapsed (period) between MotionControlModule runs (milliseconds)
     */
    static constexpr std::chrono::milliseconds kPeriod{static_cast<int>(1000 / kFrequency)};

    /**
     * Priority used by RTOS
     */
    static constexpr int kPriority = 3;

    /**
     * Constructor for MotionControlModule
     * @param batteryVoltage Shared memory location containing data on battery voltage and critical status
     * @param imuData Shared memory location containing linear acceleration and angular velocity along/about X,Y, and Z axes
     * @param motionCommand Shared memory location containing dribbler rotation, x and y linear velocity, z angular velocity
     * @param motorFeedback Shared memory location containing encoder counts and currents to each wheel motor
     * @param motorCommand Shared memory location containing wheel motor duty cycles and dribbler rotation speed
     */
    MotionControlModule(LockedStruct<BatteryVoltage>& batteryVoltage,
                        LockedStruct<IMUData>& imuData,
                        LockedStruct<MotionCommand>& motionCommand,
                        LockedStruct<MotorFeedback>& motorFeedback,
                        LockedStruct<MotorCommand>& motorCommand);

    /**
     * Code to run when called by RTOS once per system tick (`kperiod`)
     *
     * Uses IMU Data, motion data, motor data, and battery voltage data to update robot state estimation
     * and motor controllers.
     */
    void entry() override;

private:
    /**
     * Checks the update time versus the current time and timout given below
     */
    bool isRecentUpdate(uint32_t lastUpdateTime);

    LockedStruct<BatteryVoltage>& batteryVoltage;
    LockedStruct<IMUData>& imuData;
    LockedStruct<MotionCommand>& motionCommand;
    LockedStruct<MotorFeedback>& motorFeedback;
    LockedStruct<MotorCommand>& motorCommand;

    DribblerController dribblerController;
    RobotController robotController;
    RobotEstimator robotEstimator;

    Eigen::Matrix<float, 4, 1> prevCommand;

    /**
     * Max amount of time that can elapse from the latest
     * command from the radio (milliseconds)
     *
     * This is a safety feature to prevent the motors from moving
     * if the motion control dies
     */
    static constexpr uint32_t COMMAND_TIMEOUT = 250;
};


/*
 * filter_ext.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <scitbx/array_family/flex_types.h>
#include <dials/algorithms/filtering/filter.h>

namespace dials { namespace algorithms { namespace filter { namespace boost_python {

  using namespace boost::python;

  using scitbx::af::flex_bool;

  af::shared<bool> by_detector_mask_multipanel_wrapper(
    const af::const_ref<std::size_t> &panel,
    const af::const_ref<int6> bboxes,
    boost::python::tuple mask_tuple,
    int2 scan_range) {
    af::shared<af::const_ref<bool, af::c_grid<2> > > mask(len(mask_tuple));
    for (std::size_t i = 0; i < mask.size(); ++i) {
      flex_bool temp = extract<flex_bool>(mask_tuple[i]);
      DIALS_ASSERT(temp.accessor().all().size() == 2);
      mask[i] = af::const_ref<bool, af::c_grid<2> >(
        &temp[0], af::c_grid<2>(temp.accessor().all()[0], temp.accessor().all()[1]));
    }

    return by_detector_mask_multipanel(panel, bboxes, mask.const_ref(), scan_range);
  }

  void export_is_zeta_valid() {
    def("is_zeta_valid",
        (bool (*)(vec3<double>, vec3<double>, vec3<double>, double)) & is_zeta_valid,
        (arg("m2"), arg("s0"), arg("s1"), arg("zeta_min")));
    def("is_zeta_valid",
        (bool (*)(const CoordinateSystem &, double)) & is_zeta_valid,
        (arg("cs"), arg("zeta_min")));
    def("is_zeta_valid",
        (bool (*)(const Goniometer &, const BeamBase &, vec3<double>, double))
          & is_zeta_valid,
        (arg("g"), arg("b"), arg("s1"), arg("zeta_min")));
  }

  void export_is_xds_small_angle_valid() {
    def("is_xds_small_angle_valid",
        (bool (*)(vec3<double>, vec3<double>, vec3<double>, double))
          & is_xds_small_angle_valid,
        (arg("m2"), arg("s0"), arg("s1"), arg("delta_m")));
    def("is_xds_small_angle_valid",
        (bool (*)(const CoordinateSystem &, double)) & is_xds_small_angle_valid,
        (arg("cs"), arg("delta_m")));
    def("is_xds_small_angle_valid",
        (bool (*)(const Goniometer &, const BeamBase &, vec3<double>, double))
          & is_xds_small_angle_valid,
        (arg("g"), arg("b"), arg("s1"), arg("delta_m")));
  }

  void export_is_xds_angle_valid() {
    def(
      "is_xds_angle_valid",
      (bool (*)(vec3<double>, vec3<double>, vec3<double>, double)) & is_xds_angle_valid,
      (arg("m2"), arg("s0"), arg("s1"), arg("delta_m")));
    def("is_xds_angle_valid",
        (bool (*)(const CoordinateSystem &, double)) & is_xds_angle_valid,
        (arg("cs"), arg("delta_m")));
    def("is_xds_angle_valid",
        (bool (*)(const Goniometer &, const BeamBase &, vec3<double>, double))
          & is_xds_angle_valid,
        (arg("g"), arg("b"), arg("s1"), arg("delta_m")));
  }

  void export_filter_list() {
    def("by_zeta", &by_zeta, (arg("g"), arg("b"), arg("r"), arg("min_zeta")));
    def("by_xds_small_angle",
        &by_xds_small_angle,
        (arg("g"), arg("b"), arg("r"), arg("delta_m")));
    def("by_xds_angle", &by_xds_angle, (arg("g"), arg("b"), arg("r"), arg("delta_m")));
    def(
      "by_bbox_volume",
      (af::shared<bool>(*)(const af::const_ref<int6> &, std::size_t)) & by_bbox_volume,
      (arg("bbox"), arg("num")));
    def("by_bbox_volume",
        (af::shared<bool>(*)(const af::const_ref<int6> &)) & by_bbox_volume,
        (arg("bbox")));

    def("is_bbox_outside_image_range", &is_bbox_outside_image_range);
    def("does_bbox_contain_bad_pixels", &does_bbox_contain_bad_pixels);
    def("is_bbox_valid", &is_bbox_valid);
    def("by_detector_mask", &by_detector_mask);
    def("by_detector_mask", &by_detector_mask_multipanel_wrapper);

    def("by_resolution_at_centroid", &by_resolution_at_centroid);

    def("by_shoebox_mask", &by_shoebox_mask);
  }

  BOOST_PYTHON_MODULE(dials_algorithms_filter_ext) {
    export_is_zeta_valid();
    export_is_xds_small_angle_valid();
    export_is_xds_angle_valid();
    export_filter_list();
  }

}}}}  // namespace dials::algorithms::filter::boost_python


/*    Copyright (c) 2010-2017, 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_MAIN

#include <iomanip>

#include <Tudat/SimulationSetup/tudatEstimationHeader.h>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <Eigen/Core>
#include "Tudat/Basics/testMacros.h"

#include "Tudat/SimulationSetup/PropagationSetup/propagationLambertTargeterFullProblem.h"
#include "Tudat/SimulationSetup/PropagationSetup/propagationPatchedConicFullProblem.h"
#include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositions.h"
#include "Tudat/Astrodynamics/TrajectoryDesign/trajectory.h"
#include "Tudat/Astrodynamics/TrajectoryDesign/exportTrajectory.h"
#include "Tudat/Astrodynamics/TrajectoryDesign/planetTrajectory.h"

namespace tudat
{

namespace unit_tests
{

using namespace tudat;

//! Test of the full propagation of a trajectory
BOOST_AUTO_TEST_SUITE( testFullPropagationTrajectory )

//! Test of the full propagation of a MGA trajectory
BOOST_AUTO_TEST_CASE( testFullPropagationMGA )
{

    std::cout.precision(20);

    //    std::cout << "Cassini trajectory: " << "\n\n";

    // Specify number and type of legs.
    int numberOfLegs = 6;
    std::vector< transfer_trajectories::TransferLegType > legTypeVector;
    legTypeVector.resize( numberOfLegs );
    legTypeVector[ 0 ] = transfer_trajectories::mga_Departure;
    legTypeVector[ 1 ] = transfer_trajectories::mga_Swingby;
    legTypeVector[ 2 ] = transfer_trajectories::mga_Swingby;
    legTypeVector[ 3 ] = transfer_trajectories::mga_Swingby;
    legTypeVector[ 4 ] = transfer_trajectories::mga_Swingby;
    legTypeVector[ 5 ] = transfer_trajectories::capture;

    // Name of the bodies involved in the trajectory
    std::vector< std::string > nameBodiesTrajectory;
    nameBodiesTrajectory.push_back("Earth");
    nameBodiesTrajectory.push_back("Venus");
    nameBodiesTrajectory.push_back("Mars");
    nameBodiesTrajectory.push_back("Earth");
    nameBodiesTrajectory.push_back("Jupiter");
    nameBodiesTrajectory.push_back("Saturn");

    std::vector< std::string > centralBody; centralBody.push_back( "Sun" );
    std::string bodyToPropagate = "spacecraft";

    spice_interface::loadStandardSpiceKernels( );

    // Define gravitational parameter for each transfer body.
    std::vector< double > gravitationalParametersTransferBodies;
    gravitationalParametersTransferBodies.push_back( simulation_setup::createGravityFieldModel( simulation_setup::getDefaultGravityFieldSettings(
                                                                                "Earth", TUDAT_NAN, TUDAT_NAN), "Earth")->getGravitationalParameter());
    gravitationalParametersTransferBodies.push_back( simulation_setup::createGravityFieldModel( simulation_setup::getDefaultGravityFieldSettings(
                                                                                "Venus", TUDAT_NAN, TUDAT_NAN), "Venus")->getGravitationalParameter());
    gravitationalParametersTransferBodies.push_back( simulation_setup::createGravityFieldModel( simulation_setup::getDefaultGravityFieldSettings(
                                                                                "Venus", TUDAT_NAN, TUDAT_NAN), "Venus")->getGravitationalParameter());
    gravitationalParametersTransferBodies.push_back( simulation_setup::createGravityFieldModel( simulation_setup::getDefaultGravityFieldSettings(
                                                                                "Earth", TUDAT_NAN, TUDAT_NAN), "Earth")->getGravitationalParameter());
    gravitationalParametersTransferBodies.push_back( simulation_setup::createGravityFieldModel( simulation_setup::getDefaultGravityFieldSettings(
                                                                                "Jupiter", TUDAT_NAN, TUDAT_NAN), "Jupiter")->getGravitationalParameter());
    gravitationalParametersTransferBodies.push_back( simulation_setup::createGravityFieldModel( simulation_setup::getDefaultGravityFieldSettings(
                                                                                "Saturn", TUDAT_NAN, TUDAT_NAN), "Saturn")->getGravitationalParameter());

    // Create variable vector.
    std::vector< double > variableVector;
    variableVector.push_back(-789.8117 * physical_constants::JULIAN_DAY); variableVector.push_back(158.302027105278 * physical_constants::JULIAN_DAY);
    variableVector.push_back(449.385873819743 * physical_constants::JULIAN_DAY); variableVector.push_back(54.7489684339665 * physical_constants::JULIAN_DAY);
    variableVector.push_back(1024.36205846918 * physical_constants::JULIAN_DAY); variableVector.push_back(4552.30796805542 * physical_constants::JULIAN_DAY);
    variableVector.push_back(1.0 * physical_constants::JULIAN_DAY);

    double initialTime = variableVector[0];

    // Define ephemerides for the transfer bodies
    std::vector< ephemerides::EphemerisPointer > ephemerisVectorTransferBodies;
    ephemerides::EphemerisPointer ephemerisEarth = std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter) ;
    ephemerides::EphemerisPointer ephemerisVenus = std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus) ;
    ephemerides::EphemerisPointer ephemerisJupiter = std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::jupiter) ;
    ephemerides::EphemerisPointer ephemerisSaturn = std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::saturn) ;
    ephemerisVectorTransferBodies.push_back( ephemerisEarth );
    ephemerisVectorTransferBodies.push_back( ephemerisVenus );
    ephemerisVectorTransferBodies.push_back( ephemerisVenus );
    ephemerisVectorTransferBodies.push_back( ephemerisEarth );
    ephemerisVectorTransferBodies.push_back( ephemerisJupiter );
    ephemerisVectorTransferBodies.push_back( ephemerisSaturn );

    // Create body map.
    simulation_setup::NamedBodyMap bodyMap = propagators::setupBodyMapFromUserDefinedEphemeridesForPatchedConicsTrajectory(centralBody[0],
            bodyToPropagate, nameBodiesTrajectory, ephemerisVectorTransferBodies, gravitationalParametersTransferBodies, "ECLIPJ2000" );

    // Create acceleration map.
    std::vector< basic_astrodynamics::AccelerationMap > accelerationMap = propagators::setupAccelerationMapPatchedConicsTrajectory(
                nameBodiesTrajectory.size(), centralBody[0], bodyToPropagate, bodyMap);

    // Create departure and capture variables.
    std::vector< double > semiMajorAxes;
    semiMajorAxes.push_back( std::numeric_limits< double >::infinity( ) ); semiMajorAxes.push_back( 1.0895e8 / 0.02 );
    std::vector< double > eccentricities;
    eccentricities.push_back( 0.0 ); eccentricities.push_back( 0.98 );

    // Create minimum pericenter radii vector
    std::vector< double > minimumPericenterRadii;
    minimumPericenterRadii.push_back( 6778000.0 ); minimumPericenterRadii.push_back( 6351800.0 ); minimumPericenterRadii.push_back( 6351800.0 );
    minimumPericenterRadii.push_back( 6778000.0 ); minimumPericenterRadii.push_back( 600000000.0 ); minimumPericenterRadii.push_back( 600000000.0 );

    // Define integrator settings.
    double fixedStepSize = 3600.0;
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared < numerical_integrators::IntegratorSettings < > > ( numerical_integrators::rungeKutta4, initialTime, fixedStepSize);

    // Compute difference between patched conics trajectory and full problem at departure and at arrival for each leg.
    for( int terminationType = 0; terminationType < 2; terminationType++ )
    {
        std::map< int, std::map< double, Eigen::Vector6d > > patchedConicsResultForEachLeg;
        std::map< int, std::map< double, Eigen::Vector6d > > fullProblemResultForEachLeg;
        std::map< int, std::map< double, Eigen::VectorXd > > dependentVariableResultForEachLeg;

        propagators::fullPropagationPatchedConicsTrajectory(
                    bodyMap, accelerationMap, nameBodiesTrajectory,
                    centralBody[0], bodyToPropagate, legTypeVector, variableVector, minimumPericenterRadii,
                semiMajorAxes, eccentricities, integratorSettings, patchedConicsResultForEachLeg, fullProblemResultForEachLeg,
                dependentVariableResultForEachLeg, static_cast< bool >( terminationType ) );

        for( auto itr : patchedConicsResultForEachLeg )
        {
            for( auto innerItr : itr.second )
            {
                Eigen::Vector6d stateDifference = patchedConicsResultForEachLeg[ itr.first ][ innerItr.first ] -
                        fullProblemResultForEachLeg[ itr.first ][ innerItr.first ];
                for( int i = 0; i < 3; i++ )
                {
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( i ) ), 1.0 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( i + 3 ) ), 1.0E-6 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( i ) ), 1.0 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( i + 3 ) ), 1.0E-6 );
                }
            }
        }

        int numberOfLegs = patchedConicsResultForEachLeg.size( );
        auto legIterator = patchedConicsResultForEachLeg.begin( );
        auto nextLegIterator = patchedConicsResultForEachLeg.begin( );
        nextLegIterator++;

        for( int i = 0; i < numberOfLegs - 1; i++ )
        {
            double legTimeDifference =
                    nextLegIterator->second.begin( )->first - legIterator->second.rbegin( )->first;
            Eigen::Vector6d legStateDifference =
                    nextLegIterator->second.begin( )->second - legIterator->second.rbegin( )->second;

            if( terminationType == 0 )
            {
                BOOST_CHECK_SMALL( std::fabs( legTimeDifference ), 1.0E-6 );
                if( i != 1 )
                {
                    BOOST_CHECK_SMALL( std::fabs( legStateDifference.segment( 0, 3 ).norm( ) ), 1.0 );
                }
            }
            else
            {
                BOOST_CHECK_EQUAL( std::fabs( legTimeDifference ) > 1.0E5, true );
            }

//            std::cout<<"Time diff. "<<legTimeDifference<<std::endl;

//            std::cout<<std::setprecision( 16 )<<"State. "<<( legIterator->second.rbegin( )->second ).transpose( )<<std::endl; //Wrong
//            std::cout<<std::setprecision( 16 )<<"State. "<<( nextLegIterator->second.begin( )->second ).transpose( )<<std::endl; //Correct
//            std::cout<<std::setprecision( 16 )<<"Venus state. "<<ephemerisVenus->getCartesianState( nextLegIterator->second.begin( )->first ).transpose( )<<std::endl<<std::endl;

//            std::cout<<"State diff. "<<legStateDifference.transpose( )<<std::endl<<std::endl;

            legIterator++;
            nextLegIterator++;
        }
//        std::cout<<std::endl<<std::endl;
    }
}

//! Test of the full propagation of a MGA trajectory including deep-space manoeuvres
BOOST_AUTO_TEST_CASE( testFullPropagationMGAwithDSM )
{

    //    std::cout << "Messenger trajectory: " << "\n\n";

    // Specify number and type of legs.
    int numberOfLegs = 5;
    std::vector< transfer_trajectories::TransferLegType > legTypeVector;
    legTypeVector.resize( numberOfLegs );
    legTypeVector[ 0 ] = transfer_trajectories::mga1DsmVelocity_Departure;
    legTypeVector[ 1 ] = transfer_trajectories::mga1DsmVelocity_Swingby;
    legTypeVector[ 2 ] = transfer_trajectories::mga1DsmVelocity_Swingby;
    legTypeVector[ 3 ] = transfer_trajectories::mga1DsmVelocity_Swingby;
    legTypeVector[ 4 ] = transfer_trajectories::capture;

    // Name of the bodies involved in the trajectory
    std::vector< std::string > nameBodiesAndManoeuvresTrajectory;
    nameBodiesAndManoeuvresTrajectory.push_back("Earth");
    nameBodiesAndManoeuvresTrajectory.push_back("DSM1");
    nameBodiesAndManoeuvresTrajectory.push_back("Earth");
    nameBodiesAndManoeuvresTrajectory.push_back("DSM2");
    nameBodiesAndManoeuvresTrajectory.push_back("Venus");
    nameBodiesAndManoeuvresTrajectory.push_back("DSM3");
    nameBodiesAndManoeuvresTrajectory.push_back("Venus");
    nameBodiesAndManoeuvresTrajectory.push_back("DSM4");
    nameBodiesAndManoeuvresTrajectory.push_back("Mercury");

    std::vector< std::string > transferBodyTrajectory;
    transferBodyTrajectory.push_back("Earth");
    transferBodyTrajectory.push_back("Earth");
    transferBodyTrajectory.push_back("Venus");
    transferBodyTrajectory.push_back("Venus");
    transferBodyTrajectory.push_back("Mercury");

    std::vector< std::string > centralBody; centralBody.push_back( "Sun" );
    std::string bodyToPropagate = "spacecraft";

    // Create body map.
    simulation_setup::NamedBodyMap bodyMap = propagators::setupBodyMapFromEphemeridesForPatchedConicsTrajectory(centralBody[0],
            bodyToPropagate, transferBodyTrajectory);

    // Create acceleration map.
    std::vector< basic_astrodynamics::AccelerationMap > accelerationMap = propagators::setupAccelerationMapPatchedConicsTrajectory(
                transferBodyTrajectory.size(), centralBody[0], bodyToPropagate, bodyMap);

    // Create variable vector.
    std::vector< double > variableVector;

    // Add the time of flight and start epoch.
    variableVector.push_back( 1171.64503236 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 399.999999715 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 178.372255301 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 299.223139512 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 180.510754824 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 1.0); // The capture time is irrelevant for the final leg.

    // Add the additional variables.
    // 1st leg.
    variableVector.push_back( 0.234594654679 );
    variableVector.push_back( 1408.99421278 );
    variableVector.push_back( 0.37992647165 * 2 * 3.14159265358979 );
    variableVector.push_back( std::acos(  2 * 0.498004040298 - 1. ) - 3.14159265358979 / 2 );
    // 2nd leg.
    variableVector.push_back( 0.0964769387134 );
    variableVector.push_back( 1.35077257078 );
    variableVector.push_back( 1.80629232251 * 6.378e6 );
    variableVector.push_back( 0.0 );
    // 3rd leg.
    variableVector.push_back( 0.829948744508);
    variableVector.push_back( 1.09554368115 );
    variableVector.push_back( 3.04129845698 * 6.052e6 );
    variableVector.push_back( 0.0 );
    // 4th leg.
    variableVector.push_back( 0.317174785637 );
    variableVector.push_back( 1.34317576594 );
    variableVector.push_back( 1.10000000891 * 6.052e6 );
    variableVector.push_back( 0.0 );

    // Create minimum pericenter radii vector
    std::vector< double > minimumPericenterRadii;
    minimumPericenterRadii.push_back( TUDAT_NAN ); minimumPericenterRadii.push_back( TUDAT_NAN ); minimumPericenterRadii.push_back( TUDAT_NAN );
    minimumPericenterRadii.push_back( TUDAT_NAN ); minimumPericenterRadii.push_back( TUDAT_NAN );

    // Create departure and capture variables.
    std::vector< double > semiMajorAxes;
    semiMajorAxes.push_back( std::numeric_limits< double >::infinity( ) ); semiMajorAxes.push_back( std::numeric_limits< double >::infinity( ) );
    std::vector< double > eccentricities;
    eccentricities.push_back( 0.0 ); eccentricities.push_back( 0.0 );

    // Define integrator settings.
    double initialTime = 0.0;
    double fixedStepSize = 1000.0;
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared < numerical_integrators::IntegratorSettings < > >(
                numerical_integrators::rungeKutta4, initialTime, fixedStepSize );

    // Compute difference between patched conics trajectory and full problem at departure and at arrival for each leg.
    // Compute difference between patched conics trajectory and full problem at departure and at arrival for each leg.
    for( int terminationType = 0; terminationType < 2; terminationType++ )
    {
        std::map< int, std::map< double, Eigen::Vector6d > > patchedConicsResultForEachLeg;
        std::map< int, std::map< double, Eigen::Vector6d > > fullProblemResultForEachLeg;
        std::map< int, std::map< double, Eigen::VectorXd > > dependentVariableResultForEachLeg;

        //    std::map< int, std::pair< Eigen::Vector6d, Eigen::Vector6d > > differenceStateArrivalAndDeparturePerLeg =
        tudat::propagators::fullPropagationPatchedConicsTrajectory(
                    bodyMap, accelerationMap, transferBodyTrajectory, centralBody[0], bodyToPropagate, legTypeVector, variableVector,
                minimumPericenterRadii, semiMajorAxes, eccentricities, integratorSettings, patchedConicsResultForEachLeg,
                fullProblemResultForEachLeg, dependentVariableResultForEachLeg,
                static_cast< bool >( terminationType ) );

        for( auto itr : patchedConicsResultForEachLeg )
        {
            for( auto innerItr : itr.second )
            {
                Eigen::Vector6d stateDifference = patchedConicsResultForEachLeg[ itr.first ][ innerItr.first ] -
                        fullProblemResultForEachLeg[ itr.first ][ innerItr.first ];
                for( int i = 0; i < 3; i++ )
                {
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( i ) ), 1.0 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( i + 3 ) ), 1.0E-6 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( i ) ), 1.0 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( i + 3 ) ), 1.0E-6 );
                }
            }
        }

        int numberOfLegs = patchedConicsResultForEachLeg.size( );
        auto legIterator = patchedConicsResultForEachLeg.begin( );
        auto nextLegIterator = patchedConicsResultForEachLeg.begin( );
        nextLegIterator++;

        for( int i = 0; i < numberOfLegs - 1; i++ )
        {
            double legTimeDifference =
                    nextLegIterator->second.begin( )->first - legIterator->second.rbegin( )->first;
            Eigen::Vector6d legStateDifference =
                    nextLegIterator->second.begin( )->second - legIterator->second.rbegin( )->second;
//            std::cout<<"Time diff. "<<legTimeDifference<<std::endl;
//            std::cout<<"State diff. "<<legStateDifference.transpose( )<<std::endl<<std::endl;

            if( terminationType == 0 )
            {
                BOOST_CHECK_SMALL( std::fabs( legTimeDifference ), 1.0E-6 );
                BOOST_CHECK_SMALL( std::fabs( legStateDifference.segment( 0, 3 ).norm( ) ), 1.0 );
            }
            else if( i % 2 == 0 )
            {
                BOOST_CHECK_SMALL( std::fabs( legTimeDifference ), 1.0E-6 );
                BOOST_CHECK_SMALL( std::fabs( legStateDifference.segment( 0, 3 ).norm( ) ), 1.0 );
            }
            else
            {
                BOOST_CHECK_EQUAL( std::fabs( legTimeDifference ) > 1.0E5, true );
            }

            legIterator++;
            nextLegIterator++;
        }
//        std::cout<<std::endl<<std::endl;
    }
}

}

}

}


#include <boost/proto/debug.hpp>
#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/include/functions/fma.hpp>
#include <boost/simd/include/functions/optimize.hpp>

int main()
{
    using boost::simd::pack;
    const pack<float> a, b, c, d;

    boost::proto::display_expr(
      boost::simd::optimize
      (
        a + b * (a + b * (a + b * (a + b * c)))
      + b * (b * (b * (b * c + a) + a) + a) + a
      )
    );
}


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file orea/aggregation/dimcalculator.hpp
    \brief Dynamic Initial Margin calculator base class
    \ingroup analytics
*/

#pragma once

#include <orea/aggregation/collatexposurehelper.hpp>
#include <orea/cube/cubeinterpretation.hpp>
#include <orea/cube/inmemorycube.hpp>
#include <orea/scenario/aggregationscenariodata.hpp>

#include <ored/portfolio/nettingsetmanager.hpp>
#include <ored/portfolio/portfolio.hpp>
#include <ored/report/report.hpp>

#include <ql/time/date.hpp>

#include <boost/shared_ptr.hpp>

namespace ore {
namespace analytics {
using namespace QuantLib;
using namespace QuantExt;
using namespace data;
using namespace std;

//! Dynamic Initial Margin Calculator base class
/*!
  Derived classes implement a constructor with the relavant additional input data
  and a build function that performs the DIM calculations for all netting sets and
  along all paths.
*/
class DynamicInitialMarginCalculator {
public:
    DynamicInitialMarginCalculator(
        //! Driving portfolio consistent with the cube below
        const boost::shared_ptr<Portfolio>& portfolio,
        //! NPV cube resulting from the Monte Carlo simulation loop
        const boost::shared_ptr<NPVCube>& cube,
        //! Interpretation of the cube, regular NPV, MPoR grid etc
        const boost::shared_ptr<CubeInterpretation>& cubeInterpretation,
        //! Additional output of the MC simulation loop with numeraires, index fixings, FX spots etc
        const boost::shared_ptr<AggregationScenarioData>& scenarioData,
        //! VaR quantile, e.g. 0.99 for 99%
        Real quantile = 0.99,
        //! VaR holding period in calendar days
        Size horizonCalendarDays = 14,
	//! Actual t0 IM by netting set used to scale the DIM evolution, no scaling if the argument is omitted
	const std::map<std::string, Real>& currentIM = std::map<std::string, Real>());

    virtual ~DynamicInitialMarginCalculator() {}

    //! Model implied t0 DIM by netting set, does not need a call to build() before
    virtual map<string, Real> unscaledCurrentDIM() = 0;

    //! t0 IM by netting set, as provided as an arguments
    map<string, Real> currentIM() { return currentIM_; }

    //! Compute dynamic initial margin along all paths and fill result structurues
    virtual void build() = 0;

    //! DIM evolution report
    virtual void exportDimEvolution(ore::data::Report& dimEvolutionReport);

    //! DIM by nettingSet, date, sample returned as a regular NPV cube
    const boost::shared_ptr<NPVCube>& dimCube() { return dimCube_; }

    //! DIM matrix by date and sample index for the specified netting set
    const vector<vector<Real>>& dynamicIM(const string& nettingSet);

    //! Cash flow matrix by date and sample index for the specified netting set
    const vector<vector<Real>>& cashFlow(const string& nettingSet);

    //! Expected DIM vector by date for the specified netting set
    const vector<Real>& expectedIM(const string& nettingSet);

    //! Get the implied netting set specific scaling factors
    const std::map<std::string, Real>& getInitialMarginScaling() { return nettingSetScaling_; }

protected:
    boost::shared_ptr<Portfolio> portfolio_;
    boost::shared_ptr<NPVCube> cube_, dimCube_;
    boost::shared_ptr<CubeInterpretation> cubeInterpretation_;
    boost::shared_ptr<AggregationScenarioData> scenarioData_;
    Real quantile_;
    Size horizonCalendarDays_;
    map<string, Real> currentIM_;

    bool cubeIsRegular_;
    Size datesLoopSize_;
    vector<string> nettingSetIds_;
    map<string, Real> nettingSetScaling_;

    // For each netting set: matrix of values by date and sample, aggregated over trades
    map<string, vector<vector<Real>>> nettingSetNPV_;
    map<string, vector<vector<Real>>> nettingSetCloseOutNPV_;
    map<string, vector<vector<Real>>> nettingSetFLOW_;
    map<string, vector<vector<Real>>> nettingSetDeltaNPV_;
    map<string, vector<vector<Real>>> nettingSetDIM_;

    // For each netting set: vector of values by date, aggregated over trades and samples
    map<string, vector<Real>> nettingSetExpectedDIM_;

};

} // namespace analytics
} // namespace ore


#include <FaceDetectorOpencv.hpp>
//#include <trace.hpp>
//#include <utils.hpp>
//#include <ModernPosit.h>
//#include <boost/program_options.hpp>
using namespace cv::dnn;
using namespace cv;
  const float BBOX_SCALE = 0.3f;
  const cv::Size FACE_SIZE = cv::Size(256,256);
  const cv::Scalar meanVal(104.0, 177.0, 123.0);

void FaceDetectorOpencv::detectFaceOpenCVDNN(const cv::Mat& frameOpenCVDNN,  std::vector<Rect> &faces,
    Net net)
{
    int frameHeight = frameOpenCVDNN.rows;
    int frameWidth = frameOpenCVDNN.cols;
#ifdef CAFFE
        cv::Mat inputBlob = cv::dnn::blobFromImage(frameOpenCVDNN, inScaleFactor, cv::Size(inWidth, inHeight), meanVal, false, false);
#else
        cv::Mat inputBlob = cv::dnn::blobFromImage(frameOpenCVDNN, inScaleFactor, cv::Size(inWidth, inHeight), meanVal, true, false);
#endif

    net.setInput(inputBlob, "data");
    cv::Mat detection = net.forward("detection_out");

    cv::Mat detectionMat(detection.size[2], detection.size[3], CV_32F, detection.ptr<float>());
    faces.clear();
    for(int i = 0; i < detectionMat.rows; i++)
    {
        float confidence = detectionMat.at<float>(i, 2);

        if(confidence > confidenceThreshold)
        {
            int x1 = static_cast<int>(detectionMat.at<float>(i, 3) * frameWidth);
            int y1 = static_cast<int>(detectionMat.at<float>(i, 4) * frameHeight);
            int x2 = static_cast<int>(detectionMat.at<float>(i, 5) * frameWidth);
            int y2 = static_cast<int>(detectionMat.at<float>(i, 6) * frameHeight);

            //faces.push_back(upm::FaceAnnotation());
            //faces[i].bbox.pos = cv::Rect2f(x1,y1,x2-x1,y2-y1);
            std::cout<<"xyxy"<<x1<<" "<<y1<<" "<<x2<<" "<<y2<<std::endl;
            faces.push_back(Rect(Point2i(x1,y1),Point2i(x2,y2)));
            cv::rectangle(frameOpenCVDNN, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0),2, 4);
        }
    }

}

  void FaceDetectorOpencv::load(cv::CommandLineParser parser)
{
  _faceDetectorConfigFile = samples::findFile( parser.get<String>("face_ssd_config_file") );
  _faceDetectorWeightFile = samples::findFile( parser.get<String>("face_ssd_weight_file") );

  std::cout<<"configfile:"<<_faceDetectorConfigFile<<std::endl;
  /*
    if (not face_cascade.load(_cascade_name)) {
      std::cerr << "Cannot load cascade classifier: " << _cascade_name << std::endl;
    }
  */

  net = cv::dnn::readNetFromTensorflow(_faceDetectorWeightFile, _faceDetectorConfigFile);

}

void
FaceDetectorOpencv::process
  (
  cv::Mat frameOpenCVDNN,
  std::vector<Rect> &faces
  )
{
  detectFaceOpenCVDNN(frameOpenCVDNN,  faces, net);
  //faceDetector(frameOpenCVDNN,  faces, face_cascade);

}


#include <memory>
#include <string>

#include <Eigen/Core>
#include <aslam/common/hash-id.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <maplab-common/unique-id.h>
#include <posegraph/unique-id.h>

#include <vi-map/unique-id.h>
#include <vi-map/vi_map.pb.h>

#include "localization-summary-map/localization-summary-map.h"
#include "localization-summary-map/localization-summary-map.pb.h"

namespace summary_map {

class LocalizationSummaryMapTest : public ::testing::Test {
 protected:
  virtual void SetUp() {}

  void constructLocalizationSummaryMap();
  void serializeAndDeserialize();

  summary_map::LocalizationSummaryMap::Ptr initial_summary_map_;
  summary_map::LocalizationSummaryMap::Ptr summary_map_from_msg_;
};

void LocalizationSummaryMapTest::serializeAndDeserialize() {
  proto::LocalizationSummaryMap summary_map_proto, proto_from_msg;
  CHECK(initial_summary_map_);
  initial_summary_map_->serialize(&summary_map_proto);
  std::string serialized_summary_map = summary_map_proto.SerializeAsString();

  proto_from_msg.ParseFromString(serialized_summary_map);

  summary_map_from_msg_.reset(new summary_map::LocalizationSummaryMap());
  summary_map_from_msg_->deserialize(
      initial_summary_map_->id(), proto_from_msg);
}

void LocalizationSummaryMapTest::constructLocalizationSummaryMap() {
  initial_summary_map_.reset(new summary_map::LocalizationSummaryMap());

  summary_map::LocalizationSummaryMapId id;
  common::generateId(&id);
  initial_summary_map_->setId(id);

  constexpr int kNumLandmarks = 50;
  Eigen::Matrix3Xd G_landmark_position;
  G_landmark_position.resize(Eigen::NoChange, kNumLandmarks);
  G_landmark_position.setRandom();
  initial_summary_map_->setGLandmarkPosition(G_landmark_position);

  constexpr int kNumObservations = 120;
  constexpr int kNumDescriptorDimensions = 10;
  Eigen::MatrixXf descriptors;
  descriptors.resize(kNumDescriptorDimensions, kNumObservations);
  descriptors.setRandom();
  initial_summary_map_->setProjectedDescriptors(descriptors);

  constexpr int kNumKeyframes = 10;
  Eigen::Matrix<unsigned int, Eigen::Dynamic, 1> observer_indices;
  observer_indices.resize(kNumObservations, Eigen::NoChange);
  for (int i = 0; i < kNumObservations; ++i) {
    observer_indices(i, 0) = i % kNumKeyframes;
  }
  initial_summary_map_->setObserverIndices(observer_indices);

  Eigen::Matrix3Xd observer_positions;
  observer_positions.setRandom(3, observer_indices.cols());
  initial_summary_map_->setGObserverPosition(observer_positions);

  Eigen::Matrix<unsigned int, Eigen::Dynamic, 1> observation_to_landmark_index;
  observation_to_landmark_index.resize(kNumObservations, Eigen::NoChange);
  for (int i = 0; i < kNumObservations; ++i) {
    observation_to_landmark_index(i, 0) = i % kNumLandmarks;
  }
  initial_summary_map_->setObservationToLandmarkIndex(
      observation_to_landmark_index);
}

TEST_F(
    LocalizationSummaryMapTest,
    LocalizationSummaryMapSerializeAndDeserializeTest) {
  constructLocalizationSummaryMap();
  serializeAndDeserialize();

  EXPECT_EQ(initial_summary_map_->id(), summary_map_from_msg_->id());
  EXPECT_NEAR_EIGEN(
      initial_summary_map_->GLandmarkPosition(),
      summary_map_from_msg_->GLandmarkPosition(), 1e-10);
  EXPECT_NEAR_EIGEN(
      initial_summary_map_->GObserverPosition(),
      summary_map_from_msg_->GObserverPosition(), 1e-10);
  EXPECT_NEAR_EIGEN(
      initial_summary_map_->projectedDescriptors(),
      summary_map_from_msg_->projectedDescriptors(), 1e-10);
  EXPECT_TRUE(
      initial_summary_map_->observerIndices() ==
      summary_map_from_msg_->observerIndices());
  EXPECT_TRUE(
      initial_summary_map_->observationToLandmarkIndex() ==
      summary_map_from_msg_->observationToLandmarkIndex());

  EXPECT_EQ(*initial_summary_map_, *summary_map_from_msg_);
}

TEST_F(
    LocalizationSummaryMapTest,
    LocalizationSummaryMapSerializeAndDeserializeNotEqualTest) {
  constructLocalizationSummaryMap();
  serializeAndDeserialize();

  constexpr int kNumLandmarks = 50;
  Eigen::Matrix3Xd G_landmark_position;
  G_landmark_position.resize(Eigen::NoChange, kNumLandmarks);
  G_landmark_position.setRandom();
  initial_summary_map_->setGLandmarkPosition(G_landmark_position);

  EXPECT_NE(*initial_summary_map_, *summary_map_from_msg_);
}

}  // namespace summary_map

MAPLAB_UNITTEST_ENTRYPOINT


// Copyright (c) 2019 yshurik
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/assign/list_of.hpp>

#include "base58.h"
#include "rpcserver.h"
#include "txdb.h"
#include "init.h"
#include "main.h"
#include "net.h"
#include "keystore.h"
#include "wallet.h"

#include "pegops.h"
#include "pegdata.h"

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/split.hpp>

using namespace std;
using namespace boost;
using namespace boost::assign;
using namespace json_spirit;

void printpegshift(const CFractions & frPegShift,
                   const CPegLevel & peglevel,
                   Object & result)
{
    int64_t nValue = frPegShift.Positive(nullptr).Total();
    CFractions frLiquid = frPegShift.HighPart(peglevel, nullptr);
    CFractions frReserve = frPegShift.LowPart(peglevel, nullptr);

    int64_t nLiquidNegative = 0;
    int64_t nLiquidPositive = 0;
    frLiquid.Negative(&nLiquidNegative);
    frLiquid.Positive(&nLiquidPositive);

    int64_t nReserveNegative = 0;
    int64_t nReservePositive = 0;
    frReserve.Negative(&nReserveNegative);
    frReserve.Positive(&nReservePositive);

    int64_t nLiquidPart = std::min(nLiquidPositive, -nLiquidNegative);
    int64_t nReservePart = std::min(nReservePositive, -nReserveNegative);

    result.push_back(Pair("pegshift_absvalue", nValue));
    result.push_back(Pair("pegshift_liquidpart", nLiquidPart));
    result.push_back(Pair("pegshift_reservepart", nReservePart));

    CPegData pegdata;
    pegdata.fractions = frPegShift;
    pegdata.peglevel = peglevel;
    pegdata.nLiquid = frPegShift.High(peglevel);
    pegdata.nReserve = frPegShift.Low(peglevel);

    result.push_back(Pair("pegshift_pegdata", pegdata.ToString()));
}

void printpeglevel(const CPegLevel & peglevel,
                   Object & result)
{
    result.push_back(Pair("peglevel", peglevel.ToString()));

    Object info;
    info.push_back(Pair("cycle", peglevel.nCycle));
    info.push_back(Pair("peg", peglevel.nSupply));
    info.push_back(Pair("pegnext", peglevel.nSupplyNext));
    info.push_back(Pair("pegnextnext", peglevel.nSupplyNextNext));
    info.push_back(Pair("shift", peglevel.nShift));
    info.push_back(Pair("shiftlastpart", peglevel.nShiftLastPart));
    info.push_back(Pair("shiftlasttotal", peglevel.nShiftLastTotal));
    result.push_back(Pair("peglevel_info", info));
}

void printpegbalance(const CPegData & pegdata,
                     Object & result,
                     string prefix)
{
    int64_t nValue      = pegdata.fractions.Total();
    int64_t nNChange    = pegdata.fractions.NChange(pegdata.peglevel);

    int16_t nValueHli   = pegdata.fractions.HLI();
    int16_t nLiquidHli  = pegdata.fractions.HighPart(pegdata.peglevel, nullptr).HLI();
    int16_t nReserveHli = pegdata.fractions.LowPart(pegdata.peglevel, nullptr).HLI();

    result.push_back(Pair(prefix+"value", nValue));
    result.push_back(Pair(prefix+"value_hli", nValueHli));
    result.push_back(Pair(prefix+"liquid", pegdata.nLiquid));
    result.push_back(Pair(prefix+"liquid_hli", nLiquidHli));
    result.push_back(Pair(prefix+"reserve", pegdata.nReserve));
    result.push_back(Pair(prefix+"reserve_hli", nReserveHli));
    result.push_back(Pair(prefix+"nchange", nNChange));
    result.push_back(Pair(prefix+"pegdata", pegdata.ToString()));
}

void printpegtxout(const CPegData & pegdata,
                   Object & result,
                   string prefix)
{
    int64_t nValue      = pegdata.fractions.Total();

    // network peg in next cycle
    int pegn_liquid = pegdata.peglevel.nSupplyNext - pegdata.peglevel.nBuffer;
    int pegn_reserve = pegdata.peglevel.nSupplyNext;

    int64_t nLiquid = pegdata.fractions.High(pegn_liquid);
    int64_t nReserve = pegdata.fractions.Low(pegn_reserve);

    int16_t nValueHli   = pegdata.fractions.HLI();
    int16_t nLiquidHli  = pegdata.fractions.HighPart(pegn_liquid, nullptr).HLI();
    int16_t nReserveHli = pegdata.fractions.LowPart(pegn_reserve, nullptr).HLI();

    result.push_back(Pair(prefix+"value", nValue));
    result.push_back(Pair(prefix+"value_hli", nValueHli));
    result.push_back(Pair(prefix+"nliquid", nLiquid));
    result.push_back(Pair(prefix+"nliquid_hli", nLiquidHli));
    result.push_back(Pair(prefix+"nreserve", nReserve));
    result.push_back(Pair(prefix+"nreserve_hli", nReserveHli));
    result.push_back(Pair(prefix+"pegdata", pegdata.ToString()));
}


//
// Copyright Antoine Leblanc 2010 - 2015
// Distributed under the MIT license.
//
// http://nauths.fr
// http://github.com/nicuveo
// mailto://antoine.jp.leblanc@gmail.com
//

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Includes

#include <cstdlib>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "nauths/mcl/mcl.hh"

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Macros

#define CREATE(R, D, S)                         \
  case mcl::spaces::S:                          \
  {                                             \
    mcl::S color;                               \
    fill(color);                                \
    return color;                               \
  }                                             \

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Local code

namespace
{

  typedef mcl::Endomorphism<mcl::Color> Endo;

  const Endo grey        = mcl::grey<mcl::Color>();
  const Endo gray        = mcl::gray<mcl::Color>();
  const Endo opaque      = mcl::opaque<mcl::Color>();
  const Endo transparent = mcl::transparent<mcl::Color>();
  const Endo alpha25     = mcl::alpha<mcl::Color>(0.25);
  const Endo alpha50     = mcl::alpha<mcl::Color>(0.50);
  const Endo alpha75     = mcl::alpha<mcl::Color>(0.75);
  const Endo light25     = mcl::lightness<mcl::Color>(25);
  const Endo light50     = mcl::lightness<mcl::Color>(50);
  const Endo light75     = mcl::lightness<mcl::Color>(75);
  const Endo cround      = mcl::round<mcl::Color>();
  const Endo clamp       = mcl::clamp<mcl::Color>();

  template <typename T>
  T random()
  {
    return T((rand() % 10000 + 1) / 10000.);
  }

  template <>
  mcl::byte random()
  {
    return (rand() >> 8) % 256;
  }

  template <typename T, std::size_t N>
  void fill(std::array<T, N>& color)
  {
    for (auto& x : color)
      x = random<T>();
  }

  template <typename T>
  T make()
  {
    T color;
    fill(color);
    return color;
  }

  mcl::Color make(mcl::SpaceType s)
  {
    switch (s)
    {
      BOOST_PP_SEQ_FOR_EACH(CREATE, _, MCL_SPACES)
    }

    return mcl::Color();
  }

  mcl::Color make()
  {
    return make((mcl::SpaceType) (rand() % (mcl::spaces::XYZ + 1)));
  }

}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Implementation

BOOST_AUTO_TEST_SUITE(transform)

BOOST_AUTO_TEST_CASE(gray_equal_grey)
{
  srand(42);
  for (int i = 0; i < 100; ++i)
  {
    mcl::Color c = make();
    BOOST_REQUIRE_EQUAL(grey(c), gray(c));
  }
}

BOOST_AUTO_TEST_CASE(endos)
{
  srand(42);

  for (int i = 0; i < 100; ++i)
  {
    mcl::Color c0 = make();
    mcl::Color c1 = c0 | clamp | grey | cround | transparent | gray | opaque | light25 | alpha25 | light50 | alpha50 | light75 | alpha75;
    mcl::Color c2 = (alpha75, light75, alpha50, light50, alpha25, light25, opaque, gray, transparent, cround, grey, clamp)(c0);
    mcl::Color c3 = alpha75(light75(alpha50(light50(alpha25(light25(opaque(gray(transparent(cround(grey(clamp(c0))))))))))));
    BOOST_REQUIRE_EQUAL(c1, c2);
    BOOST_REQUIRE_EQUAL(c2, c3);
    BOOST_REQUIRE_EQUAL(c1, c3);
  }
}

BOOST_AUTO_TEST_CASE(endos_2)
{
  srand(42);

  std::vector<Endo> endos = {alpha75, light75, alpha50, light50};
  Endo e1 = (endos[0], endos[1], endos[2], endos[3]);
  Endo e2 = mcl::concat<Endo>(endos);

  for (int i = 0; i < 100; ++i)
  {
    mcl::Color c0 = make();
    mcl::Color c1 = e1(c0);
    mcl::Color c2 = e2(c0);
    BOOST_REQUIRE_EQUAL(c1, c2);
  }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef NOVIGRA
#include <boost/python.hpp>
#include <string>
#include "inf_def_visitor.hxx"

#include <opengm/inference/intersection_based_inf.hxx>
#include <param/intersection_based_param.hxx>

#ifdef WITH_CPLEX
#include "opengm/inference/auxiliary/fusion_move/permutable_label_fusion_mover.hxx"
#endif

template<class GEN>
void export_intersection_based_t( InfSetup & setup, const std::string & genName){

    typedef opengm::IntersectionBasedInf<typename GEN::GraphicalModelType, GEN> INF;
    setup.hyperParameters= StringVector(1,genName);
    const std::string baseName("IntersectionBased");
    const std::string tBaseName = baseName +  std::string("_") + genName;
    const std::string name = std::string("_")+tBaseName;
    exportInfParam<INF>(name.c_str()); // "IntersectionBased"
    // export inferencePyIntersectionBasedInf
    class_< INF>(name.c_str(),init<const typename GEN::GraphicalModelType & >())
    .def(InfSuite<INF>(baseName,setup))
    ;
}

template<class GEN>
void export_intersection_based_proposal_param( InfSetup & setup, const std::string & genName){

    setup.hyperParameters= StringVector(1,genName);
    const std::string baseName("FusionBased");
    const std::string tBaseName = baseName +  std::string("_") + genName;
    const std::string name = std::string("_")+tBaseName+std::string("_ProposalParam");
    exportInfParam<GEN>(name.c_str()); // "IntersectionBased"
}

template<class GM,class ACC>
void export_intersection_based(){
    using namespace boost::python;
    import_array();
    append_subnamespace("solver");

    // documentation
    InfSetup setup;
    setup.cite       = "";
    setup.algType    = "fusion-moves";
    setup.hyperParameterKeyWords        = StringVector(1,std::string("generator"));
    setup.hyperParametersDoc            = StringVector(1,std::string("proposal generator"));
    // parameter of inference will change if hyper parameter changes
    setup.hasInterchangeableParameter   = false;

    #ifndef NOVIGRA
    typedef opengm::proposal_gen::RandomizedHierarchicalClustering<GM, opengm::Minimizer>   RHCGen;
    typedef opengm::proposal_gen::RandomizedWatershed<GM, opengm::Minimizer>                RWSGen;
    #endif

    typedef opengm::proposal_gen::QpboBased<GM, opengm::Minimizer>                QpboGen;

    typedef opengm::proposal_gen::WeightRandomization<typename GM::ValueType> WeightRand;

    typedef typename  WeightRand::Parameter PyWeightRand;

    enum_<typename PyWeightRand::NoiseType> ("_WeightRandomization_NoiseType_")
        .value("normalAdd",    PyWeightRand::NormalAdd)
        .value("uniformAdd",   PyWeightRand::UniformAdd)
        .value("normalMult",  PyWeightRand::NormalMult)
        .value("none",  PyWeightRand::None)
    ;

    class_<PyWeightRand>("_WeightRandomizerParameter_", init<>())
        .def_readwrite("noiseType",&PyWeightRand::noiseType_)
        .def_readwrite("noiseParam",&PyWeightRand::noiseParam_)
        .def_readwrite("seed",&PyWeightRand::seed_)
        .def_readwrite("ignoreSeed",&PyWeightRand::ignoreSeed_)
    ;

    exportInfParam<  opengm::PermutableLabelFusionMove<GM, ACC> >("_PermutableLabelFusionMove"); // "IntersectionBased"

    #ifndef NOVIGRA
    // RandomizedHierarchicalClustering
    {
        setup.isDefault=true;
        const std::string genName("randomizedHierarchicalClustering");
        typedef RHCGen GEN;

        export_intersection_based_proposal_param<GEN>(setup, genName);
        export_intersection_based_t<GEN>(setup, genName);
    }
    // RandomizedHierarchicalClustering
    {
        setup.isDefault=false;
        const std::string genName("randomizedWatershed");
        typedef RWSGen GEN;

        export_intersection_based_proposal_param<GEN>(setup, genName);
        export_intersection_based_t<GEN>(setup, genName);
    }
    #endif
     // Qpbo Based
    #ifdef WITH_QPBO
    {
        setup.isDefault=false;
        const std::string genName("qpboBased");
        typedef QpboGen GEN;

        export_intersection_based_proposal_param<GEN>(setup, genName);
        export_intersection_based_t<GEN>(setup, genName);
    }
    #endif

}

template void export_intersection_based<opengm::python::GmAdder,opengm::Minimizer>();
#endif


// __BEGIN_LICENSE__
// Copyright (C) 2006-2010 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <iostream>
#include <time.h>
#include <sys/stat.h>

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <boost/math/distributions/gamma.hpp>

#include <vw/Core.h>
#include <vw/Image.h>
#include <vw/FileIO.h>
#include <vw/Cartography.h>
#include <vw/Math.h>
using namespace vw;
using namespace vw::cartography;

#include <vw/Photometry/Reconstruct.h>
#include <vw/Photometry/Camres.h>
#include <vw/Photometry/Misc.h>
using namespace vw::photometry;


/*
 * Copyright 2018-2019 Autoware Foundation. All rights reserved.
 *
 * 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 "op_perception_simulator_core.h"

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <boost/random.hpp>
#include <boost/math/distributions/normal.hpp>
#include "op_ros_helpers/op_ROSHelpers.h"

#include "op_utility/UtilityH.h"
#include "math.h"
#include "op_planner/MatrixOperations.h"

namespace PerceptionSimulatorNS
{

typedef boost::mt19937 ENG;
typedef boost::normal_distribution<double> NormalDIST;
typedef boost::variate_generator<ENG, NormalDIST> VariatGEN;

constexpr double SIMU_OBSTACLE_WIDTH = 1.5;
constexpr double SIMU_OBSTACLE_LENGTH = 1.5;
constexpr double SIMU_OBSTACLE_HEIGHT = 1.4;
constexpr double SIMU_OBSTACLE_POINTS_NUM = 50;
constexpr int SIMU_OBSTACLE_ID = 100001;

OpenPlannerSimulatorPerception::OpenPlannerSimulatorPerception()
{
	m_bSetSimulatedObj = false;
	nh.getParam("/op_perception_simulator/simObjNumber" , m_DecParams.nSimuObjs);
	nh.getParam("/op_perception_simulator/GuassianErrorFactor" , m_DecParams.errFactor);
	nh.getParam("/op_perception_simulator/pointCloudPointsNumber" , m_DecParams.nPointsPerObj);

	pub_DetectedObjects = nh.advertise<autoware_msgs::CloudClusterArray>("cloud_clusters",1);

	sub_simulated_obstacle_pose_rviz = nh.subscribe("/clicked_point", 1, &OpenPlannerSimulatorPerception::callbackGetRvizPoint,	this);

	for(int i=1; i <= m_DecParams.nSimuObjs; i++)
	{
		std::ostringstream str_pose;
		//str_pose << "/op_car_simulator" << i << "/sim_box_pose_" << i;
		str_pose << "/sim_box_pose_" << i;
		std::cout << "Subscribe to Topic : " <<  str_pose.str() <<  std::endl;

		ros::Subscriber _sub;
		_sub =  nh.subscribe(str_pose.str(), 10, &OpenPlannerSimulatorPerception::callbackGetSimuData, this);
		sub_objs.push_back(_sub);
	}

	ros::Subscriber _sub;
	_sub =  nh.subscribe("/sim_box_pose_ego", 10, &OpenPlannerSimulatorPerception::callbackGetSimuData, this);
	sub_objs.push_back(_sub);

	std::cout << "OpenPlannerSimulatorPerception initialized successfully " << std::endl;

}

OpenPlannerSimulatorPerception::~OpenPlannerSimulatorPerception()
{
}

void OpenPlannerSimulatorPerception::callbackGetRvizPoint(const geometry_msgs::PointStampedConstPtr& msg)
{
	tf::StampedTransform transform;
	PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);

	geometry_msgs::Pose point;
	point.position.x = msg->point.x + transform.getOrigin().x();
	point.position.y = msg->point.y + transform.getOrigin().y();
	point.position.z = msg->point.z + transform.getOrigin().z();
	point.orientation =  tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);

	m_SimulatedCluter = GenerateSimulatedObstacleCluster(SIMU_OBSTACLE_WIDTH , SIMU_OBSTACLE_LENGTH, SIMU_OBSTACLE_HEIGHT, SIMU_OBSTACLE_POINTS_NUM, point);
	m_SimulatedCluter.id = SIMU_OBSTACLE_ID;
	m_SimulatedCluter.score = 0; //zero velocity
	m_SimulatedCluter.indicator_state = 3; // default indicator value

	m_bSetSimulatedObj = true;
}

void OpenPlannerSimulatorPerception::callbackGetSimuData(const geometry_msgs::PoseArray &msg)
{
	int obj_id = -1;
	double actual_speed = 0;
	double actual_steering  = 0;
	int indicator = 3;

	if(msg.poses.size() > 0 )
	{
		obj_id = msg.poses.at(0).position.x;
		actual_speed = msg.poses.at(0).position.y;
		actual_steering = msg.poses.at(0).position.z;
	}

	if(msg.poses.size() == 4)
	{
		indicator = msg.poses.at(3).orientation.w;
	}

	if(obj_id < 0)
		return;

	int index = -1;
	for(unsigned int i = 0; i < m_ObjClustersArray.clusters.size() ; i++ )
	{
		if((int)m_ObjClustersArray.clusters.at(i).id == obj_id)
		{
			index = i;
			break;
		}
	}

	timespec t;
	UtilityHNS::UtilityH::GetTickCount(t);
	srand(t.tv_nsec);
	int nPoints = m_DecParams.nPointsPerObj + (rand()%POINT_CLOUD_ADDTIONAL_ERR_NUM - POINT_CLOUD_ADDTIONAL_ERR_NUM/2);

	autoware_msgs::CloudCluster c = GenerateSimulatedObstacleCluster(msg.poses.at(2).position.y, msg.poses.at(2).position.x, msg.poses.at(2).position.z, nPoints, msg.poses.at(1));
	c.id = obj_id;
	c.score = actual_speed;
	c.indicator_state = indicator;

	if(index >= 0) // update existing
	{
		m_ObjClustersArray.clusters.at(index) = c;
		m_keepTime.at(index).second = OBJECT_KEEP_TIME;
	}
	else
	{
		m_ObjClustersArray.clusters.push_back(c);
		m_keepTime.push_back(std::make_pair(c.id, OBJECT_KEEP_TIME));
	}
}

autoware_msgs::CloudCluster OpenPlannerSimulatorPerception::GenerateSimulatedObstacleCluster(const double& width, const double& length, const double& height, const int& nPoints, const geometry_msgs::Pose& centerPose)
{
	autoware_msgs::CloudCluster cluster;

	timespec t;
	UtilityHNS::UtilityH::GetTickCount(t);
	srand(t.tv_nsec);

	ENG eng(t.tv_nsec);
	NormalDIST dist_x(0, m_DecParams.errFactor);
	VariatGEN gen_x(eng, dist_x);

	cluster.centroid_point.point.x = centerPose.position.x + gen_x();
	cluster.centroid_point.point.y = centerPose.position.y + gen_x();
	cluster.centroid_point.point.z = centerPose.position.z;

	cluster.avg_point.point.x = centerPose.position.x;
	cluster.avg_point.point.y = centerPose.position.y;
	cluster.avg_point.point.z = centerPose.position.z;

	double yaw_angle = tf::getYaw(centerPose.orientation);
	cluster.estimated_angle = yaw_angle;

	cluster.dimensions.x = width;
	cluster.dimensions.y = length;
	cluster.dimensions.z = height;
	pcl::PointCloud<pcl::PointXYZI> point_cloud;

	PlannerHNS::Mat3 rotationMat(yaw_angle);
	PlannerHNS::Mat3 translationMat(cluster.avg_point.point.x, cluster.avg_point.point.y);

	for(int i=1; i < nPoints; i++)
	{
		UtilityHNS::UtilityH::GetTickCount(t);
		PlannerHNS::WayPoint center_p;
		srand(t.tv_nsec);

		center_p.pos.x = ((double)(rand()%100)/100.0 - CONTOUR_DISTANCE_ERROR);
		center_p.pos.x *= width;

		srand(t.tv_nsec/i);
		center_p.pos.y = ((double)(rand()%100)/100.0 - CONTOUR_DISTANCE_ERROR);
		center_p.pos.y *= length;

		srand(t.tv_nsec/i*i);
		center_p.pos.z = ((double)(rand()%100)/100.0 - CONTOUR_DISTANCE_ERROR)* height;

		center_p.pos = rotationMat*center_p.pos;
		center_p.pos = translationMat*center_p.pos;

		pcl::PointXYZI p;
		p.x = center_p.pos.x;
		p.y = center_p.pos.y;
		p.z = center_p.pos.z;

		point_cloud.points.push_back(p);
	}

	pcl::toROSMsg(point_cloud, cluster.cloud);

	return cluster;
}

void OpenPlannerSimulatorPerception::MainLoop()
{

	ros::Rate loop_rate(15);

	while (ros::ok())
	{
		ros::spinOnce();

		//clean old data
		for(unsigned int i = 0 ; i < m_keepTime.size(); i++)
		{
			if(m_keepTime.at(i).second <= 0)
			{
				m_keepTime.erase(m_keepTime.begin()+i);
				m_ObjClustersArray.clusters.erase(m_ObjClustersArray.clusters.begin()+i);
				i--;
			}
			else
				m_keepTime.at(i).second -= 1;
		}

		if(m_bSetSimulatedObj)
		{
			m_AllObjClustersArray = m_ObjClustersArray;
			m_AllObjClustersArray.clusters.push_back(m_SimulatedCluter);
			pub_DetectedObjects.publish(m_AllObjClustersArray);
		}
		else
		{
			pub_DetectedObjects.publish(m_ObjClustersArray);
		}

		loop_rate.sleep();
	}
}

}


/*******************************************************************************
 * Copyright (c) 2014, 2015  IBM Corporation and others
 *
 * 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 <Eigen/SparseCore>
#include <boost/bimap.hpp>
#include "ImageHolder.hpp"
#include "LocException.hpp"
#include <opencv2/flann/flann.hpp>
#include <opencv2/opencv.hpp>

namespace loc{

    double ImageHolder::Point::distance(const Point& left, const Point& right){
        double x1 = left.x;
        double y1 = left.y;
        double x2 = right.x;
        double y2 = right.y;
        double sq = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
        double dist = std::sqrt(sq);
        return dist;
    }

    ImageHolderMode ImageHolder::mode_ = light;
    bool ImageHolder::precomputesIndex = false;

    Color::Color(uint8_t r, uint8_t g, uint8_t b){
        r_ = r;
        g_ = g;
        b_ = b;
    }

    static const std::vector<Color> colorList{
        color::white,
        color::black,
        color::red,
        color::lime,
        color::blue,
        color::yellow,
        color::green

        /*
        color::olive,
        color::fuchsia,
        color::silver,
        color::aqua,
        color::gray,
        color::purple,
        color::navy,
        color::teal,
        color::maroon
        */
    };

    static const std::vector<Color> colorsToIndices{
        color::red,
        color::lime,
        color::blue,
        color::yellow,
        color::green
    };

    bool Color::equals(const Color& c)const{
        if( this->b_ == c.b_
           && this->g_ == c.g_
           && this->r_ == c.r_
           ){
            return true;
        }else{
            return false;
        }
    }

    bool Color::operator==(const Color& right) const{
        return this->equals(right);
    }

    int Color::rgbcode() const{
        int t = 65536*this->r_ + 256*this->g_ + this->b_;
        return t;
    }

    bool Color::operator<(const Color& right) const{
        return this->rgbcode() < right.rgbcode();
    }

    bool ImageHolder::checkValid(int y, int x) const
    {
        if(0<=y && y<rows() && 0<=x && x<cols()){
            return true;
        }else{
            return false;
        }
    }

    class IndexWrapper: public cv::flann::Index{
    public:
        IndexWrapper(const IndexWrapper& idx) : Index(idx){};
        IndexWrapper(): Index(){}
        IndexWrapper(cv::InputArray data,
                     const cv::flann::IndexParams& params,
                     cvflann::flann_distance_t distType):
                    Index(data, params, distType){}
        ~IndexWrapper(){};
    };

    class ImageHolder::Impl{
    protected:
        std::map<Color, ImageHolder::Points> mColorPointsMap;

        std::map<Color, cv::Mat> mColorDataMap;
        mutable std::mutex mtx_;
        mutable std::map<Color, std::shared_ptr<IndexWrapper>> mColorIndexMap;

    public:
        virtual ~Impl() = default;
        virtual int rows() const = 0;
        virtual int cols() const = 0;
        virtual Color get(int y, int x) const = 0;
        virtual std::vector<Point> getPoints(const Color& c) const = 0;

        virtual void setUpIndices(){
            for(const Color& c: colorsToIndices){
                this->setUpIndexForColor(c);
            }
        }

        virtual void setUpIndexForColor(const Color& c){
            std::lock_guard<std::mutex> lock(mtx_);
            if(mColorPointsMap.count(c)==0){
                auto points = getPoints(c);
                int n = static_cast<int>(points.size());
                mColorPointsMap[c] = points;
                if(n>0){
                    cv::Mat data = cv::Mat::zeros(n, 2, CV_32FC1);
                    for(int j=0; j<n; j++){
                        data.at<float>(j,0) = points.at(j).x;
                        data.at<float>(j,1) = points.at(j).y;
                    }
                    mColorDataMap[c] = data;
                    if(ImageHolder::precomputesIndex){
                        mColorIndexMap[c] = std::make_shared<IndexWrapper>(mColorDataMap[c], cv::flann::KDTreeIndexParams(), cvflann::FLANN_DIST_EUCLIDEAN);
                    }
                }
            }
        }

        virtual Points findClosestPoints(const Color& c, const ImageHolder::Point& p, int k = 1) const{

            std::lock_guard<std::mutex> lock(mtx_);

            if(mColorPointsMap.count(c)==0){
                LocException ex("Index is not set for the input color");
                BOOST_THROW_EXCEPTION(ex);
            }

            const Points& points = mColorPointsMap.at(c);
            if(points.size()==0){
                return Points(0);
            }

            cv::Mat query = (cv::Mat_<float>(1,2) << p.x , p.y);
            std::vector<int> indices;
            std::vector<float> dists;

            {
                if(ImageHolder::precomputesIndex){
                    if(mColorIndexMap.count(c)==0){
                        cv::Mat data = mColorDataMap.at(c);
                        mColorIndexMap[c] = std::make_shared<IndexWrapper>(data, cv::flann::KDTreeIndexParams(), cvflann::FLANN_DIST_EUCLIDEAN);
                    }else{
                        mColorIndexMap.at(c)->knnSearch(query, indices, dists, k);
                    }
                }else{
                    cv::Mat data = mColorDataMap.at(c);
                    cv::flann::Index idx(data, cv::flann::KDTreeIndexParams(), cvflann::FLANN_DIST_EUCLIDEAN);
                    idx.knnSearch(query, indices, dists, k);
                }
            }

            Points psRet(k);
            for(int i=0; i<k; i++){
                int index = indices.at(0);
                Point pret = points.at(index);
                psRet[i] = pret;
            }
            return psRet;
        }
    };

    class ImageHolder::ImplHeavy : public ImageHolder::Impl{
        std::string name_;
        cv::Mat mat_;

    public:
        ImplHeavy(){}
        ImplHeavy(const std::string& filepath, const std::string& name){
            name_ = name;
            mat_ = cv::imread(filepath);
            if(mat_.empty()){
                LocException ex("Failed to read the image file at " + filepath);
                BOOST_THROW_EXCEPTION(ex);
            }

            this->setUpIndices();
        }
        ~ImplHeavy() = default;

        int rows() const{
            return mat_.rows;
        }
        int cols() const{
            return mat_.rows;
        }
        Color get(int y, int x) const{
            cv::Vec3b vec = mat_.at<cv::Vec3b>(y, x);
            uint8_t b = vec[0];
            uint8_t g = vec[1];
            uint8_t r = vec[2];
            Color color(r,g,b);
            return color;
        }

        std::vector<Point> getPoints(const Color& c) const{
            cv::Mat binary;
            cv::inRange(mat_, cv::Scalar(c.b_, c.g_, c.r_), cv::Scalar(c.b_, c.g_, c.r_), binary);
            cv::Mat idx;
            cv::findNonZero(binary, idx);
            std::vector<Point> points;
            for(int i=0; i<idx.total(); i++){
                Point p;
                p.x = idx.at<cv::Point>(i).x;
                p.y = idx.at<cv::Point>(i).y;
                points.push_back(p);
            }
            return points;
        }
    };

    class ImageHolder::ImplLight : public ImageHolder::Impl{
        std::string name_;
        Eigen::SparseMatrix<uint8_t> mat_;
        boost::bimap<Color, uint8_t> colorIntBM;
    public:
        ImplLight(){
            using bm_type = boost::bimap<Color, uint8_t>;
            for(int i=0; i<colorList.size(); i++){
                const Color& c = colorList.at(i);
                colorIntBM.insert( bm_type::value_type(c, i));
            }
        }

        ImplLight(const std::string& filepath, const std::string& name) : ImplLight(){

            typedef Eigen::Triplet<uint8_t> Triplet;

            name_ = name;
            int rows, cols;
            std::vector<Triplet> tripletList;
            {
                cv::Mat image = cv::imread(filepath);
                cols = image.cols;
                rows = image.rows;
                for(int y=0; y<image.rows; y++){
                    for(int x=0; x<image.cols; x++){
                        cv::Vec3b vec = image.at<cv::Vec3b>(y, x);
                        int b = vec[0];
                        int g = vec[1];
                        int r = vec[2];
                        Color color(r,g,b);
                        uint8_t code = colorToUint8(color);
                        if(code!=0){
                            tripletList.push_back(Triplet(y,x,code));
                        }
                    }
                }
                mat_ = Eigen::SparseMatrix<uint8_t>(rows, cols);
                image.release();
            }

            mat_.setFromTriplets(tripletList.begin(), tripletList.end());

            this->setUpIndices();
        }
        ~ImplLight() = default;

        uint8_t colorToUint8(const Color& color) const{
            for(int i=0; i<colorList.size(); i++){
                const Color& c = colorList.at(i);
                if(color.equals(c)){
                    return i;
                }
            }
            return 0;
        }

        Color uint8ToColor(uint8_t i) const{
            if(i<colorList.size()){
                return colorList.at(i);
            }else{
                return colorList.at(0);
            }
        }

        uint8_t colorToUint8BM(const Color& color) const{
            if(colorIntBM.left.count(color) != 0){
                return colorIntBM.left.at(color);
            }else{
                return colorIntBM.left.at(color::white);
            }
        }

        Color uint8ToColorBM(uint8_t i) const{
            if(colorIntBM.right.count(i) != 0){
                return colorIntBM.right.at(0);
            }else{
                return colorIntBM.right.at(i);
            }
        }

        int rows() const{
            return mat_.rows();
        }
        int cols() const{
            return mat_.cols();
        }

        Color get(int y, int x) const{
            uint8_t code = mat_.coeff(y,x);
            Color color = uint8ToColor(code);
            return color;
        }

        std::vector<Point> getPoints(const Color& c) const{
            Points points;
            uint8_t code_q = colorToUint8(c);
            for(int k=0; k<mat_.outerSize(); k++){
                auto iter = Eigen::SparseMatrix<uint8_t>::InnerIterator(mat_, k);
                for(; iter; ++iter){
                    uint8_t code = iter.value();
                    if(code == code_q){
                        Point p;
                        p.x = iter.col();
                        p.y = iter.row();
                        points.push_back(p);
                    }
                }
            }
            return points;
        }
    };

    ImageHolder::ImageHolder(){
        if(mode_ == light){
            impl.reset(new ImplLight());
        }else if (mode_ == heavy){
            impl.reset(new ImplHeavy());
        }else{
            BOOST_THROW_EXCEPTION(LocException("Unknown ImageHolderMode."));
        }
    }

    ImageHolder::ImageHolder(const std::string& filepath, const std::string& name){
        if(mode_ == light){
            std::cout << "ImageHolder::ImplLight is instantiated." << std::endl;
            impl.reset(new ImplLight(filepath, name));
        }else if (mode_ == heavy){
            std::cout << "ImageHolder::ImplHeavy is instantiated." << std::endl;
            impl.reset(new ImplHeavy(filepath, name));
        }else{
            BOOST_THROW_EXCEPTION(LocException("Unknown ImageHolderMode."));
        }
    }

    ImageHolder::~ImageHolder(){}

    void ImageHolder::setMode(ImageHolderMode mode){
        mode_ = mode;
    }

    int ImageHolder::rows() const{
        return impl->rows();
    }

    int ImageHolder::cols() const{
        return impl->cols();
    }

    Color ImageHolder::get(int y, int x) const{
        return impl->get(y, x);
    }

    void ImageHolder::setUpIndexForColor(const loc::Color &c){
        impl->setUpIndexForColor(c);
    }

    std::vector<ImageHolder::Point> ImageHolder::getPoints(const Color& c) const{
        return impl->getPoints(c);
    }

    ImageHolder::Points ImageHolder::findClosestPoints(const Color& c, const ImageHolder::Point& p, int k) const{
        return impl->findClosestPoints(c, p, k);
    }

    void ImageHolder::setPrecomputesIndex(bool precomputesIdx){
        ImageHolder::precomputesIndex = precomputesIdx;
    }
}


#ifndef WARP_POINT_2D
#define WARP_POINT_2D

#pragma once

#include <pcl/registration/warp_point_rigid.h>
#include <boost/shared_ptr.hpp>

namespace pcl {
namespace registration {
  /** \brief @b WarpPoint2D enables 3D (1D rotation around yaw + 2D translation)
   * transformations for points.
   *
   * \note The class is templated on the source and target point types as well as on the
   * output scalar of the transformation matrix (i.e., float or double). Default: float.
   * \author Radu B. Rusu
   * \ingroup registration
   */
  template<typename PointSourceT, typename PointTargetT, typename Scalar = float>
  class WarpPoint2D : public WarpPointRigid<PointSourceT, PointTargetT, Scalar>
  {
  public:
    using Matrix4 = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::Matrix4;
    using VectorX = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::VectorX;

    using Ptr = boost::shared_ptr<WarpPoint2D<PointSourceT, PointTargetT, Scalar>>;
    using ConstPtr =
      boost::shared_ptr<const WarpPoint2D<PointSourceT, PointTargetT, Scalar>>;

    /** \brief Constructor. */
    WarpPoint2D() : WarpPointRigid<PointSourceT, PointTargetT, Scalar>(2) {}

    /** \brief Empty destructor */
    ~WarpPoint2D() {}

    /** \brief Set warp parameters.
     * \param[in] p warp parameters (tx ty rz)
     */
    void setParam(const VectorX &p) override
    {
      assert(p.rows() == this->getDimension());
      Matrix4 &trans = this->transform_matrix_;

      trans = Matrix4::Identity();

      // Copy the rotation and translation components
      trans.block(0, 3, 4, 1) = Eigen::Matrix<Scalar, 4, 1>(p[0], p[1], 0, 1.0);
    }
  };
}// namespace registration
}// namespace pcl

#endif /* WARP_POINT_2D */


// MIT License
//
// Copyright (c) 2019 ibis-ssl
//
// 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.
//
// Created by hans on 2019/10/24.
//
#include <world_model/ball_info.h>
#include <world_model/world_model.h>
#include <boost/numeric/interval.hpp>
#include <field_analysis/CMD14TDP_3/conditions.h>
#include <utils/interval.h>
#include <world_model/tool.h>

namespace condition {

    float ShootConditions::getSuccessRate(const WorldModel &world_model, Point target) {
        PassRestriction restriction;
        restriction.maximum_vel = 8.0f;
        restriction.minimum_vel = 0.0f;
        float total_rate = 1.0f;
        float rate;

        std::clamp(rate = getC1SuccessRate(world_model, target, restriction), 0.f, 1.f);
        total_rate *= rate;

        std::clamp(rate = getC2SuccessRate(world_model, target, restriction), 0.f, 1.f);
        total_rate *= rate;

//        std::clamp(rate = getC3SuccessRate(world_model, target, restriction),0.f,1.f);
//        total_rate *= rate;
//
        std::clamp(rate = getC4SuccessRate(world_model, target, restriction), 0.f, 1.f);
        total_rate *= rate;

        return total_rate;
    }

    float ShootConditions::getSuccessRate(int map_id, const WorldModel &world_model, Point target) {
        PassRestriction restriction;
        restriction.maximum_vel = 8.0f;
        restriction.minimum_vel = 0.0f;
        float rate;
        switch (map_id) {
            case 1:
                rate = getC1SuccessRate(world_model, target, restriction);
                break;

            case 2:
                rate = getC2SuccessRate(world_model, target, restriction);
                break;
            case 3:
                rate = getC3SuccessRate(world_model, target, restriction);
                break;

            case 4:
                rate = getC4SuccessRate(world_model, target, restriction);
                break;
            default:
                return 0.f;
        }
        std::clamp(rate, 0.f, 1.f);
        return rate;
    }

    /*
     * xからのシュートが敵キーパーのブロックよりも早い
     */
    float ShootConditions::getC1SuccessRate(const WorldModel &world_model, Point target, PassRestriction &restriction) {
        //TODO : 書く
        auto goalie = world_model.enemys.getGoalie();
        float goalie_angle = tool::getAngle(goalie->pose.pos - target);
        float angle_1 = tool::getAngle(Constants::Their::Goal::lower() - target);
        float angle_2 = tool::getAngle(Constants::Their::Goal::upper() - target);
        float diff_angle = std::max(std::abs(angle_1 - goalie_angle), std::abs(angle_2 - goalie_angle));

        float dist = bg::distance(target, Constants::Their::Goal::center());
        return 15.f * diff_angle / (dist + 8.0f);
    }

    /*
     * xからゴールを見た時に十分な角度を占めている
     */
    float ShootConditions::getC2SuccessRate(const WorldModel &world_model, Point target, PassRestriction &restriction) {
        Interval goal_range;

        float lower_angle = tool::getAngle(Constants::Their::Goal::lower() - target);
        float upper_angle = tool::getAngle(Constants::Their::Goal::upper() - target);

        goal_range.append(lower_angle, upper_angle);

        for (auto robot : world_model.enemys.robots) {
            if (robot->is_disappeared) {
                continue;
            }
            //targetから見てゴール側にいる敵ロボットのみ．それ以外はスキップ
            if ((robot->pose.pos - target).dot((Constants::Their::Goal::center() - target).normalized()) < 0.f) {
                continue;
            }

            float d = bg::distance(robot->pose.pos, target);
            float r = (Constants::robot_radius() * d) /
                      std::sqrt(d * d - Constants::robot_radius() * Constants::robot_radius());
            Point v_vec = tool::getVerticalVec((robot->pose.pos - target).normalized());
            float angle_1 = tool::getAngle(robot->pose.pos - target - (v_vec * r));
            float angle_2 = tool::getAngle(robot->pose.pos - target + (v_vec * r));
            goal_range.erase(std::min(angle_1, angle_2), std::max(angle_1, angle_2));
        }

        float goal_angle = goal_range.getWidth();
        //27度あったらいい方
        return goal_angle / (M_PI * 0.15f);
    }

    /*
     * 敵にシュートをブロックされない
     */
    float ShootConditions::getC3SuccessRate(const WorldModel &world_model, Point target, PassRestriction &restriction) {
        //TODO : 書く
        return 1.f;
    }

    /*
     * 敵にボールを奪われない
     */
    float ShootConditions::getC4SuccessRate(const WorldModel &world_model, Point target, PassRestriction &restriction) {
        float min_dist = 1000.f, dist;
        for (auto robot : world_model.enemys.robots) {
            dist = bg::distance(target, robot->pose.pos);
            min_dist = std::min(min_dist, dist);
        }
        return min_dist / 2.f;
    }
}