/******************************************************************************
 * Copyright 2016, Georgia Tech Research Corporation, Atlanta, Georgia 30332
 * All Rights Reserved
 ******************************************************************************/
#include <iostream>
#include <memory>
#include <limits>

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

#include <scrimmage/plugin_manager/RegisterPlugin.h>
#include <scrimmage/entity/Entity.h>
#include <memory>

#include <scrimmage/math/Angles.h>
#include <scrimmage/parse/MissionParse.h>
#include <scrimmage/common/Random.h>

#include <scrimmage/pubsub/Network.h>
#include <scrimmage/pubsub/Publisher.h>
#include <scrimmage/pubsub/Subscriber.h>
#include <scrimmage/pubsub/Message.h>

#include "CaptureTheFlagInteraction.h"

#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>

#include <scrimmage/common/RTree.h>
#include <scrimmage/autonomy/Autonomy.h>
#include <scrimmage/common/Utilities.h>
#include <scrimmage/common/Random.h>

#include <scrimmage/math/State.h>
#include <scrimmage/common/Utilities.h>
#include <scrimmage/parse/ParseUtils.h>

#include <scrimmage/proto/ProtoConversions.h>
#include <scrimmage/proto/Shape.pb.h>

#include <scrimmage/pubsub/Message.h>
#include <scrimmage-gtri-share/msgs/Fire.pb.h>
#include <scrimmage-gtri-share/msgs/BaseCollision.pb.h>

#include <scrimmage/msgs/Event.pb.h>
namespace sm = scrimmage_msgs;

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

namespace sc = scrimmage;
namespace sp = scrimmage_proto;
namespace sgs = scrimmage_gtri_share;

REGISTER_PLUGIN(scrimmage::EntityInteraction, CaptureTheFlagInteraction,
                CaptureTheFlagInteraction_plugin)

CaptureTheFlagInteraction::CaptureTheFlagInteraction()
{
}

double rel_azimuth(Eigen::Vector3d &tgt_pos, sc::State &state) {
    Eigen::Vector3d diff = tgt_pos - state.pos();
    double az_to_tgt = atan2(diff(1), diff(0));
    double az = state.quat().yaw();
    return sc::Angles::angle_diff_rad(az, az_to_tgt);
}

bool CaptureTheFlagInteraction::create_cube(std::string cube_name,
                                            std::map<std::string,std::string> &plugin_params,
                                            Eigen::Vector3d color)
{
    std::vector<std::string> points_str;
    bool exist = sc::get_vec(cube_name+"/point", plugin_params, points_str);
    if (!exist) {
        cout << "Can't find cube:" << cube_name << endl;
        return false;
    }

    // Get the cube's points:
    std::vector<Eigen::Vector3d> points;
    for (std::string p : points_str) {
        std::vector<double> vec;
        if (sc::str2vec<double>(p, " ,", vec, 3)) {
            // Convert to x/y/z
            double x, y, z;
            proj_->Forward(vec[1], vec[0], vec[2], x, y, z);
            points.push_back(Eigen::Vector3d(x,y,z));
        }
    }

    if (points.size() != 8) {
        cout << "Invalid number of cube points: " << points.size() << endl;
        return false;
    }

    CubeInfo cube;
    cube.points = points;
    cube.compute_dots();
    cubes_[cube_name] = cube;

    if (show_cubes_) {
        const int num_faces = 6;
        const int vert_per_face = 4;
        int vert_lookup[num_faces][vert_per_face] = {
            {0,1,2,3},
            {0,1,5,4},
            {0,3,7,4},
            {6,5,4,7},
            {6,2,1,5},
            {6,7,3,2},
        };

        for (int f = 0; f < num_faces; f++) {
            sc::ShapePtr shape(new sp::Shape);
            shape->set_type(sp::Shape::Polygon);
            shape->set_opacity(0.2);
            sc::set(shape->mutable_color(), color[0], color[1], color[2]);
            shape->set_persistent(true);
            for (int r = 0; r < vert_per_face; r++) {
                sp::Vector3d * p = shape->add_point();
                sc::set(p, points[vert_lookup[f][r]]);
            }
            shapes_.push_back(shape);
        }
    }
    return true;
}

bool CaptureTheFlagInteraction::init(std::map<std::string,std::string> &mission_params,
                                     std::map<std::string,std::string> &plugin_params)
{
    base_collision_range_ = sc::get("base_collision_range", plugin_params, 0.0);

    double fire_rate = sc::get("fire_rate_max", plugin_params, -1);
    fire_period_min_ = 1 / (fire_rate == 0 ? -1 : fire_rate);

    fire_range_max_ = sc::get("fire_range_max", plugin_params, 100);
    offensive_fire_range_max_ = sc::get("offensive_fire_range_max", plugin_params, 100);
    defensive_fire_range_max_ = sc::get("defensive_range_max", plugin_params, 150);
    enable_cube_effects_ = sc::get<bool>("enable_cube_effects", plugin_params, false);

    fire_fov_width_ = sc::get("fire_fov_width", plugin_params, sc::Angles::deg2rad(10));
    fire_fov_height_ = sc::get("fire_fov_height", plugin_params, sc::Angles::deg2rad(10));

    fire_width_noise_ = sc::get("fire_width_noise", plugin_params, 0.1);
    fire_height_noise_ = sc::get("fire_height_noise", plugin_params, 0.1);
    fire_effective_range_ = sc::get("fire_effective_range", plugin_params, 10.0);

    entity_radius_ = std::stod(plugin_params.at("entity_radius"));

    fire_rounds_max_ = sc::get("fire_rounds_max", plugin_params, -1);
    std::string hit_detect_str = sc::get("hit_detection", plugin_params, (std::string)"cone");

    if (boost::iequals(hit_detect_str, "noisy_cone")) {
        hit_detection_ = HitDetection::noisy_cone;
    } else if (boost::iequals(hit_detect_str, "cone")) {
        hit_detection_ = HitDetection::cone;
    } else if (boost::iequals(hit_detect_str, "shrinking_cone")) {
        hit_detection_ = HitDetection::shrinking_cone;
    } else if (boost::iequals(hit_detect_str, "acs")) {
        hit_detection_ = HitDetection::acs;
    } else {
        std::string msg =
            "need a hit detection method in CaptureTheFlagInteraction, got \"" + hit_detect_str + "\"";
        throw std::runtime_error(msg);
    }

    show_cubes_ = sc::get<bool>("show_cubes", plugin_params, false);

    // Draw persistent shapes, if desired:
    shapes_.clear();

    if (!create_cube("blue_defense_cube", plugin_params,
                     Eigen::Vector3d(0,0,255))) return false;

    if (!create_cube("red_defense_cube", plugin_params,
                     Eigen::Vector3d(255,0,0))) return false;

    // Setup subscribers
    sub_fire_ = create_subscriber("Fire");
    sub_ent_pres_end_ = create_subscriber("EntityPresentAtEnd");

    // Setup publishers
    pub_base_coll_ = create_publisher("BaseCollision");
    pub_fire_result_ = create_publisher("FireResult");
    pub_distancefrombase_ = create_publisher("DistanceFromBase");

    return true;
}

bool CaptureTheFlagInteraction::step_entity_interaction(std::list<sc::EntityPtr> &ents,
                                                        double t, double dt)
{
    shapes_.clear();
    if (ents.empty()) {
        return true;
    }

    std::map<int, sc::EntityPtr> int_to_ent_map;
    for (sc::EntityPtr &ent : ents) {
        int_to_ent_map[ent->id().id()] = ent;
    }

    // Account for aircraft colliding with bases
    if (ents.front()->autonomies().empty()) {
        cout << "CaptureTheFlagInteraction:  cannot get autonomy" << endl;
        return true;
    }
    sc::RTreePtr &rtree = ents.front()->autonomies().front()->rtree();
    std::vector<sc::ID> neighbors;

    if (mp_) {
        for (auto &kv : mp_->team_info()) {
            int base_team_id = kv.first;

            for (Eigen::Vector3d &base_pos : kv.second.bases) {
                rtree->neighbors_in_range(base_pos, neighbors, base_collision_range_);

                for (sc::ID &id : neighbors) {
//                    std::cout << id.id() << " has hit base " << base_team_id << std::endl;
                    if(int_to_ent_map.count(id.id())){
                        sc::EntityPtr &ent = int_to_ent_map[id.id()];
                        ent->collision();

                        auto msg = std::make_shared<sc::Message<sgs::BaseCollision>>();
                        msg->data.set_entity_id(id.id());
                        msg->data.set_base_id(base_team_id);
                        if (base_team_id == id.team_id()) {
                            msg->data.set_type(sgs::BaseCollision::Friendly);
                        } else {
                            msg->data.set_type(sgs::BaseCollision::Enemy);
                        }
                        publish_immediate(t, pub_base_coll_, msg);
                    }
                }
            }
        }
    }

    for (auto msg : sub_ent_pres_end_->pop_msgs<sc::Message<sm::EntityPresentAtEnd>>()) {
        for (sc::EntityPtr &ent : ents) {
            if (ent->id().id() == msg->data.entity_id()){
                for (auto &kv : mp_->team_info()) {
                    int base_team_id = kv.first;
                    if(base_team_id != ent->id().team_id()){
                        for(Eigen::Vector3d &base_pos : kv.second.bases){
                            auto msg = std::make_shared<sc::Message<sgs::DistanceFromBase>>();
                            msg->data.set_entity_id(ent->id().id());
                            msg->data.set_base_id(base_team_id);
                            msg->data.set_distance((base_pos-ent->state()->pos()).norm());
                            publish_immediate(t, pub_distancefrombase_, msg);
                        }
                    }
                }
            }
        }
    }

    // Process fire messages
    for (auto msg : sub_fire_->pop_msgs<sc::Message<sgs::Fire>>()) {
        int src_id = msg->data.source_id();

        // Limit fire rate
        auto it_prev_fire = prev_fire_.find(src_id);
        if (fire_period_min_ > 0) {
            // Continue if a previous fire event has been recorded and the
            // minimum fire period has not passed
            if (it_prev_fire != prev_fire_.end() &&
                (t - it_prev_fire->second) < fire_period_min_) {
                continue;
            }
        }

        // limit ammunition
        auto it_num_fires = num_fires_.find(src_id);
        if (fire_rounds_max_ > 0) {
            if (it_num_fires == num_fires_.end()) {
                it_num_fires = num_fires_.insert(std::make_pair(src_id, 0)).first;
            } else if (it_num_fires->second >= fire_rounds_max_) {
                continue;
            }
        }

        // Increment the number of fires for this aircraft
        it_num_fires->second++;

        // If a previous fire time has not been recorded, insert it. If not,
        // just update the time in the iterator
        if (it_prev_fire == prev_fire_.end()) {
            prev_fire_.insert(std::make_pair(src_id, t));
        } else {
            it_prev_fire->second = t;
        }

        int target_id = msg->data.target_id();
        auto it_target_ent = int_to_ent_map.find(target_id);
        if (it_target_ent == int_to_ent_map.end()) {
            continue;
        }

        sc::EntityPtr &target_ent = it_target_ent->second;

        int src_team_id = (*team_lookup_)[src_id];
        bool is_friendly = target_ent->id().team_id() == src_team_id;

        auto it_src_ent = int_to_ent_map.find(src_id);
        if (it_src_ent == int_to_ent_map.end()) {
            std::cout << "Warning: couldn't find source id in map" << std::endl;
            continue;
        }
        sc::EntityPtr &src_ent = it_src_ent->second;

        // Add a line between the two to show the fire event
        sc::ShapePtr attempt_shape(new sp::Shape());
        attempt_shape->set_type(sp::Shape::Line);
        sc::set(attempt_shape->mutable_color(), 255,255,255);
        attempt_shape->set_opacity(1.0);

        sc::add_point(attempt_shape, src_ent->state()->pos());
        sc::add_point(attempt_shape, target_ent->state()->pos());
        shapes_.push_back(attempt_shape);

        bool is_hit = false;
        Eigen::Vector3d diff = src_ent->state()->pos() - target_ent->state()->pos();

        double range = diff.norm();

        if (hit_detection_ == HitDetection::shrinking_cone) {

            Eigen::Vector3d rel_pos = src_ent->state()->rel_pos_local_frame(target_ent->state()->pos());
            double dist = rel_pos.norm();
            double dist_xy = rel_pos.head<2>().norm();
            double az = atan2(rel_pos(1), rel_pos(0));
            double el = atan2(rel_pos(2), dist_xy);

            if (dist == 0) {
                is_hit = true;
            } else {
                double rel_mx = std::abs(atan(entity_radius_ / diff.norm()));
                is_hit = std::abs(az) < rel_mx / 2.0 && std::abs(el) < rel_mx / 2.0;
            }
        } else if (hit_detection_ == HitDetection::noisy_cone) {
            sc::State aim = *src_ent->state();   // we want a copy so no reference
            sc::Quaternion &quat = aim.quat();

            double yaw_noise = random_->rng_normal()*fire_width_noise_;
            double pitch_noise = random_->rng_normal()*fire_height_noise_;

            quat.set(quat.roll(), quat.pitch() + pitch_noise, quat.yaw() + yaw_noise);

            double fov_width = (exp(-range/fire_effective_range_) * M_PI);
            double fov_height = fov_width;

            sc::ShapePtr fire_shape(new sp::Shape);
            fire_shape->set_type(sp::Shape::Line);
            sc::set(fire_shape->mutable_color(), 0, 0, 255);
            fire_shape->set_opacity(1.0);
            sc::add_point(fire_shape, src_ent->state()->pos());
            sc::add_point(fire_shape, aim.pos());
            shapes_.push_back(fire_shape);

            if (aim.InFieldOfView(*target_ent->state(), fov_width, fov_height)) {
                is_hit = true;
            }
        } else if (hit_detection_ == HitDetection::cone) {
            if (range <= fire_range_max_ &&
                src_ent->state()->InFieldOfView(*target_ent->state(), fire_fov_width_, fire_fov_height_)) {
                is_hit = true;
            }
        } else if (hit_detection_ == HitDetection::acs) {
            // ACS detection operates in altitude deconflicted context, so need
            // to remove z-component
            diff(2) = 0;
            range = diff.norm();

            bool possible_hit =
                std::abs(src_ent->state()->rel_az(target_ent->state()->pos())) < fire_fov_width_;
            if (possible_hit) {
                if (enable_cube_effects_) {
                    double cube_range_max = -1;
                    if (cubes_["blue_defense_cube"].contains(src_ent->state()->pos())) {
                        if (src_team_id == 1) {
                            cube_range_max = defensive_fire_range_max_;
                        } else if (src_team_id == 2) {
                            cube_range_max = offensive_fire_range_max_;
                        } else {
                            cout << "CAPTURE THE FLAG WARNING: CAN'T HANDLE MORE THAN TWO TEAMS" << endl;
                        }
                    } else if (cubes_["red_defense_cube"].contains(src_ent->state()->pos())) {
                        if (src_team_id == 1) {
                            cube_range_max = offensive_fire_range_max_;
                        } else if (src_team_id == 2) {
                            cube_range_max = defensive_fire_range_max_;
                        } else {
                            cout << "CAPTURE THE FLAG WARNING: CAN'T HANDLE MORE THAN TWO TEAMS" << endl;
                        }
                    }

                    // Based on the modified fire range, is the
                    // target within range? Also, is the target in
                    // either cube?
                    if (range <= cube_range_max &&
                        (cubes_["blue_defense_cube"].contains(target_ent->state()->pos()) ||
                         cubes_["red_defense_cube"].contains(target_ent->state()->pos()))) {
                        is_hit = true;
                    }
                } else {
                    if (range <= fire_range_max_) {
                        is_hit = true;
                    }
                }
            }
        }

        if (is_hit) {
            sc::ShapePtr hit_shape(new sp::Shape);
            hit_shape->set_type(sp::Shape::Line);
            sc::set(hit_shape->mutable_color(), 255, 0, 0);
            hit_shape->set_opacity(1.0);
            hit_shape->set_ttl(10);
            sc::add_point(hit_shape, src_ent->state()->pos());
            sc::add_point(hit_shape, target_ent->state()->pos());
            shapes_.push_back(hit_shape);

            bool is_alive_before = target_ent->is_alive();
            target_ent->hit();

            auto hit_msg = std::make_shared<sc::Message<sgs::FireResult>>();
            hit_msg->data.set_source_id(src_id);
            hit_msg->data.set_target_id(target_id);
            if (is_friendly) {
                hit_msg->data.set_type(sgs::FireResult::FriendlyHit);
            } else {
                hit_msg->data.set_type(sgs::FireResult::EnemyHit);
            }
            publish_immediate(t, pub_fire_result_, hit_msg);

            if (is_alive_before && !(target_ent->is_alive())) {
                auto kill_msg = std::make_shared<sc::Message<sgs::FireResult>>();
                kill_msg->data.set_source_id(src_id);
                kill_msg->data.set_target_id(target_id);
                if (is_friendly) {
                    kill_msg->data.set_type(sgs::FireResult::FriendlyKill);
                } else {
                    kill_msg->data.set_type(sgs::FireResult::EnemyKill);
                }
                publish_immediate(t, pub_fire_result_, kill_msg);
            }
        } else {
            auto miss_msg = std::make_shared<sc::Message<sgs::FireResult>>();
            miss_msg->data.set_source_id(src_id);
            miss_msg->data.set_target_id(target_id);
            if (is_friendly) {
                miss_msg->data.set_type(sgs::FireResult::FriendlyMiss);
            } else {
                miss_msg->data.set_type(sgs::FireResult::EnemyMiss);
            }
            publish_immediate(t, pub_fire_result_, miss_msg);
        }
    }

    return true;
}

bool CaptureTheFlagInteraction::collision_exists(std::list<sc::EntityPtr> &ents,
                                                 Eigen::Vector3d &p)
{
    // This interaction plugin should be used with SimpleCollision in the
    // mission.xml file to get startup collisions
    return false;
}


#include "cape_pairwise_aligner.h"

#include <blitz/tinyvec-et.h>
#include <algorithm>
#include <ext/functional>
#include "parameter_functions.h"
#include "pairwise_dna_alignment.h"
#include "statistics.h"
#include "hash_factory.h"
#include "hash_hit_data.h"
#include "fragment_algorithm.h"
#include "fragment_functions.h"
#include "fragment_selectors.h"
#include "algorithm_extra.h"
#include "alignment_functions.h"
#include "sequence_statistics.h"
#include "sequence_statistics_functions.h"
#include "entropy.h"
#include "io.h"
#include "dynamic_fragment_score_worker.h"
#include "custom_fragment_score_worker.h"

fragment cape_pairwise_aligner::ensure_minimum_for_composition( input_data &in, fragment &f ) {
   fragment new_f = f;
   expand_closed_interval( in.a.seq->data, new_f[0], min_composition_window );
   expand_closed_interval( in.b.seq->data, new_f[1], min_composition_window );
   return new_f;
}

/*
triplet_table<logint> cape_pairwise_aligner::compute_conditionals( input_data &in, scored_fragment &f ) {
   triplet_table<unsigned int> freq;
   if( f[0].a > 0 ) {
      freq = in.a.stats->tri[f[0].b] - in.a.stats->tri[f[0].a-1];
   } else {
      freq = in.a.stats->tri[f[0].b];
   }

   if( f[1].a > 0 ) {
      freq += in.b.stats->tri[f[1].b] - in.b.stats->tri[f[1].a-1];
   } else {
      freq += in.b.stats->tri[f[1].b];
   }

   triplet_table<logint> result;

   for( dna_alpha::alphabet_type::const_iterator i = dna_alpha::alphabet.begin(); i != dna_alpha::alphabet.end(); ++i ) {
   for( dna_alpha::alphabet_type::const_iterator j = dna_alpha::alphabet.begin(); j != dna_alpha::alphabet.end(); ++j ) {
      double asize = 0.0;
      for( dna_alpha::alphabet_type::const_iterator k = dna_alpha::alphabet.begin(); k != dna_alpha::alphabet.end(); ++k ) {
         asize += freq(k->index(),j->index(),i->index());
      }
      for( dna_alpha::alphabet_type::const_iterator k = dna_alpha::alphabet.begin(); k != dna_alpha::alphabet.end(); ++k ) {
         //std::cerr << dna_alpha::decode(*k) << dna_alpha::decode(*j) << dna_alpha::decode(*i) << "\t";
         result(k->index(),j->index(),i->index()) = ((double)freq(k->index(),j->index(),i->index()))/asize;
      }
   }
   }
   return result;
}
*/

void cape_pairwise_aligner::set_active_parameter_sets( unsigned int nsets ) {
	// This list is 1 based. Remember to subtract one for each parameter.
#ifndef VITERBI_EXTENSIONS
	static unsigned int const greedy_opt[] = {106, 88, 150, 62, 59, 77, 103, 145, 44, 121, 132, 31, 84, 93, 72,
		131, 146, 75, 28, 54, 61, 78, 134, 138, 142, 46, 58, 63, 115, 119 };
#else
	// If we are using Viterbi segmentation, then we use a list optimized for SW.
	static unsigned int const greedy_opt[] = {52, 82, 8, 97, 20, 36, 50, 68, 113, 81, 83, 9, 66, 19, 24, 109,
		13, 49, 67, 100, 145, 18, 12, 15, 21, 25, 41, 65, 72, 84};
#endif
	assert( nsets > 0 );
	assert( nsets <= 30 );
	parameter_sets.clear();
	for( unsigned int i = 0; i < nsets; ++i ) {
		parameter_sets.push_back( all_parameter_sets[greedy_opt[i]-1] );
	}
}

void cape_pairwise_aligner::create_parameter_sets() {
	using namespace std;
	vector<double> gap_lengths, gap_opens, distances;
	gap_opens.push_back( 0.03125 ); // -5
	gap_opens.push_back( 0.0625  ); // -4
	gap_opens.push_back( 0.125   ); // -3
	gap_opens.push_back( 0.25    ); // -2

	gap_lengths.push_back( 1.333    ); // -2
	gap_lengths.push_back( 2.0      ); // -1
	gap_lengths.push_back( 3.414214 ); // -0.5
	gap_lengths.push_back( 6.285214 ); // -0.25

	distances.push_back( 0.1 );
	distances.push_back( 0.2 );
	distances.push_back( 0.3 );
	distances.push_back( 0.4 );
	distances.push_back( 0.5 );
	distances.push_back( 0.6 );
	distances.push_back( 0.7 );
	distances.push_back( 0.8 );
	distances.push_back( 0.9 );
	distances.push_back( 1.0 );

	all_parameter_sets.clear();
	for( vector<double>::const_iterator d = distances.begin(); d != distances.end(); ++d )
		for( vector<double>::const_iterator a = gap_opens.begin(); a != gap_opens.end(); ++a )
			for( vector<double>::const_iterator L = gap_lengths.begin(); L != gap_lengths.end(); ++L )
				all_parameter_sets.push_back( simple_nw_model_parameters(*d,*a,*L) );
}

cape_pairwise_aligner::seed_parameters cape_pairwise_aligner::computeOSP( input_data &in, fragment &f  ) {
   using namespace std;
   // expand fragment to minimum size
   fragment fcomp = ensure_minimum_for_composition( in, f );

   // estimate region composition
   blitz::TinyVector<double,4> q = composition_frequency(*(in.a.stats),fcomp[0]) + composition_frequency(*(in.b.stats),fcomp[1]);
   q /= (double) (fcomp[0].size() + fcomp[1].size() );

   // compute match probability for a column
   double pr_match = dot(q,q);
   double area = (double)f[0].size() * (double)f[1].size();
   unsigned int w = (unsigned int) (log(random_sample/area)/log(pr_match));
   unsigned int l = (unsigned int) ((double)w*1.4);
   if( l < 5 ) l = 5;      // minimum length for random seeds is 5
   if( w < 3 ) w = 3;      // minimum weight 3
   unsigned int expected_random_hits = (unsigned int)(pow( pr_match, (double)w )*area);
   return seed_parameters(w,l,expected_random_hits);
}

cape_pairwise_aligner::cape_pairwise_aligner( int k ) :	mma(k), mmt(k),
random_sample(2000),
double_random_sample(4000),
extension_algorithm(cape_pairwise_aligner::FW_LOCAL),
MAX_AREA(500*500),
MAX_DISTANCE_FROM_CHAIN(200),
EXT(200),
min_composition_window(2000),
training_passes(5),
verbose(false),
custom_scoring(false),
disable_random_model(false),
nthreads(1)
{
   q = 0.25, 0.25, 0.25, 025;
	THRESHOLD.set_base(70.0);
	SHORT_CIRCUIT_THRESHOLD.set_base(200.0);

	create_parameter_sets();
	set_active_parameter_sets( 10 );
	entropy random;
   unsigned int rseed = (unsigned int) random.get_int();
   if( rseed == 0 ) rseed = 1; // bug in old boost doesn't allow 0 as a seed
   base_generator::result_type brseed(rseed);
   baseg = base_generator_ptr( new base_generator( brseed ) );
}

void cape_pairwise_aligner::set_required_region_size( unsigned int i ) {
   MAX_AREA = (double)i*(double)i;
}

void cape_pairwise_aligner::set_random_sample( double rs ) {
   random_sample = rs;
	double_random_sample = (unsigned int)rs*2;
}

void cape_pairwise_aligner::set_custom_scoring( bool b ) {
   custom_scoring = b;
}

void cape_pairwise_aligner::set_threshold( double t ) {
   THRESHOLD.set_base(t);
}

void cape_pairwise_aligner::set_training_passes( unsigned int t ) {
	training_passes = t;
}

void cape_pairwise_aligner::set_chain_filter_distance( int t ) {
	MAX_DISTANCE_FROM_CHAIN = t;
}

void cape_pairwise_aligner::set_num_threads( int n ) {
	nthreads = n;
}

void cape_pairwise_aligner::set_extension_limit( int limit ) {
   //fal.set_extension_limit(limit);
   //swl.set_extension_limit(limit);
}

mm_trainer const &cape_pairwise_aligner::train( dna_sequence_ptr seqa, dna_sequence_ptr seqb ) {
	using namespace std;

	// preprocess and package input data
   input_data indata( seqa, seqb );

	region_pair_vector_ptr segments = find_segments( indata );

	if( disable_random_model ) mmt.disable_random_model();

	cerr << "INITIAL PARAMETERS" << endl << endl;
	mmt.pretty_print_parameters( cerr );

	// now we train on all the regions
	cerr  << endl << "Training on " << segments->size() << " regions." << endl;

	ereal prev_sc = 0.0;
	for( unsigned int j = 0; j < training_passes; ++j ) {
		cerr << endl << "Pass " << (j+1) << " of " << training_passes << "." << endl;
		mmt.start_training();
		mmt.constrained_bw_train( segments->begin(), segments->end(), nthreads );
		ereal pr = mmt.end_training();

		cerr << "Forward pr:" << pr.as_base() << ", " << (double)pr << endl;
		mmt.write_parameters( cerr );

		// tie parameters
		for( int k = 0; k < mmt.num_models(); ++k ) {
			nw_model_parameters_ptr nwp = mmt.get_parameters(k);
			mmt.set_parameters(k,*nwp,(int)mmt.get_region_length(k));
		}
		mmt.set_random_param((int)mmt.get_random_length_a(), (int)mmt.get_random_length_b(), mmt.get_random_distribution());

		// check if we are finished
		if( fabs( (pr.as_base()-prev_sc.as_base()) ) < 1 ) break;
		prev_sc = pr;
	}

	return mmt;
}

cape_pairwise_aligner::region_pair_vector_ptr cape_pairwise_aligner::find_segments( input_data &indata ) {
	using namespace std;
	// start with one large region (implicit anchors at each end)
   scored_fragment_ptr_list anchors;
   scored_fragment_ptr front(new scored_fragment()), back(new scored_fragment());
	// we use 0,0 so that the midpoint is a valid sequence position
   front->add( closed_interval(0,0) );
   front->add( closed_interval(0,0) );
	// right side is always taken to be midpoint-1, so this indicates the end of the sequence
   back->add( closed_interval(indata.a.seq->data.size(),indata.a.seq->data.size()) );
   back->add( closed_interval(indata.b.seq->data.size(),indata.b.seq->data.size()) );
   anchors.push_back( front );
   anchors.push_back( back );

   double random_sample_original = random_sample;
   scored_fragment_ptr_list_iter prev = anchors.begin(), i = ++anchors.begin();
   while ( i != anchors.end() ) {
      // if area too small, get new anchors for it
      if( interior_area( **prev, **i ) > MAX_AREA ) {
         // if either sequence has >=40% repeats, then don't mask them out
         scored_fragment ifrag = interior( **prev, **i );
         //bool use_mask =
			//(fraction_repeats(*(indata.a.stats), ifrag[0]) >= 0.40  || fraction_repeats(*(indata.b.stats), ifrag[1]) >= 0.40)
			//? false : true;
         scored_fragment_ptr_list_ptr new_anchors = find_anchors( indata, interior( **prev, **i ), true );
         anchors.splice( i, *new_anchors );

         // adjust i
         i = prev;
         ++i;

			// small enough, skip to next range
      } else {
         random_sample = random_sample_original;
         ++i;
         ++prev;
      }
   }
	// now filter out as many guessed fragments as possible
	if( anchors.size() > 2 ) {
		prev = anchors.begin();
		i = ++anchors.begin();
		scored_fragment_ptr_list_iter j = i;
		++j;

		while( j != anchors.end() ) {
			if( (**i).score() < THRESHOLD && interior_area(**prev,**j) <= MAX_AREA ) {
				i = anchors.erase(i);
				++j;
			} else {
				++prev;
				++i;
				++j;
			}
		}
	}

   cerr << endl << "Finished anchoring with " << anchors.size() << " anchors." << endl << endl;

	// first we create a set of sequence regions
	region_pair_vector_ptr segments( new region_pair_vector() );

   for( prev = anchors.begin(), i = ++anchors.begin(); i != anchors.end(); ++i, ++prev ) {
      scored_fragment region = midpoint_interior( **prev, **i );
		if( verbose ) cerr << region << endl;
      assert( region[0].a < region[0].b );
      assert( region[1].a < region[1].b );

		dna_sequence_region_ptr region_a( new dna_sequence_region(indata.a.seq,region[0]) );
		dna_sequence_region_ptr region_b( new dna_sequence_region(indata.b.seq,region[1]) );
		segments->push_back( make_pair(region_a,region_b) );
	}
	return segments;

}

std::pair<pairwise_dna_alignment,annotation_ptr> cape_pairwise_aligner::align_and_annotate( dna_sequence_ptr seqa, dna_sequence_ptr seqb ) {
	using namespace std;

	// preprocess and package input data
   input_data indata( seqa, seqb );

	region_pair_vector_ptr segments = find_segments( indata );

	if( disable_random_model ) mma.disable_random_model();

	// do alignment
	cerr << "Aligning " << segments->size() << " regions." << endl;

	annotation_ptr ann;
	dna_alignment_sequence_ptr alignmenta( new dna_alignment_sequence() );
   dna_alignment_sequence_ptr alignmentb(new dna_alignment_sequence() );
   pairwise_dna_alignment final = pairwise_dna_alignment( alignmenta, alignmentb, 0.0 );

	mma.set_interface_size(4);
	tie(final,ann) = mma.constrained_align_and_annotate( segments->begin(), segments->end() );
	cerr << "Done." << endl;
   return make_pair( final, ann );
}

// here we filter by distance to optimal chain
scored_fragment_ptr_vector_ptr cape_pairwise_aligner::chain_filter( scored_fragment_ptr_vector &fragments ) {
	using namespace std;

	// perhaps the problem with sorting is that the reference counting gets borked and thus a fragment
	// is accidentally deleted. If so, keeping a copy of each pointer here should stop it.
	scored_fragment_ptr_vector copy_test = fragments;

	// hold the final result
	scored_fragment_ptr_vector_ptr result( new scored_fragment_ptr_vector() );
	assert( fragments.size() > 0 );

	ereal basic_score;
	basic_score.set_base(200.0);
	for( scored_fragment_ptr_vector_iter i = fragments.begin(); i != fragments.end(); ++i ) (**i).set_score(basic_score );

   // first find optimal chain
	fragment_algorithm::optimal_chain( fragments, back_inserter(*result), fragment_algorithm::gap_score_cf(1.0 - 1.0/mma.get_random_length_a()) );
	//fragment_algorithm::optimal_chain_fast( fragments, back_inserter(*result) );
//	fragment_algorithm::optimal_chain( fragments, back_inserter(chain) );

	//for( scored_fragment_ptr_vector_iter i = fragments.begin(); i != fragments.end(); ++i ) assert( i->get() != 0 );
	assert( result->size() > 0 );
	return result;
}

void cape_pairwise_aligner::score_fragments( input_data &in, scored_fragment_ptr_vector_ptr fragments, bool use_mask ) {
	using namespace std;

	// score each fragment
	concurrent_queue<scored_fragment_ptr> fqueue;
	copy( fragments->begin(), fragments->end(), back_inserter(fqueue) );
	fqueue.shutdown_when_empty();
	boost::thread_group workers;

	vector<nw_model_parameters> nwsets;
	for( int i = 0; i < mma.num_models(); ++i ) nwsets.push_back( mma.get_parameters(i) );
	custom_fragment_score_worker worker( nwsets, in, fqueue );
	for( int i = 0; i < nthreads; ++i ) workers.create_thread(worker);

   workers.join_all();
}

scored_fragment_ptr_list_ptr cape_pairwise_aligner::find_anchors( input_data &in, scored_fragment f, bool use_mask ) {
    using namespace std;
    using namespace __gnu_cxx;
    using namespace boost;

    seed_parameters p = computeOSP(in,f);
    random_seed_factory sf( get<1>(p) );
    seed myseed = sf.create( get<0>(p) );

    hash_factory<hash_hit_data,dna_sequence> hsh( myseed );
    hsh.set_use_mask( use_mask );
    hash_hit_data_ptr sequence_hash = hsh.create( in.a.seq, in.b.seq, f );
    scored_fragment_ptr_vector_ptr all_fragments( new scored_fragment_ptr_vector() );
    fragment_algorithm::create_fragments( *sequence_hash, back_inserter(*all_fragments) );
    scored_fragment_ptr_vector_ptr fragments = all_fragments;

    // score each fragment
    cerr << f << " w: " << get<0>(p) << " l: " << get<1>(p) << " expected/found: " << get<2>(p) << "/" << fragments->size()
		  << " p: " << myseed.pattern_string();
    if( use_mask ) cerr << " repeats masked,";
    cerr << " scoring..." << endl;

    if( all_fragments->size() == 0 ) throw runtime_error("cape_pairwise_anchor: found no fragments");

    // use chain filter to reduce number of test points
    if( MAX_DISTANCE_FROM_CHAIN > 0 ) {
		fragments = chain_filter( *all_fragments );
		cerr << "filtered fragments: " << (all_fragments->size()-fragments->size()) << endl;
	}

    // clear score on each fragment
	for( scored_fragment_ptr_vector_iter i = all_fragments->begin(); i != all_fragments->end(); ++i ) (**i).set_score(0.0);

	score_fragments( in, fragments, use_mask );

	// split fragments into good ones and poor scoring ones
	scored_fragment_ptr_vector good_fragments;
	scored_fragment_ptr_vector poor_fragments;
	split_copy( *fragments, score_below(THRESHOLD), back_inserter(poor_fragments), back_inserter(good_fragments) );

   // filter out any fragments with score below the sample deviation
   cerr << "before filter: " << fragments->size();
   cerr << "\tafter filter: " << good_fragments.size();

  /* for( scored_fragment_ptr_vector_iter i = good_fragments.begin(); i != good_fragments.end(); ++i ) {
      cerr << **i << "\t" <<(**i).score() << endl;
   }
   for( fragment_ptr_vector_iter i = fragments->begin(); i != fragments->end(); ++i ) {
      cerr << (**i)[0].a << "\t" <<(**i)[1].a << endl;
   }
   exit(1);*/

   // chain fragments
   scored_fragment_ptr_list_ptr chain( new scored_fragment_ptr_list() );
	fragment_algorithm::optimal_chain( good_fragments, front_inserter(*chain), fragment_algorithm::null_cf() );

	cerr << "\tchain size: " << chain->size() << endl;

	// Now we guess if we must. For each region in the chain, if we will end up
	// using the same parameter set as we just did, we may as well just guess now
	// and avoid duplicate work. We keep guessing until either the tiling is dense
	// enough or we will use a new seed on the next pass.

	// First we add a start and end fragment to our chain.
	pair<scored_fragment_ptr,scored_fragment_ptr> ends = build_ends(f);
	chain->push_front(ends.first);
	chain->push_back(ends.second);

	// iterate over each area
	for( scored_fragment_ptr_list_iter i = ++(chain->begin()), prev = chain->begin(); i != chain->end(); ) {
		// skip if area is small enough
		if( interior_area( **prev, **i ) <= MAX_AREA ) { ++prev; ++i; continue; }

		// otherwise get the parameters we will use for it
		fragment ifrag = interior( **prev, **i );
		seed_parameters next_p = computeOSP(in,ifrag);

		// if we will use a new weight, leave for next round
		if( get<0>(next_p) != get<0>(p) ) { ++prev; ++i; continue; }

		// Otherwise we need to choose a random fragment in the region from all_fragments.
		// First we filter out fragments that can't be in the set.
		scored_fragment_ptr_vector_ptr filtered( new scored_fragment_ptr_vector() );
		for( scored_fragment_ptr_vector_iter j = all_fragments->begin(); j != all_fragments->end(); ++j )
			if( (**prev >> **j) && (**j >> **i) ) filtered->push_back( *j );

		// We didn't find any good fragment to insert, and will repeat this search next time.
		// So we are really out of luck now. This should never happen of course, but just in case.
		if( filtered->size() == 0 ) {
			throw runtime_error("cape_pairwise_anchor: found no fragments causing endless loop");
		}

		if( verbose ) cerr << "Rescoring a possible " << filtered->size() << " fragments for filling." << endl;
		// Score any unscored fragments.
		score_fragments(in, filtered, use_mask);

		/*
		// Try to chain again.
		scored_fragment_ptr_list_ptr subchain( new scored_fragment_ptr_list() );
		fragment_algorithm::optimal_chain( *filtered, front_inserter(*subchain), fragment_algorithm::null_cf() );

		if( subchain->size() > 0 ) {
			cerr << "chained: " << subchain->size() << endl;

		    chain->splice( i, *subchain );
			i = prev;
			++i;
		} else {
			// SHOULD NEVER GET HERE
			assert(0 && "error in anchoring" );
*/
			// We sort the fragments by score.
			sort( filtered->begin(), filtered->end(), by_increasing_fragment_score() );
	//		cerr << filtered->back()->score() << "\t" << filtered->front()->score() << endl;

			// choose the best scoring fragment
			if( verbose ) cerr << "greedily chose " << *(filtered->back()) << endl;
			i = chain->insert( i, filtered->back() );
//		}
	}

	// remove the front and back fake anchors
	chain->pop_front(); chain->pop_back();

	// output and return our final chain
   if( verbose ) for( scored_fragment_ptr_list_citer i = chain->begin(); i != chain->end(); ++i ) {
      cerr << **i << "\t" << (**i).parameter_set() << endl;
   }

   return chain;
}

void cape_pairwise_aligner::set_parameters_from_file( std::string filename ) {
   using namespace std;
   ifstream in( filename.c_str() );
   if( in.fail() ) throw runtime_error("set_parameters_from_file: could not open parameter file");

	vector<double> mdist;
	mdist.push_back( read<double>(in) );
	int rand_a = (int)read<double>(in);
	int rand_b = (int)read<double>(in);
	blitz::TinyVector<double,4> q;
	q = read<double>(in), read<double>(in), read<double>(in), read<double>(in);
	mma.set_random_param(rand_a, rand_b, q);
	mmt.set_random_param(rand_a, rand_b, q);

	int nmodels = read<int>(in);
	for( int j = 0; j < nmodels; ++j ) {
		mdist.push_back( read<double>(in) );
		int length = (int)read<double>(in);
		nw_model_parameters param;
		in >> param;
		mma.set_parameters( j, param, length );
		mmt.set_parameters( j, param, length );
	}
	mma.set_region_dist( mdist );
	mmt.set_region_dist( mdist );
}

void cape_pairwise_aligner::initialize_parameters_from_file( std::string filename ) {
   using namespace std;
   ifstream infile(filename.c_str());
	if( infile.fail() ) throw runtime_error("initialize_parameters_from_file: could not open initialization file");
	unsigned int nmodels = read<unsigned int>(infile);

	for( unsigned int k = 0; k < nmodels; ++k ) {
		nw_model_parameters newp;
		double d = read<double>(infile);
		newp.pr_open = read<double>(infile);
		newp.mean_gap_length = read<double>(infile);
		unsigned int explicit_background = read<unsigned int>(infile);

		if( explicit_background ) {
			newp.q(0) = read<double>(infile);
			newp.q(1) = read<double>(infile);
			newp.q(2) = read<double>(infile);
			newp.q(3) = read<double>(infile);
		} else newp.q = q;
		newp.set_p_hky( d, 1.0 );
		mma.set_parameters( k, newp, 200 );
		mmt.set_parameters( k, newp, 200 );
	}

	infile.close();
}

void cape_pairwise_aligner::mma_disable_random_model() {
	disable_random_model = true;
}

void cape_pairwise_aligner::set_background(  blitz::TinyVector<double,4> nq ) {
   for( int i = 0; i < 4; ++i ) q(i) = nq(i);
}


// Copyright (c) 2021 The Dash Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <bls/bls.h>
#include <consensus/validation.h>
#include <evo/mnhftx.h>
#include <evo/specialtx.h>
#include <primitives/transaction.h>
#include <uint256.h>
#include <util/strencodings.h>

#include <boost/test/unit_test.hpp>
#include <test/setup_common.h>

#include <cstdint>
#include <vector>

bool VerifyMNHFTx(const CTransaction& tx, CValidationState& state)
{
    MNHFTxPayload mnhfTx;
    if (!GetTxPayload(tx, mnhfTx)) {
        return state.DoS(100, false, REJECT_INVALID, "bad-mnhf-payload");
    }

    if (mnhfTx.nVersion == 0 || mnhfTx.nVersion > MNHFTxPayload::CURRENT_VERSION) {
        return state.DoS(100, false, REJECT_INVALID, "bad-mnhf-version");
    }

    return true;
}

static CMutableTransaction CreateMNHFTx(const uint256& mnhfTxHash, const CBLSSignature& cblSig, const uint16_t& versionBit)
{
    MNHFTxPayload extraPayload;
    extraPayload.nVersion = 1;
    extraPayload.signal.nVersion = versionBit;
    extraPayload.signal.quorumHash = mnhfTxHash;
    extraPayload.signal.sig = cblSig;

    CMutableTransaction tx;
    tx.nVersion = 3;
    tx.nType = TRANSACTION_MNHF_SIGNAL;
    SetTxPayload(tx, extraPayload);

    return tx;
}

BOOST_FIXTURE_TEST_SUITE(specialtx_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(verify_mnhf_specialtx_tests)
{
    int count = 10;
    uint16_t ver = 2;

    std::vector<CBLSSignature> vec_sigs;
    std::vector<CBLSPublicKey> vec_pks;
    std::vector<CBLSSecretKey> vec_sks;

    CBLSSecretKey sk;
    uint256 hash = GetRandHash();
    for (int i = 0; i < count; i++) {
        sk.MakeNewKey();
        vec_pks.push_back(sk.GetPublicKey());
        vec_sks.push_back(sk);
    }

    CBLSSecretKey ag_sk = CBLSSecretKey::AggregateInsecure(vec_sks);
    CBLSPublicKey ag_pk = CBLSPublicKey::AggregateInsecure(vec_pks);

    BOOST_CHECK(ag_sk.IsValid());
    BOOST_CHECK(ag_pk.IsValid());

    uint256 verHash = uint256S(itostr(ver));
    auto sig = ag_sk.Sign(verHash);
    BOOST_CHECK(sig.VerifyInsecure(ag_pk, verHash));

    const CMutableTransaction tx = CreateMNHFTx(hash, sig, ver);
    CValidationState state;
    BOOST_CHECK(VerifyMNHFTx(CTransaction(tx), state));
}

BOOST_AUTO_TEST_SUITE_END()


#include "MicroManagerTerran.h"

#include <BWAPI.h>
//#include <BWTA.h>
#include <math.h>
#include <boost/foreach.hpp>

#include "Common.h"

MicroManagerTerran::MicroManagerTerran(){

	Graph.clear();

	BOOST_FOREACH( BWTA::Region* reg, BWTA::getRegions() ){

		MoveGraphT newG;
		newG.reg = reg;
		newG.Center = reg->getCenter();
		Graph.push_back( newG );
	}

	for( int i=0; i<Graph.size(); i++){

		/*
		for(int j=0; j<Graph[i].reg->getReachableRegions().size(); j++){

			for(int k=0; k<Graph.size(); k++){

			}
		}
		*/

	}

}

void MicroManagerTerran::drawInfo()
{

   	for( int i=0; i<Graph.size(); i++){

		Broodwar->drawCircleMap( Graph[i].Center.x(), Graph[i].Center.y() ,  4 , BWAPI::Colors::White , true );

		BOOST_FOREACH( BWTA::Chokepoint* choke , Graph[i].reg->getChokepoints() ){

			Broodwar->drawLineMap(choke->getRegions().first->getCenter().x(), choke->getRegions().first->getCenter().y() , choke->getRegions().second->getCenter().x(), choke->getRegions().second->getCenter().y() , BWAPI::Colors::White );

		}
		/*
		BOOST_FOREACH( BWTA::Region* reg , Graph[i].reg->getReachableRegions() ){
			Broodwar->drawLineMap( Graph[i].Center.x(), Graph[i].Center.y() , reg->getCenter().x(), reg->getCenter().y() , BWAPI::Colors::White );
		}
		*/

	}

}

void MicroManagerTerran::onFrame()
{

	for(int i=0; i<ProdMan->BuildingsQueue.size(); i++){

		if( ProdMan->BuildingsQueue[i].type == BWAPI::UnitTypes::Terran_Command_Center
			&& Broodwar->self()->completedUnitCount( BWAPI::UnitTypes::Terran_Command_Center ) >= 2 ){
			bool already = false;
			for(int j=0; j<DefenceBases.size(); j++){
				if(  ProdMan->BuildingsQueue[i].buildLocation == DefenceBases[j].CClocation ){
					already = true;
				}
			}

			if( already == false
				&& BWTA::getRegion( ProdMan->BuildingsQueue[i].buildLocation  ) != InfoMan->NatRegion
				&& BWTA::getRegion( ProdMan->BuildingsQueue[i].buildLocation  ) != InfoMan->OurMainregion
				){
				InfluenceBaseT newInfluenceBase;
				newInfluenceBase.CClocation = ProdMan->BuildingsQueue[i].buildLocation;
				newInfluenceBase.DefenceScoreNeeded = 0;
				newInfluenceBase.reg = BWTA::getRegion( ProdMan->BuildingsQueue[i].buildLocation  );
				MicroSquadTerran newDsquad;
				BWAPI::Position bestPos = BWAPI::Positions::Unknown;
				int closestEbase = 99999;
				int furthestOurbase = 0;
				BOOST_FOREACH( BWTA::Chokepoint * choke, newInfluenceBase.reg->getChokepoints() ){
					if( choke->getCenter().getDistance( InfoMan->PosOurBase ) > furthestOurbase ){
						furthestOurbase = choke->getCenter().getDistance( InfoMan->PosOurBase );
						bestPos = choke->getCenter();
					}
					/*
					if( choke->getCenter().getDistance( MacroMan->attackPos ) < closestEbase ){
						closestEbase = choke->getCenter().getDistance( MacroMan->attackPos );
						bestPos = choke->getCenter();
					}
					*/
				}
				newDsquad.AttackTarget = bestPos;
				newInfluenceBase.defenceSquad = newDsquad;
				DefenceBases.push_back( newInfluenceBase );
			}

		}

	}

	for(int j=0; j<DefenceBases.size(); j++){
		int defScore = 0;
		if(  DefenceBases[j].reg != NULL ){
			BWAPI::Position cent = DefenceBases[j].reg->getCenter();
			BOOST_FOREACH( BWAPI::Unit* enemy, Broodwar->enemy()->getUnits() ){
				if( cent.getDistance( enemy->getPosition() ) < 20*32 ){
					defScore += enemy->getType().buildScore();
				}
			}
			/*
			BOOST_FOREACH( BWAPI::Unit* enemy, Broodwar->getUnitsInRadius(   DefenceBases[j].reg->getCenter(), 20*32 ) ){
				if( enemy->getPlayer() == Broodwar->enemy() ){
					defScore += enemy->getType().buildScore();
				}
			}
			*/
		}

		int VulturesNeeded = 0 + ( defScore / 75 );

		if( VulturesNeeded > 7 ){
			VulturesNeeded = 7;
		}
		int GoliathsNeeded = 0;
		if (defScore > 0 ){
			GoliathsNeeded = 1;
		}

		for(int m=0; m<DefenceBases[j].defenceSquad.Vultures.size(); m++){
			if(  !DefenceBases[j].defenceSquad.Vultures[m]->exists() ){
				DefenceBases[j].defenceSquad.Vultures.erase( DefenceBases[j].defenceSquad.Vultures.begin() + m);
				m--;
				continue;
			}
		}

		for(int m=0; m<DefenceBases[j].defenceSquad.Goliaths.size(); m++){
			if(  !DefenceBases[j].defenceSquad.Goliaths[m]->exists() ){
				DefenceBases[j].defenceSquad.Goliaths.erase( DefenceBases[j].defenceSquad.Goliaths.begin() + m);
				m--;
				continue;
			}
		}

		if( DefenceBases[j].defenceSquad.Vultures.size() < VulturesNeeded ){
			if( MacroMan->Squads[0].Vultures.size() > 0 ){
				DefenceBases[j].defenceSquad.Vultures.push_back( MacroMan->Squads[0].Vultures[ MacroMan->Squads[0].Vultures.size() - 1 ]  );
				MacroMan->Squads[0].Vultures.pop_back();
			}
		}

		if( DefenceBases[j].defenceSquad.Vultures.size() > VulturesNeeded ){
			if(  DefenceBases[j].defenceSquad.Vultures.size() > 0 ){
				MacroMan->AddUnit( DefenceBases[j].defenceSquad.Vultures[ DefenceBases[j].defenceSquad.Vultures.size() - 1 ] );
				DefenceBases[j].defenceSquad.Vultures.pop_back();
			}
		}

		if( DefenceBases[j].defenceSquad.Goliaths.size() < GoliathsNeeded ){
			if( MacroMan->Squads[0].Goliaths.size() > 0 ){
				DefenceBases[j].defenceSquad.Goliaths.push_back( MacroMan->Squads[0].Goliaths[ MacroMan->Squads[0].Goliaths.size() - 1 ]  );
				MacroMan->Squads[0].Goliaths.pop_back();
			}
		}

		if( DefenceBases[j].defenceSquad.Goliaths.size() > GoliathsNeeded ){
			if(  DefenceBases[j].defenceSquad.Goliaths.size() > 0 ){
				MacroMan->AddUnit( DefenceBases[j].defenceSquad.Goliaths[ DefenceBases[j].defenceSquad.Goliaths.size() - 1 ] );
				DefenceBases[j].defenceSquad.Goliaths.pop_back();
			}
		}

		OnFrameTerranSquadDefendExpand( DefenceBases[j].defenceSquad );

	}

	//drawInfo();

}

void MicroManagerTerran::OnFrameTerranSquadDefendExpand(  MicroSquadTerran squad  )
{

	for(unsigned int i=0; i<squad.Vultures.size(); i++){
		//Broodwar->printf("%s",Marines[i]->getOrder().c_str() );

		std::set<Unit*> EnemyClose = squad.Vultures[i]->getUnitsInRadius( 9*32 );
		Unit* closest = NULL;
		int closeDist = 9999999;
		bool Bunker = false;
		BOOST_FOREACH( Unit* close, EnemyClose )
        {
			if(  close->getType().isBuilding() &&  close->getType() != BWAPI::UnitTypes::Terran_Bunker){
				continue;
			}
			if(  close->getType() == BWAPI::UnitTypes::Terran_Bunker
				&& close->getPlayer() == Broodwar->enemy()
				&& squad.Vultures[i]->getDistance( close->getPosition() ) < 8*32  ){
				Bunker = true;
			}
			if( close->getPlayer() == Broodwar->enemy() && closeDist > squad.Vultures[i]->getDistance( close->getPosition() )  ){
				closeDist = squad.Vultures[i]->getDistance( close->getPosition() );
				closest = close;
			}
		}

	  // if we can't shoot, run away  if(kite)
		//  Squads[0].Vultures[i]->getHitPoints() < 25 &&
		//( closeDist < 4*32 &&  Squads[0].Vultures[i]->getGroundWeaponCooldown() != 0 )
		 // || ( closest != NULL &&  Squads[0].Vultures[i]->getGroundWeaponCooldown() != 0 )
	  if (   false )
	  {
		//BWAPI::Position fleePosition(Marines[i].marine->getPosition() - closest->getPosition() + Marines[i].marine->getPosition());
		  BWAPI::Position fleePosition( InfoMan->PosOurBase );

		//BWAPI::Broodwar->drawLineMap(Marines[i]->getPosition().x(), Marines[i]->getPosition().y(),
			//fleePosition.x(), fleePosition.y(), BWAPI::Colors::Cyan);

		squad.Vultures[i]->move( fleePosition );
		continue;
		//Marines[i].state = M_Fleeing;
	  }

		//if there are no enemies nearby, attack;
		if( closest == NULL ||  squad.Vultures[i]->getGroundWeaponCooldown() == 0  ){ //|| closeDist > 3*32
			if( InfoMan->EnemyBase != BWAPI::TilePositions::Unknown ){
				//BWAPI::Position attackPos = BWAPI::Position( InfoMan->EnemyBase );

				if( squad.Vultures[i]->getPosition().getDistance( squad.AttackTarget ) > 6*32 ){
			      //Marines[i].state = M_Attacking;
			      //BWAPI::Position attackPos = BWAPI::Position( InfoMan->EnemyBase );
                  if( closest != NULL ){
					  if( squad.Vultures[i]->getOrderTarget() == NULL ){
                        squad.Vultures[i]->attack(  closest );
					  }
					  else if( squad.Vultures[i]->getOrderTarget()->getID() != closest->getID() ){
		                squad.Vultures[i]->attack(  closest );
					  }
				  } else {

					  //prevent spam
					  if( squad.Vultures[i]->getOrder() != BWAPI::Orders::AttackMove &&
						  squad.Vultures[i]->getOrder() != BWAPI::Orders::AttackTile &&
						  squad.Vultures[i]->getOrder() != BWAPI::Orders::AttackUnit ){

		                  squad.Vultures[i]->attack( squad.AttackTarget );

					  }
				  }
				  //Broodwar->printf("Attack");
				}
			}
			continue;
		}

	}

	for(unsigned int i=0; i<squad.Goliaths.size(); i++){
		//Broodwar->printf("%s",Marines[i]->getOrder().c_str() );

		std::set<Unit*> EnemyClose = squad.Goliaths[i]->getUnitsInRadius( 15*32 );
		Unit* closest = NULL;
		int closeDist = 9999999;
		bool Bunker = false;
		BOOST_FOREACH( Unit* close, EnemyClose )
        {
			if(  close->getType().isBuilding() &&  close->getType() != BWAPI::UnitTypes::Terran_Bunker){
				continue;
			}
			if(  close->getType() == BWAPI::UnitTypes::Terran_Bunker
				&& close->getPlayer() == Broodwar->enemy()
				&& squad.Goliaths[i]->getDistance( close->getPosition() ) < 8*32  ){
				Bunker = true;
			}
			if( close->getPlayer() == Broodwar->enemy() && closeDist > squad.Goliaths[i]->getDistance( close->getPosition() )  ){
				closeDist = squad.Goliaths[i]->getDistance( close->getPosition() );
				closest = close;
			}
		}

	  // if we can't shoot, run away  if(kite)
		//  || (Squads[0].Goliaths[i]->getHitPoints() < 25 && closest != NULL &&  Squads[0].Goliaths[i]->getGroundWeaponCooldown() != 0 )
	  if ( ( closeDist < 2*32 &&  squad.Goliaths[i]->getGroundWeaponCooldown() != 0 )  )
	  {
		//BWAPI::Position fleePosition(Marines[i].marine->getPosition() - closest->getPosition() + Marines[i].marine->getPosition());
		  BWAPI::Position fleePosition( InfoMan->PosOurBase );

		//BWAPI::Broodwar->drawLineMap(Marines[i]->getPosition().x(), Marines[i]->getPosition().y(),
			//fleePosition.x(), fleePosition.y(), BWAPI::Colors::Cyan);

		squad.Goliaths[i]->move( fleePosition );
		continue;
		//Marines[i].state = M_Fleeing;
	  }

		//if there are no enemies nearby, attack;
		if( closest == NULL ||  squad.Goliaths[i]->getGroundWeaponCooldown() == 0  ){ //|| closeDist > 3*32
			if( InfoMan->EnemyBase != BWAPI::TilePositions::Unknown ){
				//BWAPI::Position attackPos = BWAPI::Position( InfoMan->EnemyBase );

				if( squad.Goliaths[i]->getPosition().getDistance( squad.AttackTarget ) > 6*32 ){
			      //Marines[i].state = M_Attacking;
			      //BWAPI::Position attackPos = BWAPI::Position( InfoMan->EnemyBase );
                  if( closest != NULL ){
					  if( squad.Goliaths[i]->getOrderTarget() == NULL ){
                        squad.Goliaths[i]->attack(  closest );
					  }
					  else if( squad.Goliaths[i]->getOrderTarget()->getID() != closest->getID() ){
		                squad.Goliaths[i]->attack(  closest );
					  }
				  } else {

					  //prevent spam
					  if( squad.Goliaths[i]->getOrder() != BWAPI::Orders::AttackMove &&
						  squad.Goliaths[i]->getOrder() != BWAPI::Orders::AttackTile &&
						  squad.Goliaths[i]->getOrder() != BWAPI::Orders::AttackUnit ){

		                  squad.Goliaths[i]->attack(  squad.AttackTarget );

					  }
				  }
				  //Broodwar->printf("Attack");
				}
			}
			continue;
		}

	}

}


#ifndef AMGCL_MPI_COARSENING_RUNTIME_HPP
#define AMGCL_MPI_COARSENING_RUNTIME_HPP

/*
The MIT License

Copyright (c) 2012-2018 Denis Demidov <dennis.demidov@gmail.com>

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.
*/

/**
 * \file   amgcl/mpi/coarsening/aggregation.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  Runtime wrapper for distributed coarsening schemes.
 */

#include <boost/property_tree/ptree.hpp>

#include <amgcl/util.hpp>
#include <amgcl/mpi/util.hpp>
#include <amgcl/mpi/distributed_matrix.hpp>
#include <amgcl/mpi/coarsening/aggregation.hpp>
#include <amgcl/mpi/coarsening/smoothed_aggregation.hpp>

namespace amgcl {
namespace runtime {
namespace mpi {
namespace coarsening {

enum type {
    aggregation,
    smoothed_aggregation
};

std::ostream& operator<<(std::ostream &os, type s)
{
    switch (s) {
        case aggregation:
            return os << "aggregation";
        case smoothed_aggregation:
            return os << "smoothed_aggregation";
        default:
            return os << "???";
    }
}

std::istream& operator>>(std::istream &in, type &s)
{
    std::string val;
    in >> val;

    if (val == "aggregation")
        s = aggregation;
    else if (val == "smoothed_aggregation")
        s = aggregation;
    else
        throw std::invalid_argument("Invalid coarsening value. Valid choices are: "
                "aggregation, smoothed_aggregation.");

    return in;
}

template <class Backend>
struct wrapper {
    typedef amgcl::mpi::distributed_matrix<Backend> matrix;
    typedef boost::property_tree::ptree params;

    type c;
    void *handle;

    wrapper(params prm = params())
        : c(prm.get("type", smoothed_aggregation)), handle(0)
    {
        if (!prm.erase("type")) AMGCL_PARAM_MISSING("type");

        switch (c) {
            case aggregation:
                {
                    typedef amgcl::mpi::coarsening::aggregation<Backend> C;
                    handle = static_cast<void*>(new C(prm));
                }
                break;
            case smoothed_aggregation:
                {
                    typedef amgcl::mpi::coarsening::smoothed_aggregation<Backend> C;
                    handle = static_cast<void*>(new C(prm));
                }
                break;
            default:
                throw std::invalid_argument("Unsupported coarsening type");
        }
    }

    ~wrapper() {
        switch(c) {
            case aggregation:
                {
                    typedef amgcl::mpi::coarsening::aggregation<Backend> C;
                    delete static_cast<C*>(handle);
                }
                break;
            case smoothed_aggregation:
                {
                    typedef amgcl::mpi::coarsening::smoothed_aggregation<Backend> C;
                    delete static_cast<C*>(handle);
                }
                break;
            default:
                break;
        }
    }

    std::tuple< std::shared_ptr<matrix>, std::shared_ptr<matrix> >
    transfer_operators(const matrix &A) {
        switch (c) {
            case aggregation:
                {
                    typedef amgcl::mpi::coarsening::aggregation<Backend> C;
                    return static_cast<C*>(handle)->transfer_operators(A);
                }
            case smoothed_aggregation:
                {
                    typedef amgcl::mpi::coarsening::smoothed_aggregation<Backend> C;
                    return static_cast<C*>(handle)->transfer_operators(A);
                }
            default:
                throw std::invalid_argument("Unsupported partition type");
        }
    }

    std::shared_ptr<matrix>
    coarse_operator(const matrix &A, const matrix &P, const matrix &R) const {
        switch (c) {
            case aggregation:
                {
                    typedef amgcl::mpi::coarsening::aggregation<Backend> C;
                    return static_cast<C*>(handle)->coarse_operator(A, P, R);
                }
            case smoothed_aggregation:
                {
                    typedef amgcl::mpi::coarsening::smoothed_aggregation<Backend> C;
                    return static_cast<C*>(handle)->coarse_operator(A, P, R);
                }
            default:
                throw std::invalid_argument("Unsupported partition type");
        }
    }
};

template <class Backend>
unsigned block_size(const wrapper<Backend> &w) {
    switch (w.c) {
        case aggregation:
            {
                typedef amgcl::mpi::coarsening::aggregation<Backend> C;
                return block_size(*static_cast<const C*>(w.handle));
            }
        case smoothed_aggregation:
            {
                typedef amgcl::mpi::coarsening::smoothed_aggregation<Backend> C;
                return block_size(*static_cast<const C*>(w.handle));
            }
        default:
            throw std::invalid_argument("Unsupported coarsening type");
    }
}

} // namespace coarsening
} // namespace mpi
} // namespace runtime
} // namespace amgcl

#endif


#include <numpy_eigen/boost_python_headers.hpp>
#include <aslam/backend/OptimizerBase.hpp>
#include <aslam/backend/util/OptimizerProblemManagerBase.hpp>
#include <aslam/backend/OptimizerCallbackManager.hpp>
#include <aslam/backend/Optimizer.hpp>
#include <aslam/backend/Optimizer2.hpp>
#include <aslam/backend/OptimizerRprop.hpp>
#include <aslam/backend/OptimizerBFGS.hpp>
#include <aslam/backend/ScalarNonSquaredErrorTerm.hpp>
#include <aslam/python/ExportOptimizerCallbackEvent.hpp>
#include <boost/shared_ptr.hpp>
#include <sm/PropertyTree.hpp>

// some wrappers:
Eigen::VectorXd b(const aslam::backend::Optimizer * o)
{
	return o->b();
}
Eigen::VectorXd dx(const aslam::backend::Optimizer * o)
{
	return o->dx();
}
Eigen::VectorXd rhs(const aslam::backend::Optimizer * o)
{
	return o->rhs();
}

template <typename T>
std::string toString(const T& t) {
  std::ostringstream os;
  os << t;
  return os.str();
}

void exportOptimizer()
{
    using namespace boost::python;
    using namespace aslam::backend;

    class_<SolutionReturnValue>("SolutionReturnValue")
        .def_readwrite("JStart",&SolutionReturnValue::JStart)
        .def_readwrite("JFinal",&SolutionReturnValue::JFinal)
        .def_readwrite("iterations",&SolutionReturnValue::iterations)
        .def_readwrite("failedIterations",&SolutionReturnValue::failedIterations)
        .def_readwrite("lmLambdaFinal",&SolutionReturnValue::lmLambdaFinal)
        .def_readwrite("dXFinal",&SolutionReturnValue::dXFinal)
        .def_readwrite("dJFinal",&SolutionReturnValue::dJFinal)
        .def_readwrite("linearSolverFailure",&SolutionReturnValue::linearSolverFailure)
        ;

    class_<Optimizer, boost::shared_ptr<Optimizer> >("Optimizer",init<>())
        .def(init<OptimizerOptions>())
        .def("setProblem", static_cast<void(Optimizer::*)(boost::shared_ptr<OptimizationProblemBase>)>(&Optimizer::setProblem))

        /// \brief initialize the optimizer to run on an optimization problem.
        ///        This should be called before calling optimize()
        .def("initialize", &Optimizer::initialize)

        /// \brief initialize the linear solver specified in the optimizer options.
        .def("initializeLinearSolver", &Optimizer::initializeLinearSolver)

        /// \brief Run the optimization
        .def("optimize", &Optimizer::optimize)
        .def("optimizeDogLeg", &Optimizer::optimizeDogLeg)

        .def("buildGnMatrices", &Optimizer::buildGnMatrices)
        /// \brief Get the optimizer options.
        .add_property("options", make_function(&Optimizer::options,return_internal_reference<>()))

        /// \brief return the full Hessian
        .def("H", &Optimizer::H, return_internal_reference<>())

        /// \brief return the full rhs
        .def("rhs", &rhs)

        /// \brief return the reduced system lhs
        .def("A", &Optimizer::A, return_internal_reference<>())

        /// \brief return the reduced system rhs
        .def("b", &b)

        /// \brief return the reduced system dx
        .def("dx", &dx)

        /// The value of the objective function.
        .def("J", &Optimizer::J)

        // \todo Covariance calculations
        // void computeCovariances();
        // const Eigen::MatrixXd & getDenseBlockCovariance(int di1, int di2);
        // Eigen::MatrixXd getSparseSparseCovariance(int si1, int si2);
        // Eigen::MatrixXd getDenseSparseCovariance(int di, int si);

        /// \brief Evaluate the error at the current state.
        .def("evaluateError", &Optimizer::evaluateError)

        /// \brief Get dense design variable i.
        .def("denseVariable", &Optimizer::denseVariable, return_internal_reference<>())
        /// \brief Get sparse design variable i
        .def("sparseVariable", &Optimizer::sparseVariable, return_internal_reference<>())

        /// \brief how many dense design variables are involved in the problem
        .def("numDenseDesignVariables", &Optimizer::numDenseDesignVariables)

        /// \brief how many sparse design variables are involved in the problem
        .def("numSparseDesignVariables", &Optimizer::numSparseDesignVariables)

        .def("printTiming", &Optimizer::printTiming)

        .def("computeCovariances", &Optimizer::computeCovariances)
        .def("computeDiagonalCovariances", &Optimizer::computeDiagonalCovariances)
        // \todo Think of a nice way to expose this to Python
        //.def("computeCovarianceBlocks", &Optimizer::computeCovarianceBlocks)
        .def("getCovarianceBlock", &Optimizer::getCovarianceBlock,return_internal_reference<>())
        .def("P", &Optimizer::P,return_internal_reference<>())
        .def("getCovariance", &Optimizer::getCovariance,return_internal_reference<>())

        ;

    class_<Optimizer2, boost::shared_ptr<Optimizer2> >("Optimizer2",init<>())
        .def(init<Optimizer2Options>())
        .def("setProblem", &Optimizer2::setProblem)

        /// \brief initialize the optimizer to run on an optimization problem.
        ///        This should be called before calling optimize()
        .def("initialize", &Optimizer2::initialize)

        /// \brief initialize the linear solver specified in the optimizer options.
        .def("initializeLinearSolver", &Optimizer2::initializeLinearSolver)

        /// \brief Run the optimization
        .def("optimize", &Optimizer2::optimize)
        //.def("optimizeDogLeg", &Optimizer2::optimizeDogLeg)

        /// \brief Get the optimizer options.
        .add_property("options", make_function(&Optimizer2::options,return_internal_reference<>()))

        /// \brief return the full Hessian
        //.def("H", &Optimizer2::H, return_internal_reference<>())

        /// \brief return the full rhs
        //.def("rhs", &rhs)

        /// \brief return the reduced system lhs
        //.def("A", &Optimizer2::A, return_internal_reference<>())

        /// \brief return the reduced system rhs
        //.def("b", &b)

        /// \brief return the reduced system dx
        //.def("dx", &dx)

        /// The value of the objective function.
        .def("J", &Optimizer2::J)

        // \todo Covariance calculations
        // void computeCovariances();
        // const Eigen::MatrixXd & getDenseBlockCovariance(int di1, int di2);
        // Eigen::MatrixXd getSparseSparseCovariance(int si1, int si2);
        // Eigen::MatrixXd getDenseSparseCovariance(int di, int si);
        .def("computeCovariances", &Optimizer2::computeCovariances)
        .def("computeDiagonalCovariances", &Optimizer2::computeDiagonalCovariances)

        /// \brief Evaluate the error at the current state.
        .def("evaluateError", &Optimizer2::evaluateError)

        /// \brief Get dense design variable i.
        .def("densignVariable", &Optimizer2::designVariable, return_internal_reference<>())

        /// \brief how many dense design variables are involved in the problem
        .def("numDesignVariables", &Optimizer2::numDesignVariables)

        .def("printTiming", &Optimizer2::printTiming)
        .def("computeHessian", &Optimizer2::computeHessian)

        ;

    class_<OptimizerOptionsBase, boost::shared_ptr<OptimizerOptionsBase> >("OptimizerOptionsBase", init<>("OptimizerOptionsBase(): Constructor"))
        .def(init<const sm::PropertyTree&>("OptimizerOptionsBase(sm::PropertyTree pt): Constructor"))
        .def_readwrite("convergenceGradientNorm", &OptimizerOptionsBase::convergenceGradientNorm)
        .def_readwrite("convergenceDeltaX", &OptimizerOptionsBase::convergenceDeltaX)
        .def_readwrite("convergenceDeltaError", &OptimizerOptionsBase::convergenceDeltaError)
        .def_readwrite("maxIterations",&OptimizerOptionsBase::maxIterations)
        .def_readwrite("numThreadsJacobian", &OptimizerOptionsBase::numThreadsJacobian)
        .def_readwrite("numThreadsError", &OptimizerOptionsBase::numThreadsError)
        .def("__str__", &toString<OptimizerOptionsBase>)
        ;

    enum_<ConvergenceStatus>("ConvergenceStatus")
        .value("IN_PROGRESS", ConvergenceStatus::IN_PROGRESS)
        .value("FAILURE", ConvergenceStatus::FAILURE)
        .value("GRADIENT_NORM", ConvergenceStatus::GRADIENT_NORM)
        .value("DX", ConvergenceStatus::DX)
        .value("DOBJECTIVE", ConvergenceStatus::DOBJECTIVE)
        ;

    class_<OptimizerStatus, boost::shared_ptr<OptimizerStatus> >("OptimizerStatus")
        .def_readwrite("convergence",&OptimizerStatus::convergence)
        .def_readwrite("numIterations",&OptimizerStatus::numIterations)
        .def_readwrite("numJacobianEvaluations",&OptimizerStatus::numJacobianEvaluations)
        .def_readwrite("numErrorEvaluations",&OptimizerStatus::numErrorEvaluations)
        .def_readwrite("gradientNorm",&OptimizerStatus::gradientNorm)
        .def_readwrite("maxDeltaX",&OptimizerStatus::maxDeltaX)
        .def_readwrite("error",&OptimizerStatus::error)
        .def_readwrite("deltaError",&OptimizerStatus::deltaError)
        .def("success", &OptimizerStatus::success)
        .def("failure", &OptimizerStatus::failure)
        .def("__str__", &toString<OptimizerStatus>)
        ;

    class_<OptimizerBase, boost::shared_ptr<OptimizerBase>, boost::noncopyable >("OptimizerBase", no_init)

        .def("setProblem", pure_virtual(&OptimizerBase::setProblem),
             "Set up to work on the optimization problem.")

        .def("checkProblemSetup", pure_virtual(&OptimizerBase::checkProblemSetup),
             "Do a bunch of checks to see if the problem is well-defined. This includes checking that every error term is hooked up to design variables and running "
             "finite differences on error terms where this is possible.")

        .def("initialize", &OptimizerBase::initialize,
             "Initialize the optimizer to run on an optimization problem. optimize() will call initialize() upon the first call.")

        .def("isInitialized", pure_virtual(&OptimizerBase::isInitialized),
             "Initialize the optimizer to run on an optimization problem. optimize() will call initialize() upon the first call.")

        .def("reset", &OptimizerBase::reset,
             "Reset internal states but don't re-initialize the whole problem")

        .def("optimize", pure_virtual(&OptimizerBase::optimize),
             "Run the optimization")

        .add_property("status", make_function(&OptimizerBase::getStatus, return_internal_reference<>()),
                      "Status of the optimizer")

        .add_property("options", make_function(&OptimizerBase::getOptions, return_internal_reference<>()), &OptimizerBase::setOptions,
                      "Options of the optimizer")

        .def("isConverged", &OptimizerBase::isConverged,
             "Has the optimizer converged?")

        .def("isFailed", &OptimizerBase::isFailed,
            "Has the optimizer failed?")

        .def("isInProgress", &OptimizerBase::isInProgress,
             "Is the optimizer still running?")

        .add_property("callback", make_function(&OptimizerBase::callback, return_internal_reference<>()),
                      "Callback manager")
        ;

    class_<OptimizerProblemManagerBase, boost::shared_ptr<OptimizerProblemManagerBase>, bases<OptimizerBase>, boost::noncopyable >("OptimizerProblemManagerBase", no_init)
        ;

    enum_<OptimizerOptionsRprop::Method>("RpropMethod")
        .value("RPROP_PLUS", OptimizerOptionsRprop::Method::RPROP_PLUS)
        .value("RPROP_MINUS", OptimizerOptionsRprop::Method::RPROP_MINUS)
        .value("IRPROP_MINUS", OptimizerOptionsRprop::Method::IRPROP_MINUS)
        .value("IRPROP_PLUS", OptimizerOptionsRprop::Method::IRPROP_PLUS)
        ;

    class_<OptimizerOptionsRprop, boost::shared_ptr<OptimizerOptionsRprop>, bases<OptimizerOptionsBase> >("OptimizerOptionsRprop", init<>())
        .def_readwrite("etaMinus",&OptimizerOptionsRprop::etaMinus)
        .def_readwrite("etaPlus",&OptimizerOptionsRprop::etaPlus)
        .def_readwrite("initialDelta",&OptimizerOptionsRprop::initialDelta)
        .def_readwrite("minDelta",&OptimizerOptionsRprop::minDelta)
        .def_readwrite("maxDelta",&OptimizerOptionsRprop::maxDelta)
        .def_readwrite("regularizer", &OptimizerOptionsRprop::regularizer)
        .def_readwrite("method", &OptimizerOptionsRprop::method)
        .def_readwrite("useDenseJacobianContainer", &OptimizerOptionsRprop::useDenseJacobianContainer)
        .def("__str__", &toString<OptimizerOptionsRprop>)
        ;

    aslam::python::exportEvent<callback::event::DESIGN_VARIABLE_UPDATE_COMPUTED>("EVENT_DESIGN_VARIABLE_UPDATE_COMPUTED", "");

    class_<OptimizerRprop, boost::shared_ptr<OptimizerRprop>, bases<OptimizerProblemManagerBase> >("OptimizerRprop", init<>("OptimizerRprop(): Constructor with default options"))
        .def(init<const OptimizerOptionsRprop&>("OptimizerRprop(OptimizerOptionsRprop options): Constructor with custom options"))
        .def(init<const sm::PropertyTree&>("OptimizerRprop(PropertyTree propertyTree): Constructor from sm::PropertyTree"))
        ;
    implicitly_convertible< boost::shared_ptr<OptimizerRprop>, boost::shared_ptr<const OptimizerRprop> >();

    class_<LineSearchOptions, boost::shared_ptr<LineSearchOptions> >("LineSearchOptions", init<>())
        .def_readwrite("c1WolfeCondition",&LineSearchOptions::c1WolfeCondition)
        .def_readwrite("c2WolfeCondition", &LineSearchOptions::c2WolfeCondition)
        .def_readwrite("maxStepLength", &LineSearchOptions::maxStepLength)
        .def_readwrite("minStepLength", &LineSearchOptions::minStepLength)
        .def_readwrite("xtol", &LineSearchOptions::xtol)
        .def_readwrite("initialStepLength", &LineSearchOptions::initialStepLength)
        .def_readwrite("nMaxIterWolfe1", &LineSearchOptions::nMaxIterWolfe1)
        .def_readwrite("nMaxIterWolfe2", &LineSearchOptions::nMaxIterWolfe2)
        .def_readwrite("nMaxIterZoom", &LineSearchOptions::nMaxIterZoom)
        .def("__str__", &toString<LineSearchOptions>)
        ;

    class_<OptimizerOptionsBFGS, boost::shared_ptr<OptimizerOptionsBFGS>, bases<OptimizerOptionsBase> >("OptimizerOptionsBFGS", init<>())
        .def_readwrite("linesearch", &OptimizerOptionsBFGS::linesearch)
        .def_readwrite("useDenseJacobianContainer", &OptimizerOptionsBFGS::useDenseJacobianContainer)
        .def_readwrite("regularizer", &OptimizerOptionsBFGS::regularizer)
        .def("__str__", &toString<OptimizerOptionsBFGS>)
        ;

    class_<OptimizerBFGS, boost::shared_ptr<OptimizerBFGS>, bases<OptimizerProblemManagerBase> >("OptimizerBFGS", init<>("OptimizerBFGS(): Constructor with default options"))
        .def(init<const OptimizerOptionsBFGS&>("OptimizerBFGSOptions(OptimizerOptionsBFGS options): Constructor with custom options"))
        .def(init<const sm::PropertyTree&>("OptimizerRprop(PropertyTree propertyTree): Constructor from sm::PropertyTree"))
        ;
    implicitly_convertible< boost::shared_ptr<OptimizerBFGS>, boost::shared_ptr<const OptimizerBFGS> >();

}


/*
 * Collision sphere file
 *
 * This file is part of the "SoftPixel Engine" (Copyright (c) 2008 by Lukas Hermanns)
 * See "SoftPixelEngine.hpp" for license information.
 */

#include "SceneGraph/Collision/spCollisionSphere.hpp"
#include "SceneGraph/Collision/spCollisionCapsule.hpp"
#include "SceneGraph/Collision/spCollisionCylinder.hpp"
#include "SceneGraph/Collision/spCollisionCone.hpp"
#include "SceneGraph/Collision/spCollisionBox.hpp"
#include "SceneGraph/Collision/spCollisionPlane.hpp"
#include "SceneGraph/Collision/spCollisionMesh.hpp"

#include <boost/foreach.hpp>

namespace sp
{
namespace scene
{

CollisionSphere::CollisionSphere(
    CollisionMaterial* Material, SceneNode* Node, f32 Radius) :
    CollisionNode   (Material, Node, COLLISION_SPHERE   ),
    Radius_         (Radius                             )
{
    if (Radius_ < math::ROUNDING_ERROR)
        throw "Collision spheres must have a radius larger than 0.0";
}
CollisionSphere::~CollisionSphere()
{
}

s32 CollisionSphere::getSupportFlags() const
{
    return COLLISIONSUPPORT_ALL;
}

f32 CollisionSphere::getMaxMovement() const
{
    return getRadius() * 0.8f;
}

bool CollisionSphere::checkIntersection(const dim::line3df &Line, SIntersectionContact &Contact) const
{
    const dim::vector3df SpherePos(getPosition());

    /* Make an intersection test with the line and this sphere */
    if (math::CollisionLibrary::checkLineSphereIntersection(Line, SpherePos, getRadius(), Contact.Point))
    {
        Contact.Normal = (Contact.Point - SpherePos).normalize();
        Contact.Object = this;
        return true;
    }

    return false;
}

bool CollisionSphere::checkIntersection(const dim::line3df &Line, bool ExcludeCorners) const
{
    /* Make an intersection test with the line and this sphere */
    dim::vector3df Point;

    if (math::CollisionLibrary::checkLineSphereIntersection(Line, getPosition(), getRadius(), Point))
        return ExcludeCorners ? checkCornerExlusion(Line, Point) : true;

    return false;
}

/*
 * ======= Private: =======
 */

bool CollisionSphere::checkCollisionToSphere(const CollisionSphere* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Store transformation */
    const dim::vector3df SpherePos(getPosition());
    const dim::vector3df OtherPos(Rival->getPosition());

    /* Check if this object and the other collide with each other */
    return checkPointDistanceDouble(
        SpherePos, OtherPos, getRadius() + Rival->getRadius(), Rival->getRadius(), Contact
    );
}

bool CollisionSphere::checkCollisionToCapsule(const CollisionCapsule* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Store transformation */
    const dim::vector3df SpherePos(getPosition());
    const dim::line3df RivalLine(Rival->getLine());

    /* Get the closest point from this sphere to the capsule */
    const dim::vector3df ClosestPoint = RivalLine.getClosestPoint(SpherePos);

    /* Check if this object and the other collide with each other */
    return checkPointDistanceDouble(
        SpherePos, ClosestPoint, getRadius() + Rival->getRadius(), Rival->getRadius(), Contact
    );
}

//!TODO! -> incomplete
bool CollisionSphere::checkCollisionToCylinder(const CollisionCylinder* Rival, SCollisionContact &Contact) const
{
    #if 0

    if (!Rival)
        return false;

    /* Store transformation */
    const dim::vector3df SpherePos(getPosition());
    const dim::line3df RivalLine(Rival->getLine());

    /* Get the closest point from this sphere to the cylinder */
    dim::vector3df ClosestPoint;
    const dim::ELinePointRelations Relation = RivalLine.getClosestPoint(SpherePos, ClosestPoint);

    //...

    #endif

    return false;
}

//!TODO! -> incomplete
bool CollisionSphere::checkCollisionToCone(const CollisionCone* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Store transformation */
    const dim::vector3df SpherePos(getPosition());
    const dim::line3df RivalLine(Rival->getLine());

    /* Get the closest point from this sphere to the cone */
    dim::vector3df ClosestPoint;
    const dim::ELinePointRelations Relation = RivalLine.getClosestPoint(SpherePos, ClosestPoint);

    if (Relation == dim::LINE_RELATION_END)
    {
        /* Check if this object and the other collide with each other */
        if (checkPointDistanceSingle(SpherePos, ClosestPoint, getRadius(), Contact))
            return true;
    }
    else if (Relation == dim::LINE_RELATION_BETWEEN)
    {
        /* Get the closest point from this sphere to the angular line (cone's outer cover) */
        const dim::line3df AngularLine(
            RivalLine.Start + (SpherePos - ClosestPoint).setLength(Rival->getRadius()),
            RivalLine.End
        );

        ClosestPoint = AngularLine.getClosestPoint(SpherePos);

        /* Check if this object and the other collid with each other */
        if (checkPointDistanceSingle(SpherePos, ClosestPoint, getRadius(), Contact))
            return true;
    }

    /* Check if this object collides with the cone's bottom */

    //...

    return false;
}

bool CollisionSphere::checkCollisionToBox(const CollisionBox* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Store transformation */
    const dim::matrix4f Mat(Rival->getTransformation().getPositionRotationMatrix());
    const dim::matrix4f InvMat(Mat.getInverse());

    const dim::aabbox3df Box(Rival->getBox().getScaled(Rival->getScale()));
    const dim::vector3df SpherePos(getPosition());
    const dim::vector3df SphereInvPos(InvMat * SpherePos);

    /* Get the closest point from this box and the box */
    if (Box.isPointInside(SphereInvPos))
        return false;

    const dim::vector3df Point = math::CollisionLibrary::getClosestPoint(Box, SphereInvPos);

    /* Check if this object and the other collide with each other */
    if (math::getDistanceSq(Point, SphereInvPos) < math::pow2(getRadius()))
    {
        Contact.Point = Mat * Point;

        /* Compute normal and impact together to avoid calling square-root twice */
        Contact.Normal = SpherePos - Contact.Point;
        Contact.Impact = Contact.Normal.getLength();

        if (Contact.Impact < math::ROUNDING_ERROR)
            return false;

        Contact.Normal *= (1.0f / Contact.Impact);
        Contact.Impact = getRadius() - Contact.Impact;

        return true;
    }

    return false;
}

bool CollisionSphere::checkCollisionToPlane(const CollisionPlane* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Store transformation */
    const dim::vector3df SpherePos(getPosition());
    const dim::plane3df RivalPlane(
        Rival->getTransformation().getPositionRotationMatrix() * Rival->getPlane()
    );

    /* Check if this object and the other collide with each other */
    const f32 Distance = RivalPlane.getPointDistance(SpherePos);

    if (Distance > 0.0f && Distance < getRadius())
    {
        /* Compute point, normal and impact separately */
        Contact.Point   = RivalPlane.getClosestPoint(SpherePos);
        Contact.Normal  = RivalPlane.Normal;
        Contact.Impact  = getRadius() - Distance;
        return true;
    }

    return false;
}

bool CollisionSphere::checkCollisionToMesh(const CollisionMesh* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Check if rival mesh has a tree-hierarchy */
    KDTreeNode* RootTreeNode = Rival->getRootTreeNode();

    if (!RootTreeNode)
        return false;

    /* Store transformation */
    const dim::vector3df SpherePos(getPosition());
    const video::EFaceTypes CollFace(Rival->getCollFace());

    const dim::matrix4f RivalMat(Rival->getTransformation());
    const dim::matrix4f RivalMatInv(RivalMat.getInverse());

    const dim::vector3df SpherePosInv(RivalMatInv * SpherePos);

    f32 DistanceSq = math::pow2(getRadius());
    SCollisionFace* ClosestFace = 0;
    dim::vector3df ClosestPoint;

    #ifndef _DEB_NEW_KDTREE_
    std::map<SCollisionFace*, bool> FaceMap;
    #endif

    /* Get tree node list */
    std::list<const TreeNode*> TreeNodeList;

    RootTreeNode->findLeafList(
        TreeNodeList, SpherePosInv, (RivalMatInv.getScale() * getRadius()).getMax()
    );

    /* Check collision with triangles of each tree-node */
    foreach (const TreeNode* Node, TreeNodeList)
    {
        /* Get tree node data */
        CollisionMesh::TreeNodeDataType* TreeNodeData = static_cast<CollisionMesh::TreeNodeDataType*>(Node->getUserData());

        if (!TreeNodeData)
            continue;

        /* Check collision with each triangle */
        #ifndef _DEB_NEW_KDTREE_
        foreach (SCollisionFace* Face, *TreeNodeData)
        #else
        foreach (SCollisionFace &NodeFace, *TreeNodeData)
        #endif
        {
            #ifndef _DEB_NEW_KDTREE_
            /* Check for unique usage */
            if (FaceMap.find(Face) != FaceMap.end())
                continue;

            FaceMap[Face] = true;
            #else
            SCollisionFace* Face = &NodeFace;
            #endif

            /* Check for face-culling */
            if (Face->isBackFaceCulling(CollFace, SpherePosInv))
                continue;

            /* Make sphere-triangle collision test */
            const dim::vector3df CurClosestPoint(
                math::CollisionLibrary::getClosestPoint(RivalMat * Face->Triangle, SpherePos)
            );

            /* Check if this is a potentially new closest face */
            const f32 CurDistSq = math::getDistanceSq(CurClosestPoint, SpherePos);

            if (CurDistSq < DistanceSq)
            {
                /* Store link to new closest face */
                DistanceSq      = CurDistSq;
                ClosestPoint    = CurClosestPoint;
                ClosestFace     = Face;
            }
        }
    }

    /* Check if a collision has been detected */
    if (ClosestFace)
    {
        /* Compute point, normal and impact separately */
        Contact.Normal  = (RivalMat * ClosestFace->Triangle).getNormal();
        Contact.Point   = ClosestPoint;
        Contact.Impact  = getRadius() - (SpherePos - Contact.Point).getLength();
        Contact.Face    = ClosestFace;
        return true;
    }

    return false;
}

bool CollisionSphere::checkAnyCollisionToMesh(const CollisionMesh* Rival) const
{
    if (!Rival)
        return false;

    /* Check if rival mesh has a tree-hierarchy */
    KDTreeNode* RootTreeNode = Rival->getRootTreeNode();

    if (!RootTreeNode)
        return false;

    /* Store transformation */
    const dim::vector3df SpherePos(getPosition());
    const video::EFaceTypes CollFace(Rival->getCollFace());

    const dim::matrix4f RivalMat(Rival->getTransformation());
    const dim::matrix4f RivalMatInv(RivalMat.getInverse());

    const dim::vector3df SpherePosInv(RivalMatInv * SpherePos);

    const f32 RadiusSq = math::pow2(getRadius());

    #ifndef _DEB_NEW_KDTREE_
    std::map<SCollisionFace*, bool> FaceMap;
    #endif

    /* Get tree node list */
    std::list<const TreeNode*> TreeNodeList;

    RootTreeNode->findLeafList(
        TreeNodeList, SpherePosInv, (RivalMatInv.getScale() * getRadius()).getMax()
    );

    /* Check collision with triangles of each tree-node */
    foreach (const TreeNode* Node, TreeNodeList)
    {
        /* Get tree node data */
        CollisionMesh::TreeNodeDataType* TreeNodeData = static_cast<CollisionMesh::TreeNodeDataType*>(Node->getUserData());

        if (!TreeNodeData)
            continue;

        /* Check collision with each triangle */
        #ifndef _DEB_NEW_KDTREE_
        foreach (SCollisionFace* Face, *TreeNodeData)
        #else
        foreach (SCollisionFace &NodeFace, *TreeNodeData)
        #endif
        {
            #ifndef _DEB_NEW_KDTREE_
            /* Check for unique usage */
            if (FaceMap.find(Face) != FaceMap.end())
                continue;

            FaceMap[Face] = true;
            #else
            SCollisionFace* Face = &NodeFace;
            #endif

            /* Check for face-culling */
            if (Face->isBackFaceCulling(CollFace, SpherePosInv))
                continue;

            /* Make sphere-triangle collision test */
            const dim::vector3df CurClosestPoint(
                math::CollisionLibrary::getClosestPoint(Face->Triangle, SpherePosInv)
            );

            /* Check if the first collision has been detected and return on succeed */
            if (math::getDistanceSq(CurClosestPoint, SpherePosInv) < RadiusSq)
                return true;
        }
    }

    return false;
}

void CollisionSphere::performCollisionResolvingToSphere(const CollisionSphere* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToSphere(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionSphere::performCollisionResolvingToCapsule(const CollisionCapsule* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToCapsule(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionSphere::performCollisionResolvingToCylinder(const CollisionCylinder* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToCylinder(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionSphere::performCollisionResolvingToCone(const CollisionCone* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToCone(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionSphere::performCollisionResolvingToBox(const CollisionBox* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToBox(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionSphere::performCollisionResolvingToPlane(const CollisionPlane* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToPlane(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionSphere::performCollisionResolvingToMesh(const CollisionMesh* Rival)
{
    if (!Rival)
        return;

    /* Check if rival mesh has a tree-hierarchy */
    KDTreeNode* RootTreeNode = Rival->getRootTreeNode();

    if (!RootTreeNode)
        return;

    /* Store transformation */
    const video::EFaceTypes CollFace(Rival->getCollFace());

    const dim::matrix4f RivalMat(Rival->getTransformation());
    const dim::matrix4f RivalMatInv(RivalMat.getInverse());

    dim::vector3df SpherePos(getPosition());
    dim::vector3df SpherePosInv(RivalMatInv * SpherePos);

    dim::vector3df ClosestPoint;
    const f32 RadiusSq = math::pow2(getRadius());

    #ifndef _DEB_NEW_KDTREE_
    std::map<SCollisionFace*, bool> FaceMap, EdgeFaceMap;
    #endif

    /* Get tree node list */
    std::list<const TreeNode*> TreeNodeList;

    RootTreeNode->findLeafList(
        TreeNodeList, SpherePosInv, (RivalMatInv.getScale() * getRadius()).getMax()
    );

    /* Check collision with triangle faces of each tree-node */
    foreach (const TreeNode* Node, TreeNodeList)
    {
        /* Get tree node data */
        CollisionMesh::TreeNodeDataType* TreeNodeData = static_cast<CollisionMesh::TreeNodeDataType*>(Node->getUserData());

        if (!TreeNodeData)
            continue;

        /* Check collision with each triangle face */
        #ifndef _DEB_NEW_KDTREE_
        foreach (SCollisionFace* Face, *TreeNodeData)
        #else
        foreach (SCollisionFace &NodeFace, *TreeNodeData)
        #endif
        {
            #ifndef _DEB_NEW_KDTREE_
            /* Check for unique usage */
            if (FaceMap.find(Face) != FaceMap.end())
                continue;

            FaceMap[Face] = true;
            #else
            SCollisionFace* Face = &NodeFace;
            #endif

            /* Check for face-culling */
            if (Face->isBackFaceCulling(CollFace, SpherePosInv))
                continue;

            /* Make sphere-triangle collision test */
            const dim::triangle3df Triangle(RivalMat * Face->Triangle);

            if (!math::CollisionLibrary::getClosestPointStraight(Triangle, SpherePos, ClosestPoint))
                continue;

            /* Check if this is a potentially new closest face */
            if (math::getDistanceSq(ClosestPoint, SpherePos) < RadiusSq)
            {
                /* Perform detected collision contact */
                SCollisionContact Contact;
                {
                    Contact.Point       = ClosestPoint;
                    Contact.Normal      = Triangle.getNormal();
                    Contact.Impact      = getRadius() - (SpherePos - Contact.Point).getLength();
                    Contact.Triangle    = Triangle;
                    Contact.Face        = Face;
                }
                performDetectedContact(Rival, Contact);

                if (getFlags() & COLLISIONFLAG_RESOLVE)
                {
                    /* Update sphere position */
                    SpherePos       = getPosition();
                    SpherePosInv    = RivalMatInv * SpherePos;
                }
            }
        }
    }

    /* Check collision with triangle edges of each tree-node */
    foreach (const TreeNode* Node, TreeNodeList)
    {
        /* Get tree node data */
        CollisionMesh::TreeNodeDataType* TreeNodeData = static_cast<CollisionMesh::TreeNodeDataType*>(Node->getUserData());

        if (!TreeNodeData)
            continue;

        /* Check collision with each triangle edge */
        #ifndef _DEB_NEW_KDTREE_
        foreach (SCollisionFace* Face, *TreeNodeData)
        #else
        foreach (SCollisionFace &NodeFace, *TreeNodeData)
        #endif
        {
            #ifndef _DEB_NEW_KDTREE_
            /* Check for unique usage */
            if (EdgeFaceMap.find(Face) != EdgeFaceMap.end())
                continue;

            EdgeFaceMap[Face] = true;
            #else
            SCollisionFace* Face = &NodeFace;
            #endif

            /* Check for face-culling */
            if (Face->isBackFaceCulling(CollFace, SpherePosInv))
                continue;

            /* Make sphere-triangle collision test */
            const dim::triangle3df Triangle(RivalMat * Face->Triangle);

            ClosestPoint = math::CollisionLibrary::getClosestPoint(Triangle, SpherePos);

            /* Check if this is a potentially new closest face */
            if (math::getDistanceSq(ClosestPoint, SpherePos) < RadiusSq)
            {
                /* Perform detected collision contact */
                SCollisionContact Contact;
                {
                    Contact.Point = ClosestPoint;

                    Contact.Normal = SpherePos;
                    Contact.Normal -= ClosestPoint;
                    Contact.Normal.normalize();

                    Contact.Impact      = getRadius() - (SpherePos - Contact.Point).getLength();
                    Contact.Triangle    = Triangle;
                    Contact.Face        = Face;
                }
                performDetectedContact(Rival, Contact);

                if (getFlags() & COLLISIONFLAG_RESOLVE)
                {
                    /* Update sphere position */
                    SpherePos       = getPosition();
                    SpherePosInv    = RivalMatInv * SpherePos;
                }
            }
        }
    }
}

bool CollisionSphere::checkPointDistanceSingle(
    const dim::vector3df &SpherePos, const dim::vector3df &ClosestPoint,
    f32 MaxRadius, SCollisionContact &Contact) const
{
    /* Check if this object and the other collide with each other */
    if (math::getDistanceSq(SpherePos, ClosestPoint) < math::pow2(MaxRadius))
    {
        /* Compute normal and impact together to avoid calling square-root twice */
        Contact.Normal = SpherePos;
        Contact.Normal -= ClosestPoint;

        Contact.Impact = Contact.Normal.getLength();

        if (Contact.Impact < math::ROUNDING_ERROR)
            return false;

        Contact.Normal *= (1.0f / Contact.Impact);
        Contact.Impact = getRadius() - Contact.Impact;

        Contact.Point = ClosestPoint;

        return true;
    }
    return false;
}

bool CollisionSphere::checkPointDistanceDouble(
    const dim::vector3df &SpherePos, const dim::vector3df &ClosestPoint,
    f32 MaxRadius, f32 RivalRadius, SCollisionContact &Contact) const
{
    /* Check if this object and the other collide with each other */
    if (math::getDistanceSq(SpherePos, ClosestPoint) < math::pow2(MaxRadius))
    {
        /* Compute normal and impact together to avoid calling square-root twice */
        Contact.Normal = SpherePos;
        Contact.Normal -= ClosestPoint;

        Contact.Impact = Contact.Normal.getLength();

        if (Contact.Impact < math::ROUNDING_ERROR)
            return false;

        Contact.Normal *= (1.0f / Contact.Impact);
        Contact.Impact = MaxRadius - Contact.Impact;

        Contact.Point = Contact.Normal;
        Contact.Point *= RivalRadius;
        Contact.Point += ClosestPoint;

        return true;
    }
    return false;
}

} // /namespace scene

} // /namespace sp

// ================================================================================