source: tradir/serial/test.c @ 177

Last change on this file since 177 was 177, checked in by (none), 14 years ago
File size: 8.2 KB
Line 
1#include <stdio.h>
2#include <stdlib.h>
3#include <string.h>
4#include <unistd.h>
5#include <sys/types.h>
6#include <sys/socket.h>
7#include <netinet/in.h>
8#include <netdb.h> 
9#include <stdarg.h>
10#include <assert.h>
11
12#define UBYTE   0x07
13#define BYTE    0x08
14#define INTEGER 0x09
15#define FLOAT   0x0a
16#define DOUBLE  0x0b
17#define STRING  0x0c
18#define LIST    0x0e
19
20typedef struct {
21        int32_t size;
22        int32_t pos;
23        uint8_t *data;
24} message;
25
26int sockfd;
27message msg;
28
29void error(char *msg) {
30    perror(msg);
31    exit(0);
32}
33
34void msg_new() {
35        msg.size = 0;
36        msg.pos = 0;
37        msg.data = calloc(255, sizeof(char));
38}
39
40void msg_free() {
41        free(msg.data);
42}
43
44void msg_write(uint8_t cmd, char *format, ...) {
45        va_list ap;
46        char *str;
47        int len;
48        int old = msg.size;
49        int i;
50
51        msg.size++;
52        msg.data[msg.size++] = cmd;
53        va_start(ap, format);
54        for (i = 0; i < strlen(format); i++) {
55                switch (format[i]) {
56                        case 'b' :
57                                msg.data[msg.size++] = (uint8_t) va_arg(ap, int);
58                                break;
59                        case 'i' :
60                                *(uint32_t*) &msg.data[msg.size] = htonl(va_arg(ap, uint32_t));
61                                msg.size += 4;
62                                break;
63                        case 'd' :
64                                *(double*) &msg.data[msg.size] = va_arg(ap, double);
65                                msg.size += 8;
66                                break;
67                        case 's' :
68                                str = va_arg(ap, char*);
69                                len = strlen(str);
70                                *(uint32_t*) &msg.data[msg.size] = htonl(len);
71                                msg.size += 4;
72                                memcpy(&msg.data[msg.size], str, len);
73                                msg.size += len;
74                                break;
75                        default :
76                                break;
77                }
78        }
79        va_end(ap);
80        msg.data[old] = (uint8_t) (msg.size - old);
81}
82
83void msg_send() {
84        int32_t size;
85        int res;
86
87        size = htonl(msg.size + 4);
88        res = write(sockfd, (char*) &size, 4);
89        if (res < 0) error("ERROR sending message size");
90        res = write(sockfd, msg.data, msg.size);
91        if (res < 0) error("ERROR sending message data");
92        free(msg.data);
93}
94
95void msg_recv() {
96        int32_t size;
97        int res;
98//      int i;
99
100        res = read(sockfd, &size, 4);
101        if (res < 0) error("ERROR receiving message size");
102        msg.size = ntohl(size) - 4;
103        msg.data = calloc(msg.size, sizeof(char));
104        res = read(sockfd, msg.data, msg.size);
105        if (res < 0) error("ERROR receiving message data");
106//      printf("### ");
107//      for (i = 0; i < res; i++) {
108//              printf("%02x-", msg.data[i]);
109//      }
110//      printf("\n");
111}
112
113void msg_read(uint8_t cmd, char *format, ...) {
114        va_list ap;
115        char *s;
116        uint8_t** str;
117        uint8_t*** lst;
118        uint32_t host;
119        int cnt;
120        int len;
121        int old = msg.pos;
122        uint32_t size;
123        int i;
124        int j;
125
126        size = (uint8_t) msg.data[msg.pos++];
127        if (!size) {
128                size = ntohl(*(uint32_t*) &msg.data[msg.pos]);
129                msg.pos += 4;
130        }
131//      printf("CMD: %02X DATA: %02X\n", cmd, msg.data[msg.pos]);
132        assert(cmd == msg.data[msg.pos++]);
133        va_start(ap, format);
134        for (i = 0; i < strlen(format); i++) {
135                switch (format[i]) {
136                        case 'B' :
137                                assert(va_arg(ap, int) == msg.data[msg.pos++]);
138                                break;
139                        case 'b' :
140                                *va_arg(ap, uint8_t*) = msg.data[msg.pos++];
141                                break;
142                        case 'f' :
143                                host = ntohl(*(uint32_t*) &msg.data[msg.pos]);
144                                msg.pos += 4;
145                                *va_arg(ap, float*) = *(float*) &host;
146                                break;
147                        case 'S' :
148                                len = ntohl(*(int32_t*) &msg.data[msg.pos]);
149                                msg.pos += 4;
150                                s = va_arg(ap, char*);
151                                assert(len == strlen(s) && !strncmp((char*) &msg.data[msg.pos], s, len));
152                                msg.pos += len;
153                                break;
154                        case 's' :
155                                len = ntohl(*(int32_t*) &msg.data[msg.pos]);
156                                msg.pos += 4;
157                                str = va_arg(ap, uint8_t**);
158                                *str = calloc(1 + len, sizeof(uint8_t));
159                                memcpy(*str, &msg.data[msg.pos], len);
160                                msg.pos += len;
161                                break;
162                        case 'l' :
163                                cnt = ntohl(*(uint32_t*) &msg.data[msg.pos]);
164                                msg.pos += 4;
165                                lst = va_arg(ap, uint8_t***);
166                                *lst = calloc(1 + cnt, sizeof(uint8_t*));
167                                for (j = 0; j < cnt; j++) {
168                                        len = ntohl(*(uint32_t*) &msg.data[msg.pos]);
169                                        msg.pos += 4;
170                                        (*lst)[j] = calloc(1 + len, sizeof(uint8_t));
171                                        memcpy((*lst)[j], &msg.data[msg.pos], len);
172                                        msg.pos += len;
173                                }
174                        case '#' :
175                                assert(msg.pos - old == size);
176                                break;
177                        default :
178                                break;
179                }
180        }
181        va_end(ap);
182}
183
184
185void do_close() {
186        char *desc;
187
188        msg_new();
189        msg_write(0x7f, ""); 
190        msg_send();
191        msg_recv();
192
193        msg_read(0x7f, "Bs#", 0x00, &desc);
194//      printf("CLOSE: 0x%02x-%s\n", res, desc);
195        msg_free();
196}
197
198void do_step() {
199        char *desc;
200
201        msg_new();
202        msg_write(0x01, "db", 100000.0, 0); 
203        msg_send();
204        msg_recv();
205        msg_read(0x01, "Bs#", 0x00, &desc);
206//      printf("STEP: #%s#\n", desc);
207        msg_free();
208}
209
210void do_steps(int n) {
211        int i;
212        for (i = 0; i < n; i++) {
213                do_step();
214        }
215}
216
217char **get_edges() {
218        char *desc;
219        char **edges;
220//      int i = 0;
221
222        msg_new();
223        msg_write(0xaa, "bs", 0x00, "");
224        msg_send();
225        msg_recv();
226        msg_read(0xaa, "Bs", 0x00, &desc);
227        msg_read(0xba, "BSBl#", 0x00, "", LIST, &edges);
228//      printf("EDGES: ");
229//      while (edges[i]) {
230//              printf("[%s] ", edges[i]);
231//              i++;
232//      }
233//      printf("\n");
234        msg_free();
235
236        return edges;
237}
238
239char **get_lanes() {
240        char *desc;
241        char **lanes;
242//      int i = 0;
243
244        msg_new();
245        msg_write(0xa3, "bs", 0x00, "");
246        msg_send();
247        msg_recv();
248        msg_read(0xa3, "Bs", 0x00, &desc);
249        msg_read(0xb3, "BSBl#", 0x00, "", LIST, &lanes);
250//      printf("LANES: ");
251//      while (lanes[i]) {
252//              printf("{%s} ", lanes[i]);
253//              i++;
254//      }
255//      printf("\n");
256        msg_free();
257
258        return lanes;
259}
260
261char *get_lane_edge(char *lane) {
262        char *desc;
263        char *edge;
264
265        msg_new();
266        msg_write(0xa3, "bs", 0x31, lane);
267        msg_send();
268        msg_recv();
269        msg_read(0xa3, "Bs", 0x00, &desc);
270        msg_read(0xb3, "BSBs#", 0x31, lane, STRING, &edge);
271        msg_free();
272
273        return edge;
274}
275
276float get_lane_length(char *lane) {
277        char *desc;
278        float length;
279
280        msg_new();
281        msg_write(0xa3, "bs", 0x44, lane);
282        msg_send();
283        msg_recv();
284        msg_read(0xa3, "Bs", 0x00, &desc);
285        msg_read(0xb3, "BSBf#", 0x44, lane, FLOAT, &length);
286        msg_free();
287
288        return length;
289}
290
291float *get_edge_lengths(char **edges) {
292        int count;
293        float *lengths;
294        char **lanes;
295        int i, j;
296
297        printf("HERE\n");
298        lanes = get_lanes();
299        count = 0;
300        while (edges[count++]);
301        printf("HERE\n");
302        lengths = (float*) calloc(count, sizeof(float));
303        for (i = 0; lanes[i]; i++) {
304                char *edge = get_lane_edge(lanes[i]);
305                float length = get_lane_length(lanes[i]);
306                for (j = 0; edges[j]; j++) {
307                        if (!strcmp(edges[j], edge)) {
308                                lengths[j] = length;
309                                break;
310                        }
311                }
312        }
313        for (i = 0; edges[i]; i++) {
314                printf("LENGTH OF [%s] is %f\n", edges[i], lengths[i]);
315        }
316
317        return lengths;
318}
319
320char **get_edge_vehicles(char *edge) {
321        char *desc;
322        char **vehicles;
323        int i = 0;
324
325        msg_new();
326        msg_write(0xaa, "bs", 0x12, edge);
327        msg_send();
328        msg_recv();
329        msg_read(0xaa, "Bs", 0x00, &desc);
330        msg_read(0xba, "BSBl#", 0x12, edge, LIST, &vehicles);
331//      printf("VEHICLES: 0x%02x-#%s#\n", res, desc);
332//      printf("ON #%s#: ", edge);
333//      while (vehicles[i]) {
334//              printf("<%s> ", vehicles[i]);
335//              i++;
336//      }
337//      printf("\n");
338        msg_free();
339
340        return vehicles;
341}
342
343float get_vehicle_speed(char *vehicle) {
344        char *desc;
345        float speed;
346
347        msg_new();
348        msg_write(0xa4, "bs", 0x40, vehicle);
349        msg_send();
350        msg_recv();
351        msg_read(0xa4, "Bs", 0x00, &desc);
352        msg_read(0xb4, "BSBf#", 0x40, vehicle, FLOAT, &speed);
353//      printf("SPEED: 0x%02x-#%s#\n", res, desc);
354//      printf("speed is %f\n", speed);
355        msg_free();
356
357        return speed;
358}
359
360int main(int argc, char* argv[]) {
361        FILE *fout;
362
363        int portno;
364        struct sockaddr_in serv_addr;
365        struct hostent *server;
366
367        int step;
368
369        char **edges;
370        char **vehicles;
371        float speed;
372        float *lengths;
373        int i, j;
374
375        if (argc < 3) {
376                fprintf(stderr,"usage %s hostname port\n", argv[0]);
377                exit(0);
378        }
379        portno = atoi(argv[2]);
380        sockfd = socket(AF_INET, SOCK_STREAM, 0);
381        if (sockfd < 0) 
382                error("ERROR opening socket");
383        server = gethostbyname(argv[1]);
384        if (server == NULL) {
385                fprintf(stderr,"ERROR, no such host\n");
386                exit(0);
387        }
388        bzero((char *) &serv_addr, sizeof(serv_addr));
389        serv_addr.sin_family = AF_INET;
390        bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
391        serv_addr.sin_port = htons(portno);
392        if (connect(sockfd,(struct sockaddr*) &serv_addr,sizeof(serv_addr)) < 0) 
393                error("ERROR connecting");
394
395        fout = fopen("results.txt", "w");
396
397        edges = get_edges();
398        lengths = get_edge_lengths(edges);
399        for (step = 0; step < 1; step++) {
400                printf("Step %03i ... ", step + 1);
401                do_step();
402                for (i = 0; edges[i] && i < 20; i++) {
403                        speed = 0;
404                        vehicles = get_edge_vehicles(edges[i]);
405                        for (j = 0; vehicles[j]; j++) {
406                                speed += get_vehicle_speed(vehicles[j]);
407                        }
408                        free(vehicles);
409                        speed /= j;
410                        fprintf(fout, "%f\t", speed);
411                }
412                free(edges);
413                fprintf(fout, "\n");
414                printf("OK\n");
415        }
416        do_close();
417
418        fclose(fout);
419
420        return 0;
421}
422
Note: See TracBrowser for help on using the repository browser.