root/applications/doprava/traffic_agent.h @ 1261

/*!
\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 queue_last;      
        double queue_avg_last;
        double green_time_ratio;
        double Tc;
        double last_Tc;
        double saturated_stream;
        double delta; // ztratovy cas pri prechodu na zelenou
        double delta_w;
        // udaje opro filtr
        double queue_avg; //  pruner
        double queue_w;   //  vaha

        double Ro_avg;  //  prumer
        double Ro_w;    //  vaha K

        double cars_in_avg;
        double cars_in_w;
        

        double last_queue_avg;

// pomocne funkce

        

        void init () {
                last_Tc = 0;
                saturated_stream = 0.5;
                queue = 0;
                queue_last = 0;
                // filtr
                delta = 0;
                delta_w = 0.2;
                
                queue_avg = 0;  //  prumer
                queue_w = 0.4;  //  vaha                

                Ro_avg = 0;     //  prumer
                Ro_w = 0.1;     //  vaha

                cars_in_avg = 0;
                cars_in_w = 0.4;
                
                last_queue_avg = 0;
        }

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

        void echo () {
                
                cout << getQueueName();
                
                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 << 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 ) {
                        queue_last = queue;
                        queue = ql;
                        queue_avg_last = queue_avg;
                        
                        // KALMAN                       
                        //cout << endl << "pridani fronty\t" << queue_avg << "\t" << ql << endl;
                        queue_avg += queue_w*( ql - queue_avg );                        
                        /*cout << getQueueName() << " inputs";
                        int k = 0;
                        for ( int i = 0; i < rv_inputs.length(); i ++ ) {
                        cout <<"\t"<< rv_inputs.name(i) << ": ";
                        for ( int j = 0; j < rv_inputs.size(i); j ++ ) {                                
                                cout << inputs(k) << " ";
                                k ++;
                        }
                        cout << endl;*/
                }
                if ( inputs(0) >= 0 ) {
                        cars_in_avg += cars_in_w*( inputs(0) - cars_in_avg );
                
                }
                if ( cars_in_avg > 0 && queue_avg_last > saturated_stream*last_Tc*green_time_ratio ) {
                        //double delta_d = queue_avg - queue_avg_last + last_Tc * saturated_stream - cars_in_avg*(double)last_Tc/90;                    
                        double delta_d = ( last_Tc/(90*saturated_stream)) * (cars_in_avg - queue_avg + queue_avg_last) - green_time_ratio;
                        delta += delta_w * ( delta_d - delta );
                        if ( delta < 0 )
                                delta = 0;
                }

                cout << getQueueName() << " cars in: " << inputs(0) << " " << cars_in_avg <<
                        " q_avd: " << queue_avg <<
                        " delta: " << delta <<
                        endl;
        }


        

        double getAverageQueueLength () {
                // kalmam
                return queue_avg;
        }

        double getLastAverageQueueLength () {
                return queue_avg_last;
        }

        /*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 ); }       

        

        // hodnotici funkce - suma cekaciho casu aut za 10h
        //double getWT_old ( 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;
        //}
        // stredni doba prujezdu n-teho auta ve fronte
        double getEcarWT ( const double tc, const int n ) {
                double cpg = (tc * green_time_ratio - delta) * saturated_stream; // cars per green time
                double wc = floor( n / cpg ); // number of waiting cycles               
                double ET_last_cycle = ( 1/ (2*tc) ) * ( tc*(1-green_time_ratio) + (n-wc*cpg)/saturated_stream )*( tc*(1-green_time_ratio) + (n-wc*cpg)/saturated_stream );
                double ET = wc*tc + ET_last_cycle;
                return ET;
        }

        // suma strednich hodnot cekacich casu pres auta ve fronte
        double getWT ( const double tc ) {
                double sumEWT = 0;
                //int n = round(getAverageQueueLength());
                int n = round(cars_in_avg + getAverageQueueLength());
                for ( int i = 0; i <= n; i ++ ) {
                        sumEWT += getEcarWT( tc, i );
                }
                return sumEWT;
        }


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