root/applications/doprava/traffic_agent.h @ 1139

/*!
\file
\brief Traffic Light Agents 
\author Vaclav Smidl.
*/

#ifndef TRAGE_H
#define TRAGE_H


#include <base/participants.h>

using namespace bdm;

class BaseTrafficAgent;

//! detector of traffic variables
class Lane{
public:
        Array<string> inputs;
        Array<string> outputs;
        vec input_distances;
        vec output_distances;
        vec alpha; //size of outputs
        //! percent of queue lenght (for "multiline queues")
        double beta;
        string queue;
        string sg;

        //! function loading info from Setting
        void from_setting(const Setting &set){
                UI::get(inputs,set,"inputs",UI::compulsory);
                UI::get(outputs,set,"outputs",UI::compulsory);
                UI::get(input_distances,set,"input_distances",UI::compulsory);
                UI::get(output_distances,set,"output_distances",UI::compulsory);
                UI::get(alpha,set,"alpha",UI::compulsory);
                UI::get(queue,set,"queue",UI::compulsory);
                UI::get(sg,set,"sg",UI::compulsory);
                UI::get(beta, set, "beta");
        }
};

//! class that operates on a signal group
class LaneHandler {
protected:
        //! pointer to physical lane
        const Lane &lane;
        //! agent pointer
        BaseTrafficAgent *agent;

public:
        //! actual data from the relevant signal group
        vec inputs;
        //! queue length
        double queue; 
        //! description of det_data
        RV rv_inputs;
        //! description of det_data
        RV rv_outputs;
        //! description of det_data
        RV rv_queue;
        //! link from global measured data 
        datalink agentin2input;
        //!
        datalink output2agentout;
        //!
        int queue_index;

public:
        LaneHandler(const Lane &lane0): lane(lane0){
                for (int i=0;i<lane0.inputs.length();i++){
                        rv_inputs.add(RV(lane.inputs(i), 2));
                }
                for (int i=0;i<lane0.outputs.length();i++){
                        rv_outputs.add(RV(lane.outputs(i), 2)); 
                }
                rv_queue.add(RV(lane.queue, 1)); 
                inputs.set_size(rv_inputs._dsize());
                // KOD PRO TrafficAgentCycleTime
                init();
        }


        void connect_data(BaseTrafficAgent &agent0);

        //! computes expected density in cars/s
        double expected_density();

        //! arbitrary function that computes the need of the signal group for green light in common units (number of waiting cars?)
        double expected_output(double green_time);

        

/********************************
* KOD PRO TrafficAgentCycleTime *
*********************************/
        double last_queue;
        double sum_queue_length;
        double last_sum_queue_length;
        double queue_diff_sum;
        double queue_variance_sum;
        int n_of_queues_in_length;
        int last_n_of_queues_in_length;
        double last_variance;
        double green_time_ratio;
        double Tc;
        double last_Tc;
        double saturated_stream;
        double delta; // ztratovy cas pri prechodu na zelenou
        // udaje opro kalmana
        double queue_avg; //  pruner
        double queue_w;   //  vaha
        double queue_d;   //  rozptyl
        double queue_dd;  //  snizeni verohodnosti odhadu 
        double queue_r;   //  neduveryhodnost mereni = 1

        double Ro_avg;  //  prumer
        double Ro_w;    //  vaha K
        double Ro_d;            //  rozptyl P
        double Ro_dd;           //  snizeni verohodnosti odhadu Q
        double Ro_r;            //  neduveryhodnost mereni = 1 R

        double last_queue_avg;

// pomocne funkce

        

        void init () {
                last_Tc = 0;
                saturated_stream = 0.5;
                queue = 0;
                sum_queue_length = 0;
                last_sum_queue_length = 0;
                n_of_queues_in_length = 0;
                last_n_of_queues_in_length = 0;
                queue_diff_sum = 0;
                queue_variance_sum = 0;
                last_variance = 0;
                delta = 5;
                // KALMAN
                queue_avg = 0;  //  prumer
                queue_w = 1;    //  vaha K
                queue_d = 0.9;          //  rozptyl P
                queue_dd =0.1;          //  snizeni verohodnosti odhadu Q
                queue_r = 9;            //  neduveryhodnost mereni = 1 R

                Ro_avg = 0;     //  prumer
                Ro_w = 1;       //  vaha K
                Ro_d = 0.9;             //  rozptyl P
                Ro_dd =0.1;             //  snizeni verohodnosti odhadu Q
                Ro_r = 9;               //  neduveryhodnost mereni = 1 R

                last_queue_avg = 0;
        }

        /*double abs ( double x ) {
                if ( x > 0 ) return x;
                else return -x;
        }*/

        void echo () {
                //cout << "n:" << n_of_queues_in_length << " " << getQueueName() << " q:" << getQueue() << " aq:" << getAverageQueueLength() << " diff:" << getAverageQueueGrow() << " D:" << getQueueVariance() << endl;
                /*cout << "n:" << n_of_queues_in_length 
                     << " " << getQueueName() 
                         << "LAST aq:" << last_sum_queue_length/last_n_of_queues_in_length 
                         << " aq:" << getAverageQueueLength()
                         << " diff" <<  getAverageQueueLength() - last_sum_queue_length/last_n_of_queues_in_length 
                         << endl;*/
                cout << getQueueName();
                /*if ( last_n_of_queues_in_length> 0 && n_of_queues_in_length > 0 )
                        cout << " DIFF: " << getAverageQueueLength() - last_sum_queue_length/last_n_of_queues_in_length;
                else
                        cout << " no diff available";*/
                //cout << "\tn " << n_of_queues_in_length;
                //cout << " prijizdejicich aut za 90: " << getRo()*90;
                cout << "\tgtr: " << green_time_ratio;
                cout << "\tRo: " << getRo();
                cout << "\tC: " << green_time_ratio*Tc*saturated_stream - getRo()*Tc << endl; 
                //cout << "\tWT:";
                for ( int i = -2; i <= 2; i++ ) {
                        double tc = Tc + i*8;
                        cout << tc << "\t";
                }
                cout << endl;
                for ( int i = -2; i <= 2; i++ ) {
                        double tc = Tc + i*8;
                        cout << (int)(100*getWT(tc)) << "\t";
                }
                //cout << " diff: " << getRo()*Tc - saturated_stream*getGreenTime();
                /*if ( saturated_stream*getGreenTime() >= getAverageQueueLength() )
                        cout << " projede vsechno ";
                else
                        cout << " zustanou auta ";*/
                cout << endl << endl;
        }
        

        Lane getLane () {
                return lane;
        }

        string getSG () {
                return lane.sg;
        }

        string getQueueName () {
                return lane.queue;
        }

        double getQueue() {
                return queue_avg;
        }

        void addQueueLength ( int ql ) {
                // pricita pouze namerene udaje
                if ( ql >= 0 ) {
                        last_queue = queue;
                        queue = ql;
                        queue_diff_sum += queue - last_queue;
                        //queue_variance_sum += abs(queue - last_queue);
                
                        sum_queue_length += ql;
                        n_of_queues_in_length ++;
                        // KALMAN                       
                        cout << endl << "pridani fronty\t" << queue_avg << "\t" << ql << endl;
                        queue_avg += queue_w*( ql - queue_avg );
                        queue_d += queue_dd;
                        queue_w = queue_d/(queue_d+1);
                        queue_d = (1-queue_w)*queue_d;
        
                        double Ro = getActualRo();
                        Ro_avg += Ro_w*( Ro - Ro_avg );
                        Ro_d += Ro_dd;
                        Ro_w = Ro_d/(Ro_d+1);
                        Ro_d = (1-Ro_w)*Ro_d;
                }
        }

        void resetQueue () {
                last_Tc = Tc;
                //last_variance = getQueueVariance();
                last_n_of_queues_in_length = n_of_queues_in_length;
                last_sum_queue_length = sum_queue_length;
                n_of_queues_in_length = 0;
                sum_queue_length = 0;
                queue_variance_sum = 0;
                queue_diff_sum = 0;
                // kalman
                last_queue_avg = getAverageQueueLength();
        }

        double getAverageQueueLength () {
                // kalmam
                return queue_avg;

                if( n_of_queues_in_length > 0 )
                        return sum_queue_length / n_of_queues_in_length;
                else
                        return 0.0;
        }

        double getLastAverageQueueLength () {
                if( last_n_of_queues_in_length > 0 )
                        return last_sum_queue_length / last_n_of_queues_in_length;
                else
                        return 0.0;
        }

        /*double getAverageQueueGrow () {
                return queue_diff_sum / n_of_queues_in_length;
        }*/
        
        /*double getQueueVariance () {
                return queue_variance_sum / n_of_queues_in_length;
        }*/

        double getGreenTime ( double tc ) { return green_time_ratio * tc - delta; }     
        double getGreenTime () { return getGreenTime( Tc ); }   
        
        double getRedTime ( double tc ) { return (1-green_time_ratio) * tc;     }
        double getRedTime () { return getRedTime( Tc ); }       

        double getRelativeQueueDiff () {
                if ( last_n_of_queues_in_length > 0 && n_of_queues_in_length > 0 ) {
                        
                        return getAverageQueueLength()/(Tc*(1-green_time_ratio)) - (last_sum_queue_length/last_n_of_queues_in_length)/(last_Tc*(1-green_time_ratio));
                }
                else
                        return 0;
        }

        // hodnotici funkce - suma cekaciho casu aut za 10h
        double getWT ( double  Tc ) {
          double T = 36000; // celkovy cas 10h
          double Ro = getRo();
          double Gr = green_time_ratio;
          double ss = saturated_stream;
          double WT = 0;
          double q = 0;  // zacina s nulovou frontou ?
          double ti = 0;
          double sumq = q;
          while ( ti < T ) {
                // ve fronte stoji vic aut nez je schopno odjet za zelenou
                if ( q > 0.5*ss*Tc ) {
                  WT += 0.5*ss*(Tc*Gr - delta)*(Tc*Gr - delta);
                }
                else {

                  WT += 0.5*q*(Tc*Gr - delta);
                }

                if ( (Tc*Gr - delta)*ss < q )
                  q -= (Tc*Gr - delta)*ss;
                else
                  q = 0;

                WT += q * Tc; // zbytek fronty ceka cely cyklus
                // cekani vozidel, ktera prijela za pocitany cyklus
                if ( q > 0 ) {
                  //pokud je fronta > 0, auta cekaji prumerne polovinu delky cyklu
                  WT += Ro*Tc*0.5*Tc;
                }
                else {
                  // pokud je fronta = 0 cekaji pouze auta, ktera prijela na cervenou (p=(1-Gr)) 0.5Tc(1-Gr)
                  WT += Ro*(1-Gr)*Tc*0.5*(1-Gr)*Tc;
                }
                // fronta se zvetsi o Ro*Tc
                q += Ro*Tc;
                sumq += Ro*Tc;
                ti += Tc;
          }

          //return WT;
          if ( sumq > 0 )
                return 100*(WT/(0.5*T*sumq));
          else
          return 0;
        }

        //double getWT ( double  tc ) {
        //      double T = ;
        //      double WT = 0;
        //      double cWT = 0;
        //      double q = 0;
        //      double cq = 0;
        //      double sum_t;
        //      double car_part = 0.1;
        //      double Ro = getRo();
        //      double dt = car_part/Ro;
        //      while ( sum_t < T ) {
        //              if ( q < 0 )
        //                      q = 0;
        //              WT += q * dt;
        //              // prirustek fronty za dt
        //              q += car_part;
        //              // zelena - auta projedou
        //              if ( (sum_t - tc*(int)(sum_t/tc)) > delta && )
        //                      q -= saturated_stream*dt;
        //              sum_t += dt;
        //      }
        //}
        // Odhad hustoty ( auto/sec )
        double getActualRo () {
                // pokud delka fronty mensi nez pocet aut ktere odjedou za zelenou
                if ( saturated_stream*getGreenTime() >= getAverageQueueLength() ) {
                        // pocet aut, ktera prijela pri cervene
                        return getAverageQueueLength()/getRedTime();
                }
                else {
                        //fronta se nevynuluje
                        // pocet aut = to co stihlo projet na zelenou + o co narostla fronta
                        // doba pocet_cyklu*90
                        //double T = n_of_queues_in_length*90;
                        //double cars_per_green = T*(getGreenTime()/Tc)*saturated_stream;
                        //return (cars_per_green + queue_diff_sum)/T;
                        double T = 90;
                        double cars_per_green = T*(getGreenTime()/Tc)*saturated_stream;
                        return (cars_per_green + (getLastAverageQueueLength()-getAverageQueueLength()))/T;
                }
        }

        double getRo() {
                return Ro_avg;
        }
        
        

        /***************************************
        * KONEC KODU PRO TrafficAgentCycleTime *
        ****************************************/
};

/*!
\brief  Basic Traffic Agent

*/
class BaseTrafficAgent : public Participant {

LOG_LEVEL(BaseTrafficAgent,logdata);
public:
        //! Signal Groups
        Array<Lane> lanes;

        Array<LaneHandler*> lanehs;

        //!data from messages
        vec inputs;

        //! decription of msg_data
        RV rv_inputs;

        //! data to broadcast
        vec outputs;

        //! description of broadcast dataind
        RV rv_outputs;

        vec queues;

        //! description of queues
        RV rv_queues;

        //! 
        vec green_starts;

        //!
        vec green_times;

        //!
        Array<string> green_names;

        //!
        Array<string> stage_names;

        //!
        vec stage_times;





        //! datalink from DS to input variables
        datalink ds2inputs;

        //! datalink from DS to output variables
        datalink ds2queues;

        //! action description
        RV rv_action;

        datalink_part action2ds;

        Array<string> neighbours;

        Array<RV> rv_neighbours_out;

        Array<datalink> output2neighbour;

        //! simulator's step length in seconds
        static const int step_length=90;

        //! lenght of cycle in seconds
        static const int cycle_length=80;



public:
        void validate(){

                lanehs.set_length(lanes.length());
                for (int l=0; l<lanes.length(); l++){
                        lanehs(l) = new LaneHandler(lanes(l));

                        rv_inputs.add(lanehs(l)->rv_inputs);
                        rv_outputs.add(lanehs(l)->rv_outputs);
                        rv_queues.add(lanehs(l)->rv_queue);
                }
                inputs.set_size(rv_inputs._dsize());
                outputs.set_size(rv_outputs._dsize());
                queues.set_size(rv_queues._dsize());

                for (int l=0; l<lanes.length(); l++){
                        lanehs(l)->connect_data(*this);
                }

                //for -- rv_outputs -- 
                // TODO vybrat rv pro sousedy 
                rv_neighbours_out.set_length(neighbours.length());
                output2neighbour.set_length(neighbours.length());

                for (int i=0; i<neighbours.length(); i++){
                        for (int r=0; r<rv_outputs.length(); r++){
                                int str_pos = rv_outputs.name(r).compare(neighbours(i));
                                if (str_pos>(int)neighbours(i).length()){
                                        rv_neighbours_out(i).add(rv_outputs.subselect(vec_1(r)));
                                }
                        }
                        // connect datasource
                        output2neighbour(i).set_connection(rv_neighbours_out(i), rv_outputs);
                }

                // lanehs knows RVS
                // write internal checks if all was loaded OK

        }
        void receive(const Setting &msg){
                string what;
                UI::get(what, msg, "what", UI::compulsory);

                if (what=="new_stable_state"){ // 
                        // field data 
                        // extract decription of teh received datavector
                        shared_ptr<RV> rv=UI::build<RV>(msg,"rv",UI::compulsory);
                        // find if it is needed
                        ivec ind=rv->dataind(rv_inputs); // position of rv in in_rv;
                        if (ind.length()>0){ //data are interesting
                                vec dt;
                                UI::get(dt, msg, "value",UI::compulsory); // get data
                                set_subvector(inputs, ind,  dt); //check size?
                        }                               
                } else {
                        Participant::receive(msg);
                }
        }
        void log_register(logger &L, const string &prefix){
                root::log_register ( L, prefix );
                if ( log_level[logdata]){
                        L.add_vector ( log_level, logdata, RV ( 1 ), prefix );  
                }
        }
        void log_write() const {
                if (log_level[logdata]){
                        log_level.store(logdata, inputs);
                }
        }

        void broadcast(Setting& set){
                // broadcast data to all neighbours
                for (int i=0; i<neighbours.length(); i++){
                        Setting &msg =set.add(Setting::TypeGroup);

                        // if...
                        // copy from create message
                        // create msg with fields {to=..., what=data, rv=..., value = ...}
                        UI::save ( neighbours(i), msg, "to");
                        UI::save ( (string)"new_stable_state", msg, "what");
                        UI::save ( &(rv_neighbours_out(i)), msg, "rv");
                        UI::save( output2neighbour(i).pushdown(outputs), msg, "value");
                }

        }
        void adapt(const vec &glob_dt){
                // copy data from global vector to sSGHandlers
                ds2inputs.filldown(glob_dt, inputs);
                //copy data from neighbours
                ds2queues.filldown(glob_dt, queues);
                // copy sg_length ... and others...
        }
        void act(vec &glob_ut){
                vec action; // trivial stuff
                action2ds.filldown(action,glob_ut);
        }

        void ds_register(const DS &ds){
                //register ds2output
                ds2inputs.set_connection(rv_inputs, ds._drv());
                ds2queues.set_connection(rv_queues, ds._drv());

                inputs.set_size(rv_inputs._dsize());
                action2ds.set_connection( ds._urv(), rv_action);

        }

        void from_setting(const Setting &set);
};
UIREGISTER(BaseTrafficAgent);

#endif //TRAGE_H