root/applications/doprava/traffic_agent_LQ.h @ 1421

#include "QuadraticMinimalizator.h"

class TrafficAgentLQ : public BaseTrafficAgent {
protected:
        mat A, B, Q, R;
        static const int T = 90;
        int L;
        int nOfBroadcast;
        QuadraticMinimalizator * minimizer;
        //map<string, int> queueIndex;
public:
        // FUNKCE VOLANE V main_loop NA ZACATKU
        
        void validate (){
                L = 90;
                BaseTrafficAgent::validate();

                rv_action = RV("Tc",1);
                rv_action.add( RV( stage_names, ones_i(stage_names.length()) ) );

                for ( int i=0; i<lanehs.length(); i ++ ) {
                        unsigned int index = find_index( green_names, name + "_" + lanehs(i)->getSG() );
                        lanehs(i)->green_time_ratio = green_times( index );
                }
                
                Q = diag( ones( queues.length()) );
                R = "1";
                A = diag( ones( queues.length()) );
                B = ones( queues.length(), 1);

                minimizer = new QuadraticMinimalizator(A,B,Q,R);
                
                cout << "HELLO! " << name << " IS HERE!" << endl;
                /*
                cout << "inputs " << inputs.length() << endl;           
                cout << "queues " << queues.length() << endl;
                cout << "rv_inputs" << rv_inputs.length() << endl << rv_inputs << endl;
                cout << "rv_queues" << rv_queues.length() << endl << rv_queues << endl;
                cout << "lanehs " << lanehs.length() << endl << endl;
                cout << "A B Q R " << endl << A << endl << B << endl << Q << endl << R << endl << endl;
                */
        }

        // FUNKCE VOLANE V main_loop V KAZDEM CYKLU
        void adapt (const vec &glob_dt) {
                for ( int i=0; i<lanehs.length(); i ++ ) {
                        //lanehs(i)->inputs( 0 ) = inputs( 2*find_index(rv_inputs, lanehs(i)->rv_inputs.name(0) ) );
                        //lanehs(i)->inputs( 1 ) = inputs( 2*find_index(rv_inputs, lanehs(i)->rv_inputs.name(0) ) + 1 );
                        lanehs(i)->queue = queues( find_index( rv_queues, lanehs(i)->getQueueName() ) );
                        lanehs(i)->countAvgs();
                }
                
                // jeden clen matice B
                for ( int i=0; i<queues.length(); i ++ ) {
                        B(i,0) = 0.5 * lanehs(i)->green_time_ratio;
                }

                
                
                BaseTrafficAgent::adapt(glob_dt);
        }

        
        void broadcast( Setting &set ) {
                string dummyDet = "DUMMY_DET";
                if ( nOfBroadcast == 0 ) {                      
                        for ( int i = 0; i < lanehs.length(); i++ ) {           
                                for ( int j = 0; j < lanehs(i)->rv_outputs.length(); j ++ ) {
                                        if ( dummyDet.compare(lanehs(i)->rv_outputs.name(j)) != 0 ) {                                           
                                                stringstream coefStr;
                                                coefStr << "coef" << lanehs(i)->rv_outputs.name(j);
                                                double val = 0.5 * lanehs(i)->green_time_ratio * lanehs(i)->getLane().alpha(j);
                                                sendToAll<double>(set, coefStr.str(), val);
                                        }
                                }                               
                        }
                }
                nOfBroadcast ++;
        }

        void receive(const Setting& msg){
                string what;
                UI::get(what, msg, "what", UI::compulsory);             
                if ( what.substr(0,4) == "coef" ) {
                        string inputName = what.substr(4,what.length()-4);
                        int i = find_index(rv_inputs, inputName);
                        double val;
                        UI::get(val, msg, "value");
                        B(i,0) = B(i,0) - val;
                        //cout << name << " received coefs " << inputName << " " << what << endl << val <<endl;
                }
        }

        void act (vec &glob_ut){
                //cout <<"A act"<<endl<<A<<endl;
                B = B * T * L;
                minimizer = new QuadraticMinimalizator(A, B, Q, R);
                vec computedQueues = getComputedQueues();
                vec res = minimizer->minimize(computedQueues, 100);
                //cout << "minimalization res " << name << endl << res << endl << endl;
                delete minimizer;
                int Tc = 80;
                vec action;
                action.set_size(rv_action._dsize());
                action(find_index(rv_action, "Tc")) = Tc;       
                int st;
                int stage_time_sum = 0;  // soucet delky fazi
                action(find_index(rv_action, "Tc")) = Tc;               
                for ( int i =0; i < stage_names.length(); i ++) {                       
                        if ( (i+1) < stage_names.length() ) {
                                st = round(((double)(stage_times(i)*Tc))/80.0);
                                stage_time_sum += st;
                                action(find_index(rv_action, stage_names(i))) = st;
                        }
                        else { // dopocitani posledni faze - oprava zaokrouhlovaci chyby
                                action(find_index(rv_action, stage_names(i))) = Tc - stage_time_sum;
                        }
                }
                action2ds.filldown(action,glob_ut);
        }

        void setCycleTime( int Tc, vec &glob_ut ) {
                
        }


        
// POMOCNE FCE
        vec getComputedQueues() {
                vec q = zeros( queues.length() );
                for ( int i = 0; i < lanehs.length(); i++ ) {
                        q(i) = lanehs(i)->queue_avg;
                }
                return q;
        }

        template <class T> void sendToAll( Setting &set, string msgCode, T value) {
                for ( int i = 0; i < neighbours.length(); i ++  ) {     
                        Setting & msg = set.add(Setting::TypeGroup);
                        UI::save( neighbours(i), msg, "to" );
                        UI::save (name,msg,"from");
                        UI::save( msgCode, msg, "what" );
                        UI::save( value, msg, "value" );
                }
        }

        void printVector ( RV rv_vector, vec vector, string description ) {
                cout << endl << description << " " << name << endl;
                int k = 0;
                for ( int i = 0; i < rv_vector.length(); i ++ ) {
                        cout << rv_vector.name(i) << " : ";
                        for ( int j = 0; j < rv_vector.size(i); j ++ ) {                                
                                cout << vector(k) << " ";
                                k ++;
                        }
                        cout << endl;
                }
                cout << endl;
        }

        unsigned int find_index ( RV rv_vector, string index_string ) {
                for ( unsigned int i = 0; i < rv_vector.length(); i ++) {
                        if ( rv_vector.name(i) == index_string )
                                return i;
                }
                return -1;
        }

        unsigned int find_index ( Array <string> arr, string index_string ) {
                for ( unsigned int i = 0; i < arr.length(); i ++) {
                        if ( arr(i) == index_string )
                                return i;
                }
                return -1;
        }

};

UIREGISTER(TrafficAgentLQ);