#include <ros/ros.h>
#include "ros/package.h"

#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>

#include <ssd_ros_msgs/BoundingBox.h>
#include <ssd_ros_msgs/BoundingBoxArray.h>
#include <ssd_ros_msgs/SSD.h>
#include <ssd_ros_msgs/Cluster.h>
#include <ssd_ros_msgs/ClusterArray.h>

#include <Eigen/Core>

using namespace std;
using namespace Eigen;

typedef pcl::PointXYZRGB PointA;
//typedef pcl::PointXYZINormal PointA;  // 法線情報・反射強度を利用する場合はNormal型に変更
typedef pcl::PointCloud<PointA> CloudA;
typedef pcl::PointCloud<PointA>::Ptr CloudAPtr;

ros::Publisher pub_cluster_centroid;
ros::Publisher pub_cluster_points;
ros::Publisher pub_cluster_centroid_max;
ros::Publisher pub_cluster_points_max;
ros::Publisher pub_cluster_array;

string target_frame = "/zed_left_camera";
string source_frame = "/velodyne";

bool pc_flag = false;

void getClusterInfo(CloudA pt, PointA& cluster)
{
    Vector3f centroid;
	centroid[0]=pt.points[0].x;
	centroid[1]=pt.points[0].y;
	centroid[2]=pt.points[0].z;

	Vector3f min_p;
	min_p[0]=pt.points[0].x;
	min_p[1]=pt.points[0].y;
    min_p[2]=pt.points[0].z;

	Vector3f max_p;
	max_p[0]=pt.points[0].x;
	max_p[1]=pt.points[0].y;
	max_p[2]=pt.points[0].z;

	for(size_t i=1;i<pt.points.size();i++){
        centroid[0]+=pt.points[i].x;
        centroid[1]+=pt.points[i].y;
        centroid[2]+=pt.points[i].z;
		if (pt.points[i].x<min_p[0]) min_p[0]=pt.points[i].x;
		if (pt.points[i].y<min_p[1]) min_p[1]=pt.points[i].y;
        if (pt.points[i].z<min_p[2]) min_p[2]=pt.points[i].z;

		if (pt.points[i].x>max_p[0]) max_p[0]=pt.points[i].x;
		if (pt.points[i].y>max_p[1]) max_p[1]=pt.points[i].y;
		if (pt.points[i].z>max_p[2]) max_p[2]=pt.points[i].z;
    }

	cluster.x=centroid[0]/(float)pt.points.size();
    cluster.y=centroid[1]/(float)pt.points.size();
    cluster.z=centroid[2]/(float)pt.points.size();
	// cluster.normal_x=max_p[0]-min_p[0];
	// cluster.normal_y=max_p[1]-min_p[1];
	// cluster.normal_z=max_p[2];
}

void cpu_clustering(CloudAPtr pcl_in, CloudA& cluster_centroid, CloudA& cluster_pt, CloudA& max_centroid, CloudA& max_pt)
{
    //クラスタリング
    pcl::search::KdTree<PointA>::Ptr tree (new pcl::search::KdTree<PointA>);
    tree->setInputCloud (pcl_in);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<PointA> ec;
    ec.setClusterTolerance(0.10);
    ec.setMinClusterSize(100);
    ec.setMaxClusterSize(4000);
    ec.setSearchMethod (tree);
    ec.setInputCloud (pcl_in);
    ec.extract(cluster_indices);

    // size_t max_cluster = 0;
    // size_t max_cluster_size = 0;
    size_t num = 0;

    cluster_centroid.resize(cluster_indices.size());

    // cout<<"size"<<cluster_indices.size()<<endl;
    for(size_t iii=0;iii<cluster_indices.size();iii++){
        CloudAPtr cloud_cluster (new CloudA);
        cloud_cluster->points.resize(cluster_indices[iii].indices.size());
        cluster_pt.points.resize(cluster_indices[iii].indices.size()+num);
        for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
            int p_num=cluster_indices[iii].indices[jjj];
            cloud_cluster->points[jjj]=pcl_in->points[p_num];
            cluster_pt.points[num+jjj]=pcl_in->points[p_num];
        }
        getClusterInfo(*cloud_cluster, cluster_centroid[iii]);

        if(iii==0){
             max_pt.points.resize(cluster_indices[iii].indices.size()+num);
             for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
                 int p_num=cluster_indices[iii].indices[jjj];
                 max_pt.points[num+jjj]=pcl_in->points[p_num];
             }
             max_centroid.resize(1);
             getClusterInfo(*cloud_cluster, max_centroid[iii]);
        }
        num=cluster_pt.points.size();//save previous size
    }
}

void sampling_cpu_clustering(CloudAPtr pcl_in, CloudA& cluster_centroid, CloudA& cluster_pt, CloudA& max_centroid, CloudA& max_pt)
{
    //Downsample//
    pcl::VoxelGrid<PointA> vg;
	CloudAPtr ds_cloud (new CloudA);
	vg.setInputCloud (pcl_in);
	vg.setLeafSize (0.01f, 0.01f, 0.01f);
	vg.filter (*ds_cloud);
    //downsampled point's z =>0
    vector<float> tmp_z;
    tmp_z.resize(ds_cloud->points.size());
	for(int i=0;i<(int)ds_cloud->points.size();i++){
        tmp_z[i]=ds_cloud->points[i].z;
		ds_cloud->points[i].z  = 0.0;
    }
    //Clustering//
    pcl::search::KdTree<PointA>::Ptr tree (new pcl::search::KdTree<PointA>);
    tree->setInputCloud (ds_cloud);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<PointA> ec;
    ec.setClusterTolerance (0.05); // 15cm
    ec.setMinClusterSize (10);
    ec.setMaxClusterSize (4000);
    ec.setSearchMethod (tree);
    ec.setInputCloud(ds_cloud);
    ec.extract (cluster_indices);
    //reset z value
	for(int i=0;i<(int)ds_cloud->points.size();i++)
        ds_cloud->points[i].z=tmp_z[i];
    //get cluster information//
    size_t num=0;
    cluster_centroid.resize(cluster_indices.size());
    for(size_t iii=0;iii<cluster_indices.size();iii++){
        //cluster cenrtroid
        CloudAPtr cloud_cluster (new CloudA);
        cloud_cluster->points.resize(cluster_indices[iii].indices.size());
        //cluster points
        cluster_pt.points.resize(cluster_indices[iii].indices.size()+num);
        for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
            int p_num=cluster_indices[iii].indices[jjj];
            cloud_cluster->points[jjj]=ds_cloud->points[p_num];
            cluster_pt.points[num+jjj]=ds_cloud->points[p_num];
        }
        getClusterInfo(*cloud_cluster, cluster_centroid[iii]);
        if(iii==0){
            max_pt.points.resize(cluster_indices[iii].indices.size()+num);
            for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
                int p_num=cluster_indices[iii].indices[jjj];
                max_pt.points[num+jjj]=ds_cloud->points[p_num];
            }
            max_centroid.resize(1);
            getClusterInfo(*cloud_cluster, max_centroid[iii]);
        }
        num=cluster_pt.points.size();//save previous size
    }
}

void cluster(const ssd_ros_msgs::SSDConstPtr& pc_msg)
{
    CloudAPtr pcl_cluster_points;
    CloudA pcl_cluster_centroid;
    CloudA pcl_max_centroid;
    CloudA pcl_max_points;

    ssd_ros_msgs::ClusterArray ssd_clusters;

    int grobal_count = 0;

    for(size_t i=0;i<pc_msg->ssd_array.size();i++)
    {
        int local_count = 0;
        const sensor_msgs::PointCloud2 &pc2_people = pc_msg->ssd_array[i];
        // printf("Num:%d size:%d\n", i+1, pc2_people.width);

        CloudAPtr pcl_people (new CloudA);
        CloudA cluster_points;
        CloudA cluster_centroid;
        CloudA max_centroid;
        CloudA max_points;

        // ssd_ros_msgs::Cluster ssd_cluster;

        pcl::fromROSMsg(pc2_people, *pcl_people);

        //cpu_clustering(pcl_people, cluster_centroid, cluster_points, max_centroid, max_points);
        cout<<"cpu_cluster"<<endl;
		sampling_cpu_clustering(pcl_people, cluster_centroid, cluster_points, max_centroid, max_points);
        cout<<"finish"<<endl;

		// for(size_t j=0;j<cluster_centroid.points.size();j++){
        //     PointA p = cluster_centroid.points[j];
        //     pcl_cluster_centroid.push_back(p);
        //     local_count++;
        //     grobal_count++;
        // }
        // for(size_t k=0;k<cluster_points.points.size();k++){
        //     PointA q = cluster_points.points[k];
        //     pcl_cluster_points->push_back(q);
        // }
		// if(0<pcl_cluster_points->points.size()){
		// 	boundingbox_array.push_back<CloudAPtr&>(pcl_cluster_points);
		// }
		cout<<"max"<<endl;
        for(size_t l=0;l<max_centroid.points.size();l++){
            PointA p = max_centroid.points[l];
            pcl_max_centroid.push_back(p);
        }
        for(size_t m=0;m<max_points.points.size();m++){
            PointA q = max_points.points[m];
            pcl_max_points.push_back(q);
        }

		cout<<"nest"<<endl;

        if(0<max_points.points.size()){
            ssd_ros_msgs::Cluster ssd_cluster;
            CloudA ssd_centroid;
            CloudA ssd_points;

            for(size_t l=0;l<max_centroid.points.size();l++){
                PointA p = max_centroid.points[l];
                ssd_centroid.push_back(p);
            }
            for(size_t m=0;m<max_points.points.size();m++){
                PointA q = max_points.points[m];
                ssd_points.push_back(q);
            }
            toROSMsg(ssd_centroid, ssd_cluster.centroid);
            toROSMsg(ssd_points, ssd_cluster.points);
            ssd_cluster.centroid.header.frame_id = target_frame;
            ssd_cluster.centroid.header.stamp = ros::Time::now();
            ssd_cluster.points.header.frame_id = target_frame;
            ssd_cluster.points.header.stamp = ros::Time::now();
            ssd_clusters.clusters.push_back(ssd_cluster);
        }

        // if(0<max_points.points.size()){
        //     toROSMsg(pcl_max_centroid, ssd_cluster.centroid);
        //     toROSMsg(pcl_max_points, ssd_cluster.points);
        //     ssd_cluster.centroid.header.frame_id = target_frame;
        //     ssd_cluster.centroid.header.stamp = ros::Time::now();
        //     ssd_cluster.points.header.frame_id = target_frame;
        //     ssd_cluster.points.header.stamp = ros::Time::now();
        //     ssd_clusters.clusters.push_back(ssd_cluster);
        // }

        printf("Array Num:%d Size:%d Class:%d\n",(int)i+1, (int)pc2_people.width, local_count);
    }

    printf("Total Class:%d\n", grobal_count);
    printf("Array size:%d\n", (int)ssd_clusters.clusters.size());

    // sensor_msgs::PointCloud2 cluster_centroid_ros;
    // toROSMsg(pcl_cluster_centroid, cluster_centroid_ros);
    // cluster_centroid_ros.header.frame_id=target_frame;
    // cluster_centroid_ros.header.stamp=ros::Time::now();
    // pub_cluster_centroid.publish(cluster_centroid_ros);

    // sensor_msgs::PointCloud2 cluster_points_ros;
    // toROSMsg(*pcl_cluster_points, cluster_points_ros);
    // cluster_points_ros.header.frame_id=target_frame;
    // cluster_points_ros.header.stamp=ros::Time::now();
    // pub_cluster_points.publish(cluster_points_ros);

    // boundingbox_array.publish();

    sensor_msgs::PointCloud2 max_centroid_ros;
    toROSMsg(pcl_max_centroid, max_centroid_ros);
    max_centroid_ros.header.frame_id = target_frame;
    pub_cluster_centroid_max.publish(max_centroid_ros);

    sensor_msgs::PointCloud2 max_points_ros;
    toROSMsg(pcl_max_points,max_points_ros);
    max_points_ros.header.frame_id = target_frame;
    pub_cluster_points_max.publish(max_points_ros);

    pub_cluster_array.publish(ssd_clusters);

}

ssd_ros_msgs::SSDConstPtr pc_;
void pcCallback(const ssd_ros_msgs::SSDConstPtr& msg)
{
    // printf("Human Count : %d\n", msg->ssd_array.size());
    pc_ = msg;
    pc_flag = true;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "ssd_cluster");
    ros::NodeHandle n;

    ros::Subscriber pc_sub = n.subscribe("/SSD/people",10,pcCallback);

    pub_cluster_centroid=n.advertise<sensor_msgs::PointCloud2>("/cluster/centroid",1);
    pub_cluster_points=n.advertise<sensor_msgs::PointCloud2>("/cluster/points",1);
    pub_cluster_centroid_max=n.advertise<sensor_msgs::PointCloud2>("/cluster/centroid/max",1);
    pub_cluster_points_max=n.advertise<sensor_msgs::PointCloud2>("/cluster/points/max",1);
    pub_cluster_array = n.advertise<ssd_ros_msgs::ClusterArray>("/cluster/array/ssd", 1);

    ros::Rate rate(20);

    while(ros::ok())
    {
        if(pc_flag) cluster(pc_);
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}


// Copyright 2020 Poofee (https://github.com/Poofee)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------
/*****************************************************************************
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *****************************************************************************
 *                                                                           *
 *  Authors: Poofee                                                          *
 *  Email:   poofee@qq.com                                                   *
 *  Address:                                                                 *
 *  Original Date: 2020-07-15                                                *
 *                                                                           *
 *****************************************************************************/
#include "relay1250.h"

#include <stdio.h>
#include <stdlib.h>
#include <cmath>
#if !defined(ARMA_32BIT_WORD)
#define ARMA_32BIT_WORD
#endif
#include <armadillo>
#include <vector>
#include <ctime>
#include <omp.h>

#include "SuperLU_MT.h"
#include "qcustomplot.h"
#include "slu_mt_ddefs.h"

using namespace std;
using namespace arma;

#define PI 3.14159265358979323846
#define r 2

const double miu0 = PI*4e-7;

Relay1250::Relay1250()
{

}

Relay1250::~Relay1250()
{

}

/*!
 \brief 参数初始化

*/
void Relay1250::init()
{
    /** 文件名，需不带扩展名 **/
    sprintf(fileName,"%s","JRS1250/JRS1250bgm");
    /** 初始化时间步长 **/
    current_step = 0;
    min_time = 0;
    max_time = 4e-3;
    for(double t = 1e-5; t < 3e-3; t+=1e-5){
        timesteps.emplace_back(1e-5);
    }
    for(double t = 3e-3; t <= 3e-3; t+=5e-5){
        timesteps.emplace_back(5e-5);
    }

    /** 初始化变量 **/
    current_step = 0;
    Ddisplacements.resize(timesteps.size()+1);
    Ddisplacements.at(current_step) = 0;
    displacements.resize(timesteps.size()+1);
    displacements.at(current_step) = 0;
    velocities.resize(timesteps.size()+1);
    velocities.at(current_step) = 0;
    accelerations.resize(timesteps.size()+1);
    accelerations.at(current_step) = 0;
    PhiCoil.resize(timesteps.size()+1);
    PhiCoil.at(current_step) = 0;
    ICoil.resize(timesteps.size()+1);
    ICoil.at(current_step) = 0;
    magforces.resize(timesteps.size()+1);
    magforces.at(current_step) = 0;
    UCoil.resize(timesteps.size()+1);
    UCoil.at(current_step) = 420;

    MAX_NONLINEARSTEPS = 20;
    /** 设置材料参数 **/
    CMaterial* air_material = new CMaterial;
    materialList.emplace_back(air_material);

    CMaterial* pm_material = new CMaterial;
    materialList.emplace_back(pm_material);
    pm_material->H_c = 1e6;
    pm_material->miu = 1.12;

    CMaterial* dt4e_material = new CMaterial;
    materialList.emplace_back(dt4e_material);
    dt4e_material->sigma = 1/(2e-7);
    dt4e_material->BHpoints = 20;
    double* Hdata = (double*)malloc(20*sizeof(double));
    double* Bdata = (double*)malloc(20*sizeof(double));
    /** 1250的BH曲线 **/

    CMaterial* coil_material = new CMaterial;
    materialList.emplace_back(coil_material);
    coil_material->tau = 60/(3e-3 * 17e-3);

    /** 设置映射关系 **/
    materialMap[1] = dt4e_material;/** 衔铁 **/
    materialMap[2] = coil_material;/** 线圈 **/
    materialMap[3] = dt4e_material;/** 铁芯 **/
    materialMap[4] = dt4e_material;/** 铁芯 **/
    materialMap[5] = dt4e_material;/** 铁芯 **/
    materialMap[6] = pm_material;/** 永磁 **/
    materialMap[7] = dt4e_material;/** 铁芯（导磁环） **/
    materialMap[8] = dt4e_material;/** 铁芯 **/
    materialMap[9] = air_material;/** 外部空气 **/
    materialMap[10] = air_material;/** 可压缩空气 **/
    /** 设置形变区域 **/
    tag_xiantie = 1;
    tag_air = 10;

    Precision = 1e-6;
}

/*!
 \brief 计算区域index的电磁力，如果没有指定的话，就按衔铁算。

 \param index
*/
void Relay1250::calcMagForce(int index)
{

}

/*!
 \brief 计算三角形单元的基本项

 \param index
*/
void Relay1250::makeTriangle(int index)
{
    int k,m,n;
    double p0,p1,p2,q0,q1,q2,area;
    k = pmeshele[index].n[0];
    m = pmeshele[index].n[1];
    n = pmeshele[index].n[2];

    p0 = pmeshnode[m].y - pmeshnode[n].y;
    pmeshele[index].P[0] = p0;
    p1 = pmeshnode[n].y - pmeshnode[k].y;
    pmeshele[index].P[1] = p1;
    p2 = pmeshnode[k].y - pmeshnode[m].y;
    pmeshele[index].P[2] = p2;

    q0 = pmeshnode[n].x - pmeshnode[m].x;
    pmeshele[index].Q[0] = q0;
    q1 = pmeshnode[k].x - pmeshnode[n].x;
    pmeshele[index].Q[1] = q1;
    q2 = pmeshnode[m].x - pmeshnode[k].x;
    pmeshele[index].Q[2] = q2;

    area = 0.5*abs(p1 * q2 - q1 * p2);
    pmeshele[index].AREA = area;
    pmeshele[index].rc = (pmeshnode[k].x +
                          pmeshnode[m].x +
                          pmeshnode[n].x) / 3;
    pmeshele[index].zc = (pmeshnode[k].y +
                          pmeshnode[m].y +
                          pmeshnode[n].y) / 3;

    int flag = 0;
    for (int f = 0; f < 3; f++) {
        if (pmeshnode[pmeshele[index].n[f]].x < 1e-7) {
            flag++;
        }
    }
    /** 计算三角形重心半径 **/
    if (flag == 2) {
        pmeshele[index].ydot = pmeshele[index].rc;
    } else {
        pmeshele[index].ydot  = 1 / (pmeshnode[k].x + pmeshnode[m].x);
        pmeshele[index].ydot += 1 / (pmeshnode[k].x + pmeshnode[n].x);
        pmeshele[index].ydot += 1 / (pmeshnode[m].x + pmeshnode[n].x);
        pmeshele[index].ydot = 1.5 / pmeshele[index].ydot;
    }

//    double Y11,Y12,Y13,Y22,Y23,Y33;
    pmeshele[index].Y11 = q0 * q0 + p0 * p0;
    pmeshele[index].Y12 = q0 * q1 + p0 * p1;
    pmeshele[index].Y13 = q0 * q2 + p0 * p2;
    pmeshele[index].Y22 = q1 * q1 + p1 * p1;
    pmeshele[index].Y23 = q1 * q2 + p1 * p2;
    pmeshele[index].Y33 = q2 * q2 + p2 * p2;

    pmeshele[index].Y11 /= 4. * area;
    pmeshele[index].Y12 /= 4. * area;
    pmeshele[index].Y13 /= 4. * area;
    pmeshele[index].Y22 /= 4. * area;
    pmeshele[index].Y23 /= 4. * area;
    pmeshele[index].Y33 /= 4. * area;
}

/*!
 \brief 使用传统的牛顿法进行求解

*/
void Relay1250::AxisNRsolve()
{
    /** 计时 **/
    clock_t time[10];
    int tt = 0;
    time[tt++] = SuperLU_timer_();
    /** 电路参数 **/
    double C1 = 200e-6;
    double C2 = 200e-6;
    double R1 = 1e3;
    double Rc = 28;
    double Y1= 1/R1;
    double Yc= 1/Rc;
    /** 变量 **/
    double beta = 1;
    /** 时间步循环 **/
    current_step = 0;
    double totalTime = 0;
    for(int i = 0; i < timesteps.size();i++){
        totalTime += timesteps.at(i);
        printf("Time step %d, current time is %lf s.\n",i,totalTime);
        /** remesh，要使用增量位移，向下为负，向上为正 **/
        remesh(0,Ddisplacements.at(i));
        /** 读取第i步的分网 **/
        loadMesh();
        /** 查找边界，默认一次边界 **/
        findBoundaryEdges(-1);
        findBoundaryPoints(-1);

        double* unknown_b = (double*)calloc(num_pts - boundaryPoints.size()+1, sizeof(double));
        /** 将边界点排序到末尾 **/
        int node_bdr = boundaryPoints.size();
        int node_all = allPoints.size();
        int num_dof = node_all - node_bdr + 1;/** 自由度：未知节点+电压 **/
        /** 初始化，提出读取的gmsh多余的节点以及边界点 **/
        for(int i2=0;i2<num_pts;i2++){
            pmeshnode[i2].bdr = 3;
        }
        for(int i2=0;i2<node_all;i2++){
            pmeshnode[allPoints.at(i2)].bdr = 0;
        }
        for(int i2=0;i2<node_bdr;i2++){
            pmeshnode[boundaryPoints.at(i2)].bdr = 3;
        }
        /** 计算固定区域的单元节点数目，起始编号为0 **/
        for(int i_tri = 0; i_tri < num_triangle; i_tri++){
            int i1 = num_ele-num_triangle+i_tri;
            if(pmeshele[i1].geometry_tag == tag_air){
                air_position = i_tri;
                break;
            }
        }

        umat locs(2, 9 * num_triangle);
        locs.zeros();
        mat vals(1, 9 * num_triangle);
        vec bbJz = zeros<vec>(num_pts);
        uvec node_reorder = zeros<uvec>(num_pts);
        uvec node_pos = zeros<uvec>(num_pts);
        vec bn = zeros<vec>(num_pts);
        vec A = zeros<vec>(num_pts);
        vec A_old = A;/** 上一步的大小肯定和这一步不一样 **/
        double Y11,Y12,Y13,Y22,Y23,Y33;

        for (int ibdr = 0; ibdr < num_pts; ibdr++) {
            if (pmeshnode[ibdr].bdr == 3) {
                node_bdr++;
                node_reorder(num_pts - node_bdr) = ibdr;
                node_pos(ibdr) = num_pts - node_bdr;
                pmeshnode[ibdr].A = 0;
                A(ibdr) = 0;
            } else {
                node_reorder(ibdr - node_bdr) = ibdr;
                node_pos(ibdr) = ibdr - node_bdr;
            }
        }
        /** 单元初始化，如果是非线性区域，可以考虑优化先初始值设置 **/
        for(int i_tri = 0; i_tri < num_triangle; i_tri++){
            int i1 = num_ele-num_triangle+i_tri;
            pmeshele[i1].B = 0;
            /** 对于涡流区域，没有形变，直接将miu带过来 **/
            if(i_tri < air_position){
                pmeshele[i1].miu = pmeshelelast[i1].miut;
                pmeshele[i1].miut = pmeshelelast[i1].miut;
            }else{
                pmeshele[i1].miu = miu0;
                pmeshele[i1].miut = miu0;
            }
        }
        double miuold = miu0;
        /** 非线性迭代求解 **/
        for(int non_iter = 0;non_iter < MAX_NONLINEARSTEPS;non_iter++){

            /** 有限元装配 **/
            double ce[3][3] = { {0} };/** 雅可比矩阵 **/
            double Je[3][3] = { {0} };/** 用于牛顿迭代 **/
            double Te[3][3] = { {0} };/** 涡流矩阵 **/
            double De[3] = { 0 };/** 电流密度矩阵 **/
            double dt = timesteps.at(non_iter);

            int pos = 0;
            for(int i_tri = 0; i_tri < num_triangle; i_tri++){
                int k,m,n,i1;
                i1 = num_ele-num_triangle+i_tri;
                k = pmeshele[i1].n[0];m = pmeshele[i1].n[1];n = pmeshele[i1].n[2];

                /** 计算除了磁导率部分的系数 **/
                makeTriangle(i1);

                /** 计算雅可比矩阵 **/
                double ydot = pmeshele[i1].ydot;
                double miut = pmeshele[i1].miut;
                miuold = pmeshele[i1].miu;
                /** 相当于C矩阵 **/
                Y11 = pmeshele[i1].Y11;Y22 = pmeshele[i1].Y22;Y33 = pmeshele[i1].Y33;
                Y12 = pmeshele[i1].Y12;Y13 = pmeshele[i1].Y13;Y23 = pmeshele[i1].Y23;

                /** 应当是当前迭代的mu **/
                ce[0][0] = Y11 / ydot;ce[1][1] = Y22 / ydot;ce[2][2] = Y33 / ydot;
                ce[0][1] = Y12 / ydot;ce[0][2] = Y13 / ydot;ce[1][2] = Y23 / ydot;
                ce[1][0] = ce[0][1];ce[2][0] = ce[0][2];ce[2][1] = ce[1][2];

                double v[3];
                v[0] = Y11*A(k) + Y12*A(m) + Y13*A(n);
                v[1] = Y12*A(k) + Y22*A(m) + Y23*A(n);
                v[2] = Y13*A(k) + Y23*A(m) + Y33*A(n);

                CMaterial* mat = materialMap[pmeshele[i1].geometry_tag];
                if (non_iter != 0) {
                    double tmp;
                    if (mat->BHpoints == 0) {
                        tmp = 0;
                    } else {
                        tmp = mat->getdvdB(pmeshele[i1].B);
                        if (pmeshele[i].B > 1e-9){
                            tmp /= pmeshele[i].B * pmeshele[i].AREA;//B==0?
                            tmp /= ydot * ydot * ydot;
                        }

                    }
                    Je[0][0] = v[0] * v[0] * tmp;Je[1][1] = v[1] * v[1] * tmp;Je[2][2] = v[2] * v[2] * tmp;
                    Je[0][1] = v[0] * v[1] * tmp;Je[0][2] = v[0] * v[2] * tmp;Je[1][2] = v[1] * v[2] * tmp;
                    Je[1][0] = Je[0][1];Je[2][0] = Je[0][2];Je[2][1] = Je[1][2];
                }

                /** 计算右侧向量 **/
                double dtmp = mat->tau * pmeshele[i1].AREA/3;
                De[0] = dtmp;De[1] = dtmp;De[2] = dtmp;
                double dtmp1 = 2*PI*Yc*dtmp*dtmp;
                /** 计算涡流矩阵 **/
                double etmp = mat->sigma/ydot*pmeshele[i1].AREA/12;
                Te[0][0] = etmp * 2+dtmp1;Te[1][1] = etmp* 2+dtmp1;Te[2][2] = etmp* 2+dtmp1;
                Te[0][1] = etmp+dtmp1;Te[0][2] = etmp+dtmp1;Te[1][2] = etmp+dtmp1;
                Te[1][0] = Te[0][1];Te[2][0] = Te[0][2];Te[2][1] = Te[1][2];

                for(int kk=0;kk<3;kk++){
                    for(int mm=0;mm<3;mm++){
                        Te[kk][mm] /= dt;
                        Je[kk][mm] += ce[kk][mm]/miut;
                        Je[kk][mm] += Te[kk][mm];
                    }
                }

                double jr = pmeshele[i].AREA*mat->Jr / 3;
                for (int j = 0; j < 3; j++) {
                    bbJz(pmeshele[i].n[j]) += jr;
                    /** 计算永磁部分 **/
                    bbJz(pmeshele[i].n[j]) -= mat->H_c / 2.*pmeshele[i].Q[j];
                }

                for (int row = 0; row < 3; row++) {
                    for (int col = 0; col < 3; col++) {
                        /** 判断节点是否在未知节点内 **/
                        /** 得到排序之后的编号 **/
                        int n_row = node_pos(pmeshele[i].n[row]);
                        int n_col = node_pos(pmeshele[i].n[col]);
                        if (n_row < num_pts - node_bdr && n_col < num_pts - node_bdr) {
                            locs(0, pos) = n_row;
                            locs(1, pos) = n_col;
                            vals(0, pos) = Je[row][col];
                            pos++;
                        }
                        /** 与雅可比矩阵相关的项 **/
//                        bn(pmeshele[i].n[row]) += Je[row][col] * A(pmeshele[i].n[col]);
                    }
                }
            }/** 单元循环结束 **/
            /** 右下角的点 **/
            locs(0, pos) = num_dof-1;
            locs(1, pos) = num_dof-1;
            vals(0, pos) = ((Y1+Yc)/dt-(C1+C2))/(2*PI);
            pos++;
            if (non_iter == 0) {
                locs.reshape(2, pos);
                vals.reshape(1, pos);
            }
            bn += bbJz;
            /** 调用线性求解器求解 **/
            /** 使用构造函数来生成稀疏矩阵 **/
            sp_mat X(true, locs, vals, num_pts - node_bdr, num_pts - node_bdr, true, true);

            for (int i = 0; i < num_pts - node_bdr; i++) {
                unknown_b[i] = bn(node_reorder(i));
            }
            //---------------------superLU_MT---------------------------------------
            CSuperLU_MT superlumt(num_pts - node_bdr, X, unknown_b);
            if (superlumt.solve() == 1) {
                printf("Error: superlumt.slove. Info:%d\n",superlumt.info);
                break;
            } else {
                double *sol = nullptr;
                A_old = A;
                sol = superlumt.getResult();

                for (int i = 0; i < num_pts - node_bdr; i++) {
                    pmeshnode[node_reorder(i)].A = sol[i];// / pmeshnode[i].x;//the A is r*A_real
                    A(node_reorder(i)) = sol[i];
                }
            }
            /** 更新磁场结果 **/
//            FILE *fp1 = fopen("B_T3_NR.txt", "w");
            for (int i = 0; i < num_triangle; i++) {
                double bx = 0;
                double by = 0;
                int i1 = num_ele-num_triangle+i;
                for (int j = 0; j < 3; j++) {
                    bx += pmeshele[i1].Q[j] * A(pmeshele[i1].n[j]);
                    by += pmeshele[i1].P[j] * A(pmeshele[i1].n[j]);
                }
                CMaterial* mat = materialMap[pmeshele[i1].geometry_tag];
                pmeshele[i].B = sqrt(bx*bx + by*by) / 2. / pmeshele[i].AREA / pmeshele[i1].ydot;
                pmeshele[i].Bx = bx / 2. / pmeshele[i1].AREA / pmeshele[i1].ydot;
                pmeshele[i].By = by / 2. / pmeshele[i1].AREA / pmeshele[i1].ydot;
                pmeshele[i].miut = mat->getMiu(pmeshele[i].B);
//                fprintf(fp1, "%lf \t %lf \t %lf \t %lf \t %lf\n", pmeshele[i].rc, pmeshele[i].zc, pmeshele[i].Bx, pmeshele[i].By, pmeshele[i].B);
//                y[i] = pmeshele[i].miut;
            }
//            fclose(fp1);
            double error = norm((A_old - A), 2) / norm(A, 2);
//            iter++;
            if (error < Precision) {
                break;
            }
            bn.zeros();
            pos = 0;

        }/** 牛顿迭代结束 **/

        /** 计算电磁力 **/
        calcMagForce(-1);

        /** 更新结果 **/
        current_step += 1;
        Ddisplacements.at(current_step) = 0;
        displacements.at(current_step) = 0;
        velocities.at(current_step) = 0;
        accelerations.at(current_step) = 0;
        PhiCoil.at(current_step) = 0;
        ICoil.at(current_step) = 0;
        magforces.at(current_step) = 0;
        UCoil.at(current_step) = 0;

        /** 回收空间 **/
        if(unknown_b) delete unknown_b;
    }

}

/*!
 \brief 使用传输线法进行求解

*/
void Relay1250::AxisTLMsolve()
{

}

/*!
 \brief 运行测试案例

*/
void Relay1250::run()
{
    init();
    openGeo();

    AxisNRsolve();

    /** 输出信息 **/
    outputResults();
}

/*!
 \brief 将一些结果进行输出。

*/
void Relay1250::outputResults()
{

}


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "trio_model.hpp"

#include <iterator>
#include <algorithm>
#include <cmath>
#include <random>
#include <utility>
#include <cassert>
#include <string>
#include <iostream>

#include <boost/iterator/transform_iterator.hpp>

#include "utils/maths.hpp"
#include "constant_mixture_genotype_likelihood_model.hpp"

namespace octopus { namespace model {

unsigned TrioModel::max_ploidy() noexcept
{
    return 3;
}

TrioModel::TrioModel(const Trio& trio,
                     const PopulationPriorModel& prior_model,
                     const DeNovoModel& mutation_model,
                     Options options,
                     boost::optional<logging::DebugLogger> debug_log)
: trio_ {trio}
, prior_model_ {prior_model}
, mutation_model_ {mutation_model}
, options_ {options}
, debug_log_ {debug_log}
{}

const PopulationPriorModel& TrioModel::prior_model() const noexcept
{
    return prior_model_;
}

namespace {

template <typename Container>
void clear(Container& c)
{
    c.clear();
    c.shrink_to_fit();
}

using GenotypeReference = std::reference_wrapper<const Genotype<IndexedHaplotype<>>>;

bool operator==(const GenotypeReference lhs, const GenotypeReference rhs)
{
    return lhs.get() == rhs.get();
}

bool operator<(const GenotypeReference lhs, const GenotypeReference rhs)
{
    return GenotypeLess{}(lhs.get(), rhs.get());
}

struct GenotypeRefProbabilityPair
{
    GenotypeReference genotype;
    double probability;
};

bool operator==(const GenotypeRefProbabilityPair& lhs, const GenotypeRefProbabilityPair& rhs) noexcept
{
    return lhs.probability == rhs.probability;
}
bool operator!=(const GenotypeRefProbabilityPair& lhs, const GenotypeRefProbabilityPair& rhs) noexcept
{
    return lhs.probability != rhs.probability;
}
bool operator<(const GenotypeRefProbabilityPair& lhs, const GenotypeRefProbabilityPair& rhs) noexcept
{
    return lhs.probability < rhs.probability;
}
bool operator>(const GenotypeRefProbabilityPair& lhs, const GenotypeRefProbabilityPair& rhs) noexcept
{
    return lhs.probability > rhs.probability;
}

struct GenotypeRefProbabilityPairGenotypeEqual
{
    bool operator()(const GenotypeRefProbabilityPair& lhs, const GenotypeRefProbabilityPair& rhs) const
    {
        return lhs.genotype == rhs.genotype;
    }
};

struct GenotypeRefProbabilityPairGenotypeLess
{
    bool operator()(const GenotypeRefProbabilityPair& lhs, const GenotypeRefProbabilityPair& rhs) const
    {
        return lhs.genotype < rhs.genotype;
    }
};

struct ParentsProbabilityPair
{
    GenotypeReference maternal, paternal;
    double probability, maternal_likelihood, paternal_likelihood;
};

bool operator==(const ParentsProbabilityPair& lhs, const ParentsProbabilityPair& rhs) noexcept
{
    return lhs.probability == rhs.probability;
}
bool operator!=(const ParentsProbabilityPair& lhs, const ParentsProbabilityPair& rhs) noexcept
{
    return lhs.probability != rhs.probability;
}
bool operator<(const ParentsProbabilityPair& lhs, const ParentsProbabilityPair& rhs) noexcept
{
    return lhs.probability < rhs.probability;
}
bool operator>(const ParentsProbabilityPair& lhs, const ParentsProbabilityPair& rhs) noexcept
{
    return lhs.probability > rhs.probability;
}

auto compute_likelihoods(const TrioModel::GenotypeVector& genotypes,
                         const ConstantMixtureGenotypeLikelihoodModel& model)
{
    std::vector<GenotypeRefProbabilityPair> result {};
    result.reserve(genotypes.size());
    std::transform(std::cbegin(genotypes), std::cend(genotypes), std::back_inserter(result),
                   [&] (const auto& genotype) { return GenotypeRefProbabilityPair {genotype, model.evaluate(genotype)}; });
    return result;
}

auto compute_posteriors(const std::vector<GenotypeRefProbabilityPair>& likelihoods,
                        const PopulationPriorModel& prior_model)
{
    std::vector<double> posteriors(likelihoods.size());
    std::transform(std::cbegin(likelihoods), std::cend(likelihoods), std::begin(posteriors),
                   [&] (const auto& p) { return p.probability + prior_model.evaluate({p.genotype}); });
    maths::normalise_exp(posteriors);
    std::vector<GenotypeRefProbabilityPair> result {};
    result.reserve(posteriors.size());
    std::transform(std::cbegin(likelihoods), std::cend(likelihoods), std::cbegin(posteriors), std::back_inserter(result),
                   [] (const auto& p, const auto posterior) noexcept {
                       return GenotypeRefProbabilityPair {p.genotype, posterior};
                   });
    return result;
}

template <typename T>
bool all_equal(const std::vector<T>& v)
{
    return std::adjacent_find(std::cbegin(v), std::cend(v), std::not_equal_to<> {}) == std::cend(v);
}

struct ProbabilityGetter
{
    template <typename T> const auto& operator()(const T& x) const noexcept { return x.probability; }
};

template <typename Iterator>
auto compute_lost_posterior_mass(Iterator first, Iterator first_removed, Iterator last)
{
    using boost::make_transform_iterator;
    const auto first_prob = make_transform_iterator(first, ProbabilityGetter {});
    const auto first_removed_prob = make_transform_iterator(first_removed, ProbabilityGetter {});
    const auto last_prob = make_transform_iterator(last, ProbabilityGetter {});
    return maths::log_sum_exp(first_removed_prob, last_prob) - maths::log_sum_exp(first_prob, last_prob);
}

template <typename T>
auto compute_min_joint(const std::vector<T>& zipped, const double max_log_mass_loss, boost::optional<double>& lost_log_mass)
{
    assert(!zipped.empty());
    using boost::make_transform_iterator;
    std::vector<double> cum_log_probs(make_transform_iterator(std::cbegin(zipped), ProbabilityGetter {}),
                                      make_transform_iterator(std::cend(zipped), ProbabilityGetter {}));
    maths::normalise_logs(cum_log_probs);
    const static auto log_sum_exp = [] (auto sum, auto x) { return maths::log_sum_exp(sum, x); };
    std::partial_sum(std::crbegin(cum_log_probs), std::crend(cum_log_probs), std::rbegin(cum_log_probs), log_sum_exp);
    const auto min_itr = std::upper_bound(std::crbegin(cum_log_probs), std::crend(cum_log_probs), max_log_mass_loss);
    const auto num_to_keep = static_cast<std::size_t>(std::distance(min_itr, std::crend(cum_log_probs)));
    assert(num_to_keep <= zipped.size());
    if (num_to_keep < zipped.size()) {
        assert(min_itr != std::crbegin(cum_log_probs));
        lost_log_mass = *std::prev(min_itr);
    }
    return std::max(num_to_keep, std::size_t {1});
}

template <typename T>
typename std::vector<T>::iterator
reduce(std::vector<T>& zipped, std::size_t max_joint,
       const double max_log_probability_loss,
       boost::optional<double>& lost_log_mass)
{
    max_joint = std::min(max_joint, zipped.size());
    if (all_equal(zipped)) {
        static std::default_random_engine gen {};
        std::shuffle(std::begin(zipped), std::end(zipped), gen);
        const auto min_joint = static_cast<std::size_t>(std::floor(zipped.size() * (1 - std::exp(max_log_probability_loss))));
        lost_log_mass = boost::none;
        return std::next(std::begin(zipped), std::min(max_joint, min_joint));
    } else {
        auto result = std::next(std::begin(zipped), max_joint);
        std::partial_sort(std::begin(zipped), result, std::end(zipped), std::greater<> {});
        if (max_joint < zipped.size()) {
            lost_log_mass = compute_lost_posterior_mass(std::begin(zipped), result, std::end(zipped));
        }
        if (!lost_log_mass || *lost_log_mass < max_log_probability_loss) {
            auto num_joint = compute_min_joint(zipped, max_log_probability_loss, lost_log_mass);
            assert(num_joint <= max_joint);
            result = std::next(std::begin(zipped), num_joint);
        }
        assert(result <= std::end(zipped));
        return result;
    }
}

unsigned get_sample_reduction_count(const std::size_t n)
{
    return static_cast<unsigned>(std::sqrt(n));
}

template <typename T>
struct ReducedVectorMap
{
    using Iterator = typename std::vector<T>::const_iterator;
    Iterator first, last_to_partially_join, last_to_join, last;
};

template <typename T>
auto make_reduction_map(const std::vector<T>& elements,
                        const typename std::vector<T>::const_iterator last_to_combine,
                        const TrioModel::Options& options)
{
    if (std::distance(std::cbegin(elements), last_to_combine) > 1) {
        return ReducedVectorMap<T> {std::cbegin(elements), std::next(std::cbegin(elements), 1), last_to_combine, std::cend(elements)};
    } else {
        return ReducedVectorMap<T> {std::cbegin(elements), last_to_combine, last_to_combine, std::cend(elements)};
    }
}

template <typename T>
auto reduce(std::vector<T>& zipped, const TrioModel::Options& options)
{
    auto last_to_join = std::cend(zipped);
    if (options.max_genotype_combinations) {
        last_to_join = reduce(zipped, get_sample_reduction_count(*options.max_genotype_combinations), options.max_individual_log_probability_loss);
    }
    return make_reduction_map(zipped, last_to_join, options);
}

struct UniformPriorJointProbabilityHelper
{
    std::size_t index;
    double probability;
};

bool operator==(const UniformPriorJointProbabilityHelper& lhs, const UniformPriorJointProbabilityHelper& rhs) noexcept
{
    return lhs.probability == rhs.probability;
}
bool operator!=(const UniformPriorJointProbabilityHelper& lhs, const UniformPriorJointProbabilityHelper& rhs) noexcept
{
    return lhs.probability != rhs.probability;
}
bool operator<(const UniformPriorJointProbabilityHelper& lhs, const UniformPriorJointProbabilityHelper& rhs) noexcept
{
    return lhs.probability < rhs.probability;
}
bool operator>(const UniformPriorJointProbabilityHelper& lhs, const UniformPriorJointProbabilityHelper& rhs) noexcept
{
    return lhs.probability > rhs.probability;
}

template <typename Iterator, typename UnaryOperation>
auto index_zip(Iterator first, const Iterator last, UnaryOperation op)
{
    using T = typename std::iterator_traits<Iterator>::value_type;
    using R = std::result_of_t<UnaryOperation(const T&)>;
    const auto n = static_cast<std::size_t>(std::distance(first, last));
    std::vector<std::pair<R, std::size_t>> result(n);
    for (std::size_t idx {0}; idx < n; ++idx) {
        result[idx] = std::make_pair(op(*first++), idx);
    }
    return result;
}

template <typename Iterator>
auto order_indices_by_probability(const Iterator first, const Iterator last, std::size_t k)
{
    auto zipped = index_zip(first, last, [] (const auto& pair) { return pair.probability; });
    k = std::min(zipped.size(), k);
    const auto kth = std::next(std::begin(zipped), k);
    std::partial_sort(std::begin(zipped), kth, std::end(zipped), std::greater<> {});
    std::vector<std::size_t> result(k);
    std::transform(std::begin(zipped), kth, std::begin(result), [] (const auto& p) { return p.second; });
    return result;
}

template <typename Iterator>
auto select_top_k_haplotypes(const Iterator first, Iterator last, const std::size_t k)
{
    const auto top_pair_indices = order_indices_by_probability(first, last, k);
    std::set<Haplotype> result {};
    for (const auto idx : top_pair_indices) {
        if (result.size() >= k) break;
        const auto& genotype = std::next(first, idx)->genotype.get();
        std::copy(std::cbegin(genotype), std::cend(genotype), std::inserter(result, std::begin(result)));
    }
    return result;
}

using ChildReductionMap = ReducedVectorMap<GenotypeRefProbabilityPair>;

auto select_top_k_haplotypes(const ChildReductionMap& child, const std::size_t k)
{
    return select_top_k_haplotypes(child.first, child.last_to_join, k);
}

using ParentsProbabilityPairIterator = std::vector<ParentsProbabilityPair>::iterator;

bool is_represented(const Haplotype& haplotype, const ParentsProbabilityPair& parent)
{
    return contains(parent.maternal.get(), haplotype) || contains(parent.paternal.get(), haplotype);
}

bool is_represented(const Haplotype& haplotype,
                    const ParentsProbabilityPairIterator first_parent,
                    const ParentsProbabilityPairIterator last_parent)
{
    return std::any_of(first_parent, last_parent, [&] (const auto& parent) { return is_represented(haplotype, parent); });
}

auto find_represented(const Haplotype& haplotype,
                      const ParentsProbabilityPairIterator first_parent,
                      const ParentsProbabilityPairIterator last_parent)
{
    return std::find_if(first_parent, last_parent, [&] (const auto& parent) { return is_represented(haplotype, parent); });
}

auto reduce(std::vector<ParentsProbabilityPair>& parents, const ChildReductionMap& child,
            boost::optional<double>& lost_log_mass, const TrioModel::Options& options)
{
    if (!options.max_genotype_combinations) return make_reduction_map(parents, std::cend(parents), options);
    const auto reduction_count = get_sample_reduction_count(*options.max_genotype_combinations);
    auto last_to_join = reduce(parents, reduction_count, options.max_joint_log_probability_loss, lost_log_mass);
    if (last_to_join != std::cend(parents)) {
        std::vector<UniformPriorJointProbabilityHelper> uniform_parents(parents.size());
        for (std::size_t i {0}; i < parents.size(); ++i) {
            uniform_parents[i].index = i;
            uniform_parents[i].probability = parents[i].maternal_likelihood + parents[i].paternal_likelihood;
        }
        uniform_parents.erase(reduce(uniform_parents, reduction_count, options.max_joint_log_probability_loss, lost_log_mass), std::cend(uniform_parents));
        std::deque<std::size_t> new_indices {};
        const auto num_posterior_joined = static_cast<std::size_t>(std::distance(std::begin(parents), last_to_join));
        for (const auto& p : uniform_parents) {
            if (p.index >= num_posterior_joined) {
                new_indices.push_back(p.index);
            }
        }
        uniform_parents.clear();
        if (!new_indices.empty()) {
            std::sort(std::begin(new_indices), std::end(new_indices));
            auto curr_index = num_posterior_joined;
            // Can use std::partition as it must do a single left-to-right pass due to ForwardIterator and complexity requirements
            last_to_join = std::partition(last_to_join, std::end(parents), [&] (const auto& p) {
                if (!new_indices.empty() && curr_index++ == new_indices.front()) {
                    new_indices.pop_front();
                    return true;
                } else {
                    return false;
                }
            });
            assert(new_indices.empty());
        }
        // We want to make sure haplotypes that the child may have are survive to avoid false positive de novo child haplotypes
        const auto top_child_haplotypes = select_top_k_haplotypes(child, 4);
        for (const auto& haplotype : top_child_haplotypes) {
            if (!is_represented(haplotype, std::begin(parents), last_to_join)) {
                const auto first_represented_parent = find_represented(haplotype, last_to_join, std::end(parents));
                if (first_represented_parent != std::end(parents)) {
                    std::iter_swap(first_represented_parent, last_to_join);
                    ++last_to_join;
                }
            }
        }
    }
    return make_reduction_map(parents, last_to_join, options);
}

bool are_same_ploidy(const std::vector<GenotypeRefProbabilityPair>& maternal,
                     const std::vector<GenotypeRefProbabilityPair>& paternal)
{
    assert(!maternal.empty() && !paternal.empty());
    return maternal.front().genotype.get().ploidy() == paternal.front().genotype.get().ploidy();
}

auto reduce(std::vector<GenotypeRefProbabilityPair>& likelihoods,
            const PopulationPriorModel& prior_model,
            boost::optional<double>& lost_log_mass,
            const TrioModel::Options& options)
{
    if (!options.max_genotype_combinations) return make_reduction_map(likelihoods, std::cend(likelihoods), options);
    const auto reduction_count = get_sample_reduction_count(*options.max_genotype_combinations);
    const auto last_likelihood_join = reduce(likelihoods, reduction_count, options.max_individual_log_probability_loss, lost_log_mass);
    if (last_likelihood_join == std::end(likelihoods)) {
        return make_reduction_map(likelihoods, std::cend(likelihoods), options);
    } else if (lost_log_mass && *lost_log_mass > options.max_individual_log_probability_loss) {
        return make_reduction_map(likelihoods, last_likelihood_join, options); // overflow
    } else {
        auto posteriors = compute_posteriors(likelihoods, prior_model);
        const auto last_posterior_join = reduce(posteriors, reduction_count, options.max_individual_log_probability_loss, lost_log_mass);
        assert(last_posterior_join <= std::end(posteriors));
        if (last_posterior_join != std::end(posteriors)) {
            std::sort(std::begin(likelihoods), std::end(likelihoods), GenotypeRefProbabilityPairGenotypeLess {});
            std::sort(std::begin(posteriors), last_posterior_join, GenotypeRefProbabilityPairGenotypeLess {});
            std::vector<GenotypeRefProbabilityPair> reordered_likelihoods {};
            reordered_likelihoods.reserve(posteriors.size());
            std::set_intersection(std::cbegin(likelihoods), std::cend(likelihoods),
                                  std::begin(posteriors), last_posterior_join,
                                  std::back_inserter(reordered_likelihoods),
                                  GenotypeRefProbabilityPairGenotypeLess {});
            std::set_intersection(std::cbegin(likelihoods), std::cend(likelihoods),
                                  last_posterior_join, std::end(posteriors),
                                  std::back_inserter(reordered_likelihoods),
                                  GenotypeRefProbabilityPairGenotypeLess {});
            likelihoods = std::move(reordered_likelihoods);
            auto last_join = std::next(std::cbegin(likelihoods), std::distance(std::begin(posteriors), last_posterior_join));
            return make_reduction_map(likelihoods, last_join, options);
        } else {
            return make_reduction_map(likelihoods, std::cend(likelihoods), options);
        }
    }
}

auto joint_probability(const Genotype<IndexedHaplotype<>>& mother, const Genotype<IndexedHaplotype<>>& father,
                       const PopulationPriorModel& model)
{
    return model.evaluate({std::cref(mother), std::cref(father)});
}

auto joint_probability(const GenotypeRefProbabilityPair& mother, const GenotypeRefProbabilityPair& father,
                       const PopulationPriorModel& model)
{
    return mother.probability + father.probability + joint_probability(mother.genotype, father.genotype, model);
}

template <typename T1, typename T2>
auto join_size(const ReducedVectorMap<T1>& first, const ReducedVectorMap<T2>& second) noexcept
{
    using std::distance;
    std::size_t result {0};
    result += distance(first.first, first.last_to_join) * distance(second.first, second.last_to_join);
    result += distance(first.last_to_join, first.last) * distance(second.first, second.last_to_partially_join);
    result += distance(second.last_to_join, second.last) * distance(first.first, first.last_to_partially_join);
    return result;
}

auto join(const ReducedVectorMap<GenotypeRefProbabilityPair>& maternal,
          const ReducedVectorMap<GenotypeRefProbabilityPair>& paternal,
          const PopulationPriorModel& model)
{
    std::vector<ParentsProbabilityPair> result {};
    result.reserve(join_size(maternal, paternal));
    std::for_each(maternal.first, maternal.last_to_join, [&] (const auto& m) {
        std::for_each(paternal.first, paternal.last_to_join, [&] (const auto& p) {
            result.push_back({m.genotype, p.genotype, joint_probability(m, p, model),
                              m.probability, p.probability});
        });
    });
    std::for_each(maternal.last_to_join, maternal.last, [&] (const auto& m) {
        std::for_each(paternal.first, paternal.last_to_partially_join, [&] (const auto& p) {
            result.push_back({m.genotype, p.genotype, joint_probability(m, p, model),
                              m.probability, p.probability});
        });
    });
    std::for_each(paternal.last_to_join, paternal.last, [&] (const auto& p) {
        std::for_each(maternal.first, maternal.last_to_partially_join, [&] (const auto& m) {
            result.push_back({m.genotype, p.genotype, joint_probability(m, p, model),
                              m.probability, p.probability});
        });
    });
    return result;
}

template <typename G>
bool all_haploid(const G& child, const G& mother, const G& father) noexcept
{
    return is_haploid(child) && is_haploid(mother) && is_haploid(father);
}

template <typename G>
bool all_diploid(const G& child, const G& mother, const G& father) noexcept
{
    return is_diploid(child) && is_diploid(mother) && is_diploid(father);
}

template <typename G>
bool all_triploid(const G& child, const G& mother, const G& father) noexcept
{
    return is_triploid(child) && is_triploid(mother) && is_triploid(father);
}

template <typename H, typename G>
double probability_of_child_given_haploid_parent(const H& child, const G& parent,
                                                 const DeNovoModel& mutation_model)
{
    return mutation_model.evaluate(child, parent[0]);
}

template <typename H, typename G>
double probability_of_child_given_diploid_parent(const H& child, const G& parent,
                                                 const DeNovoModel& mutation_model)
{
    static const double ln2 {std::log(2)};
    const auto p1 = mutation_model.evaluate(child, parent[0]);
    const auto p2 = mutation_model.evaluate(child, parent[1]);
    return maths::log_sum_exp(p1, p2) - ln2;
}

template <typename H, typename G>
double probability_of_child_given_diploid_parents(const H& child_from_mother, const H& child_from_father,
                                                  const G& mother, const G& father,
                                                  const DeNovoModel& mutation_model)
{
    return probability_of_child_given_diploid_parent(child_from_mother, mother, mutation_model)
            + probability_of_child_given_diploid_parent(child_from_father, father, mutation_model);
}

template <typename H, typename G>
double probability_of_child_given_triploid_parent(const H& child, const G& parent,
                                                  const DeNovoModel& mutation_model)
{
    static const double ln3 {std::log(3)};
    const auto p1 = mutation_model.evaluate(child, parent[0]);
    const auto p2 = mutation_model.evaluate(child, parent[1]);
    const auto p3 = mutation_model.evaluate(child, parent[2]);
    return maths::log_sum_exp(p1, p2, p3) - ln3;
}

template <typename H, typename G>
double probability_of_child_given_triploid_parents(const H& child1_from_mother,
                                                   const H& child2_from_mother,
                                                   const H& child_from_father,
                                                   const G& mother, const G& father,
                                                   const DeNovoModel& mutation_model)
{
    return probability_of_child_given_triploid_parent(child1_from_mother, mother, mutation_model)
           + probability_of_child_given_triploid_parent(child2_from_mother, mother, mutation_model)
           + probability_of_child_given_triploid_parent(child_from_father, father, mutation_model);
}

template <unsigned ChildPloidy, unsigned MotherPloidy, unsigned FatherPloidy>
struct ProbabilityOfChildGivenParents
{
    ProbabilityOfChildGivenParents(const DeNovoModel& mutation_model) : mutation_model {mutation_model} {}

    template <typename G>
    double operator()(const G& child, const G& mother, const G& father)
    {
        return 0;
    }

    const DeNovoModel& mutation_model;
};

template <> struct ProbabilityOfChildGivenParents<2, 2, 2>
{
    ProbabilityOfChildGivenParents(const DeNovoModel& mutation_model) : mutation_model {mutation_model} {}

    template <typename G>
    double operator()(const G& child, const G& mother, const G& father)
    {
        static const double ln2 {std::log(2)};
        const auto p1 = probability_of_child_given_diploid_parents(child[0], child[1], mother, father, mutation_model);
        const auto p2 = probability_of_child_given_diploid_parents(child[1], child[0], mother, father, mutation_model);
        return maths::log_sum_exp(p1, p2) - ln2;
    }

    const DeNovoModel& mutation_model;
};

template <> struct ProbabilityOfChildGivenParents<3, 3, 3>
{
    ProbabilityOfChildGivenParents(const DeNovoModel& mutation_model) : mutation_model {mutation_model} {}

    template <typename G>
    double operator()(const G& child, const G& mother, const G& father)
    {
        static const double ln6 {std::log(6)};
        const auto p1 = probability_of_child_given_triploid_parents(child[0], child[1], child[2], mother, father, mutation_model);
        const auto p2 = probability_of_child_given_triploid_parents(child[0], child[2], child[1], mother, father, mutation_model);
        const auto p3 = probability_of_child_given_triploid_parents(child[1], child[0], child[2], mother, father, mutation_model);
        const auto p4 = probability_of_child_given_triploid_parents(child[1], child[2], child[0], mother, father, mutation_model);
        const auto p5 = probability_of_child_given_triploid_parents(child[2], child[0], child[1], mother, father, mutation_model);
        const auto p6 = probability_of_child_given_triploid_parents(child[2], child[1], child[0], mother, father, mutation_model);
        return maths::log_sum_exp({p1, p2, p3, p4, p5, p6}) - ln6;
    }

    const DeNovoModel& mutation_model;
};

template <> struct ProbabilityOfChildGivenParents<2, 2, 1>
{
    ProbabilityOfChildGivenParents(const DeNovoModel& mutation_model) : mutation_model {mutation_model} {}

    template <typename G>
    double operator()(const G& child, const G& mother, const G& father)
    {
        static const double ln2 {std::log(2)};
        const auto p1 = probability_of_child_given_diploid_parent(child[0], mother, mutation_model);
        const auto p2 = probability_of_child_given_diploid_parent(child[1], mother, mutation_model);
        const auto p3 = probability_of_child_given_haploid_parent(child[0], father, mutation_model);
        const auto p4 = probability_of_child_given_haploid_parent(child[1], father, mutation_model);
        return maths::log_sum_exp(p1 + p4, p2 + p3) - ln2;
    }

    const DeNovoModel& mutation_model;
};

template <> struct ProbabilityOfChildGivenParents<1, 2, 1>
{
    ProbabilityOfChildGivenParents(const DeNovoModel& mutation_model) : mutation_model {mutation_model} {}

    template <typename G>
    double operator()(const G& child, const G& mother, const G& father)
    {
        return probability_of_child_given_diploid_parent(child[0], mother, mutation_model);
    }

    const DeNovoModel& mutation_model;
};

template <> struct ProbabilityOfChildGivenParents<1, 0, 1>
{
    ProbabilityOfChildGivenParents(const DeNovoModel& mutation_model) : mutation_model {mutation_model} {}

    template <typename G>
    double operator()(const G& child, const G& mother, const G& father)
    {
        return probability_of_child_given_haploid_parent(child[0], father, mutation_model);
    }

    const DeNovoModel& mutation_model;
};

template <> struct ProbabilityOfChildGivenParents<1, 1, 1>
{
    ProbabilityOfChildGivenParents(const DeNovoModel& mutation_model) : mutation_model {mutation_model} {}

    template <typename G>
    double operator()(const G& child, const G& mother, const G& father)
    {
        static const double ln2 {std::log(2)};
        const auto p1 = probability_of_child_given_haploid_parent(child[0], mother, mutation_model);
        const auto p2 = probability_of_child_given_haploid_parent(child[0], father, mutation_model);
        return maths::log_sum_exp(p1, p2) - ln2;
    }

    const DeNovoModel& mutation_model;
};

using JointProbability = TrioModel::Latents::JointProbability;

template <typename F>
auto joint_probability(const ParentsProbabilityPair& parents,
                       const GenotypeRefProbabilityPair& child,
                       F joint_probability_function)
{
    return parents.probability + child.probability
           + joint_probability_function(child.genotype.get(), parents.maternal.get(), parents.paternal.get());
}

template <typename F>
auto join(const ReducedVectorMap<ParentsProbabilityPair>& parents,
          const ReducedVectorMap<GenotypeRefProbabilityPair>& child,
          F jpdf)
{
    std::vector<JointProbability> result {};
    result.reserve(join_size(parents, child));
    std::for_each(parents.first, parents.last_to_join, [&] (const auto& p) {
        std::for_each(child.first, child.last_to_join, [&] (const auto& c) {
            result.push_back({p.maternal, p.paternal, c.genotype, joint_probability(p, c, jpdf), 0.0});
        });
    });
    std::for_each(parents.last_to_join, parents.last, [&] (const auto& p) {
        std::for_each(child.first, child.last_to_partially_join, [&] (const auto& c) {
            result.push_back({p.maternal, p.paternal, c.genotype, joint_probability(p, c, jpdf), 0.0});
        });
    });
    std::for_each(child.last_to_join, child.last, [&] (const auto& c) {
        std::for_each(parents.first, parents.last_to_partially_join, [&] (const auto& p) {
            result.push_back({p.maternal, p.paternal, c.genotype, joint_probability(p, c, jpdf), 0.0});
        });
    });
    return result;
}

auto join(const ReducedVectorMap<ParentsProbabilityPair>& parents,
          const ReducedVectorMap<GenotypeRefProbabilityPair>& child,
          const DeNovoModel& mutation_model)
{
    const auto maternal_ploidy = parents.first->maternal.get().ploidy();
    const auto paternal_ploidy = parents.first->paternal.get().ploidy();
    const auto child_ploidy    = child.first->genotype.get().ploidy();
    if (child_ploidy == 1) {
        if (paternal_ploidy == 1) {
            if (maternal_ploidy == 0) {
                return join(parents, child, ProbabilityOfChildGivenParents<1, 0, 1> {mutation_model});
            }
            if (maternal_ploidy == 1) {
                return join(parents, child, ProbabilityOfChildGivenParents<1, 1, 1> {mutation_model});
            }
            if (maternal_ploidy == 2) {
                return join(parents, child, ProbabilityOfChildGivenParents<1, 2, 1> {mutation_model});
            }
        }
    } else if (child_ploidy == 2) {
        if (maternal_ploidy == 2) {
            if (paternal_ploidy == 1) {
                return join(parents, child, ProbabilityOfChildGivenParents<2, 2, 1> {mutation_model});
            }
            if (paternal_ploidy == 2) {
                return join(parents, child, ProbabilityOfChildGivenParents<2, 2, 2> {mutation_model});
            }
        }
    } else if (child_ploidy == 3 && maternal_ploidy == 3 && paternal_ploidy == 3) {
        return join(parents, child, ProbabilityOfChildGivenParents<3, 3, 3> {mutation_model});
    }
    throw std::runtime_error {"TrioModel: unimplemented joint probability function"};
}

auto extract_probabilities(const std::vector<JointProbability>& joint_likelihoods)
{
    std::vector<double> result(joint_likelihoods.size());
    std::transform(std::cbegin(joint_likelihoods), std::cend(joint_likelihoods),
                   std::begin(result), [] (const auto& p) { return p.log_probability; });
    return result;
}

auto normalise_exp(std::vector<JointProbability>& joint_likelihoods)
{
    auto log_likelihoods = extract_probabilities(joint_likelihoods);
    const auto norm = maths::normalise_logs(log_likelihoods);
    auto iter = std::cbegin(log_likelihoods);
    for (auto& p : joint_likelihoods) {
        p.log_probability = *iter++;
        p.probability = std::exp(p.log_probability);
    }
    return norm;
}

} // namespace

namespace debug {

template <typename S>
void print(S&& stream, const std::string& sample, std::vector<GenotypeRefProbabilityPair> ps,
           std::size_t n = 5);
void print(const std::string& sample, const std::vector<GenotypeRefProbabilityPair>& ps,
           std::size_t n = 5);
template <typename S>
void print(S&& stream, std::vector<ParentsProbabilityPair> ps, std::size_t n = 5);
void print(std::vector<ParentsProbabilityPair> ps, std::size_t n = 5);
template <typename S>
void print(S&& stream, std::vector<JointProbability> ps, std::size_t n = 5);
void print(std::vector<JointProbability> ps, std::size_t n = 5);

} // namespace debug

TrioModel::InferredLatents
TrioModel::evaluate(const GenotypeVector& maternal_genotypes,
                    const GenotypeVector& paternal_genotypes,
                    const GenotypeVector& child_genotypes,
                    const HaplotypeLikelihoodArray& haplotype_likelihoods) const
{
    if (maternal_genotypes.empty()) {
        haplotype_likelihoods.prime(trio_.father());
        return evaluate_allosome(paternal_genotypes, child_genotypes, haplotype_likelihoods);
    }
    if (paternal_genotypes.empty()) {
        haplotype_likelihoods.prime(trio_.mother());
        return evaluate_allosome(maternal_genotypes, child_genotypes, haplotype_likelihoods);
    }
    assert(!maternal_genotypes.empty() && !paternal_genotypes.empty() && !child_genotypes.empty());
    const ConstantMixtureGenotypeLikelihoodModel likelihood_model {haplotype_likelihoods};
    haplotype_likelihoods.prime(trio_.mother());
    auto maternal_likelihoods = compute_likelihoods(maternal_genotypes, likelihood_model);
    haplotype_likelihoods.prime(trio_.father());
    auto paternal_likelihoods = compute_likelihoods(paternal_genotypes, likelihood_model);
    haplotype_likelihoods.prime(trio_.child());
    auto child_likelihoods = compute_likelihoods(child_genotypes, likelihood_model);
    if (debug_log_) {
        debug::print(stream(*debug_log_), "maternal", maternal_likelihoods);
        debug::print(stream(*debug_log_), "paternal", paternal_likelihoods);
        debug::print(stream(*debug_log_), "child", child_likelihoods);
    }
    boost::optional<double> lost_log_mass {};
    const auto reduced_maternal_likelihoods = reduce(maternal_likelihoods, prior_model_, lost_log_mass, options_);
    const auto reduced_paternal_likelihoods = reduce(paternal_likelihoods, prior_model_, lost_log_mass, options_);
    const auto reduced_child_likelihoods    = reduce(child_likelihoods, prior_model_, lost_log_mass, options_);
    auto parental_likelihoods = join(reduced_maternal_likelihoods, reduced_paternal_likelihoods, prior_model_);
    if (debug_log_) debug::print(stream(*debug_log_), parental_likelihoods);
    const auto reduced_parental_likelihoods = reduce(parental_likelihoods, reduced_child_likelihoods, lost_log_mass, options_);
    auto joint_likelihoods = join(reduced_parental_likelihoods, reduced_child_likelihoods, mutation_model_);
    if (debug_log_) debug::print(stream(*debug_log_), joint_likelihoods);
    const auto evidence = normalise_exp(joint_likelihoods);
    if (lost_log_mass) *lost_log_mass *= 2 * std::distance(reduced_child_likelihoods.first, reduced_child_likelihoods.last_to_join);
    return {std::move(joint_likelihoods), evidence, lost_log_mass};
}

TrioModel::InferredLatents
TrioModel::evaluate(const GenotypeVector& genotypes, const HaplotypeLikelihoodArray& haplotype_likelihoods) const
{
    return evaluate(genotypes, genotypes, genotypes, haplotype_likelihoods);
}

auto probability_of_child_given_parent(const Genotype<IndexedHaplotype<>>& child,
                                       const Genotype<IndexedHaplotype<>>& parent,
                                       const DeNovoModel& mutation_model)
{
    if (is_haploid(child) && is_haploid(parent)) {
        return mutation_model.evaluate(child[0], parent[0]);
    }
    return 0.0; // TODO
}

auto joint_probability(const GenotypeRefProbabilityPair& parent,
                       const GenotypeRefProbabilityPair& child,
                       const DeNovoModel& mutation_model)
{
    return parent.probability + child.probability
           + probability_of_child_given_parent(child.genotype, parent.genotype, mutation_model);
}

auto join(const ReducedVectorMap<GenotypeRefProbabilityPair>& parent,
          const ReducedVectorMap<GenotypeRefProbabilityPair>& child,
          const DeNovoModel& mutation_model)
{
    std::vector<JointProbability> result {};
    result.reserve(join_size(parent, child));
    std::for_each(parent.first, parent.last_to_join, [&] (const auto& p) {
        std::for_each(child.first, child.last_to_join, [&] (const auto& c) {
            result.push_back({p.genotype, p.genotype, c.genotype, joint_probability(p, c, mutation_model), 0.0});
        });
    });
    std::for_each(parent.last_to_join, parent.last, [&] (const auto& p) {
        std::for_each(child.first, child.last_to_partially_join, [&] (const auto& c) {
            result.push_back({p.genotype, p.genotype, c.genotype, joint_probability(p, c, mutation_model), 0.0});
        });
    });
    std::for_each(child.last_to_join, child.last, [&] (const auto& c) {
        std::for_each(parent.first, parent.last_to_partially_join, [&] (const auto& p) {
            result.push_back({p.genotype, p.genotype, c.genotype, joint_probability(p, c, mutation_model), 0.0});
        });
    });
    return result;
}

TrioModel::InferredLatents
TrioModel::evaluate_allosome(const GenotypeVector& parent_genotypes,
                             const GenotypeVector& child_genotypes,
                             const HaplotypeLikelihoodArray& haplotype_likelihoods) const
{
    assert(!parent_genotypes.empty() && !child_genotypes.empty());
    const ConstantMixtureGenotypeLikelihoodModel likelihood_model {haplotype_likelihoods};
    assert(haplotype_likelihoods.is_primed());
    auto child_likelihoods = compute_likelihoods(child_genotypes, likelihood_model);
    if (debug_log_) debug::print(stream(*debug_log_), "child", child_likelihoods);
    boost::optional<double> lost_log_mass {};
    const auto reduced_child_likelihoods = reduce(child_likelihoods, prior_model_, lost_log_mass, options_);
    auto parent_likelihoods = compute_likelihoods(parent_genotypes, likelihood_model);
    if (debug_log_) debug::print(stream(*debug_log_), "parent", parent_likelihoods);
    const auto reduced_parent_likelihoods = reduce(parent_likelihoods, prior_model_, lost_log_mass, options_);
    haplotype_likelihoods.prime(trio_.child());
    auto joint_likelihoods = join(reduced_parent_likelihoods, reduced_child_likelihoods, mutation_model_);
    if (lost_log_mass) *lost_log_mass *= 2 * std::distance(reduced_child_likelihoods.first, reduced_child_likelihoods.last_to_join);
    clear(parent_likelihoods);
    clear(child_likelihoods);
    const auto evidence = normalise_exp(joint_likelihoods);
    if (debug_log_) debug::print(stream(*debug_log_), joint_likelihoods);
    return {std::move(joint_likelihoods), evidence, lost_log_mass};
}

namespace debug {

template <typename S>
void print(S&& stream, const std::string& sample, std::vector<GenotypeRefProbabilityPair> ps,
           const std::size_t n)
{
    stream << sample << ":\n";
    const auto nth = std::next(std::begin(ps), std::min(ps.size(), n));
    std::partial_sort(std::begin(ps), nth, std::end(ps),
                      [] (const auto& lhs, const auto& rhs) {
                          return lhs.probability > rhs.probability;
                      });
    std::for_each(std::begin(ps), nth, [&] (const auto& p) {
        using octopus::debug::print_variant_alleles;
        print_variant_alleles(stream, p.genotype.get());
        stream << " " << p.probability << "\n";
    });
}

void print(const std::string& sample, const std::vector<GenotypeRefProbabilityPair>& ps,
           std::size_t n)
{
    print(std::cout, sample, std::move(ps), n);
}

template <typename S>
void print(S&& stream, std::vector<ParentsProbabilityPair> ps, const std::size_t n)
{
    stream << "joint parents" << ":\n";
    const auto nth = std::next(std::begin(ps), std::min(ps.size(), n));
    std::partial_sort(std::begin(ps), nth, std::end(ps),
                      [] (const auto& lhs, const auto& rhs) {
                          return lhs.probability > rhs.probability;
                      });
    std::for_each(std::begin(ps), nth, [&] (const auto& p) {
        using octopus::debug::print_variant_alleles;
        print_variant_alleles(stream, p.maternal.get());
        stream << " | ";
        print_variant_alleles(stream, p.paternal.get());
        stream << " " << p.probability << "\n";
    });
}

void print(std::vector<ParentsProbabilityPair> ps, std::size_t n)
{
    print(std::cout, std::move(ps), n);
}

template <typename S>
void print(S&& stream, std::vector<JointProbability> ps, const std::size_t n)
{
    stream << "trio top (maternal | paternal | child):" << '\n';
    const auto nth = std::next(std::begin(ps), std::min(ps.size(), n));
    std::partial_sort(std::begin(ps), nth, std::end(ps),
                      [] (const auto& lhs, const auto& rhs) {
                          return lhs.log_probability > rhs.log_probability;
                      });
    std::for_each(std::begin(ps), nth, [&] (const auto& p) {
        using octopus::debug::print_variant_alleles;
        print_variant_alleles(stream, p.maternal.get());
        stream << " | ";
        print_variant_alleles(stream, p.paternal.get());
        stream << " | ";
        print_variant_alleles(stream, p.child.get());
        stream << " " << p.log_probability << "\n";
    });
}

void print(std::vector<JointProbability> ps, std::size_t n)
{
    print(std::cout, std::move(ps), n);
}

} // namespace debug

} // namespace model
} // namespace octopus


/******************************************************************************
**
** Copyright (C) 2009-2011 Kyle Lutz <kyle.r.lutz@gmail.com>
** All rights reserved.
**
** This file is a part of the chemkit project. For more information
** see <http://www.chemkit.org>.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions
** are met:
**
**   * Redistributions of source code must retain the above copyright
**     notice, this list of conditions and the following disclaimer.
**   * Redistributions in binary form must reproduce the above copyright
**     notice, this list of conditions and the following disclaimer in the
**     documentation and/or other materials provided with the distribution.
**   * Neither the name of the chemkit project nor the names of its
**     contributors may be used to endorse or promote products derived
**     from this software without specific prior written permission.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**
******************************************************************************/

#include "graphicstransform.h"

#include <Eigen/LU>

#include <chemkit/constants.h>

namespace chemkit {

// === GraphicsTransform =================================================== //
/// \class GraphicsTransform graphicstransform.h chemkit/graphicstransform.h
/// \ingroup chemkit-graphics
/// \brief The GraphicsTransform class represents a transformation
///        matrix.

// --- Construction and Destruction ---------------------------------------- //
/// Creates a new, empty graphics transform.
///
/// The tranformation returned is:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       0 & 0 & 0 & 0 \\
///       0 & 0 & 0 & 0 \\
///       0 & 0 & 0 & 0 \\
///       0 & 0 & 0 & 0 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform::GraphicsTransform()
{
    m_matrix = new Eigen::Matrix<float, 4, 4>();

    // set transformation matrix to all zeros
    m_matrix->setZero();
}

/// Creates a new transform as a copy of \p transform.
GraphicsTransform::GraphicsTransform(const GraphicsTransform &transform)
{
    m_matrix = new Eigen::Matrix<float, 4, 4>(*transform.m_matrix);
}

/// Creates a new transform that contains \p matrix.
GraphicsTransform::GraphicsTransform(const Eigen::Matrix<float, 4, 4> &matrix)
{
    m_matrix = new Eigen::Matrix<float, 4, 4>(matrix);
}

/// Destroys the graphics transform.
GraphicsTransform::~GraphicsTransform()
{
    delete m_matrix;
}

// --- Properties ---------------------------------------------------------- //
/// Returns the data for the transform.
///
/// Use the following code to load a GraphicsTransform into
/// OpenGL:
/// \code
/// glLoadMatrixf(transform.data());
/// \endcode
const float* GraphicsTransform::data() const
{
    return m_matrix->data();
}

// --- Math ---------------------------------------------------------------- //
/// Inverts the transform.
void GraphicsTransform::invert()
{
    *m_matrix = m_matrix->inverse();
}

/// Returns the inverted version of the transform.
GraphicsTransform GraphicsTransform::inverted() const
{
    GraphicsTransform transform = *this;
    transform.invert();
    return transform;
}

/// Multiplies \p ray by the transform.
GraphicsRay GraphicsTransform::multiply(const GraphicsRay &ray) const
{
    Point3f origin = multiplyPoint(ray.origin());
    Point3f direction = multiplyVector(ray.direction());

    return GraphicsRay(origin, direction);
}

/// Multiplies \p point by the transform.
Point3f GraphicsTransform::multiplyPoint(const Point3f &point) const
{
    Eigen::Matrix<float, 4, 1> vector4;
    vector4[0] = point.x();
    vector4[1] = point.y();
    vector4[2] = point.z();
    vector4[3] = 1;

    vector4 = *m_matrix * vector4;

    return Point3f(vector4[0], vector4[1], vector4[2]);
}

/// Multiplies \p vector by the transform.
Vector3f GraphicsTransform::multiplyVector(const Vector3f &vector) const
{
    Eigen::Matrix<float, 4, 1> vector4;
    vector4[0] = vector.x();
    vector4[1] = vector.y();
    vector4[2] = vector.z();
    vector4[3] = 0;

    vector4 = *m_matrix * vector4;

    return Vector3f(vector4[0], vector4[1], vector4[2]);
}

/// Multiplies \p transform by the transform.
GraphicsTransform GraphicsTransform::multiply(const GraphicsTransform &transform) const
{
    return GraphicsTransform(*m_matrix * *transform.m_matrix);
}

Eigen::Matrix<float, 4, 1> GraphicsTransform::multiply(const Eigen::Matrix<float, 4, 1> &vector) const
{
    return *m_matrix * vector;
}

/// Multiplies \p point by the inverse of the transform.
Point3f GraphicsTransform::inverseMultiplyPoint(const Point3f &point) const
{
    Eigen::Matrix<float, 4, 1> vector4;
    vector4[0] = point.x();
    vector4[1] = point.y();
    vector4[2] = point.z();
    vector4[3] = 1;

    vector4 = m_matrix->inverse() * vector4;

    return Point3f(vector4[0], vector4[1], vector4[2]);
}

/// Multiplies \p vector by the inverse of the transform.
Vector3f GraphicsTransform::inverseMultiplyVector(const Vector3f &vector) const
{
    Eigen::Matrix<float, 4, 1> vector4;
    vector4[0] = vector.x();
    vector4[1] = vector.y();
    vector4[2] = vector.z();
    vector4[3] = 0;

    vector4 = m_matrix->inverse() * vector4;

    return Vector3f(vector4[0], vector4[1], vector4[2]);
}

Eigen::Matrix<float, 4, 1> GraphicsTransform::inverseMultiply(const Eigen::Matrix<float, 4, 1> &vector) const
{
    return m_matrix->inverse() * vector;
}

// --- Operators ----------------------------------------------------------- //
float GraphicsTransform::operator()(int row, int column) const
{
    return m_matrix->operator()(row, column);
}

float& GraphicsTransform::operator()(int row, int column)
{
    return m_matrix->operator()(row, column);
}

GraphicsRay GraphicsTransform::operator*(const GraphicsRay &ray) const
{
    return multiply(ray);
}

Point3f GraphicsTransform::operator*(const Point3f &point) const
{
    return multiplyPoint(point);
}

GraphicsTransform GraphicsTransform::operator*(const GraphicsTransform &transform) const
{
    return multiply(transform);
}

GraphicsTransform& GraphicsTransform::operator*=(const GraphicsTransform &transform)
{
    *m_matrix *= *transform.m_matrix;
    return *this;
}

GraphicsTransform& GraphicsTransform::operator=(const GraphicsTransform &transform)
{
    *m_matrix = *transform.m_matrix;
    return *this;
}

// --- Static Methods ------------------------------------------------------ //
/// Returns the identity transform.
///
/// The transformation returned is the following:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       1 & 0 & 0 & 0 \\
///       0 & 1 & 0 & 0 \\
///       0 & 0 & 1 & 0 \\
///       0 & 0 & 0 & 1 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::identity()
{
    GraphicsTransform transform;
    transform.m_matrix->setIdentity();
    return transform;
}

/// Returns a transformation matrix that represents the translation by
/// \p vector.
///
/// The transformation returned is the following:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       1 & 0 & 0 & vector_{x} \\
///       0 & 1 & 0 & vector_{y} \\
///       0 & 0 & 1 & vector_{z} \\
///       0 & 0 & 0 & 1 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::translation(const Vector3f &vector)
{
    GraphicsTransform transform = identity();

    transform(0, 3) = vector.x();
    transform(1, 3) = vector.y();
    transform(2, 3) = vector.z();

    return transform;
}

/// Returns a transform that represents a rotation by \p angle
/// degrees around \p axis.
GraphicsTransform GraphicsTransform::rotation(const Vector3f &axis, float angle)
{
    GraphicsTransform transform = identity();

    Vector3f v = axis.normalized();
    float c = cos(angle * chemkit::constants::DegreesToRadians);
    float s = sin(angle * chemkit::constants::DegreesToRadians);

    transform(0, 0) = v.x() * v.x() + (1 - v.x() * v.x()) * c;
    transform(0, 1) = v.x() * v.y() * (1 - c) - v.z() * s;
    transform(0, 2) = v.x() * v.z() * (1 - c) + v.y() * s;
    transform(1, 0) = v.x() * v.y() * (1 - c) + v.z() * s;
    transform(1, 1) = v.y() * v.y() + (1 - v.y() * v.y()) * c;
    transform(1, 2) = v.y() * v.z() * (1 - c) - v.x() * s;
    transform(2, 0) = v.x() * v.z() * (1 - c) - v.y() * s;
    transform(2, 1) = v.y() * v.z() * (1 - c) + v.x() * s;
    transform(2, 2) = v.z() * v.z() + (1 - v.z() * v.z()) * c;

    return transform;
}

/// Returns a perspective transform.
///
/// The transformation returned is the following:
/// \f[ f = cot(\frac{angle}{2}) \f]
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       \frac{f}{aspectRatio} & 0 & 0 & 0 \\
///       0 & f & 0 & 0 \\
///       0 & 0 & \frac{nearDistance+farDistance}{nearDistance-farDistance} & \frac{2 \cdot nearDistance \cdot farDistance}{nearDistance-farDistance} \\
///       0 & 0 & -1 & 0 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::perspective(float angle, float aspectRatio, float nearDistance, float farDistance)
{
    GraphicsTransform transform;

    float f = 1.0 / tan(angle / 2.0);

    transform(0, 0) = f / aspectRatio;
    transform(1, 1) = f;
    transform(2, 2) = (nearDistance + farDistance) / (nearDistance - farDistance);
    transform(2, 3) = (2 * nearDistance * farDistance) / (nearDistance - farDistance);
    transform(3, 2) = -1;

    return transform;
}

/// Returns a frustum transform.
///
/// The transformation returned is the following:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       \frac{2 \cdot nearDistance}{right-left} & 0 & \frac{right+left}{right-left} & 0 \\
///       0 & \frac{2 \cdot nearDistance}{top-bottom} & \frac{top+bottom}{top-bottom} & 0 \\
///       0 & 0 & -\frac{farDistance+nearDistance}{farDistance-nearDistance} & -\frac{2 \cdot farDistance \cdot nearDistance}{farDistance-nearDistance} \\
///       0 & 0 & -1 & 0 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::frustum(float left, float right, float top, float bottom, float nearDistance, float farDistance)
{
    GraphicsTransform transform;

    transform(0, 0) = (2 * nearDistance) / (right - left);
    transform(1, 1) = (2 * nearDistance) / (top - bottom);
    transform(2, 0) = (right + left) / (right - left);
    transform(2, 1) = (top + bottom) / (top - bottom);
    transform(2, 2) = -(farDistance + nearDistance) / (farDistance - nearDistance);
    transform(2, 3) = -(2 * farDistance * nearDistance) / (farDistance - nearDistance);
    transform(3, 2) = -1;

    return transform;
}

/// Returns a orthographic transform.
///
/// The transformation returned is the following:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       \frac{2}{right-left} & 0 & 0 & -\frac{right+left}{right-left} \\
///       0 & \frac{2}{top-bottom} & 0 & -\frac{top+bottom}{top-bottom} \\
///       0 & 0 & -\frac{2}{farDistance-nearDistance} & -\frac{farDistance+nearDistance}{farDistance-nearDistance} \\
///       0 & 0 & 0 & 1 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::orthographic(float left, float right, float top, float bottom, float nearDistance, float farDistance)
{
    GraphicsTransform transform;

    transform(0, 0) = 2.0 / (right - left);
    transform(0, 3) = -(right + left) / (right - left);
    transform(1, 1) = 2.0 / (top - bottom);
    transform(1, 3) = -(top + bottom) / (top - bottom);
    transform(2, 2) = -2.0 / (farDistance - nearDistance);
    transform(2, 3) = -(farDistance + nearDistance) / (farDistance - nearDistance);
    transform(3, 3) = 1;

    return transform;
}

} // end chemkit namespace


/*
 * Copyright (c) 2013-2014 Nicholas Corgan (n.corgan@gmail.com)
 *
 * Distributed under the MIT License (MIT) (See accompanying file LICENSE.txt
 * or copy at http://opensource.org/licenses/MIT)
 */

#include <cmath>
#include <cstdio>

#include <boost/assign.hpp>
#include <boost/format.hpp>

#include <pkmn/enums.hpp>
#include <pkmn/paths.hpp>
#include <pkmn/database/queries.hpp>
#include <pkmn/types/prng.hpp>

#include "base_pokemon_modernimpl.hpp"
#include "team_pokemon_modernimpl.hpp"

namespace pkmn
{
    team_pokemon_modernimpl::team_pokemon_modernimpl(base_pokemon::sptr base, unsigned int game, unsigned int level,
                                                     unsigned int move1, unsigned int move2,
                                                     unsigned int move3, unsigned int move4): team_pokemon_impl(base,game,level,
                                                                                move1,move2,move3,move4)
    {
        prng::sptr _rand_gen = prng::get(_generation);

        //Random individual values
        _ivHP = _rand_gen->lcrng() % 32;
        _ivATK = _rand_gen->lcrng() % 32;
        _ivDEF = _rand_gen->lcrng() % 32;
        _ivSPD = _rand_gen->lcrng() % 32;
        _ivSATK = _rand_gen->lcrng() % 32;
        _ivSDEF = _rand_gen->lcrng() % 32;

        /*
         * Random effort values within following rules:
         *  - Sum <= 510
         *  - EV <= 255
         */
        _evHP = 256;
        _evATK = 256;
        _evDEF = 256;
        _evSPD = 256;
        _evSATK = 256;
        _evSDEF = 256;
        while((_evHP+_evATK+_evDEF+_evSPD+_evSATK+_evSDEF)>510 || _evHP>255 || _evATK>255 || _evDEF>255 || _evSPD>255 || _evSATK>255 || _evSDEF>255)
        {
            _evHP = _rand_gen->lcrng() % 256;
            _evATK = _rand_gen->lcrng() % 256;
            _evDEF = _rand_gen->lcrng() % 256;
            _evSPD = _rand_gen->lcrng() % 256;
            _evSATK = _rand_gen->lcrng() % 256;
            _evSDEF = _rand_gen->lcrng() % 256;
        }

        _gender = _determine_gender();
        _nature = nature(_determine_nature());
        _determine_ability();

        _set_stats();

        /*
         * This removes the race condition where directly setting the base_pokemon::sptr's form won't
         * update the team_pokemon::sptr's stats and ability. Setting this up will make the base_pokemon's
         * set_form() function emit a signal to automatically run these functions.
         *
         * Ugly, but necessary.
         */
        base_pokemon_modernimpl* raw = reinterpret_cast<base_pokemon_modernimpl*>(_base_pkmn.get());
        raw->form_signal1.Connect(this, &team_pokemon_modernimpl::_set_stats);
        raw->form_signal2.Connect(this, &team_pokemon_modernimpl::_determine_ability);
    }

    team_pokemon_modernimpl::team_pokemon_modernimpl(const team_pokemon_modernimpl &other): team_pokemon_impl(other)
    {
        _SATK = other._SATK;
        _SDEF = other._SDEF;
        _evSATK = other._evSATK;
        _evSDEF = other._evSDEF;
        _ivSATK = other._ivSATK;
        _ivSDEF = other._ivSDEF;

        base_pokemon_modernimpl* raw = reinterpret_cast<base_pokemon_modernimpl*>(_base_pkmn.get());
        raw->form_signal1.Connect(this, &team_pokemon_modernimpl::_set_stats);
        raw->form_signal2.Connect(this, &team_pokemon_modernimpl::_determine_ability);
    }

    team_pokemon_modernimpl& team_pokemon_modernimpl::operator=(const team_pokemon_modernimpl &other)
    {
        team_pokemon_impl::operator=(other);

        _SATK = other._SATK;
        _SDEF = other._SDEF;
        _evSATK = other._evSATK;
        _evSDEF = other._evSDEF;
        _ivSATK = other._ivSATK;
        _ivSDEF = other._ivSDEF;

        base_pokemon_modernimpl* raw = reinterpret_cast<base_pokemon_modernimpl*>(_base_pkmn.get());
        raw->form_signal1.Connect(this, &team_pokemon_modernimpl::_set_stats);
        raw->form_signal2.Connect(this, &team_pokemon_modernimpl::_determine_ability);

        return (*this);
    }

    std::string team_pokemon_modernimpl::get_gender() const {return _gender;}

    nature team_pokemon_modernimpl::get_nature() const {return _nature;}

    std::string team_pokemon_modernimpl::get_ability() const {return _ability;}

    bool team_pokemon_modernimpl::is_shiny() const
    {
        int p1, p2, E, F;
        p1 = (_personality & 0xFFFF0000) >> 16;
        p2 = _personality & 0xFFFF;
        E = _tid.public_id ^ _tid.secret_id;
        F = p1 ^ p2;
        return (E ^ F) < 8;
    }

    pkmn::dict<std::string, unsigned int> team_pokemon_modernimpl::get_stats() const
    {
        pkmn::dict<std::string, unsigned int> stats;
        stats["HP"] = _HP;
        stats["Attack"] = _ATK;
        stats["Defense"] = _DEF;
        stats["Special Attack"] = _SATK;
        stats["Special Defense"] = _SDEF;
        stats["Speed"] = _SPD;

        return stats;
    }

    pkmn::dict<std::string, unsigned int> team_pokemon_modernimpl::get_EVs() const
    {
        pkmn::dict<std::string, unsigned int> EVs;
        EVs["HP"] = _evHP;
        EVs["Attack"] = _evATK;
        EVs["Defense"] = _evDEF;
        EVs["Special Attack"] = _evSATK;
        EVs["Special Defense"] = _evSDEF;
        EVs["Speed"] = _evSPD;

        return EVs;
    }

    pkmn::dict<std::string, unsigned int> team_pokemon_modernimpl::get_IVs() const
    {
        pkmn::dict<std::string, unsigned int> IVs;
        IVs["HP"] = _ivHP;
        IVs["Attack"] = _ivATK;
        IVs["Defense"] = _ivDEF;
        IVs["Special Attack"] = _ivSATK;
        IVs["Special Defense"] = _ivSDEF;
        IVs["Speed"] = _ivSPD;

        return IVs;
    }

    //TODO: adjust Meowstic's stats if gender changes
    void team_pokemon_modernimpl::set_personality(unsigned int personality)
    {
        _personality = personality;
        _determine_ability();
        _gender = _determine_gender();
        _nature = _determine_nature();
    }

    void team_pokemon_modernimpl::set_gender(std::string gender)
    {
        if(gender == "Male" or gender == "Female") _gender = gender;
    }

    void team_pokemon_modernimpl::set_nature(std::string nature_name)
    {
        //Search for given nature in database
        std::string query_string = str(boost::format("SELECT nature_id FROM nature_names WHERE name='%s'")
                                       % nature_name.c_str());
        SQLite::Statement nature_names_query(*_db, query_string.c_str());
        if(nature_names_query.executeStep())
        {
            _nature = nature(nature_name);
            _set_stats();
            return;
        }

        //If not in nature_names, check in abilities
        query_string = str(boost::format("Select id FROM natures WHERE identifier='%s'")
                           % nature_name.c_str());
        SQLite::Statement natures_query(*_db, query_string.c_str());
        if(natures_query.executeStep())
        {
            _nature = nature(nature_name);
            _set_stats();
        }
    }

    void team_pokemon_modernimpl::set_ability(std::string ability)
    {
        //Search for given ability in database
        std::string query_string = str(boost::format("SELECT ability_id FROM ability_names WHERE name='%s'")
                                       % ability.c_str());
        SQLite::Statement ability_names_query(*_db, query_string.c_str());
        if(ability_names_query.executeStep())
        {
            _ability = ability;
            return;
        }

        //If not in ability_names, check in abilities
        query_string = str(boost::format("Select id FROM abilities WHERE identifier='%s'")
                           % ability);
        SQLite::Statement abilities_query(*_db, query_string.c_str());
        if(abilities_query.executeStep())
        {
            ability[0] = tolower(ability[0]);
            _ability = ability;
        }
    }

    void team_pokemon_modernimpl::set_using_hidden_ability(bool val)
    {
        _has_hidden_ability = val;
        _determine_ability();
    }

    void team_pokemon_modernimpl::set_EV(std::string EV, unsigned int val)
    {
        if(val > 255) throw std::runtime_error("Gen 3-6 EV's must be 0-255.");

        if(EV == "HP")
        {
            _evHP = val;
            _HP = _get_hp();
        }
        else if(EV == "Attack")
        {
            _evATK = val;
            _ATK = _get_stat("Attack", _evATK, _ivATK);
        }
        else if(EV == "Defense")
        {
            _evDEF = val;
            _DEF = _get_stat("Defense", _evDEF, _ivDEF);
        }
        else if(EV == "Speed")
        {
            _evSPD = val;
            _SPD = _get_stat("Speed", _evSPD, _ivSPD);
        }
        else if(EV == "Special Attack")
        {
            _evSATK = val;
            _SATK = _get_stat("Special Attack", _evSATK, _ivSATK);
        }
        else if(EV == "Special Defense")
        {
            _evSDEF = val;
            _SDEF = _get_stat("Special Defense", _evSDEF, _ivSDEF);
        }
    }

    void team_pokemon_modernimpl::set_IV(std::string IV, unsigned int val)
    {
        if(val > 31) throw std::runtime_error("Gen 3-6 IV's must be 0-31.");

        if(IV == "HP")
        {
            _ivHP = val;
            _HP = _get_hp();
        }
        else if(IV == "Attack")
        {
            _ivATK = val;
            _ATK = _get_stat("Attack", _evATK, _ivATK);
        }
        else if(IV == "Defense")
        {
            _ivDEF = val;
            _DEF = _get_stat("Defense", _evDEF, _ivDEF);
        }
        else if(IV == "Speed")
        {
            _ivSPD = val;
            _SPD = _get_stat("Speed", _evSPD, _ivSPD);
        }
        else if(IV == "Special Attack")
        {
            _ivSATK = val;
            _SATK = _get_stat("Special Attack", _evSATK, _ivSATK);
        }
        else if(IV == "Special Defense")
        {
            _ivSDEF = val;
            _SDEF = _get_stat("Special Defense", _evSDEF, _ivSDEF);
        }
    }

    void team_pokemon_modernimpl::set_form(unsigned int form)
    {
        _base_pkmn->set_form(form);
    }

    void team_pokemon_modernimpl::set_form(std::string form)
    {
        _base_pkmn->set_form(form);
    }

    unsigned int team_pokemon_modernimpl::_get_hp() const
    {
        pkmn::dict<std::string, unsigned int> stats = _base_pkmn->get_base_stats();

        unsigned int hp_val = int(floor(((double(_ivHP) + (2.0*double(stats["HP"])) + (0.25*double(_evHP)) + 100.0)
                            * double(_level))/100.0 + 10.0));
        return hp_val;
    }

    unsigned int team_pokemon_modernimpl::_get_stat(std::string stat, unsigned int EV, unsigned int IV) const
    {
        pkmn::dict<std::string, unsigned int> stats = _base_pkmn->get_base_stats();

        unsigned int stat_val = int(ceil(((((double(IV) + 2.0*double(stats[stat]) + 0.25*double(EV))
                              * double(_level))/100.0) + 5.0) * _nature[stat]));

        return stat_val;
    }

    void team_pokemon_modernimpl::_set_stats()
    {
        if(_base_pkmn->get_species_id() == Species::SHEDINJA) _HP = 1;
        else _HP = _get_hp();
        _ATK = _get_stat("Attack", _evATK, _ivATK);
        _DEF = _get_stat("Defense", _evDEF, _ivDEF);
        _SPD = _get_stat("Speed", _evSPD, _ivSPD);
        _SATK = _get_stat("Special Attack", _evSATK, _ivSATK);
        _SDEF = _get_stat("Special Defense", _evSDEF, _ivSDEF);
    }

    void team_pokemon_modernimpl::_determine_ability()
    {
        string_pair_t abilities = _base_pkmn->get_abilities();
        std::string hidden_ability = _base_pkmn->get_hidden_ability();

        if(_base_pkmn->get_species_id() == Species::NONE or _base_pkmn->get_species_id() == Species::INVALID){
            _ability = "None";
        }
        else if(_has_hidden_ability and _generation >= 5) _ability = hidden_ability;
        else _ability = ((_personality % 2) == 0) ? abilities.first : abilities.second;
    }

    std::string team_pokemon_modernimpl::_determine_gender() const
    {
        if(_base_pkmn->get_chance_male() + _base_pkmn->get_chance_female() == 0
           or _base_pkmn->get_species_id() == Species::NONE
           or _base_pkmn->get_species_id() == Species::INVALID) return "Genderless";
        else if(_base_pkmn->get_chance_male() == 1.0) return "Male";
        else if(_base_pkmn->get_chance_female() == 1.0) return "Female";
        else
        {
            if((_personality % 256) > int(floor(255*(1-_base_pkmn->get_chance_male())))) return "Male";
            else return "Female";
        }

        //Should never get here, this stops Clang from complaining
        return "Male";
    }

    std::string team_pokemon_modernimpl::_determine_nature() const
    {
        return database::get_nature_name(_personality % 24);
    }
} /* namespace pkmn */