// *****************************************************************************
//  Copyright (C): Christian Nastasi, 2011                                    
//  Author(s): Christian Nastasi                                              
//  Developed at the:                                                         
//  - Retis LAB, TeCIP Institute, Scuola Superiore Sant'Anna (Pisa)           
//  - School of Elec Eng and Computer Science, Queen Mary University (London) 
//  This file is distributed under the terms in the attached LICENSE_2 file.  
//  If you do not find this file, copies can be found by writing to:          
//  - c.nastasi@sssup.it                                                      
//  - nastasichr@gmail.com                                                    
//  - andrea.cavallaro@eecs.qmul.ac.uk                                        
// *****************************************************************************

#ifndef __WiseCameraDPF_h__
#define __WiseCameraDPF_h__ 

#include <vector>
#include <map>
#include <sstream>
#include <fstream>
#include "WiseCameraSimplePeriodicTracker.h"
#include "WiseCameraDPFMessage_custom.h"
#include "wise_utils_gmm.h"

class WiseCameraDPF : public WiseCameraSimplePeriodicTracker {
private:
	struct state_t {
		double x;
		double y;
		inline std::string str() const  
		{
			std::ostringstream os;
			os << "(" << x << ", " << y << ")";
			return os.str();
		}
	};
	struct particle_set_t {
		std::vector<state_t> particles;
		std::vector<double> weights;
	};
	struct node_ctrl_t {
		bool initialized;
		bool initialized_once;
		bool last_node;
		bool very_first_node;
		bool first_node;
		std::string next_node;
		bool use_gmm_input;
		unsigned int aggregation_step;
		bool detection_miss;
		bool first_step_failed;
		bool target_lost;
		bool first_node_candidate;
		multimap<double, string> first_node_candidates;
		double first_start_time;
		double very_first_start_time;
	};
	struct rng_draw_t {
		double rand_x;
		double rand_y;
	};
	typedef state_t measurement_t;
	typedef std::vector<WiseUtils::Gmm::Component> gmm_t;

private:
	static std::ofstream *final_writer;
	static std::ofstream *partial_writer;
	static std::vector<rng_draw_t> rng_draw;
	
	bool trace_particles;
	unsigned count_draws;
	unsigned n_targets;
	unsigned n_particles;
	double particle_spread;
	int n_gmm_components;

	// Assuming l -> target
	std::vector<state_t> init_states; 		// x_0
	std::vector<measurement_t> target_measurements; // z_l
	std::vector<state_t> target_states; 		// x_l
	std::vector<particle_set_t> particle_sets;	// P_l = {p,w}_l
	std::vector<node_ctrl_t> node_controls;
	std::vector<WiseUtils::Gmm*> gmm_clusterers;
	std::vector<gmm_t> gmm_mixtures;
	// Support structures
	double *cumulative_w;	// for resampling
	double *random_u;	// for resampling
	std::vector<state_t> ground_truths; 
	double *flat_particles; // A vector could be used, but this is faster

public: 
	virtual ~WiseCameraDPF();

protected:
	virtual void handleMacControlMessage(cMessage *);

	virtual void at_startup();
	virtual void at_timer_fired(int index) {} ;
	virtual void at_tracker_init();
	virtual void at_tracker_first_sample();
	virtual void at_tracker_end_first_sample();
	virtual void at_tracker_sample();
	virtual void at_tracker_end_sample();
	virtual void make_measurements(const std::vector<WiseTargetDetection>&);
	virtual void process_network_message(WiseApplicationPacket *);
	virtual void handleDirectApplicationMessage(WiseApplicationPacket *);

private:
	void init_structures();

	void start_first_node_selection(unsigned);
	void complete_first_node_selection(unsigned);
	void perform_first_step(unsigned);
	void perform_intermediate_step(unsigned);
	void perform_last_step(unsigned);
	void select_next_node(unsigned);

	void store_truth(unsigned);
	void log_final_result(unsigned);
	void log_partial_result(unsigned);

	void draw_from_gmm(const gmm_t &c, state_t &s);
	double likelihood(const measurement_t &z, const state_t &x_p);
	void process_model(state_t &x);

	void draw_initial_particles(unsigned, particle_set_t&);
	void importance_sampling(particle_set_t&, const measurement_t&);
	void resampling(particle_set_t&);
	void predict_and_update(particle_set_t &ps, const measurement_t &z);
	void estimate(unsigned, const particle_set_t&);
	void create_gmm(unsigned);

	bool check_visibility(const state_t &, const WiseCameraInfo &) const;
	double visibility_score(const state_t &, const WiseCameraInfo &,
				bool normalized=false) const;

	void send_information(unsigned);
	void send_target_lost(unsigned);
};
#endif // __WiseCameraDPF_h__	
