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

#include "WiseCameraTrackerTest.h"

Define_Module(WiseCameraTrackerTest);

#define LOGGER logger << "[" << simTime() << "] @ " << self << " : "

ofstream WiseCameraTrackerTest::logger;

/* ************************************************************************** */
/*                 EVENTS from (camera) simple periodic tracker               */
/* ************************************************************************** */
void WiseCameraTrackerTest::at_startup() 
{
	// Called upon simulation start-up
	if (!logger.is_open())
		logger.open("myLog.txt");
	LOGGER << "START-UP" << endl;

	// Read module parameter
	some_parameter = par("some_parameter");
	// Access related-module parameters (read number of targets)
	cModule *network = getParentModule()->getParentModule();
	n_targets = network->par("numPhysicalProcesses");
}

void WiseCameraTrackerTest::at_tracker_init() 
{
	// Called upon tracker init
	LOGGER << "INIT" << endl;

	// TODO: internal structure initialization should take place here!

	tracking_step = 0;
}

void WiseCameraTrackerTest::at_tracker_first_sample() 
{
	// Called when the first sample (image) is ready
	LOGGER << "FIRST SAMPLE" << endl;
	
	// We can:
	// 1- either put here any first-sample specific code
	// 2- or simply treat the first sample as the others 
	at_tracker_sample(); // option 2
}

void WiseCameraTrackerTest::at_tracker_end_first_sample() 
{
	// Called when the first sample (image) is no longer valid
	LOGGER << "END FIRST SAMPLE" << endl;

	// We can:
	// 1- either put here any first-sample specific code
	// 2- or simply treat the first sample as the others 
	at_tracker_end_sample(); // option 2
}

void WiseCameraTrackerTest::at_tracker_sample() 
{
	// Called when a new sample (image) is ready
	LOGGER << "NEW SAMPLE" << endl;
	
	// TODO: a tracking iteration generally starts here!

	tracking_step++;
	// NOTE: Measurement has been made through the make_measurements() 
	//       method and is available in the observation vector
	for(vector<measurement_t>::const_iterator m = observations.begin();
	    m != observations.end(); ++m) {
		LOGGER << "\t\tDetection for Target " << m->tid <<  
        		     " is (" << m->x << "," << m->y << "). This should"
			     " be used to write a tracker :)" << endl;

		// Example: Send some information in broadcasts
		WiseCameraTrackerTestMessage *pkt;
		pkt = new WiseCameraTrackerTestMessage("Wise Test Packet", 
						       APPLICATION_PACKET);
		pkt->setTrackingStep(tracking_step);
		pkt->setTargetID(m->tid);
		pkt->setPositionX(m->x);
		pkt->setPositionY(m->y);

		pkt->setByteLength(100); // Packet size must be set explicitly

		// Send in broadcast:
		// 1- either using the network (normal case)
		// 2- or using the DirectApplication mechanism (idealistic comm)
		send_message(pkt); 	 			// option 1
		//sendDirectApplicationMessage(pkt, "-1");	// option 2
	}
}

void WiseCameraTrackerTest::at_tracker_end_sample() 
{
	// Called when a new sample (image) is no longer valid
	LOGGER << "END NEW SAMPLE" << endl;

	// TODO: a tracking iteration generally ends here!
}

void WiseCameraTrackerTest::make_measurements(
					const vector<WiseTargetDetection>&dt)
{
	// Called to properly read the sample: when a new sample is available 
	// a measurement has to be created from it
	LOGGER << "MAKE MEASUREMENT" << endl;

	// Fill the observation vector with the measurement
	observations.clear();
	for(vector<WiseTargetDetection>::const_iterator d = dt.begin();
	    d != dt.end(); ++d) {
		// Measurement is: Target ID, and box's center (image plane)
		measurement_t m;
		double w_x, w_y, t_x, t_y;
		m.tid = d->target_id;
		m.x = (d->bb_x_tl + d->bb_x_br) / 2;
		m.y = (d->bb_y_tl + d->bb_y_br) / 2;
		observations.push_back(m);
		w_x = (d->ext_bb_x_tl + d->ext_bb_x_br) / 2;
		w_y = (d->ext_bb_y_tl + d->ext_bb_y_br) / 2;
		t_x = (d->true_bb_x_tl + d->true_bb_x_br) / 2;
		t_y = (d->true_bb_y_tl + d->true_bb_y_br) / 2;
		LOGGER << "\t\tTarget ID = " << m.tid << endl <<
        		     "\t\tImage plane coordinates: (" << 
			     m.x << "," << m.y << ")" << endl <<
        		     "\t\tWorld coordinates: (" << 
			     w_x << "," << w_y << ")" << endl <<
        		     "\t\tTrue coordinates: (" << 
			     t_x << "," << t_y << ")" << endl;
	}
} 

void WiseCameraTrackerTest::process_network_message(WiseApplicationPacket *pkt) 
{
	// Called when a packet is received from the network
	LOGGER << "PROCESS NETWORK MESSAGE" << endl;

	WiseCameraTrackerTestMessage *m;
	m = check_and_cast<WiseCameraTrackerTestMessage*>(pkt);
	AppNetInfoExchange_type ctl =
			m->getAppNetInfoExchange();

	LOGGER << "RX from " << ctl.source << ". Packet content: ["
		  "  k=" << m->getTrackingStep() <<
		  "  TID=" << m->getTargetID() << 
		  "  Pos=(" << m->getPositionX() << "," << m->getPositionX() <<
		  ")  ]" << endl;

	// TODO: the received information might be used to write the distributed
	//       tracking algorithm
} 

void WiseCameraTrackerTest::handleDirectApplicationMessage(
						     WiseApplicationPacket *pkt)
{
	// Called when a DirectApplication Message is received
	LOGGER << "HANDLE DIRECT APPLICATION MESSAGE" << endl;

	// In this case we treat the message received through DirectApplication
	// as a normal network message. However, the two mechanisms could be
	// used independently.
	process_network_message(pkt);
}
