// *****************************************************************************
//  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 "WiseCameraSimplePeriodicTracker.h"
#include "WiseUtils.h"
#include "WiseDebug.h"

#define ALARM_SENSOR_SAMPLE 	0
#define ALARM_END_SAMPLE_PERIOD 1

#define perr_app(m) opp_error("\n[WiseCameraSimplePeriodicTracker]:\n "m)
#define perr(m) opp_error(m)
#define BASE_TRACE trace() << "SIMPLE PERIODIC TRACKER : "

void WiseCameraSimplePeriodicTracker::startup() {
    WiseCameraApplication::startup();
    //BASE_TRACE << "startup : overlapping_fovs = " <<
    //	      WiseUtils::print_vector(overlapping_fov_cameras);
    network_ready = false;
    sampling_time_step = par("sampling_time");
    sample_lifetime = par("sample_lifetime");
    if (sample_lifetime < 0 || sample_lifetime > sampling_time_step)
        sample_lifetime = sampling_time_step;
    sampling_end2start_time = sampling_time_step - sample_lifetime;
    fsm_state = FSM_INIT;
    at_startup();
}

void WiseCameraSimplePeriodicTracker::finishSpecific() {
    WiseCameraApplication::finishSpecific();
}

void WiseCameraSimplePeriodicTracker::notifyNeighbourDiscovered() {
    if (!neighbour_discover) {
        // In this case we just build a fake connectivity map all-to-all
        for (int i = 0; i < n_nodes; i++) {
            ostringstream oss;
            oss << i;
            neighbours.push_back(oss.str());
        }
    }
    BASE_TRACE << "notifyNeighbourDiscovered : neighbours = " << WiseUtils::print_vector(neighbours);
    vector<string>::const_iterator n = neighbours.begin();
    for (n = neighbours.begin(); n != neighbours.end(); ++n)
        connectivity_map[*n] = true;
    fsm_tracker(INVALID, true);
}

void WiseCameraSimplePeriodicTracker::timerFiredCallback(int index) {
    switch (index) {
        case ALARM_SENSOR_SAMPLE:
            requestSensorReading();
            break;
        case ALARM_END_SAMPLE_PERIOD:
            fsm_tracker(INVALID);
            break;
        default:
            at_timer_fired(index);
    }
}

//void WiseCameraSimplePeriodicTracker::fromNetworkLayer(WiseApplicationPacket *p, const char *src, double rssi,
void WiseCameraSimplePeriodicTracker::fromNetworkLayer(cPacket* msg, const char* src, double rssi, double lqi) {
    WiseApplicationPacket *p = check_and_cast <WiseApplicationPacket*>(msg);


    if (!network_ready)
        return;
    AppNetInfoExchange_type ctl = p->getAppNetInfoExchange();
    double l = SIMTIME_DBL(simTime() - ctl.timestamp);
    BASE_TRACE << "RX packet:" " from=" << string(src) << " rssi=" << rssi << " lqi=" << lqi << " app2app_delay=" << l;
    process_network_message(p);
}

void WiseCameraSimplePeriodicTracker::handleSensorReading(WiseCameraMessage *m) {
    string type = m->getCameraSampleType();
    sample_type_t t;
    if (type == "WiseCameraDetections") {
        t = DETECTIONS;
        const WiseCameraDetectionsMessage *s;
        s = check_and_cast<const WiseCameraDetectionsMessage*>(m);
        handle_detections(s);
    }
    else {
        perr_app("WiseCameraMessage is of invalid type");
    }
    fsm_tracker(t);
}

void WiseCameraSimplePeriodicTracker::handle_detections(const WiseCameraDetectionsMessage *m) {
    unsigned len = m->getDetectionsArraySize();
    for (unsigned i = 0; i < len; i++) {
        WiseTargetDetection d = m->getDetections(i);
        detections.push_back(d);
        if (!d.valid)
            continue;
        BASE_TRACE << "Detection: BB: " << " X_tl=" << d.bb_x_tl << " Y_tl=" << d.bb_y_tl << " X_br=" << d.bb_x_br
                << " Y_br=" << d.bb_y_br;
        BASE_TRACE << "Detection: ext BB: " << " X_tl=" << d.ext_bb_x_tl << " Y_tl=" << d.ext_bb_y_tl << " X_br="
                << d.ext_bb_x_br << " Y_br=" << d.ext_bb_y_br;
        BASE_TRACE << "Detection: true BB: " << " X_tl=" << d.true_bb_x_tl << " Y_tl=" << d.true_bb_y_tl << " X_br="
                << d.true_bb_x_br << " Y_br=" << d.true_bb_y_br;
    }
}

void WiseCameraSimplePeriodicTracker::fsm_tracker(sample_type_t t, bool restart) {
    if (restart)
        fsm_state = FSM_INIT;
    switch (fsm_state) {
        case FSM_INIT:
            BASE_TRACE << "FSM : INIT";
            tracking_step = 0;
            at_tracker_init();
            network_ready = true;
            detections.clear();
            setTimer(ALARM_SENSOR_SAMPLE, 0);
            fsm_state = FSM_WAIT_FIRST_SAMPLE;
            break;
        case FSM_WAIT_FIRST_SAMPLE:
            BASE_TRACE << "FSM : WAIT_FIRST_SAMPLE";
            if (t == DETECTIONS)
                make_measurements(detections);
            at_tracker_first_sample();
            setTimer(ALARM_END_SAMPLE_PERIOD, sample_lifetime);
            fsm_state = FSM_WAIT_END_FIRST_SAMPLE;
            break;
        case FSM_WAIT_END_FIRST_SAMPLE:
            BASE_TRACE << "FSM : WAIT_END_FIRST_SAMPLE";
            at_tracker_end_first_sample();
            detections.clear();
            setTimer(ALARM_SENSOR_SAMPLE, sampling_end2start_time);
            fsm_state = FSM_WAIT_SAMPLE;
            break;
        case FSM_WAIT_SAMPLE:
            BASE_TRACE << "FSM : WAIT_SAMPLE (step = " << tracking_step << ")";
            if (t == DETECTIONS)
                make_measurements(detections);
            at_tracker_sample();
            setTimer(ALARM_END_SAMPLE_PERIOD, sample_lifetime);
            fsm_state = FSM_WAIT_END_SAMPLE;
            break;
        case FSM_WAIT_END_SAMPLE:
            BASE_TRACE << "FSM : WAIT_END_SAMPLE (step = " << tracking_step << ")";
            at_tracker_end_sample();
            tracking_step++;
            detections.clear();
            setTimer(ALARM_SENSOR_SAMPLE, sampling_end2start_time);
            fsm_state = FSM_WAIT_SAMPLE;
            break;
    }
}

void WiseCameraSimplePeriodicTracker::send_message(WiseApplicationPacket *m) {
    map<string, bool>::const_iterator n = connectivity_map.begin();
    // This messages will trigger the event (simulation) AFTER
    // other pending messages that at the SAME sim_time have prio < 10
    // (the default prio=0)
    m->setSchedulingPriority(10);
    // Broadcast send
    toNetworkLayer(m->dup(), "-1");
    delete m;
}

void WiseCameraSimplePeriodicTracker::send_message(WiseApplicationPacket *m, const std::string &dst) {
    map<string, bool>::const_iterator n = connectivity_map.find(dst);
    if (n == connectivity_map.end())
        return; // overlapping fov but NOT a neighbour
    // This messages will trigger the event (simulation) AFTER
    // other messages pending at the SAME sim_time with prio < 10
    // (the default prio=0)
    m->setSchedulingPriority(10);
    // Unicast send
    toNetworkLayer(m, dst.c_str());
}

