#include "kalmanfilter.h"
#include <Eigen/Cholesky>

const double KalmanFilter::chi2inv95[10] = {
    0,
    3.8415,
    5.9915,
    7.8147,
    9.4877,
    11.070,
    12.592,
    14.067,
    15.507,
    16.919
};
KalmanFilter::KalmanFilter()
{
    int ndim = 4;
    double dt = 1.;

    _motion_mat = Eigen::MatrixXf::Identity(8, 8);
    for(int i = 0; i < ndim; i++) {
        _motion_mat(i, ndim+i) = dt;
    }
    _update_mat = Eigen::MatrixXf::Identity(4, 8);

    this->_std_weight_position = 1. / 20;
    this->_std_weight_velocity = 1. / 160;
}

KAL_DATA KalmanFilter::initiate(const DETECTBOX &measurement)
{
    DETECTBOX mean_pos = measurement;
    DETECTBOX mean_vel;
    for(int i = 0; i < 4; i++) mean_vel(i) = 0;

    KAL_MEAN mean;
    for(int i = 0; i < 8; i++){
        if(i < 4) mean(i) = mean_pos(i);
        else mean(i) = mean_vel(i - 4);
    }

    KAL_MEAN std;
    std(0) = 2 * _std_weight_position * measurement[3];
    std(1) = 2 * _std_weight_position * measurement[3];
    std(2) = 1e-2;
    std(3) = 2 * _std_weight_position * measurement[3];
    std(4) = 10 * _std_weight_velocity * measurement[3];
    std(5) = 10 * _std_weight_velocity * measurement[3];
    std(6) = 1e-5;
    std(7) = 10 * _std_weight_velocity * measurement[3];

    KAL_MEAN tmp = std.array().square();
    KAL_COVA var = tmp.asDiagonal();
    return std::make_pair(mean, var);
}

void KalmanFilter::predict(KAL_MEAN &mean, KAL_COVA &covariance)
{
    //revise the data;
    DETECTBOX std_pos;
    std_pos << _std_weight_position * mean(3),
            _std_weight_position * mean(3),
            1e-2,
            _std_weight_position * mean(3);
    DETECTBOX std_vel;
    std_vel << _std_weight_velocity * mean(3),
            _std_weight_velocity * mean(3),
            1e-5,
            _std_weight_velocity * mean(3);
    KAL_MEAN tmp;
    tmp.block<1,4>(0,0) = std_pos;
    tmp.block<1,4>(0,4) = std_vel;
    tmp = tmp.array().square();
    KAL_COVA motion_cov = tmp.asDiagonal();
    KAL_MEAN mean1 = this->_motion_mat * mean.transpose();
    KAL_COVA covariance1 = this->_motion_mat * covariance *(_motion_mat.transpose());
    covariance1 += motion_cov;

    mean = mean1;
    covariance = covariance1;
}

KAL_HDATA KalmanFilter::project(const KAL_MEAN &mean, const KAL_COVA &covariance)
{
    DETECTBOX std;
    std << _std_weight_position * mean(3), _std_weight_position * mean(3),
            1e-1, _std_weight_position * mean(3);
    KAL_HMEAN mean1 = _update_mat * mean.transpose();
    KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose());
    Eigen::Matrix<float, 4, 4> diag = std.asDiagonal();
    diag = diag.array().square().matrix();
    covariance1 += diag;
//    covariance1.diagonal() << diag;
    return std::make_pair(mean1, covariance1);
}

KAL_DATA
KalmanFilter::update(
        const KAL_MEAN &mean,
        const KAL_COVA &covariance,
        const DETECTBOX &measurement)
{
    KAL_HDATA pa = project(mean, covariance);
    KAL_HMEAN projected_mean = pa.first;
    KAL_HCOVA projected_cov = pa.second;

    //chol_factor, lower =
    //scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False)
    //kalmain_gain =
    //scipy.linalg.cho_solve((cho_factor, lower),
    //np.dot(covariance, self._upadte_mat.T).T,
    //check_finite=False).T
    Eigen::Matrix<float, 4, 8> B = (covariance * (_update_mat.transpose())).transpose();
    Eigen::Matrix<float, 8, 4> kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4
    Eigen::Matrix<float, 1, 4> innovation = measurement - projected_mean; //eg.1x4
    auto tmp = innovation*(kalman_gain.transpose());
    KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix();
    KAL_COVA new_covariance = covariance - kalman_gain*projected_cov*(kalman_gain.transpose());
    return std::make_pair(new_mean, new_covariance);
}

Eigen::Matrix<float, 1, -1>
KalmanFilter::gating_distance(
        const KAL_MEAN &mean,
        const KAL_COVA &covariance,
        const std::vector<DETECTBOX> &measurements,
        bool only_position)
{
    KAL_HDATA pa = this->project(mean, covariance);
    if(only_position) {
        printf("not implement!");
        exit(0);
    }
    KAL_HMEAN mean1 = pa.first;
    KAL_HCOVA covariance1 = pa.second;

//    Eigen::Matrix<float, -1, 4, Eigen::RowMajor> d(size, 4);
    DETECTBOXSS d(measurements.size(), 4);
    int pos = 0;
    for(DETECTBOX box:measurements) {
        d.row(pos++) = box - mean1;
    }
    Eigen::Matrix<float, -1, -1, Eigen::RowMajor> factor = covariance1.llt().matrixL();
    Eigen::Matrix<float, -1, -1> z = factor.triangularView<Eigen::Lower>().solve<Eigen::OnTheRight>(d).transpose();
    auto zz = ((z.array())*(z.array())).matrix();
    auto square_maha = zz.colwise().sum();
    return square_maha;
}


/*  This file is part of libDAI - http://www.libdai.org/
 *
 *  libDAI is licensed under the terms of the GNU General Public License version
 *  2, or (at your option) any later version. libDAI is distributed without any
 *  warranty. See the file COPYING for more details.
 *
 *  Copyright (C) 2010  Joris Mooij  [joris dot mooij at libdai dot org]
 */

#include <dai/alldai.h>
#include <fstream>
#include "dai/emrun.h"
#include <dai/util.h>
#include <string>
#include <sys/stat.h>
#include <time.h>
#include <iomanip>      // std::setprecision
#include <sstream>
#include <cstdlib>
#include <algorithm>
#include<math.h>
#include <string>
#include <iostream>

#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/bimap/bimap.hpp>
#include <boost/bimap/unordered_set_of.hpp>
#include <boost/bimap/multiset_of.hpp>
#include <boost/config.hpp>
#include <boost/bimap.hpp>
#include <boost/optional.hpp>
#include <boost/none.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/list_inserter.hpp>
using namespace std;
using namespace dai;
using namespace boost;
// Define a random number generator and initialize it with a reproducible
// seed.
mt19937 generator(42);

// Define a uniform random number distribution which produces "double"
// values between 0 and 1 (0 inclusive, 1 exclusive).
boost::uniform_real<> uni_dist(0,1);
boost::variate_generator<mt19937&, boost::uniform_real<> > uni(generator, uni_dist);

boost::normal_distribution<> nd(0.0, 1.0);
boost::variate_generator<mt19937&, boost::normal_distribution<> > normal(generator, nd);

// GA - initialization
size_t max_gen = 5; // no. of generations
size_t population_size =4; // population size
Real pc;// = 0.8; // crossover probability - between 0.6 an 0.9
Real pm;// = 0.3; // mutation probability - between 1/pop.size and 1/chromosome_length
int population_no=0; // current population id

// 1. Creating initial population of EM runs
string alarmfgloc;
char* gibbsfile;// = "/home/priya/libDAI-0.3.1/alarm/500/5.tab";
char* networkname ; //= "alarm";
size_t maxIters =1000; // maximum number of iterations
char* emfile;
ofstream out;
EMRun* runname;
vector <EMRun*> layer;
vector <EMRun*> layer_curr;
vector <EMRun*> layer_prev;
vector <EMRun*> layer_parent;
dai::hash_map<int, FactorGraph> hashmap;
int *arrval;
//
// Generate a random number between 0 and 1
// return a uniform number in [0,1].
double unifRand()
{
    return rand()/double(RAND_MAX);
}
void initializeEM()
{
    size_t i =0,j=0;
	cout<<"Initial start EM"<<endl;
	while(i<population_size)
	{
        string number;
        stringstream num;
        num << i;
        number = num.str();
        string dirname;
        stringstream p_id;
        p_id << population_no;
        dirname = p_id.str();
        string fgfileloc =  alarmfgloc+"/"+networkname+"_"+number+".fg";
        const char* fgfile = &fgfileloc[0];
        FactorGraph fg;
        fg.ReadFromFile(fgfile);
        runname = new EMRun(networkname, i, fg, gibbsfile,emfile);
        // 2. Run EM
        boost::thread threadid(boost::bind(&EMRun::startEM, boost::ref(runname), maxIters));
        layer.push_back(runname);
        sleep(1);
        // 4. Next individual
        cout<<" run "<<i<<" started"<<endl;
        i=i+1;
    }
}
void printFactors()
{
    vector<EMRun*>::iterator it1;
    for (int i=0;i<population_size;i++)
    {
       FactorGraph fg = hashmap[i];
    cout<<"------ Printing ------"<<i<<endl;
    for( size_t I = 0; I < fg.nrFactors(); I++ )
    {
        Factor P = fg.factor( I );
        dai::TProb<double> myvector = P.p();
        for (dai::TProb<double>::iterator it = myvector.begin();it != myvector.end(); ++it)
            std::cout << ' ' << *it;
        std::cout << '\n';
    }
    }
}
void startEM()
{
   // printFactors();
    for (int i=0;i<population_size;i++)
    {
        FactorGraph fg = hashmap[i];
        double runid = i;
        cout<<"Starting EM "<<runid<<endl;
        runname = new EMRun(networkname, i, fg, gibbsfile,emfile);
        // 2. Run EM
        boost::thread threadid(boost::bind(&EMRun::startEM, boost::ref(runname), maxIters));
        layer.push_back(runname);
        sleep(1);
	}

}
std::vector<EMRun*>::iterator compareRuns(std::vector<EMRun*>::iterator *it)
{
    if(layer_prev.size() !=0 )
      {
        cout<<" iters = "<<(**it)->getIters()<<endl;
        double iters = (**it)->getIters();
        double val = (**it)->getRun();
        cout<<" em run stopped "<<val<<endl;
        double ll = (**it)->getLL();
        vector<EMRun*>::iterator itprev;
        std::vector<Real> llTraceprev;
        // get the pervious run

        for (itprev=layer_prev.begin(); itprev != layer_prev.end(); ++itprev)
        {
            double runid = (*itprev)->getRun();
            if(val==runid)
            {
                //out1<<runid<<" "<<iters<<" ";

                llTraceprev = (*itprev)->lastlog();
                cout<<val<<" "<<"runid = "<<(*itprev)->getRun()<<"  "<<" "<<(*itprev)->getIters()<<endl;
                break;
            }
        }
        //check if current EM run's LL is greater or equal to prev EM run's LL

        double llprev;
        cout<<" size = "<<llTraceprev.size()<<endl;
        if((llTraceprev.size()-1)>iters)
            llprev = llTraceprev.at(iters);
        else
            llprev = llTraceprev.at(llTraceprev.size()-1);
       // cout<<" LL for prev run = "<<llprev<<endl;
        if(ll>=llprev)
        {
            cout<<" current LL "<<ll<<" is good"<<endl;
            boost::thread thread7(boost::bind(&EMRun::resumeEM, boost::ref(**it)));
            cout<<" em run resumed"<<val<<endl;
        }
        else
        {
            cout<<" current LL "<<ll<<" is not good"<<endl;
            **it = *itprev;
            (*itprev)->forceStopped();
            cout<<" em run not resumed"<<val<<endl;
        }

    }
    return *it;
    ////////////
}
void checkEMforTermination()
{
    layer_prev.assign(layer_curr.begin(), layer_curr.end());
	layer_curr.clear();
    int i=0;
	while (true){
	    //cout<<"in while loop "<<layer.size()<<endl;
        i++;
	    vector<EMRun*>::iterator it;
	    for (it=layer.begin(); it != layer.end(); ++it){
//            if((*it)->getIters() > 2*i)
//            it = compareRuns(&it);
//            cout<<" iters = "<<(*it)->getIters()<<endl;
//
	      //cout<<"layer size "<<layer.size()<<endl;
	      if((*it)->hasTerminated()) { //if it has terminated, set the run aside

              double ll = (*it)->getLL();
              double iters = (*it)->getIters();
              double runid = (*it)->getRun();
              //cout<<"em run "<<runid<<" terminated "<<iters<<endl;
              std::vector<Real> llTrace = (*it)->lastlog();
              cout<<"Run number "<<runid<<" ll = "<<ll<<" iters = "<<iters<<endl;
              InfAlg* inf1 = (*it)->getInf();
              FactorGraph fg1 = inf1->fg();
              //cout<<fg1<<endl;
              sleep(1);
              out<<pc<<" "<<pm<<" "<<gibbsfile<<" "<<runid<<" "<<iters<<" "<<ll<<" "<<endl;
              layer_curr.push_back(*it);
              layer.erase(it);
              if (layer.size() == 0){break;}
              else { it = layer.begin(); }
          }
            //we stop the run
        else if(((*it)->getIters()) == maxIters){
            double ll = (*it)->getLL();
            double iters = (*it)->getIters();
            double runid = (*it)->getRun();
            sleep(1);
            //cout<<"Run number "<<number<<" has reached maximum iterations so stopped"<<endl;
            //out<<pc<<" "<<pm<<" "<<gibbsfile<<" "<<runid<<" "<<iters<<" "<<ll<<" "<<endl;
		    layer_curr.push_back(*it);
            if(!(*it)->isForceStop())
            {
                boost::thread thread5(boost::bind(&EMRun::stopEM, boost::ref(*it)));

            }
            layer.erase(it);
            //	if (layer.size() == 0){break;}
            //	else { it = layer.begin(); }
	      }

	 }
	 if (layer.size() == 0){ //cout<<"breaking while"<<endl;
		break; }
	 else { //cout<<"iterating"<<endl;
		}
	  sleep(1);
	}
    layer_parent.assign(layer_curr.begin(), layer_curr.end());
}

void doMutation() // open each child file and apply mutation if required
{
    //cout<<"Mutation Begins"<<endl;
    double pmut=0;
    vector<EMRun*>::iterator it_C;
    //printFactors();
    for (it_C=layer_parent.begin(); it_C != layer_parent.end(); ++it_C){
        double runid = (*it_C)->getRun();
        InfAlg* inf = (*it_C)->getInf();
        FactorGraph fg = hashmap[runid];//inf->fg();
        std::vector<int> mutateFactors;
        mutateFactors.clear();
        mutateFactors = (*it_C)->getmutateFactors();
        int noMutations =0;
        //cout<<" Run id "<<runid;
        //cout<<"______"<<mutateFactors.size()<<endl;
        cout<<"Mutation at ";
        for( size_t I = 0; I < fg.nrFactors(); I++ )
            {
//                std::vector<int>::iterator it;
//                it = find (mutateFactors.begin(), mutateFactors.end(), I);
//                if (it != mutateFactors.end())
//                {
//                    //std::cout << "Element found in myvector: " << *it << '\n';
//                    pmut=1;
//                }
//                else
//                {
//                    //  std::cout << "Element not found in myvector\n";
//                    pmut=0;
//                }
                pmut = uni(); // generates random a value between 0 and 1 for child1
                //cout<<pmut<<" "<<endl;
                int no_states = fg.factor( I ).nrStates();// total no. of states
                int no_arr_arrset =0; // factor_states
                int arr_size =1; //states of parents before factor_pos
                int arr_set =0; //states of parents after factor_pos
                int arr_set_states =0; // no. of states till factor_pos

                //cout<<fg.factor( I ).nrStates()<<endl;
                Factor P = fg.factor( I );
                VarSet vs = P.vars();
//                cout<<"vars = "<<vs<<endl;
//                cout << "{";
                for( VarSet::const_iterator v = vs.begin(); v != vs.end(); v++ )
                {
                   // cout << (v != vs.begin() ? ", " : "") << *v;
                    if(v->label()<I)
                        arr_size = arr_size * v->states();
                    if(v->label() == I)
                    {
                       // cout<<" states = "<<v->states()<<endl;
                        no_arr_arrset = v->states();
                        arr_set_states = arr_size * v->states();
                        break;
                    }
                }
                //cout << "}"<<endl;
                arr_set = no_states/arr_set_states;
                //cout<<"no. of states "<<no_states<<"\t"<<"array set "<<arr_set<<"\t"<<"arr_set_states "<<arr_set_states<<"\t"<<"no_arr_arrset "<<no_arr_arrset<<"\t"<<"array size "<<arr_size<<endl;

                if(pmut>pm) //do mutation handling the constraint that the states of the random variable sum to 1.
                {
                noMutations++;
                cout<<" "<<I;
                int k=0;
                while(k<arr_set)
                {
                    //Random values are generated---~----~----
                    std::vector< double * > Arrays1;
                    for(int l=0; l<no_arr_arrset; l++)
                    {
                        Arrays1.push_back( new double[arr_size]);
                    }
                    vector<double*>::iterator it;
                    for(int m=0; m<arr_size; m++)
                    {
                        it=Arrays1.begin();
                        std::vector<double> vec;
                        for(int i=0; i<no_arr_arrset; i++)
                        {
                            vec.push_back(uni());
                        }
                        const double total = std::accumulate(vec.begin(), vec.end(), 0.0);
                        for (double& value: vec)
                        {
                            double* arr = *it;
                            value /= total;
                            arr[m] = value;
                            ++it;
                           // cout<<value <<endl;
                        }
                        // cout<<endl;
                        vec.clear();
                    }
                    //Random values are generated---x----x----
                    //Random or actual values will be updated -----~------~
                    vector<double*>::iterator it11;
                    it11=Arrays1.begin();
                    int stat=0;
                    for(int l=0; l<no_arr_arrset; l++)
                    {
                        double* arr = *it11;
                        for(int m=0; m<arr_size; m++)
                        {
                            stringstream v;
                            v << fixed << setprecision(12) << arr[m];
                            //c1<<stat<<"   "<<v.str()<<endl;
                            //cout<< "replaced"<<v.str()<<endl;
                            P.set(stat,arr[m]);
                            stat++;
                        }
                        ++it11;
                    }
                    //Random values are updated ------x------x--------
                    k++;
                } // end of while
                } // end of if
                fg.setFactor(I,P);
            } // end of factors
        cout<<" Total mutations "<<noMutations<<" runid "<<runid<<endl;
        hashmap[runid]=fg;
    } // end of factorgraphs
     //printFactors();
    //cout<<"Mutation Done"<<endl;
}

void doCrossover()
{
    //cout<<"Crossover Begins"<<endl;
	// compute crossover probability
    double pcross = uni(); // generates random a value between 0 and 1
	//cout<<"Crossover probability "<<pcross <<endl;
	// 4. Select 2 parents randomly
	const int LOW = 0;
	const int HIGH = population_size-1;
    set<int> myset;
	set<int>::iterator it;
	size_t p=0;
	arrval = new int[population_size];
    /*Declare variable to hold seconds on clock.*/
	time_t seconds;
	/*Get value from system clock and place in seconds variable.*/
	time(&seconds);
	/*Convert seconds to a unsigned integer.*/
	srand(time(0));

    for (int n=0;n<population_size;n++)
    {
        int val = rand() % (HIGH - LOW + 1) + LOW;
        it=myset.find(val);
        arrval[n] = val;
        if(it==myset.end()) // if the value is not in the set
        {
            myset.insert(val);
            p++;
            if(p == 2)
            {
               // cout <<" Random value is"<<arrval[n]<<" and "<< arrval[n-1];
                EMRun* r1 = layer_parent.at(n);
                EMRun* r2 = layer_parent.at(n-1);
                double runid = (r1)->getRun();
                InfAlg* inf = (r1)->getInf();
                FactorGraph fg = inf->fg();
                double runid1 = (r2)->getRun();
                InfAlg* inf1 = (r2)->getInf();
                FactorGraph fg1 = inf1->fg();
                // randomly select a crossover point
                int cr_pt = (rand() % fg.nrVars());
               // cout<<" Crossover point is "<< cr_pt<<endl;
                p=0;
                if(pcross>pc)
                {
                for( size_t I = cr_pt; I < fg.nrFactors(); I++ )
                {
                    Factor P = fg.factor( I );
                    dai::TProb<double> myvector = P.p();
                    Factor P1 = fg1.factor( I );
                    dai::TProb<double> myvector1 = P1.p();
                    dai::TProb<double>::iterator it1 = myvector1.begin();
                    dai::TProb<double>::iterator it = myvector.begin();
                    for (int state=0;state<myvector.size();state++)
                    {
                        P.set(state,*it1);
                        P1.set(state,*it);
                        ++it1;++it;
                    }
                    fg.setFactor(I,P);
                    fg1.setFactor(I,P1);
                }
                }
                int val = arrval[n];int val1 = arrval[n-1];
                hashmap[val]=fg;
                hashmap[val1]=fg1;
            }
        }
        else
        {
            n=n-1;
            continue;
        }
    }
    //cout<<"Crossover Done"<<endl;
    //printFactors();

}

void replacementStrategy()
{
 //cout<<"In replacement strategy"<<endl;
 double ll, ll1;
 string ssiters,ssiters1,ssll1,ssll,ssrunid, ssrunid1;
 layer_parent.clear();
 for( size_t n = 0; n < population_size; n++ )
 {
	int val = arrval[n];
	//cout<<"val = "<<val<<endl;
	vector<EMRun*>::iterator it, it1;
	int i=0;
	for (it=layer_prev.begin(); it != layer_prev.end(); ++it)
    {
	      ll = (*it)->getLL();
	      double iters = (*it)->getIters();
	      double runid = (*it)->getRun();
	      stringstream sll;
	      stringstream siters;
              stringstream srunid;
	      sll<<ll; ssll = sll.str();
	      siters <<iters; ssiters = siters.str();
	      srunid <<runid; ssrunid = srunid.str();
	     // cout<<"runid = "<<ssrunid<<"  "<<ssll<<" "<<ssiters<<endl;
	      if(val==runid)
	      {
              //out1<<runid<<" "<<iters<<" ";
            //  cout<<val<<" "<<"runid = "<<ssrunid<<"  "<<ssll<<" "<<ssiters<<endl;
              break;
	      }
	}
    for (it1=layer_curr.begin(); it1 != layer_curr.end(); ++it1)
    {
	      ll1 = (*it1)->getLL();
	      double iters1 = (*it1)->getIters();
	      double runid1 = (*it1)->getRun();
              stringstream sll1;
              stringstream siters1;
              stringstream srunid1;
	      sll1<<ll1; ssll1 = sll1.str();
	      siters1 <<iters1; ssiters1 = siters1.str();
	      srunid1 <<runid1; ssrunid1 = srunid1.str();
	     // cout<<"runid = "<<ssrunid1<<"  "<<ssll1<<" "<<ssiters1<<endl;
	      if(val==runid1)
	      {
             // out1<<runid1<<" "<<iters1<<" ";
             // cout<<val<<" "<<"runid = "<<ssrunid1<<"  "<<ssll1<<" "<<ssiters1<<endl;
              break;
	      }
	      i++;
	}
	if(ll<=ll1)
	{
		//cout<<"Child  has a better LL so leave the child as it is."<<endl;
        out<<population_no<<"  "<<ssrunid1<<"  "<<ssll1<<" "<<ssiters1<<endl;
	}
	else
	{
       // cout<<"Replace child with parent."<<endl;
        out<<population_no<<"  "<<ssrunid<<"  "<<ssll<<" "<<ssiters<<endl;
		*it1=*it; //replacing the object in the layer_curr vector

	}
  }
  layer_parent.assign(layer_curr.begin(), layer_curr.end());

}

int main(int argc, char *argv[]) {
    generator.seed(static_cast<unsigned int>(std::time(0)));

    gibbsfile = argv[1];
    stringstream ss2(argv[2]);
    alarmfgloc = ss2.str();
    istringstream ss( argv[3] );
    ss >> pm;
    stringstream ss1( argv[4] );
    ss1 >> pc;
    const char* fname =argv[5];
    networkname = argv[6];
    emfile = argv[7];
    out.open(fname);

    time_t timer1, timer2;
    time(&timer1);
    clock_t tStart = clock();

    initializeEM();

    cout<<"population no"<<population_no<<endl;
    while(population_no<max_gen)
    {
          checkEMforTermination();
         if(population_no>0)
            replacementStrategy();
          doCrossover();
          doMutation();
          startEM();
          population_no=population_no+1;
          cout<<"population no"<<population_no<<endl;
   }

    time(&timer2);
    double t = difftime(timer2,timer1);
    double t_c = (clock() - tStart)/CLOCKS_PER_SEC;
    out<<"Time taken: "<<t<<"s"<<endl;
    out<<" Processor Time taken: "<<t_c<<"s"<<endl;
    out.close();
    return 0;
}


#include <stdlib.h>
#include <iostream>
#include <thread>
#include <string>
#include <cmath>

#include <inttypes.h>

#include <boost/tr1/random.hpp>
#include "utils.h"

#include "WordEmbed.h"

#define EXP_TABLE_SIZE 1000
#define MAX_EXP 6
#define MAX_CODE_LENGTH 40
#define MAX_SENTENCE_LENGTH 1000

class Word2Vec : public WordEmbed {
    private:
        unsigned int layer1_size;
        int window;
        real sample;
        int hs, negative;
        unsigned int num_threads;
        unsigned int iter;
        real alpha;
        unsigned int classes;
        int  cbow;
        std::string wordvector_file;

        int64_t word_count_actual;

        real starting_alpha;

        std::array<real,(EXP_TABLE_SIZE + 1)> expTable;

        clock_t start;

        std::vector<real> syn0, syn1, syn1neg;

        // Tables
        std::array<int, table_size> table;
    public:
        Word2Vec (
                std::string train_file, //Use text data from <file> to train the model
                std::string output_file = "", //Use <file> to save the resulting word vectors / word clusters
                unsigned int min_count = 5, //This will discard words that appear less than <int> times; default is 5
                int debug_mode = 2, //Set the debug mode (default = 2 = more info during training)
                int binary = 0, //Save the resulting vectors in binary moded; default is 0 (off)
                std::string save_vocab_file = "", //The vocabulary will be saved to <file>
                std::string read_vocab_file = "", //The vocabulary will be read from <file>, not constructed from the training data
                unsigned int layer1_size = 100, //Set size of word vectors; default is 100
                unsigned int window = 5, //Set max skip length between words; default is 5
                real sample = 1e-3, //Set threshold for occurrence of words. Those that appear with higher frequency in the training data will be randomly down-sampled; default is 1e-3, useful range is (0, 1e-5)
                int hs = 0, //Use Hierarchical Softmax; default is 0 (not used)
                int negative = 5, //Number of negative examples; default is 5, common values are 3 - 10 (0 = not used)
                unsigned int num_threads = 12, //Use <int> threads (default 12)
                int iter = 5, //Run more training iterations (default 5)
                real alpha = 0.05, //Set the starting learning rate; default is 0.025 for skip-gram and 0.05 for CBOW
                unsigned int classes = 0,//Output word classes rather than word vectors; default number of classes is 0 (vectors are written)
                int cbow = 0, //Use the continuous bag of words model; default is 1 (use 0 for skip-gram model)
                std::string wordvector_file = "" //Read the trained word vector file
                ):
            WordEmbed (train_file, output_file, min_count, debug_mode, binary,save_vocab_file,read_vocab_file),
            layer1_size (layer1_size),
            window (window),
            sample (sample),
            hs (hs),
            negative (negative),
            num_threads (num_threads),
            iter (iter),
            alpha (alpha),
            classes (classes),
            cbow (cbow),
            wordvector_file (wordvector_file)
        {
            min_reduce = 1,
            vocab_size = 0;
            train_words = 0;
            word_count_actual=0;
            file_size = 0;

            ready_to_train = false;
            if( CheckReady() ) ready_to_train = true;
        };

        void TrainModel ();

        void CreateBinaryTree ();

        void InitUnigramTable ();
        void InitNet ();
        void TrainModelThread (int tid);
};

//vocab cn word codelen point code

// Create binary Huffman tree using the word counts
// Frequent words will have short unique binary codes
void Word2Vec::CreateBinaryTree() {
    // Words are not sorted at the beginning
    int64_t a, b, i, min1i, min2i, pos1, pos2;
    int64_t point[MAX_CODE_LENGTH];
    char code[MAX_CODE_LENGTH];

    std::vector<int64_t> count;
    std::vector<int64_t> binary;
    std::vector<int64_t> parent_node;

    count.reserve(vocab_size * 2 + 1);
    binary.reserve(vocab_size * 2 + 1);
    parent_node.reserve(vocab_size * 2 +1);

    for(a = 0; a < vocab_size; a++) count[a] = vocab[a].cn; // count word frequency
    for(a = vocab_size; a < 2 * vocab_size; a++) count[a] = 1e15; // should be larger than any number in the vocabulary count
    pos1 = vocab_size - 1;
    pos2 = vocab_size; //

    // following algorithm constructs the Huffman tree by adding one node at a time
    for(a = 0; a < vocab_size - 1; a++) {
        // First, find two smallest nodes 'min1, min2'
        if(pos1 >= 0) {
            if(count[pos1] < count[pos2]) {
                min1i = pos1;
                pos1--;
            } else {
                min1i = pos2;
                pos2++;
            }
        } else {
            min1i = pos2;
            pos2++;
        }
        if(pos1 >= 0) {
            if(count[pos1] < count[pos2]) {
                min2i = pos1;
                pos1--;
            } else {
                min2i = pos2;
                pos2++;
            }
        } else {
            min2i = pos2;
            pos2++;
        }
        count[vocab_size + a] = count[min1i] + count[min2i]; // Creating parent node with summed frequency
        parent_node[min1i] = vocab_size + a;
        parent_node[min2i] = vocab_size + a; // Note that having a common parent node
        binary[min2i] = 1; // min2i will point to root node at the final state
    }
    // Now assign binary code to each vocabulary word
    for(a = 0; a < vocab_size; a++) {
        b = a;
        i = 0;
        while(1) {
            code[i] = binary[b];
            point[i] = b;
            i++;
            b = parent_node[b];
            if(b == vocab_size * 2 - 2) break;
        }
        vocab[a].codelen = i;
        vocab[a].point[0] = vocab_size - 2;
        for(b = 0; b < i; b++) {
            vocab[a].code[i - b - 1] = code[b];
            vocab[a].point[i - b] = point[b] - vocab_size;
        }
    }
}

// Begin of Learning Net

void Word2Vec::InitUnigramTable() {
    unsigned int i;
    double train_words_pow = 0;
    double d1, power = 0.75;
    int64_t sum = 0;
    for (unsigned int a = 0; a < vocab_size; a++) train_words_pow += pow(vocab[a].cn, power);
    for (unsigned int a = 0; a < vocab_size; a++) sum += vocab[a].cn;
    std::cout << sum << std::endl;
    i = 0;
    d1 = pow(vocab[i].cn, power) / train_words_pow;
    for (unsigned int a = 0; a < table_size; a++) {
        table[a] = i;
        if (a / (double)table_size > d1) {
            i++;
            d1 += pow(vocab[i].cn, power) / train_words_pow;
        }
        if (i >= vocab_size) i = vocab_size - 1;
    }
}

void Word2Vec::InitNet() {
    // The name EXP_TABLE is totally mis-guiding.
    for (int i = 0; i < EXP_TABLE_SIZE; i++) {
        expTable[i] = exp((i / (real)EXP_TABLE_SIZE * 2 - 1) * MAX_EXP);
        expTable[i] = expTable[i] / (expTable[i] + 1);
    }

    syn0.reserve((int64_t)vocab_size * layer1_size);
    if(hs) {
        syn1.reserve((int64_t)vocab_size * layer1_size);
        for(unsigned int a = 0; a < vocab_size; a++)
            for(unsigned int b = 0; b < layer1_size; b++)
                syn1[a * layer1_size + b] = 0;
    }
    if(negative > 0) {
        syn1neg.reserve((int64_t)vocab_size * layer1_size);
        for(unsigned int a = 0; a < vocab_size; a++)
            for(unsigned int b = 0; b < layer1_size; b++)
                syn1neg[a * layer1_size + b] = 0;
    }

    for(unsigned int a = 0; a < vocab_size; a++)
        for(unsigned int b = 0; b < layer1_size; b++)
            syn0[a * layer1_size + b] = (rand()/(double)RAND_MAX - 0.5) / layer1_size;

    //CreateBinaryTree();
}

void Word2Vec::TrainModelThread(int tid){
    int64_t a, b, d, word, last_word, sentence_length = 0, sentence_position = 0;
    int64_t word_count = 0, last_word_count = 0, sen[MAX_SENTENCE_LENGTH + 1];
    int64_t l1, l2, c, target, label, local_iter = iter;
    uint64_t next_random;
    real f, g;
    clock_t now;

    boost::mt19937 rand_engine_int;    // rand engine
    boost::uniform_int<> rand_int(0, table_size);    // set range.
    boost::variate_generator<boost::mt19937, boost::uniform_int<>> rand_gen_int(rand_engine_int, rand_int);

    boost::mt19937 rand_engine_double;  // rand engine
    boost::uniform_real<> rand_double(0.0, 1.0);
    boost::variate_generator<boost::mt19937, boost::uniform_real<>> rand_gen_double(rand_engine_double, rand_double);

    std::vector<real> neu1;
    std::vector<real> neu1e;

    neu1.reserve(layer1_size);
    neu1e.reserve(layer1_size);

    std::ifstream inFile(train_file, std::ifstream::in | std::ifstream::binary);
    inFile.seekg(file_size / (int64_t)num_threads * (int64_t)tid);
    while(1) {
        if(word_count - last_word_count > 10000) {
            word_count_actual += word_count - last_word_count;
            last_word_count = word_count;
            if((debug_mode > 1)) {
                now = clock();
                printf("%cAlpha: %f  Progress: %.2f%%  Words/thread/sec: %.2fk   ", 13, alpha,
                        word_count_actual / (real)(iter * train_words + 1) * 100,
                        word_count_actual / ((real)(now - start + 1) / (real)CLOCKS_PER_SEC * 1000));
                fflush(stdout);
            }
            alpha = starting_alpha * (1 - word_count_actual / (real)(iter * train_words + 1));
            if(alpha < starting_alpha * 0.0001) alpha = starting_alpha * 0.0001;
        }
        if(sentence_length == 0) {
            while(1) {
                word = ReadWordIndex(inFile);
                if(inFile.eof()) break;
                if(word == -1) continue;
                word_count++;
                if(word == 0) break;
                // The subsampling randomly discards frequent words while keeping the ranking same
                if(sample > 0) {
                    real ran = (sqrt(vocab[word].cn / (sample * train_words)) + 1) * (sample * train_words) / vocab[word].cn;
                    if(ran < rand_gen_double()) continue;
                }
                sen[sentence_length] = word;
                sentence_length++;
                if(sentence_length >= MAX_SENTENCE_LENGTH) break;
            }
            sentence_position = 0;
        }
        if(inFile.eof() || (word_count > train_words / num_threads )) {
            word_count_actual += word_count - last_word_count;
            local_iter--;
            if(local_iter == 0) break;
            word_count = 0;
            last_word_count = 0;
            sentence_length = 0;
	    if(inFile.eof()) {
	      inFile.clear();
	      inFile.seekg(file_size / (int64_t)num_threads * (int64_t)tid);
	    }
	    else {
	      inFile.seekg(file_size / (int64_t)num_threads * (int64_t)tid);
	    }
            continue;
        }
        word = sen[sentence_position];
        if(word == -1) continue;
        // Network Initialization
        for(c = 0; c < layer1_size; c++) neu1[c] = 0;
        for(c = 0; c < layer1_size; c++) neu1e[c] = 0;

        b = rand_gen_int() % window;
        // Skip-gram
        {
            for(a = b; a < window * 2 + 1 - b; a++) if(a != window) {
                c = sentence_position - window + a;
                if(c < 0) continue;
                if(c >= sentence_length) continue;
                last_word = sen[c];
                if(last_word == -1) continue;
                l1 = last_word * layer1_size;
                // Hierarchical Softmax
                if(hs) for(d = 0; d < vocab[word].codelen; d++) {
                    f = 0;
                    l2 = vocab[word].point[d] * layer1_size;
                    // Propagate hidden -> output
                    for(c = 0; c < layer1_size; c++) f += syn0[c + l1] * syn1[c + l2];
                    if(f <= -MAX_EXP) continue;
                    else if(f >= MAX_EXP) continue;
                    else f = expTable[(int)((f + MAX_EXP) * (EXP_TABLE_SIZE / MAX_EXP / 2))];
                    // g is the gradient multiplied by the learning rate
                    g = (1 - vocab[word].code[d] - f) * alpha;
                    // Backpropagate errors output -> hidden
                    for(c = 0; c < layer1_size; c++) neu1e[c] += g * syn1[c + l2];
                    // Learn weights hidden -> output
                    for(c = 0; c < layer1_size; c++) syn1[c + l2] += g * syn0[c +l1];
                }
                // Negative Sampling
                if(negative > 0) for (d = 0; d < negative + 1 ; d++) {
                    if(d == 0) {
		      target = word;
		      label = 1;
                    } else {
		      next_random = rand_gen_int();
		      target = table[next_random % table_size];
		      if(target == 0) target = next_random % (vocab_size - 1) + 1;
		      if(target == word) continue;
		      label = 0;
                    }
                    l2 = target * layer1_size;
                    f = 0;
                    for(c = 0; c < layer1_size; c++) f += syn0[c + l1] * syn1neg[c + l2];
                    if(f > MAX_EXP) g = (label - 1) * alpha;
                    else if(f < -MAX_EXP) g = (label - 0) * alpha;
                    else g = (label - expTable[(int)((f + MAX_EXP) * (EXP_TABLE_SIZE / MAX_EXP / 2))]) * alpha;
                    for (c = 0; c < layer1_size; c++) neu1e[c] += g * syn1neg[c + l2];
                    for (c = 0; c < layer1_size; c++) syn1neg[c + l2] += g * syn0[c + l1];
                }
                // Learn weights input -> hidden
                for(c = 0; c < layer1_size; c++) syn0[c + l1] += neu1e[c];
            }
        }
        sentence_position++;
        if(sentence_position >= sentence_length) {
            sentence_length = 0;
            continue;
        }
    }
    inFile.close();
}

void Word2Vec::TrainModel() {
    std::ofstream outFile;

    std::vector<std::thread> th;
    starting_alpha = alpha;

    std::cout << "Starting training using file " << train_file << std::endl;
    if(read_vocab_file != "") ReadVocab();
    else ExtractVocabFromTrainFile();
    if(save_vocab_file != "") SaveVocab();
    if(output_file[0] == 0) return;

    // Initialization
    InitNet();
    if(negative > 0)
        InitUnigramTable();

    start = clock(); // start to measure time

    for(int i = 0; i < num_threads; i++) {
      th.push_back(std::thread(&Word2Vec::TrainModelThread, this, i));
    }

    for(auto &t : th) {
      t.join();
    }

    //TrainModelThread(); // For the single thread

    outFile.open(output_file, std::ofstream::out | std::ofstream::binary);
    if(classes == 0) {
        outFile << vocab_size << " " << layer1_size << std::endl;
        for(unsigned int a = 0; a < vocab_size; a++) {
            outFile << vocab[a].word << " ";
            if(binary) for(unsigned int b = 0; b < layer1_size; b++) outFile << syn0[a * layer1_size + b] << " ";
            else for(unsigned int b = 0; b < layer1_size; b++) outFile << syn0[a * layer1_size + b] << " ";
            outFile << std::endl;
        }
    } else {
        // Run K-means on the word vectors
        unsigned int clcn = classes, iter = 10, closeid;
        real closev, x;

        std::vector<int> centcn, cl;
        std::vector<real> cent;

        centcn.reserve(classes);
        cl.reserve(vocab_size);
        cent.reserve(classes * layer1_size);

        for (unsigned int a = 0; a < vocab_size; a++) cl[a] = a % clcn;
        for (unsigned int a = 0; a < iter; a++) {
            for (unsigned int b = 0; b < clcn * layer1_size; b++) cent[b] = 0;
            for (unsigned int b = 0; b < clcn; b++) centcn[b] = 1;
            for (unsigned int c = 0; c < vocab_size; c++) {
                for (unsigned int d = 0; d < layer1_size; d++) cent[layer1_size * cl[c] + d] += syn0[c * layer1_size + d];
                centcn[cl[c]]++;
            }
            for (unsigned int b = 0; b < clcn; b++) {
                closev = 0;
                for (unsigned int c = 0; c < layer1_size; c++) {
                    cent[layer1_size * b + c] /= centcn[b];
                    closev += cent[layer1_size * b + c] * cent[layer1_size * b + c];
                }
                closev = sqrt(closev);
                for (unsigned int c = 0; c < layer1_size; c++) cent[layer1_size * b + c] /= closev;
            }
            for (unsigned int c = 0; c < vocab_size; c++) {
                closev = -10;
                closeid = 0;
                for (unsigned int d = 0; d < clcn; d++) {
                    x = 0;
                    for (unsigned int b = 0; b < layer1_size; b++) x += cent[layer1_size * d + b] * syn0[c * layer1_size + b];
                    if (x > closev) {
                        closev = x;
                        closeid = d;
                    }
                }
                cl[c] = closeid;
            }
        }
        // Save the K-means classes
        for (unsigned int a = 0; a < vocab_size; a++) outFile << vocab[a].word << "    " << cl[a] << std::endl;

    }
    outFile.close();
}

// End of Learning Net

// main function arguments

void printHelp() {
    std::cout << "c++ word2vector implementation \n\n";
    std::cout << "Options:\n";
    std::cout << "Parameters for training:\n";
    std::cout << "\t-train <file>\n";
    std::cout << "\t\tUse text data from <file> to train the model\n";
    std::cout << "\t-output <file>\n";
    std::cout << "\t\tUse <file> to save the resulting word vectors / word clusters\n";
    std::cout << "\t-size <int>\n";
    std::cout << "\t\tSet size of word vectors; default is 100\n";
    std::cout << "\t-window <int>\n";
    std::cout << "\t\tSet max skip length between words; default is 5\n";
    std::cout << "\t-sample <float>\n";
    std::cout << "\t\tSet threshold for occurrence of words. Those that appear with higher frequency in the training data\n";
    std::cout << "\t\twill be randomly down-sampled; default is 1e-3, useful range is (0, 1e-5)\n";
    std::cout << "\t-hs <int>\n";
    std::cout << "\t\tUse Hierarchical Softmax; default is 0 (not used)\n";
    std::cout << "\t-negative <int>\n";
    std::cout << "\t\tNumber of negative examples; default is 5, common values are 3 - 10 (0 = not used)\n";
    std::cout << "\t-threads <int>\n";
    std::cout << "\t\tUse <int> threads (default 12)\n";
    std::cout << "\t-iter <int>\n";
    std::cout << "\t\tRun more training iterations (default 5)\n";
    std::cout << "\t-min-count <int>\n";
    std::cout << "\t\tThis will discard words that appear less than <int> times; default is 5\n";
    std::cout << "\t-alpha <float>\n";
    std::cout << "\t\tSet the starting learning rate; default is 0.025 for skip-gram and 0.05 for CBOW\n";
    std::cout << "\t-classes <int>\n";
    std::cout << "\t\tOutput word classes rather than word vectors; default number of classes is 0 (vectors are written)\n";
    std::cout << "\t-debug <int>\n";
    std::cout << "\t\tSet the debug mode (default = 2 = more info during training)\n";
    std::cout << "\t-binary <int>\n";
    std::cout << "\t\tSave the resulting vectors in binary moded; default is 0 (off)\n";
    std::cout << "\t-save-vocab <file>\n";
    std::cout << "\t\tThe vocabulary will be saved to <file>\n";
    std::cout << "\t-read-vocab <file>\n";
    std::cout << "\t\tThe vocabulary will be read from <file>, not constructed from the training data\n";
    std::cout << "\t-cbow <int>\n";
    std::cout << "\t\tUse the continuous bag of words model; default is 1 (use 0 for skip-gram model)\n";
    std::cout << "\t-wordvector <file>\n";
    std::cout << "\t\tRead the trained word vector file\n";
    std::cout << "\nExamples:\n";
    std::cout << "./word2vec -train data.txt -output vec.txt -size 200 -window 5 -sample 1e-4 -negative 5 -hs 0 -binary 0 -cbow 1 -iter 3\n\n";

}

int argpos(const char *str, int argc, char **argv) {
    std::string s_str(str);
    std::vector<std::string> s_argv(argv,argv+argc);

    int i=0;
    while (i<argc) {
        if(s_str == s_argv[i]) break;
        i++;
    }
    if(i == argc) i=0;

    return i;
}

Word2Vec *arg_to_w2v(int argc, char **argv) {
    int i;

    int layer1_size = 100;
    std::string train_file ="", save_vocab_file = "", read_vocab_file = "";
    int debug_mode =2, binary = 0, cbow = 0;
    real alpha = 0.05;
    std::string output_file = "";
    int window = 5;
    real sample = 1e-3;
    int hs = 0, negative = 5, num_threads = 12;
    int64_t iter = 5;
    int min_count = 5;
    int64_t classes = 0;
    std::string wordvector_file = "";

    if ((i = argpos("-size", argc, argv)) > 0) layer1_size = atoi(argv[i + 1]);
    if ((i = argpos("-train", argc, argv)) > 0) train_file = argv[i + 1];
    if ((i = argpos("-save-vocab", argc, argv)) > 0) save_vocab_file = argv[i + 1];
    if ((i = argpos("-read-vocab", argc, argv)) > 0) read_vocab_file = argv[i + 1];
    if ((i = argpos("-debug", argc, argv)) > 0) debug_mode = atoi(argv[i + 1]);
    if ((i = argpos("-binary", argc, argv)) > 0) binary = atoi(argv[i + 1]);
    if ((i = argpos("-cbow", argc, argv)) > 0) cbow = atoi(argv[i + 1]);
    if (cbow) alpha = 0.05; else alpha = 0.025;
    if ((i = argpos("-alpha", argc, argv)) > 0) alpha = atof(argv[i + 1]);
    if ((i = argpos("-output", argc, argv)) > 0) output_file = argv[i + 1];
    if ((i = argpos("-window", argc, argv)) > 0) window = atoi(argv[i + 1]);
    if ((i = argpos("-sample", argc, argv)) > 0) sample = atof(argv[i + 1]);
    if ((i = argpos("-hs", argc, argv)) > 0) hs = atoi(argv[i + 1]);
    if ((i = argpos("-negative", argc, argv)) > 0) negative = atoi(argv[i + 1]);
    if ((i = argpos("-threads", argc, argv)) > 0) num_threads = atoi(argv[i + 1]);
    if ((i = argpos("-iter", argc, argv)) > 0) iter = atoi(argv[i + 1]);
    if ((i = argpos("-min-count", argc, argv)) > 0) min_count = atoi(argv[i + 1]);
    if ((i = argpos("-classes", argc, argv)) > 0) classes = atoi(argv[i + 1]);
    if ((i = argpos("-wordvector", argc, argv)) > 0) wordvector_file = argv[i + 1];

    Word2Vec *w2v = new Word2Vec (
                train_file,
                output_file,
                min_count,
                debug_mode,
                binary,
                save_vocab_file,
                read_vocab_file,
                layer1_size,
                window,
                sample,
                hs,
                negative,
                num_threads,
                iter,
                alpha,
                classes,
                cbow,
                wordvector_file );

    return w2v;
}

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

    std::srand(11596521);
    Word2Vec *w2v;

    if(argc < 2) { printHelp(); return 1; }
    else {
        w2v = arg_to_w2v(argc, argv);
    }

    w2v->TrainModel();

    return 0;
}


/*
 * Aircraft.cpp
 *
 *  Created on: 11-Jul-2008
 *      Author: malem303
 */

#include "Aircraft.h"
#include "Constants.h"
#include "Utils.h"
#include "ControlRigging.h"
#include "AircraftFactory.h"

#include <math.h>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <list>

Aircraft::Aircraft(bool usePhi) : usePhi(usePhi)
{
	atmosphericConditions = new AtmosphericConditions();
	centerOfGravity = AircraftFactory::createCenterOfGravity(Utils::toVec3(10, 0, 5), Constants::ZERO_VEC_3, Constants::ZERO_VEC_3, Constants::ZERO_VEC_3);
    fuselage = AircraftFactory::createFuselage();
	horizontalStabilizer = AircraftFactory::createHorizontalStabilizer();
	verticalStabilizer = AircraftFactory::createVerticalStabilizer();
	mainRotor = AircraftFactory::createMainRotor();
	tailRotor = AircraftFactory::createTailRotor();
}

Aircraft::~Aircraft() {

	delete atmosphericConditions;
	delete fuselage;
	delete mainRotor;
	delete tailRotor;
	delete horizontalStabilizer;
	delete verticalStabilizer;
	delete centerOfGravity;
}
void Aircraft::updateControls(double collectivePitchStickPosition, double longitudinalSwashplateStickPosition, double lateralSwashplateStickPosition, double tailRotorCollectivePitchStick)
{
	mainRotor->setCollectivePitchStickPosition(collectivePitchStickPosition);
	mainRotor->setLongitudinalSwashplateStickPosition(longitudinalSwashplateStickPosition);
	mainRotor->setLateralSwashplateStickPosition(lateralSwashplateStickPosition);
	tailRotor->setCollectivePitchStickPosition(tailRotorCollectivePitchStick);
}

void Aircraft::updatePhysics()
{

	updateAtmosphericConditions();

	centerOfGravity->resetForces();
	centerOfGravity->updateVelocities();

	fuselage->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	horizontalStabilizer->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	verticalStabilizer->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	mainRotor->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	tailRotor->updatePhysics(*centerOfGravity, atmosphericConditions->getDensity());

	updateWashVelocities();

	centerOfGravity->addGravityForces();

}

void Aircraft::updateWashVelocities()
{
	// TODO : find a less hard-coded way to compute that

	// Mutual influences :

	double density = atmosphericConditions->getDensity();
	vector<double> externalWashAgularVelocity = Constants::ZERO_VEC_3;
	vector<double> externalWashVelocity(3);

	// mainRotor on fuselage
	externalWashVelocity = mainRotor->computeExternalWashInfluence(fuselage, density);
	fuselage->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

	// fuselage on mainRotor
	vector<double> fuselageSelfInducedWashVelocity = fuselage->computeSelfInducedWashVelocity(density);

	externalWashVelocity = 	Utils::toVec3(0.0, 0.0, fuselageSelfInducedWashVelocity(2));
	mainRotor->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

	// mainRotor and fuselage on horizontalStabilizer
	externalWashVelocity = mainRotor->computeExternalWashInfluence(horizontalStabilizer, density) + Utils::toVec3(fuselageSelfInducedWashVelocity(0), 0.0, 0.0);
	horizontalStabilizer->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

	// verticalStabilizer on tailRotor
	vector<double> verticalStabilizerSelfInducedVelocity = verticalStabilizer->computeSelfInducedWashVelocity(density);

	externalWashVelocity = 	Utils::toVec3(0.0, verticalStabilizerSelfInducedVelocity(1), 0.0);
	tailRotor->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

	// tailRotor on verticalStabilizer
	vector<double> tailRotorSelfInducedWashVelocity = tailRotor->computeSelfInducedWashVelocity(density);

	externalWashVelocity = 	Utils::toVec3(0.0, tailRotorSelfInducedWashVelocity(1), 0.0);
	verticalStabilizer->updtateWashVelocities(density, externalWashVelocity, externalWashAgularVelocity);

}

void Aircraft::updateAtmosphericConditions()
{
	// TODO: this is a stub!
	// The atmospheric condition change according to meteo (pressure, wind, temperature) and altitude, see Dreier 2007, chap 5
}

void Aircraft::printForces()
{
	using namespace std;

	cout << "----------------------------" << endl;

	cout << "fext" << centerOfGravity->getForcesIELA() << endl;
	cout << "fuse" << fuselage->getForcesIELA() << endl;
	cout << "wing" << horizontalStabilizer->getForcesIELA() << endl;
	cout << "fin " << verticalStabilizer->getForcesIELA() << endl;
	cout << "main " << mainRotor->getForcesIELA() << endl;
	cout << "tail " << tailRotor->getForcesIELA() << endl;

	cout << "----------------------------" << endl;
}

void Aircraft::printWashes()
{
	using namespace std;
	cout << "----------Wash velocities ------------------" << endl;

	cout << "CoG" << centerOfGravity->getWashVelocityIELA() << endl;
	cout << "fuse" << fuselage->getWashVelocityIELA() << endl;
	cout << "wing" << horizontalStabilizer->getWashVelocityIELA() << endl;
	cout << "fin " << verticalStabilizer->getWashVelocityIELA() << endl;
	cout << "main " << mainRotor->getWashVelocityIELA() << endl;
	cout << "tail " << tailRotor->getWashVelocityIELA() << endl;

	cout << "----------------------------" << endl;
}
vector<double> Aircraft::buildTrimVector()
{
	vector<double> trimVector(6);

	trimVector(0) = mainRotor->getCollectivePitch();
	trimVector(1) = mainRotor->getLongitudinalSwashplateAngle();
	trimVector(2) = mainRotor->getLateralSwashplateAngle();
	trimVector(3) = tailRotor->getCollectivePitch();

	vector<double> angularPositionEarthAxes = centerOfGravity->getAngularPositionEarthAxes();
	if(usePhi)
		trimVector(4) =  angularPositionEarthAxes(0);
	else
		trimVector(4) =  angularPositionEarthAxes(2);

	trimVector(5) =  angularPositionEarthAxes(1);

	return trimVector;
}

void Aircraft::applyTrimVector(vector<double> trimVector)
{
	mainRotor->setCollectivePitch(trimVector(0));
	mainRotor->setLongitudinalSwashplateAngle(trimVector(1));
	mainRotor->setLateralSwashplateAngle(trimVector(2));
	tailRotor->setCollectivePitch(trimVector(3));

	vector<double> angularPositionEarthAxes = centerOfGravity->getAngularPositionEarthAxes();
	if(usePhi)
		angularPositionEarthAxes(0) = trimVector(4);
	else
		angularPositionEarthAxes(2) = trimVector(4);

	angularPositionEarthAxes(1) = trimVector(5);

	updatePhysics();
	centerOfGravity->updateDerivatives();

}

vector<double> Aircraft::buildAccelerationVector()
{
	vector<double> invertialVelocityDerivative = centerOfGravity->getInertialVelocityDerivativeCenterOfGravity();
	vector<double> invertialAngularVelocityDerivative = centerOfGravity->getInertialAngularVelocityDerivativeCenterOfGravity();

	vector<double> accelerationVector(6);

	for(int i = 0; i < 3; i++)
	{
		accelerationVector(i) = invertialVelocityDerivative(i);
		accelerationVector(i + 3) = invertialAngularVelocityDerivative(i);
	}
	return accelerationVector;

}

double Aircraft::jacobianTrim(vector<double> initialPositionEarthAxes, vector<double> initialAngularPositionEarthAxes, vector<double> initialVelocityEarthAxes, vector<double> initialAngularVelocityEarthAxes)
{
	centerOfGravity->setPositionEarthAxes(initialPositionEarthAxes);
	centerOfGravity->setAngularPositionEarthAxes(initialAngularPositionEarthAxes);
	centerOfGravity->setVelocityEarthAxes(initialVelocityEarthAxes);
	centerOfGravity->setAngularVelocityEarthAxes(initialAngularVelocityEarthAxes);

	vector<double> currentTrimVector = buildTrimVector();
	vector<double> perturbedTrimVector = currentTrimVector;
	applyTrimVector(perturbedTrimVector);

	vector<double> currentAccelerationVector = buildAccelerationVector();
	vector<double> perturbedAccelerationVector = currentAccelerationVector;
	std::cout << "acc : " << currentAccelerationVector << std::endl;

	double accelerationVectorNorm = Utils::vectorNorm(perturbedAccelerationVector);

	matrix<double> jacobianMatrix(6,6);
	matrix<double> accelerationMatrixMinusPerturbation(6,6);
	matrix<double> accelerationMatrixPlusPerturbation(6,6);

	int numberOfIterationLeft = 20;
	const double PERTURBATION_SIZE = 0.01;
	vector<double> trimGradient = Constants::ZERO_VEC_3;

	while (accelerationVectorNorm > 0.001 && numberOfIterationLeft > 0)
	{

		for(int jacobianRowIndex = 0; jacobianRowIndex < 6; jacobianRowIndex++)
		{
			for(int jacobianColumnIndex = 0; jacobianColumnIndex < 6; jacobianColumnIndex++)
			{
				perturbedTrimVector(jacobianColumnIndex) = currentTrimVector(jacobianColumnIndex) + PERTURBATION_SIZE;
				applyTrimVector(perturbedTrimVector);
				printForces();
				printWashes();
				perturbedAccelerationVector = buildAccelerationVector();
				accelerationMatrixMinusPerturbation(jacobianRowIndex, jacobianColumnIndex) = perturbedAccelerationVector(jacobianRowIndex);

				perturbedTrimVector(jacobianColumnIndex) = currentTrimVector(jacobianColumnIndex) - PERTURBATION_SIZE;
				applyTrimVector(perturbedTrimVector);
				printForces();
				printWashes();
				perturbedAccelerationVector = buildAccelerationVector();
				accelerationMatrixPlusPerturbation(jacobianRowIndex, jacobianColumnIndex) = perturbedAccelerationVector(jacobianRowIndex);

//				applyTrimVector(currentTrimVector);
				perturbedTrimVector = currentTrimVector;

			}
		}

		jacobianMatrix = (accelerationMatrixPlusPerturbation - accelerationMatrixMinusPerturbation) / (2 * PERTURBATION_SIZE);

		using namespace std;

//		cout << "jac : " << jacobianMatrix << endl;

		trimGradient = prod(Utils::invertMatrix(jacobianMatrix), currentAccelerationVector);

		currentTrimVector -= trimGradient;

		applyTrimVector(currentTrimVector);
		currentAccelerationVector = buildAccelerationVector();

		cout << "grad : " << trimGradient << endl;
		cout << "trim : " << perturbedTrimVector << endl;
		cout << "acc : " << currentAccelerationVector << endl;

		numberOfIterationLeft--;
		accelerationVectorNorm = Utils::vectorNorm(currentAccelerationVector);

		cout << "norm = " << accelerationVectorNorm << endl;

	}

	return accelerationVectorNorm;
}


/*
Copyright Benjamin Perret ESIEE Paris 2017

benjamin.perret@esiee.fr

This software is a computer program whose purpose is to compute isotonic/
monotonic regression on tree ordering.

This software is governed by the CeCILL-B license under French law and
abiding by the rules of distribution of free software.  You can  use,
modify and/ or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and  rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty  and the software's author,  the holder of the
economic rights,  and the successive licensors  have only  limited
liability.

In this respect, the user's attention is drawn to the risks associated
with loading,  using,  modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean  that it is complicated to manipulate,  and  that  also
therefore means  that it is reserved for developers  and  experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and,  more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL-B license and that you accept its terms.

 */

#include <vector>
#include <boost/heap/binomial_heap.hpp>

#include "morto.h"

namespace morto {

	using namespace std;

	void testOrDie(bool b, string msg)
	{
		if (!b)
			throw MortoException{ msg };
	}

	/**
	 * Generic UnionFind with path compression and union by rank on integer indicis.
	 */
	class UnionFind {
	private:
		vector<size_t> parent;
		vector<int> rank;

	public:
		/**
		Create a new family of size singleton sets.
		*/
		UnionFind(size_t size = 0)
		{
			if (size > 0)
			{
				for (size_t i = 0; i < size; ++i)
					parent.push_back(i);
				rank.resize(size, 0);
			}
		}

		/**
		Create a new singleton and return the index of its (single and canonical) node.
		Complexity O(1) (amortized)
		*/
		/*
		size_t createSet()
		{
			size_t size = parent.size();
			parent.push_back(size);
			rank.push_back(0);
			return size;
		}
		*/

		/**
		Finds and return the canonical node of the given nodeIndex and performs path compression.
		Complexity O(a(n)) (amortized)
		*/
		size_t findCanonical(size_t nodeIndex)
		{
			size_t save = nodeIndex;
			while (parent[nodeIndex] != nodeIndex)
				nodeIndex = parent[nodeIndex];
			while (parent[save] != save)
			{
				size_t tmp = save;
				save = parent[save];
				parent[tmp] = nodeIndex;
			}
			return nodeIndex;
		}

		/**
		Performs the union of two sets (given by their canonical nodes) with rank optimisation.
		Returns the index of the canonical node of the union.
		Complexity O(1)
		*/
		auto setUnion(size_t canonicalNodeIndex1, size_t canonicalNodeIndex2)
		{
			if (rank[canonicalNodeIndex1] > rank[canonicalNodeIndex2])
			{
				swap(canonicalNodeIndex1, canonicalNodeIndex2);
			}
			else if (rank[canonicalNodeIndex1] == rank[canonicalNodeIndex2])
			{
				rank[canonicalNodeIndex2]++;
			}
			parent[canonicalNodeIndex1] = canonicalNodeIndex2;
			return make_pair(canonicalNodeIndex2, canonicalNodeIndex1);
		}
	};

	//forward declaration
	struct HeapNode;

	using heap_t = boost::heap::binomial_heap<HeapNode>;

	struct HeapNode {
		double value;
		size_t nodeIndex;
	};

	bool operator<(const HeapNode & lo, const HeapNode & ro)
	{
		return lo.value < ro.value;
	}

	/**
	* Internal tree node, merges information on nodes, blocks and heaps...
	*/
	struct TreeNode {
		/**
		 * Index of the parent node
		 */
		size_t parent = 0;

		/**
		 * Weight
		 */
		double weight = 1;

		/**
		 * Input value
		 */
		double value = 0;

		/**
		 * Handle on the HeapNode associated to this node in the heap of its parent
		 */
		heap_t::handle_type handle;

		/**
		 * Heap of HeapNodes associated to every child (only valid if the node is the canonical node associated to its block)
		 */
		heap_t heap;

		/**
		 * Sum of weights of the nodes in the block (only valid if the node is the canonical node associated to its block)
		 */
		double blockTotalWeight = 1;

		/**
		* Weighted sum of the nodes in the block (only valid if the node is the canonical node associated to its block)
		*/
		double blockWeightedSum = 0;

		/**
		 * \brief Average weight of the block (only valid if the node is the canonical node associated to its block)
		 * \return Average weight of the block
		 */
		double getAverageWeight() const {
			return blockWeightedSum / blockTotalWeight;
		}

		void mergeWith(TreeNode * node)
		{
			heap.merge(node->heap);
			blockTotalWeight += node->blockTotalWeight;
			blockWeightedSum += node->blockWeightedSum;

		}
	};

	/**
	 * Internal class to represent the tree.
	 * Essentially a constructor.
	 */
	class Tree {
	private:
		vector<TreeNode> nodes;

	public:
		/**
		 * \brief Create a new tree from agnostic representation.
		 * \param parents Parent relation between nodes.
		 * Preconditions :
		 *		- parents.size() > 0
		 *		- "node i is the parent of node j" <=> parents[j] == i
		 *		- "node i is the root" <=> parents[i] == i
		 *		- nodes are in topological order : parents[i] >= i
		 * \param values Input data associated to nodes.
		 * The observed value at node i is values[i].
		 * Preconditions :
		 *		- values.size() == parents.size()
		 * \param weights Weights associated to nodes.
		 * If weights.size() == 0 , every node is assumed to have the same weight.
		 * Else, the weight of node i is weights[i].
		 * Preconditions :
		 *		- weights.size() == parents.size() || weights.empty()
		 */
		Tree(const vector<size_t> & parents, const vector<double> & values, const vector<double> & weights)
		{

			size_t nnodes = parents.size();
			nodes.resize(nnodes);

			if (!weights.empty())
			{
				for (size_t i = 0; i < nnodes; ++i)
				{
					nodes[i].weight = weights[i];
					nodes[i].blockTotalWeight = weights[i];
				}
			}

			for (size_t i = 0; i < nnodes; ++i)
			{
				nodes[i].parent = parents[i];
				nodes[i].value = values[i];
				nodes[i].blockWeightedSum = nodes[i].weight * nodes[i].value;
				if (parents[i] != i)
				{
					//nodes[parents[i]].children.push_back(i);
					nodes[i].handle = nodes[parents[i]].heap.push({ values[i], i });
				}

			}
		}

		/**
		 * \brief Get the node of index i
		 * \param i A valid node index (Precondition 0 <= i < tree.size())
		 * \return A reference to node i
		 */
		TreeNode * getNode(size_t i)
		{
			return &nodes[i];
		}

		/**
		 * \brief Number of nodes in the tree
		 * \return Number of nodes in the tree
		 */
		size_t size() const
		{
			return nodes.size();
		}
	};

	/**
	 * \brief Compute isotonic regression on the given tree.
	 * \param tree Input Tree, properly initialized.
	 * \return vector<double> containing the regression values for each node
	 */
	vector<double> IRT_BIN(Tree & tree)
	{
		size_t nnodes = tree.size();

		UnionFind uf(nnodes); // Block maintenance

		for (size_t i = 0; i < nnodes; ++i) // from leaves to root
		{

			size_t canonicali = uf.findCanonical(i); // index of the representative tree node for the block containing node i
			TreeNode * canonicalNode = tree.getNode(canonicali);

			// while we have violators among our children, fuse current block with the block of the most important violator
			while (!canonicalNode->heap.empty() && canonicalNode->getAverageWeight() < canonicalNode->heap.top().value)
			{
				size_t k = canonicalNode->heap.top().nodeIndex; // index of violator child k
				canonicalNode->heap.pop();

				size_t canonicalk = uf.findCanonical(k); // index of the representative tree node for the block containing node k

				auto resUnion = uf.setUnion(canonicali, canonicalk); // merge blocks containing i and k
				auto newCanonicalIndex = resUnion.first; // index of the new representative node (either i or k)
				auto otherIndex = resUnion.second; // index of the node that is not representative (either i or k)

				// update local variable after merge
				canonicalNode = tree.getNode(newCanonicalIndex);
				canonicali = newCanonicalIndex;

				// merge block information
				TreeNode * otherNode = tree.getNode(otherIndex);
				canonicalNode->mergeWith(otherNode);

			}

			// update parent heap to reflect the new weight of the block containing node i
			TreeNode * currentNode = tree.getNode(i);
			if (currentNode->parent != i)
			{
				TreeNode * node = tree.getNode(currentNode->parent);
				node->heap.update(currentNode->handle, { canonicalNode->getAverageWeight(), i });
			}
		}

		vector<double> res;
		for (size_t i = 0; i < nnodes; ++i)
		{
			res.push_back(tree.getNode(uf.findCanonical(i))->getAverageWeight());
		}
		return res;
	}

	bool testRelationDomain(const vector<size_t> & parents)
	{
		size_t maxV = parents.size() - 1;
		for (size_t i = 0; i < parents.size(); ++i)
		{
			if (parents[i]<0 || parents[i]>maxV) // size_t should be unsigned but well
				return false;
		}
		return true;
	}

	bool testTopologicalOrder(const vector<size_t> & parents)
	{
		for (size_t i = 0; i < parents.size(); ++i)
		{
			if (parents[i] < i)
				return false;
		}
		return true;
	}

    bool testPositiveWeights(const vector<double> & weights)
    {
        for (auto v: weights)
            if(v<=0)
                return false;
        return true;
    }
	/*bool testSingleRoot(const vector<int> & parents)
	{
		int nparents = 0;
		for (int i = 0; i<parents.size(); ++i)
		{
			if (parents[i] == i)
			{
				nparents++;
				if (nparents > 1)
					return false;
			}
		}
		return true;
	}*/

	vector<double> monotonicRegressionOnTree(const vector<size_t> & parents, const vector<double> & values, const vector<double> & weights)
	{
		testOrDie(parents.size() > 0, "Size of parents must be strictily greater than 0.");
		testOrDie(values.size() == parents.size(), "values and parents must have the same size.");
		testOrDie(weights.empty() || weights.size() == parents.size(), "weights must either be empty or have the same size as parents.");
		testOrDie(testRelationDomain(parents), "The domain of the parents relation is invalid: there exists i such that parents[i]<0 or parents[i]>=parents.size()");
		testOrDie(testTopologicalOrder(parents), "The parent relation is not given in a topological order: there exists i such that parents[i]<i");
		if(weights.size()>0)
			testOrDie(testPositiveWeights(weights),"Weights must be strictly positive.");
        //testOrDie(testSingleRoot(parents), "The parent relationcontains more than one root : there exists several i such that parents[i]==i");

		Tree tree = Tree(parents, values, weights);
		return IRT_BIN(tree);
	}

} // namespace morto
/*

using namespace morto;
using namespace std;

template<typename T>
ostream & operator<<(ostream & o, const vector<T> & vec)
{
	o << "{";
	for (size_t i = 0; i < vec.size() - 1; ++i)
		o << vec[i] << ", ";
	if (!vec.empty())
		o << vec.back();
	o << "}";
	return o;
}

void testIncreasing()
{
	vector<size_t> parents = { 4,4,6,6,7,7,7,7 };
	//vector<double> values = { 0,1,2,3,4,5,6,7 };
	vector<double> values = { 13,14,6,8,11,7,5,10 };
	vector<double> result = monotonicRegressionOnTree(parents, values);
	cout << result << endl;
}

int main(int argc, char ** argv)
{
	testIncreasing();
	getchar();
	return 0;
}
*/