projects/microb2009/sensorboard/img_processing.c

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <inttypes.h>
   4
   5 #include <aversive/pgmspace.h>
   6 #include <aversive/error.h>
   7
   8 #include <stdint.h>
   9
  10 #include <stdio.h>
  11 #include <stdlib.h>
  12 #include <math.h>
  13
  14 #include <vect_base.h>
  15 #include <lines.h>
  16 #include <polygon.h>
  17
  18 #include "img_processing.h"
  19
  20
  21 #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__)
  22
  23 #ifndef HOST_VERSION
  24
  25 #include <aversive.h>
  26 #include <pwm_ng.h>
  27 #include <pid.h>
  28 #include <time.h>
  29 #include <quadramp.h>
  30 #include <blocking_detection_manager.h>
  31 #include <rdline.h>
  32
  33 #include <control_system_manager.h>
  34 #include <adc.h>
  35 #include <spi.h>
  36 #include <ax12.h>
  37
  38 #include "main.h"
  39
  40 #define IMG_DEBUG(args...) DEBUG(E_USER_IMGPROCESS, args)
  41 #define IMG_NOTICE(args...) NOTICE(E_USER_IMGPROCESS, args)
  42 #define IMG_ERROR(args...) ERROR(E_USER_IMGPROCESS, args)
  43
  44 #else
  45
  46 #define IMG_DEBUG(args...) debug_printf(args)
  47 #define IMG_NOTICE(args...) debug_printf(args)
  48 #define IMG_ERROR(args...) debug_printf(args)
  49
  50 #endif
  51
  52
  53
  54
  55 #define OBJECT_MINIMUM_DEMI_PERIMETER (2*3)
  56 /* store object in pool if never seen
  57  * returns:
  58  * 1 if new object
  59  * 0 if already known
  60  */
  61 int store_obj(Object_bb* tab_o,  int total, Object_bb*o)
  62 {
  63         uint8_t i;
  64
  65         if (o->x_max - o->x_min + o->y_max - o->y_min < OBJECT_MINIMUM_DEMI_PERIMETER)
  66                 return 0;
  67
  68         for (i=0;i<total;i++){
  69                 if (!memcmp(&tab_o[i], o, sizeof(Object_bb)))
  70                         return 0;
  71
  72                 if (tab_o[i].x_min==0 && tab_o[i].x_max==0 &&
  73                     tab_o[i].y_min==0 && tab_o[i].y_max==0){
  74                         memcpy(&tab_o[i], o, sizeof(Object_bb));
  75                         return 1;
  76                 }
  77
  78         }
  79         return 0;
  80 }
  81
  82 /* step around object of given color and compute its bounding box */
  83 void object_find_bounding_box(unsigned char* data,
  84                               int16_t x_in, int16_t y_in,
  85                               int16_t start_x, int16_t start_y,
  86                               int16_t color, int16_t color_w,Object_bb* o)
  87 {
  88         int16_t pos_x = start_x;
  89         int16_t pos_y = start_y;
  90         int16_t start =0;
  91         int16_t len = 0;
  92         int16_t count_stand = 0;
  93
  94         vect_t v, vi;
  95
  96
  97         v.x = 1;
  98         v.y = 0;
  99
 100         vi = v;
 101
 102         o->x_max = 0;
 103         o->y_max = 0;
 104         o->x_min = x_in;
 105         o->y_min = y_in;
 106
 107
 108         while(1){
 109                 if (pos_x == start_x && pos_y == start_y){
 110                         count_stand++;
 111                         if (count_stand>4)
 112                                 break;
 113                         if( v.x == vi.x && v.y == vi.y && start)
 114                                 break;
 115                 }
 116
 117                 if (pos_x<o->x_min)
 118                         o->x_min = pos_x;
 119                 if (pos_y<o->y_min)
 120                         o->y_min = pos_y;
 121                 if (pos_x>o->x_max)
 122                         o->x_max = pos_x;
 123                 if (pos_y>o->y_max)
 124                         o->y_max = pos_y;
 125
 126                 /* is next pixel is good color */
 127                 if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){
 128                         pos_x = pos_x+v.x;
 129                         pos_y = pos_y+v.y;
 130                         len++;
 131                         vect_rot_retro(&v);
 132                         start = 1;
 133                         continue;
 134                 }
 135                 vect_rot_trigo(&v);
 136         }
 137
 138         o->len = len;
 139 }
 140
 141
 142
 143
 144
 145 /* step around object of given color and computes its polygon */
 146 void object_find_poly(unsigned char* data,
 147                       int16_t x_in, int16_t y_in,
 148                       int16_t start_x, int16_t start_y,
 149                       int16_t color, int16_t color_w, Object_poly* o)
 150 {
 151         int16_t pos_x = start_x;
 152         int16_t pos_y = start_y;
 153         int16_t start =0;
 154         int16_t len = 0;
 155         int16_t count_stand = 0;
 156         uint16_t pt_step, pt_num;
 157         vect_t v, vi;
 158
 159         v.x = 1;
 160         v.y = 0;
 161
 162         vi = v;
 163
 164         pt_step = o->len/POLY_MAX_PTS + 1;
 165
 166         pt_num = 0;
 167
 168         while(1){
 169                 if (pos_x == start_x && pos_y == start_y){
 170                         count_stand++;
 171                         if (count_stand>4)
 172                                 break;
 173                         if( v.x == vi.x && v.y == vi.y && start)
 174                                 break;
 175                 }
 176
 177                 /* is next pixel is good color */
 178                 if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){
 179                         pos_x = pos_x+v.x;
 180                         pos_y = pos_y+v.y;
 181                         len++;
 182
 183                         if (len >pt_num*pt_step){
 184                                 o->pts[pt_num].x = pos_x;
 185                                 o->pts[pt_num].y = pos_y;
 186                                 pt_num+=1;
 187                                 if (pt_num>=POLY_MAX_PTS)
 188                                         break;
 189                         }
 190
 191                         vect_rot_retro(&v);
 192                         start = 1;
 193                         continue;
 194                 }
 195                 vect_rot_trigo(&v);
 196         }
 197
 198         o->pts_num = pt_num;
 199
 200 }
 201
 202 #define PT_LEFT 0
 203 #define PT_RIGHT 2
 204 #define PT_TOP 1
 205 #define PT_DOWN 3
 206
 207 /* return most left/right/top/down pts indexes of given polygon */
 208 void object_find_extrem_points_index(Object_poly* o,
 209                                      unsigned int *pts)
 210 {
 211         unsigned int i;
 212         for (i=0;i<4;i++)
 213                 pts[i] = 0;
 214
 215
 216         for (i=1;i<o->pts_num;i++){
 217                 if (o->pts[i].x < o->pts[pts[PT_LEFT]].x)
 218                         pts[PT_LEFT] = i;
 219                 if (o->pts[i].x > o->pts[pts[PT_RIGHT]].x)
 220                         pts[PT_RIGHT] = i;
 221                 if (o->pts[i].y < o->pts[pts[PT_TOP]].y)
 222                         pts[PT_TOP] = i;
 223                 if (o->pts[i].y > o->pts[pts[PT_DOWN]].y)
 224                         pts[PT_DOWN] = i;
 225         }
 226
 227 }
 228
 229
 230 #define NUM_CALIPERS 4
 231 /* for debug purpose: display a vector on image */
 232 void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in,
 233                   vect_t *v, point_t p)
 234 {
 235         unsigned int i;
 236         float n;
 237         int16_t x, y;
 238         float coef=1.0;
 239
 240         if (!v->x && !v->y)
 241                 return;
 242
 243         n = vect_norm(v);
 244         coef = 1/n;
 245         for (i=0;i<5;i++){
 246                 x = p.x;
 247                 y = p.y;
 248
 249                 x+=(float)((v->x)*(float)i*(float)coef);
 250                 y+=(float)((v->y)*(float)i*(float)coef);
 251                 if ((x== p.x) && (y == p.y))
 252                         buf[y*x_in+x] = 0x0;
 253                 else
 254                         buf[y*x_in+x] = 0x0;
 255         }
 256
 257 }
 258
 259 #define CAL_X 3
 260 #define CAL_Y 0
 261
 262
 263 /* compute minimum rectangle area including the given convex polygon */
 264 void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out,
 265                             vect_t *v_out, vect_t *r1, vect_t*r2)
 266 {
 267         vect_t calipers[NUM_CALIPERS];
 268         vect_t edges[NUM_CALIPERS];
 269
 270         unsigned int i;
 271         unsigned int calipers_pts_index[NUM_CALIPERS];
 272         float angles[NUM_CALIPERS];
 273         float min_angle;
 274         float total_rot_angle = 0;
 275         int caliper_result_index;
 276
 277         vect_t res1, res2;
 278         float ps, n1, n2, caliper_n;
 279
 280         float aera_tmp;
 281         /* XXX hack sould be max*/
 282         float aera_min=0x100000;
 283
 284         object_find_extrem_points_index(o, calipers_pts_index);
 285
 286         calipers[0].x = 0;
 287         calipers[0].y = 1;
 288
 289         calipers[1].x = -1;
 290         calipers[1].y = 0;
 291
 292         calipers[2].x = 0;
 293         calipers[2].y = -1;
 294
 295         calipers[3].x = 1;
 296         calipers[3].y = 0;
 297
 298
 299         while (total_rot_angle <= M_PI/2){
 300
 301                 for (i=0;i<NUM_CALIPERS;i++){
 302                         /* compute polygon edge vector */
 303                         edges[i].x = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].x -
 304                                 o->pts[calipers_pts_index[i]].x;
 305                         edges[i].y = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].y -
 306                                 o->pts[calipers_pts_index[i]].y;
 307
 308                         /* compute angle between caliper and polygon edge */
 309                         angles[i] = vect_get_angle(&edges[i], &calipers[i]);
 310                 }
 311
 312                 /* find min angle */
 313                 min_angle = angles[0];
 314                 caliper_result_index = 0;
 315                 for (i=1;i<NUM_CALIPERS;i++){
 316                         if (angles[i]<min_angle){
 317                                 min_angle = angles[i];
 318                                 caliper_result_index = i;
 319                         }
 320                 }
 321
 322                 /* rotate calipers */
 323                 calipers[caliper_result_index] = edges[caliper_result_index];
 324
 325                 for (i=caliper_result_index; i<caliper_result_index + o->pts_num; i++){
 326                         calipers[(i+1) % NUM_CALIPERS] = calipers[i % NUM_CALIPERS];
 327                         vect_rot_trigo(&calipers[(i+1) % NUM_CALIPERS]);
 328                 }
 329
 330                 /* update calipers point */
 331                 for (i=0;i<NUM_CALIPERS;i++){
 332                         if (angles[i]==min_angle)
 333                                 calipers_pts_index[i] = (calipers_pts_index[i] + 1) % o->pts_num;
 334                 }
 335
 336                 res1.x = o->pts[calipers_pts_index[2]].x - o->pts[calipers_pts_index[0]].x;
 337                 res1.y = o->pts[calipers_pts_index[2]].y - o->pts[calipers_pts_index[0]].y;
 338
 339                 res2.x = o->pts[calipers_pts_index[3]].x - o->pts[calipers_pts_index[1]].x;
 340                 res2.y = o->pts[calipers_pts_index[3]].y - o->pts[calipers_pts_index[1]].y;
 341
 342                 ps = vect_pscal(&res1, &calipers[CAL_X]);
 343                 n1 = vect_norm(&res1);
 344                 caliper_n = vect_norm(&calipers[CAL_X]);
 345                 caliper_n*=caliper_n;
 346
 347                 res1 = calipers[CAL_X];
 348
 349                 res1.x *= ps/(caliper_n);
 350                 res1.y *= ps/(caliper_n);
 351
 352
 353                 ps = vect_pscal(&res2, &calipers[CAL_Y]);
 354                 n1 = vect_norm(&res2);
 355
 356                 res2 = calipers[CAL_Y];
 357
 358                 res2.x *= ps/(caliper_n);
 359                 res2.y *= ps/(caliper_n);
 360
 361                 n1 = vect_norm(&res1);
 362                 n2 = vect_norm(&res2);
 363
 364                 aera_tmp = n1*n2;
 365
 366                 if (aera_min >aera_tmp){
 367                         aera_min = aera_tmp;
 368                         for (i=0;i<NUM_CALIPERS;i++){
 369                                 pts_index_out[i] = calipers_pts_index[i];
 370                         }
 371                         *v_out = calipers[0];
 372                         *r1 = res1;
 373                         *r2 = res2;
 374
 375                 }
 376                 total_rot_angle+=min_angle;
 377         }
 378
 379         return;
 380 }
 381
 382 #define COEF_CALIP 1
 383 /* transform caliper to rectangle coordinates */
 384 int object_poly_caliper_to_rectangle(Object_poly *o,
 385                                      unsigned int *pts_index_out, vect_t* caliper,
 386                                      vect_t *r1, vect_t*r2, point_t *p)
 387 {
 388         line_t l1, l2;
 389         vect_t caliper_tmp;
 390         int ret, i;
 391         double mp_x, mp_y;
 392         point_t p_int1;//, p_int2;
 393
 394         point_t p1, p2;
 395
 396         caliper_tmp = *caliper;
 397
 398         //IMG_DEBUG("cal: %" PRIi32 " %" PRIi32 "", caliper_tmp.x, caliper_tmp.y);
 399
 400         mp_x = 0;
 401         mp_y = 0;
 402
 403         /* to be precise, calc 4 intersection of 4 calipers */
 404         for (i=0;i<NUM_CALIPERS;i++){
 405                 p1.x = o->pts[pts_index_out[i]].x;
 406                 p1.y = o->pts[pts_index_out[i]].y;
 407
 408                 p2.x = p1.x+COEF_CALIP*caliper_tmp.x;
 409                 p2.y = p1.y+COEF_CALIP*caliper_tmp.y;
 410
 411                 pts2line(&p1, &p2, &l1);
 412
 413                 vect_rot_trigo(&caliper_tmp);
 414
 415                 p1.x = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].x;
 416                 p1.y = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].y;
 417
 418                 p2.x = p1.x+COEF_CALIP*caliper_tmp.x;
 419                 p2.y = p1.y+COEF_CALIP*caliper_tmp.y;
 420
 421                 pts2line(&p1, &p2, &l2);
 422
 423                 ret = intersect_line(&l1, &l2, &p_int1);
 424                 if (ret!=1)
 425                         return 0;
 426                 //IMG_DEBUG("int1 (%d): %" PRIi32 " %" PRIi32 " ", ret, p_int1.x, p_int1.y);
 427
 428                 mp_x+=p_int1.x;
 429                 mp_y+=p_int1.y;
 430
 431         }
 432
 433
 434
 435         p->x = lround(mp_x/NUM_CALIPERS);
 436         p->y = lround(mp_y/NUM_CALIPERS);
 437
 438
 439         return 1;
 440
 441 }
 442
 443 #define OBJECT_DIM 5
 444 /* split zone in many column's sized area */
 445 int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color)
 446 {
 447         int n1, n2;
 448         int i, j;
 449         int index=0;
 450         int r1_s, r2_s;
 451         point_t ptmp;
 452
 453
 454         n1 = vect_norm(r1);
 455         n2 = vect_norm(r2);
 456
 457         r1_s = n1/OBJECT_DIM;
 458         r2_s = n2/OBJECT_DIM;
 459
 460         if (!r1_s || ! r2_s)
 461                 return 0;
 462
 463         ptmp.x = p->x - r1->x/2 - r2->x/2 + (r1->x/(r1_s*2))+(r2->x/(r2_s*2));
 464         ptmp.y = p->y - r1->y/2 - r2->y/2 + (r1->y/(r1_s*2))+(r2->y/(r2_s*2));
 465
 466         for(i=0;i<r1_s;i++){
 467                 for(j=0;j<r2_s;j++){
 468                         zones[index].p.x = ptmp.x + (i*r1->x)/r1_s+(j*r2->x)/r2_s;
 469                         zones[index].p.y = ptmp.y + (i*r1->y)/r1_s+(j*r2->y)/r2_s;
 470                         zones[index].h = color;
 471                         zones[index].valid = 1;
 472
 473                         index++;
 474                         if (index>=max_zone)
 475                                 return index;
 476
 477
 478                 }
 479         }
 480
 481         return index;
 482 }
 483
 484 #define OBJECT_SEMI_DIM (OBJECT_DIM/2)
 485 #define MIN_SURFACE_PERCENT 50
 486 #define HIGHER_MAX_PIXEL 5
 487
 488
 489 int zone_has_enought_pixels(unsigned char* data, int16_t x_in, int16_t y_in, zone* z)
 490 {
 491         int x, y;
 492         uint16_t count, total_pix, higher_pixels;
 493
 494         count = 0;
 495         total_pix=0;
 496         higher_pixels = 0;
 497
 498         for (x = -OBJECT_SEMI_DIM;
 499              (x <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL);
 500              x++){
 501                 for (y = -OBJECT_SEMI_DIM;
 502                      (y <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL);
 503                      y++){
 504                         total_pix++;
 505                         if (data[x_in * (y + z->p.y) + x + z->p.x] == z->h)
 506                                 count++;
 507
 508                         if (data[x_in * (y + z->p.y) + x + z->p.x] > z->h)
 509                                 higher_pixels++;
 510
 511                 }
 512
 513         }
 514
 515         IMG_DEBUG("has enougth pixel (h: %d x %"PRIi32": y:%"PRIi32") total: %d/%d (tt: %d, hmax: %d)", z->h, z->p.x, z->p.y,
 516                   count, (total_pix *  MIN_SURFACE_PERCENT) / 100, total_pix, higher_pixels);
 517
 518         if ((count > (total_pix *  MIN_SURFACE_PERCENT) / 100) &&
 519             (higher_pixels <HIGHER_MAX_PIXEL))
 520                 return 1;
 521
 522         return 0;
 523 }
 524
 525 int zone_filter_min_surface(unsigned char* data, int16_t x_in, int16_t y_in,
 526                             uint8_t color, unsigned int zones_num, zone* p)
 527 {
 528         int i;
 529
 530         for (i = 0; i < zones_num ; i++){
 531                 if (zone_has_enought_pixels(data, x_in, y_in, &p[i]))
 532                         continue;
 533
 534                 p[i].valid = 0;
 535         }
 536
 537         IMG_NOTICE("num zone after min surf: %d", zones_num);
 538
 539         return zones_num;
 540
 541 }
 542
 543 /* center is 15 cm radius*/
 544 #define CENTER_RADIUS 15
 545
 546 /* the complete column must be in the drop zone*/
 547 #define CENTER_MAX_DIST (15-3)
 548
 549 /* the column must not be too close from center*/
 550 #define CENTER_MIN_DIST (8)
 551
 552 int zone_filter_center(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y, int tweak_min_margin)
 553 {
 554         int i;
 555         vect_t v;
 556
 557         for (i = 0; i < zones_num; i++){
 558
 559                 v.x = p[i].p.x - center_x;
 560                 v.y = p[i].p.y - center_y;
 561                 IMG_DEBUG("square dist to center %"PRIi32" (%d %d)",
 562                           v.x*v.x + v.y*v.y,  (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin), CENTER_MAX_DIST * CENTER_MAX_DIST);
 563
 564                 if (v.x*v.x + v.y*v.y < CENTER_MAX_DIST * CENTER_MAX_DIST &&
 565                     v.x*v.x + v.y*v.y > (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin))
 566                         continue;
 567
 568                 p[i].valid = 0;
 569
 570         }
 571
 572         return zones_num;
 573 }
 574
 575 #define MAX_DIST_TO_ZONE 2
 576
 577 unsigned int zone_filter_zone_rect(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y , uint8_t working_zone)
 578 {
 579         int i;
 580
 581         for (i = 0; i < zones_num; i++){
 582
 583                 IMG_DEBUG("rct x:%"PRIi32" y:%"PRIi32" (centerx: %d)",p[i].p.x , p[i].p.y,  center_x);
 584
 585                 if ((p[i].p.x > center_x - MAX_DIST_TO_ZONE) && (p[i].h > working_zone))
 586                         continue;
 587
 588                 p[i].valid = 0;
 589         }
 590
 591         return zones_num;
 592 }
 593
 594
 595 /* delete point to render polygon convex */
 596 int object_poly_to_convex(Object_poly *o)
 597 {
 598         unsigned int i, j;
 599         vect_t v, w;
 600         int16_t z;
 601         unsigned int del_pts_num = 0;
 602
 603         for (i=0;i<o->pts_num;){
 604                 v.x = o->pts[(i + o->pts_num - 1)%o->pts_num].x - o->pts[i].x;
 605                 v.y = o->pts[(i + o->pts_num - 1)%o->pts_num].y - o->pts[i].y;
 606
 607                 w.x = o->pts[(i+1)%o->pts_num].x - o->pts[i].x;
 608                 w.y = o->pts[(i+1)%o->pts_num].y - o->pts[i].y;
 609
 610                 z = vect_pvect(&v, &w);
 611                 if (z>0){
 612                         i+=1;
 613                         continue;
 614                 }
 615
 616                 /* found a convex angle (or colinear points) */
 617                 for (j = i; j < o->pts_num-1; j++){
 618                         o->pts[j] = o->pts[j+1];
 619                 }
 620                 if (i!=0)
 621                         i-=1;
 622                 o->pts_num--;
 623                 del_pts_num++;
 624         }
 625
 626         return del_pts_num;
 627 }
 628
 629
 630
 631
 632
 633 #define DEFAULT_COLOR 255
 634 /* scan all image and find objects*/
 635 unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in,
 636                            Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj)
 637 {
 638         int16_t i, obj_num;
 639
 640         uint8_t in_color=0;
 641         int16_t etat;
 642
 643         Object_bb o;
 644         int ret;
 645
 646         obj_num = 0;
 647         /*
 648           first, delete borders
 649         */
 650         for (i=0;i<x_in;i++){
 651                 data[i] = 0;
 652                 data[(y_in - 1) * x_in + i] = 0;
 653         }
 654
 655         for (i=0;i<y_in;i++){
 656                 data[i * x_in] = 0;
 657                 data[i * x_in + x_in - 1] = 0;
 658         }
 659
 660
 661
 662         etat = 0;
 663         /*
 664           0 look for color (object or edge)
 665           1 look for edge end
 666         */
 667
 668         for (i=1;i<x_in*y_in;i++){
 669                 switch(etat){
 670                 case 0:
 671                         //we are in the dark
 672                         switch(data[i]){
 673                         case 0:
 674                                 //look for in dark
 675                                 break;
 676                                 /*
 677                                   case 1:
 678                                   case 2:
 679                                   case 0x15:
 680                                   case 0x24:
 681                                 */
 682                         default:
 683                                 in_color = data[i];
 684                                 etat = 1;
 685                                 // we found an object
 686                                 object_find_bounding_box(data, x_in, y_in, (i-1)%x_in, (i-1)/x_in, data[i], 255, &o);
 687
 688                                 ret = store_obj(sac_obj,  max_obj, &o);
 689                                 /* if new object, process rotating calipers */
 690                                 if (ret){
 691                                         sac_obj_poly[obj_num].len = o.len;
 692                                         object_find_poly(data, x_in, y_in,
 693                                                          (i-1)%x_in, (i-1)/x_in,
 694                                                          data[i], 255, &sac_obj_poly[obj_num]);
 695                                         IMG_DEBUG("%d",sac_obj_poly[obj_num].pts_num);
 696                                         object_poly_to_convex(&sac_obj_poly[obj_num]);
 697                                         /*
 698                                           for (j=0;j<sac_obj_poly[obj_num].pts_num;j++){
 699                                           data[sac_obj_poly[obj_num].pts[j].y*x_in + sac_obj_poly[obj_num].pts[j].x] = 8;
 700                                           }*/
 701                                         sac_obj_poly[obj_num].color = data[i];
 702                                         obj_num++;
 703                                 }
 704
 705                                 break;
 706
 707                         case DEFAULT_COLOR:
 708                                 //we must out of color
 709                                 break;
 710                         }
 711                         break;
 712
 713                 case 1:
 714                         if (data[i] != in_color){
 715                                 i--;
 716                                 etat = 0;
 717                         }
 718                         /*
 719                           we are in a color and want to go out of it
 720                         */
 721                         break;
 722
 723                 }
 724
 725
 726
 727         }
 728
 729
 730         return data;
 731
 732
 733 }
 734 /* space between twin tower is 13 pixels*/
 735 #define SPACE_INTER_TWIN_TOWER (13)
 736
 737 #define SPACE_INTER_TWIN_TOWER_TOLERENCE 3
 738
 739
 740
 741 /* find best twin tower for each zone */
 742 void find_twin_tower(uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE],
 743                      int16_t center_x, int16_t center_y)
 744 {
 745
 746         uint8_t i, j;
 747         uint8_t z_n;
 748         int n1, n2;
 749         unsigned int z1, z2, z3;
 750         //int32_t scal1, scal2;
 751         vect_t v, v1, v2;
 752         line_t l;
 753         point_t p;
 754         unsigned int good_zone;
 755         double dist, dist2;
 756         unsigned int current_sister;
 757
 758         /* init dummy sisters */
 759         for (i = 0; i < zones_num; i++)
 760                 for (j = 0; j < MAX_SISTER_PER_ZONE; j++)
 761                         sisters[i][j] = -1;
 762
 763
 764
 765         for (z_n = 0; z_n < zones_num; z_n++){
 766                 if (!zones[z_n].valid)
 767                         continue;
 768
 769                 current_sister = 0;
 770
 771                 for (i = 0; i < zones_num; i++){
 772
 773
 774                         /* we already have max sisters */
 775                         if (current_sister >= MAX_SISTER_PER_ZONE)
 776                                 break;
 777
 778
 779                         if (!zones[i].valid)
 780                                 continue;
 781
 782
 783                         if (i == z_n)
 784                                 continue;
 785
 786                         /* twin tower must have same high */
 787                         if (zones[i].h != zones[z_n].h)
 788                                 continue;
 789
 790                         IMG_DEBUG("test sisters (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")",
 791                                   zones[z_n].p.x,   zones[z_n].p.y,
 792                                   zones[i].p.x,   zones[i].p.y);
 793
 794
 795                         dist = sqrt( (zones[i].p.x - zones[z_n].p.x) * (zones[i].p.x - zones[z_n].p.x) +
 796                                      (zones[i].p.y - zones[z_n].p.y) * (zones[i].p.y - zones[z_n].p.y) );
 797
 798
 799                         IMG_DEBUG("sister space is %2.2f may be near %d", dist, SPACE_INTER_TWIN_TOWER);
 800
 801                         /*
 802                            twin tower must be close/far enought to drop lintel
 803                          */
 804                         if (ABS(dist - SPACE_INTER_TWIN_TOWER) > SPACE_INTER_TWIN_TOWER_TOLERENCE)
 805                                 continue;
 806
 807
 808                         pts2line(&zones[i].p, &zones[z_n].p, &l);
 809
 810                         /*
 811                            test the paralelism of the temple:
 812                            zone may be on same distance from center
 813                         */
 814
 815
 816                         v1.x = zones[z_n].p.x - center_x;
 817                         v1.y = zones[z_n].p.y - center_y;
 818
 819                         dist = vect_norm(&v1);
 820
 821                         v2.x = zones[i].p.x - center_x;
 822                         v2.y = zones[i].p.y - center_y;
 823
 824                         dist2 = vect_norm(&v2);
 825
 826                         IMG_DEBUG("zone dist %2.2f %2.2f", dist, dist2);
 827                         if (ABS(dist-dist2) > 3){
 828                                 IMG_DEBUG("bad parallelism %2.2f", ABS(dist-dist2));
 829                                 continue;
 830                         }
 831
 832
 833
 834
 835                         /* no other aligned tower to avoid dropping on a lintel
 836                          *  (3 aligned zone may mean lintel)
 837                          */
 838
 839                         good_zone = 1;
 840
 841                         for (j = 0; j < zones_num; j++){
 842                                 if (j==i ||j == z_n)
 843                                         continue;
 844
 845                                 /* if third zone, but lower */
 846                                 if (zones[j].h <= zones[i].h)
 847                                         continue;
 848
 849                                 /*
 850                                    check distance from dropping zone to
 851                                    line formed by twin towers
 852                                 */
 853
 854                                 proj_pt_line(&zones[j].p, &l, &p);
 855
 856
 857                                 /* test if projected point is in the segement */
 858
 859
 860                                 v.x = zones[z_n].p.x - zones[i].p.x;
 861                                 v.y = zones[z_n].p.y - zones[i].p.y;
 862
 863                                 v1.x = p.x - zones[i].p.x;
 864                                 v1.y = p.y - zones[i].p.y;
 865
 866                                 n1 = vect_pscal_sign(&v, &v1);
 867
 868                                 v.x = -v.x;
 869                                 v.y = -v.y;
 870
 871                                 v1.x = p.x - zones[z_n].p.x;
 872                                 v1.y = p.y - zones[z_n].p.y;
 873
 874
 875                                 n2 =vect_pscal_sign(&v, &v1);
 876
 877                                 v.x = p.x - zones[j].p.x;
 878                                 v.y = p.y - zones[j].p.y;
 879
 880                                 dist = vect_norm(&v);
 881                                 IMG_DEBUG("dist pt  h %d n: (%d %d) (%"PRIi32" %"PRIi32") to line %2.2f", zones[j].h, n1, n2, zones[j].p.x, zones[j].p.y, dist);
 882
 883
 884                                 if ((n1>=0 && n2>=0) && dist < OBJECT_DIM+2.){
 885                                         good_zone = 0;
 886                                         break;
 887                                 }
 888
 889
 890                                 /* test if zone is far from points*/
 891
 892                                 v1.x = zones[j].p.x - zones[z_n].p.x;
 893                                 v1.y = zones[j].p.y - zones[z_n].p.y;
 894
 895                                 dist = vect_norm(&v1);
 896                                 IMG_DEBUG("dist pt to z1 %2.2f", dist);
 897
 898                                 if (dist < OBJECT_DIM){
 899                                         good_zone = 0;
 900                                         break;
 901                                 }
 902
 903                                 v2.x = zones[j].p.x - zones[i].p.x;
 904                                 v2.y = zones[j].p.y - zones[i].p.y;
 905
 906
 907                                 dist = vect_norm(&v2);
 908                                 IMG_DEBUG("dist pt to z2 %2.2f", dist);
 909
 910                                 if (dist < OBJECT_DIM){
 911                                         good_zone = 0;
 912                                         break;
 913                                 }
 914
 915
 916
 917                                 z1 = i;
 918                                 z2 = z_n;
 919                                 z3 = j;
 920
 921
 922
 923
 924
 925                                 /*
 926                                   XXX may be a lintel on lintel !!
 927                                  */
 928
 929                         }
 930
 931                         if (!good_zone)
 932                                 continue;
 933
 934                         IMG_DEBUG("sisters ok (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")",
 935                                   zones[z_n].p.x,   zones[z_n].p.y,
 936                                   zones[i].p.x,   zones[i].p.y);
 937
 938
 939                         sisters[z_n][current_sister] = i;
 940                         current_sister++;
 941                 }
 942         }
 943 }
 944
 945 /* test if a higher zone is too close */
 946 int test_close_zone(uint8_t zones_num, zone* zones, unsigned int z_n)
 947 {
 948         uint8_t i;
 949         vect_t v;
 950         double dist;
 951
 952         for (i = 0; i < zones_num; i++){
 953                 if (i == z_n)
 954                         continue;
 955                 if (zones[i].h <= zones[z_n].h)
 956                         continue;
 957
 958                 v.x = zones[i].p.x - zones[z_n].p.x;
 959                 v.y = zones[i].p.y - zones[z_n].p.y;
 960
 961                 dist = vect_norm(&v);
 962                 //IMG_DEBUG("dist pt to pt %2.2f", dist);
 963
 964                 if (dist < OBJECT_DIM){
 965                         return 1;
 966                 }
 967
 968         }
 969
 970         return 0;
 971 }
 972
 973 #define MAX_COLUMN 4
 974 #define MAX_LINTEL 2
 975
 976 drop_column_zone drop_c[MAX_COLUMN];
 977 drop_lintel_zone drop_l[MAX_LINTEL];
 978
 979
 980 void reset_drop_zone(void)
 981 {
 982         memset(drop_c, 0, sizeof(drop_c));
 983         memset(drop_l, 0, sizeof(drop_l));
 984
 985 }
 986
 987
 988 void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones)
 989 {
 990         unsigned int i;
 991
 992         for (i=0;i<n_columns;i++)
 993                 IMG_NOTICE("c %d:(h:%d) (%"PRIi32" %"PRIi32") valid=%d",
 994                              i, drop_c[i].h,
 995                              zones[drop_c[i].z].p.x, zones[drop_c[i].z].p.y,
 996                              drop_c[i].valid);
 997
 998         for (i=0;i<n_lintels;i++)
 999                 IMG_NOTICE("l %d:(h:%d) (%"PRIi32" %"PRIi32") "
1000                              "(%"PRIi32" %"PRIi32") valid=%d",
1001                              i, drop_l[i].h,
1002                              zones[drop_l[i].z1].p.x, zones[drop_l[i].z1].p.y,
1003                              zones[drop_l[i].z2].p.x, zones[drop_l[i].z2].p.y, drop_l[i].valid);
1004
1005 }
1006
1007 #define MY_MAX(a, b) ((a)>(b)?(a):(b))
1008
1009
1010 #if 0
1011 #define MAX_DROP_HIGH  8
1012
1013
1014 /*
1015    recursive function to maximize points during object
1016    dropping, given lintel/column number
1017    working zone may be 1, 2 or 3
1018  */
1019 unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max,
1020                                     uint8_t n_columns, uint8_t n_lintels,
1021                                     uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone)
1022 {
1023
1024         uint8_t i, j;
1025         unsigned int points_calc;
1026         //unsigned int points_added = 0;
1027         int sister;
1028         int ret;
1029
1030
1031         /* if no more objects, return points */
1032         if (n_columns == 0 && n_lintels == 0)
1033                 return MY_MAX(points, points_max);
1034
1035         /* start by putting columns if so */
1036         for (i = 0; i < zones_num; i++){
1037                 if (zones[i].h >= MAX_DROP_HIGH)
1038                         continue;
1039
1040                 if (n_columns){
1041
1042                         ret = test_close_zone(zones_num, zones, i);
1043                         if (ret)
1044                                 continue;
1045
1046                         zones[i].h++;
1047                         points_calc = solve_objects_dropping(points + zones[i].h, points_max,
1048                                                              n_columns - 1, n_lintels,
1049                                                              zones_num, zones, sisters, working_zone);
1050
1051                         if (points_calc > points_max){
1052                                 points_max = points_calc;
1053                                 drop_c[n_columns - 1].z = i;
1054                                 drop_c[n_columns - 1].h = zones[i].h;
1055                                 drop_c[n_columns - 1].valid  = 1;
1056
1057                         }
1058                         zones[i].h--;
1059                 }
1060                 /* we must depose all columns before dropping lintels */
1061                 else if (n_lintels){
1062
1063                         /* dont drop lintel on ground */
1064                         if (zones[i].h  <= working_zone)
1065                                 continue;
1066
1067                         /* XXX todo need get second zone near selected one */
1068                         //ret = find_twin_tower(zones_num, zones, i, &sister);
1069
1070                         for (j = 0; j < MAX_SISTER_PER_ZONE; j++){
1071                                 sister = sisters[i][j];
1072                                 if (sister == -1)
1073                                         break;
1074                                 if (zones[i].h != zones[sister].h){
1075                                         sister = -1;
1076                                 }
1077
1078                         }
1079
1080                         if (sister == -1)
1081                                 continue;
1082                         //IMG_DEBUG("sister found: %d %d (h=%d %p)", i, sister, zones[i].h, &zones[i].h);
1083
1084                         zones[i].h++;
1085                         zones[sister].h++;
1086
1087
1088                         points_calc = solve_objects_dropping(points + zones[i].h * 3, points_max,
1089                                                              n_columns, n_lintels - 1,
1090                                                              zones_num, zones, sisters, working_zone);
1091
1092                         if (points_calc > points_max){
1093                                 points_max = points_calc;
1094
1095                                 drop_l[n_lintels - 1].z1 = i;
1096                                 drop_l[n_lintels - 1].z2 = sister;
1097                                 drop_l[n_lintels - 1].h = zones[i].h;
1098                                 drop_l[n_lintels - 1].valid = 1;
1099
1100                         }
1101
1102
1103                         zones[sister].h--;
1104                         zones[i].h--;
1105                 }
1106         }
1107
1108         return MY_MAX(points, points_max);
1109 }
1110 #endif
1111
1112
1113 /*  */
1114 int find_column_dropzone(uint8_t zones_num, zone* zones)
1115 {
1116         uint8_t i;
1117
1118         uint8_t z_n = 0;
1119
1120         if (zones_num <= 0)
1121                 return -1;
1122
1123         for (i = 0; i < zones_num; i++){
1124                 if (!zones[i].valid)
1125                         continue;
1126                 if (zones[i].h > zones[z_n].h)
1127                         z_n = i;
1128         }
1129
1130
1131         /*
1132            now, chose dropzone closest to robot
1133            meaning little x, big y
1134            so maximise y-x
1135          */
1136         for (i = 0; i < zones_num; i++){
1137                 if (zones[i].h != zones[z_n].h)
1138                         continue;
1139                 if (!zones[i].valid)
1140                         continue;
1141                 if (zones[i].p.y - zones[i].p.x > zones[z_n].p.y - zones[z_n].p.x)
1142                         z_n = i;
1143         }
1144
1145
1146
1147         return z_n;
1148 }
1149
1150
1151
1152 uint8_t color2h(uint8_t color)
1153 {
1154         return (0x100-color)/0x20;
1155 }
1156
1157 uint8_t h2color(uint8_t color)
1158 {
1159         return color*0x20;
1160 }
1161
1162
1163 #define NUM_ZONE_GENERATE 8
1164 #define DIST_ZONE_GEN 9
1165
1166 /*
1167   remove zone at ground level, and generate zones on
1168   a circle at X cm from center
1169  */
1170 unsigned int generate_center_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in,
1171                                           zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y)
1172 {
1173         double c_a, s_a;
1174         uint8_t i, j;
1175         double px1, py1, px2, py2;
1176
1177
1178         /* first del zone at level 2 */
1179         for (i = 0; i < zones_num; ){
1180                 if (zones[i].h!=2){
1181                         i++;
1182                         continue;
1183                 }
1184
1185                 for (j = i; j < zones_num-1; j++)
1186                         zones[j] = zones[j+1];
1187
1188                 zones_num--;
1189
1190         }
1191
1192         /* generate zones around circle  */
1193
1194         c_a = cos(2*M_PI/NUM_ZONE_GENERATE);
1195         s_a = sin(2*M_PI/NUM_ZONE_GENERATE);
1196
1197         px1 = DIST_ZONE_GEN;
1198         py1 = 0;
1199
1200         for (i = 0; i < NUM_ZONE_GENERATE; i++){
1201
1202                 zones[zones_num].p.x = center_x + px1;
1203                 zones[zones_num].p.y = center_y + py1;
1204                 zones[zones_num].h = 2;
1205                 zones[zones_num].valid = 1;
1206
1207
1208                 px2 = px1*c_a + py1*s_a;
1209                 py2 = -px1*s_a + py1*c_a;
1210
1211                 px1 = px2;
1212                 py1 = py2;
1213
1214                 /* skip zone if it is not in img */
1215                 if (zones[zones_num].p.x < 0 ||  zones[zones_num].p.y < 0 ||
1216                     zones[zones_num].p.x >= x_in ||  zones[zones_num].p.y > y_in)
1217                         continue;
1218
1219                 /* skip zone if not enougth pixels */
1220                 if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num]))
1221                         continue;
1222
1223                 zones_num++;
1224                 if (zones_num >= max_zones)
1225                         break;
1226
1227         }
1228
1229         return zones_num;
1230
1231
1232 }
1233
1234
1235 /*
1236   remove zone at ground level, and generate zones on
1237   a line at X cm from robot
1238  */
1239 unsigned int generate_rectangle_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in,
1240                                              zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y,
1241                                              uint8_t working_zone)
1242 {
1243         uint8_t i, j;
1244         uint8_t y;
1245
1246         /* first del zone at level i */
1247         for (i = 0; i < zones_num; ){
1248                 if (zones[i].h != working_zone ){
1249                         i++;
1250                         continue;
1251                 }
1252
1253                 for (j = i; j < zones_num-1; j++)
1254                         zones[j] = zones[j+1];
1255
1256                 zones_num--;
1257         }
1258
1259
1260         /* generate zones on a line  */
1261         for (y = OBJECT_DIM; y < y_in; y+=OBJECT_DIM){
1262
1263                 zones[zones_num].p.x = center_x;
1264                 zones[zones_num].p.y = y;
1265                 zones[zones_num].h = working_zone ;
1266                 zones[zones_num].valid = 1;
1267
1268                 if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num]))
1269                         continue;
1270                 zones_num++;
1271
1272                 if (zones_num >= max_zones)
1273                         break;
1274
1275         }
1276         return zones_num;
1277
1278 }
1279
1280
1281 #define MAX_DECAL_LINE 5
1282 #define ENOUGHT_ZONE_PIXEL 2
1283
1284 void recal_img_y(unsigned char* buffer, int16_t x_in, int16_t y_in,
1285                  uint8_t working_zone)
1286 {
1287         uint8_t i, j;
1288         uint8_t cpt;
1289
1290         /* recal img only for central zone */
1291         if (working_zone !=2)
1292                 return;
1293
1294         for (i = 0; i < MAX_DECAL_LINE; i++){
1295                 cpt = 0;
1296                 for (j = 0; j < x_in; j++){
1297                         if (buffer[i*x_in + j] ==2)
1298                                 cpt++;
1299                 }
1300
1301                 if (cpt >= ENOUGHT_ZONE_PIXEL)
1302                         break;
1303         }
1304
1305         memmove(buffer, &buffer[i * x_in], x_in * y_in - i*x_in);
1306         memset(&buffer[x_in * y_in - i * x_in], 0,  i*x_in);
1307 }
1308
1309
1310 #define MAX_OBJECTS 20
1311
1312 #define MAX_ZONES_PER_OBJECT 20
1313
1314
1315 uint8_t g_zone_num;
1316 zone g_all_zones[MAX_ZONES];
1317
1318 uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in,
1319                     zone * all_zones, uint8_t max_zones)
1320 {
1321
1322         int ret;
1323         int i, j;
1324
1325         zone zones[MAX_ZONES_PER_OBJECT];
1326
1327         vect_t caliper;
1328         unsigned int pts_cal[4];
1329         point_t ptmp;
1330         vect_t r1, r2;
1331         int zone_len;
1332
1333
1334         uint8_t zone_num = 0;
1335
1336         Object_bb sac_obj[MAX_OBJECTS];
1337         Object_poly sac_obj_poly[MAX_OBJECTS];
1338
1339
1340
1341         /*
1342            XXX fix: only decal for working zone 2/(1?)
1343            but we dont have info yet
1344         */
1345         recal_img_y(buffer, x_in, y_in, 2);
1346
1347         memset(sac_obj, 0, sizeof(sac_obj));
1348         memset(sac_obj_poly, 0, sizeof(sac_obj_poly));
1349
1350
1351         /* first, find polygons*/
1352         parcour_img(buffer, x_in, y_in, sac_obj, sac_obj_poly, MAX_OBJECTS);
1353
1354         /* enclose each poygon in the min area polygon
1355            then, split each rectangle in dropping zone
1356         */
1357         for (i=0;i<MAX_OBJECTS;i++){
1358
1359                 if (!sac_obj_poly[i].pts_num)
1360                         continue;
1361
1362                 IMG_DEBUG("obj: %d %d %d %d %d",
1363                           i,
1364                           sac_obj[i].x_min,
1365                           sac_obj[i].y_min,
1366                           sac_obj[i].x_max,
1367                           sac_obj[i].y_max);
1368
1369                 //IMG_DEBUG("poly pts_num: %d", sac_obj_poly[i].pts_num);
1370
1371                 object_poly_get_min_ar(&sac_obj_poly[i], &pts_cal[0], &caliper, &r1, &r2);
1372
1373                 ret = object_poly_caliper_to_rectangle(&sac_obj_poly[i], &pts_cal[0], &caliper,
1374                                                        &r1, &r2, &ptmp);
1375
1376                 if (!ret)
1377                         continue;
1378
1379                 /*
1380                 IMG_DEBUG("r: (%3"PRIi32" %3"PRIi32")  "
1381                              "(%3"PRIi32" %3"PRIi32")",
1382                              r1.x, r1.y, r2.x, r2.y);
1383                 IMG_DEBUG("intersection: %"PRIi32" %"PRIi32"",
1384                              ptmp.x, ptmp.y);
1385                 */
1386
1387                 zone_len = split_rectangle(&ptmp, &r1, &r2,
1388                                               MAX_ZONES_PER_OBJECT, &zones[0], sac_obj_poly[i].color);
1389                 //IMG_DEBUG("split ok %d", zone_len);
1390
1391                 zone_len =  zone_filter_min_surface(buffer, x_in, y_in,
1392                                                     sac_obj_poly[i].color,
1393                                                     zone_len, &zones[0]);
1394
1395                 for (j = 0; j < zone_len && zone_num < max_zones; zone_num++, j++)
1396                         all_zones[zone_num] = zones[j];
1397
1398         }
1399
1400
1401         IMG_NOTICE("num zones end: %d", zone_num);
1402
1403         return zone_num;
1404 }
1405
1406
1407 void process_img_to_zone(unsigned char *buffer, int16_t x_in, int16_t y_in)
1408 {
1409         g_zone_num = process_img(buffer, x_in, y_in,
1410                                  g_all_zones, MAX_ZONES);
1411 }
1412
1413 uint8_t filter_zones(unsigned char *buffer, int16_t x_in, int16_t y_in,
1414                      zone * all_zones, uint8_t zone_num, uint8_t max_zones,
1415                      uint8_t working_zone, int16_t center_x, int16_t center_y,
1416                      int tweak_min_margin)
1417 {
1418         uint8_t i;
1419
1420         /* first valid all zones */
1421         for (i = 0; i < zone_num; i++){
1422                 all_zones[i].valid = 1;
1423
1424                 /* filter zone lower thatn working zone */
1425                 if (all_zones[i].h  < working_zone)
1426                         all_zones[i].valid = 0;
1427         }
1428
1429
1430         /*
1431            generate correct zone at ground level
1432            (depending on working zone)
1433         */
1434         if (working_zone == 2)
1435                 zone_num =  generate_center_ground_zones(buffer, x_in, y_in,
1436                                                          all_zones, zone_num, max_zones, center_x, center_y);
1437         else
1438                 zone_num =  generate_rectangle_ground_zones(buffer, x_in, y_in,
1439                                                             all_zones, zone_num, max_zones, center_x, center_y,
1440                                                             working_zone);
1441
1442         /* filter zone position, depending on workingzone */
1443         if (working_zone == 2)
1444                 zone_num = zone_filter_center(zone_num, all_zones, center_x, center_y, tweak_min_margin);
1445         else
1446                 zone_num = zone_filter_zone_rect(zone_num, all_zones, center_x, center_y , working_zone);
1447
1448
1449
1450         /* display zones (debug purpose) */
1451
1452         for (i = 0; i < zone_num; i++){
1453                 //buffer[all_zones[i].p.y*x_in+all_zones[i].p.x] = 0x3;
1454                 IMG_NOTICE("h:%d (v:%d) x:%"PRIi32" y:%"PRIi32"", all_zones[i].h, all_zones[i].valid, all_zones[i].p.x, all_zones[i].p.y);
1455
1456         }
1457
1458         IMG_NOTICE("num zones: %d", zone_num);
1459
1460
1461
1462
1463         return zone_num;
1464 }
1465
1466
1467 /*
1468   return -1 if not column dropzone is found
1469   return column hight if found
1470 */
1471 int8_t get_column_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in,
1472                            uint8_t working_zone, int16_t center_x, int16_t center_y,
1473                            int16_t * dropzone_x, int16_t * dropzone_y)
1474 {
1475         uint8_t zone_num;
1476         int c_drop_zone;
1477
1478
1479
1480         zone_num = filter_zones(buffer, x_in, y_in,
1481                                 g_all_zones, g_zone_num, MAX_ZONES,
1482                                 working_zone, center_x, center_y,
1483                                 0);
1484
1485         c_drop_zone = find_column_dropzone(zone_num, g_all_zones);
1486
1487         if (c_drop_zone<0)
1488                 return -1;
1489
1490         *dropzone_x = g_all_zones[c_drop_zone].p.x;
1491         *dropzone_y = g_all_zones[c_drop_zone].p.y;
1492
1493         *dropzone_x = (*dropzone_x) * PIXEL2CM;
1494         *dropzone_y = (*dropzone_y) * PIXEL2CM + 30;
1495
1496         return g_all_zones[c_drop_zone].h;
1497 }
1498
1499 #define ROBOT_SEMI_INTERCOLUMN_SPACE 75
1500
1501 uint8_t is_temple_there(unsigned char * buffer, int16_t x_in, int16_t y_in,
1502                         uint8_t h, int16_t center_x, int16_t center_y)
1503 {
1504         zone z;
1505         int ret;
1506         int i;
1507
1508
1509         IMG_NOTICE("test z (mm) : x:%d y:%d", center_x, center_y);
1510         IMG_NOTICE("test z:(pix): x:%d y:%d", (int)(center_x/ PIXEL2CM), (int)(center_y/ PIXEL2CM));
1511
1512
1513         z.p.x = center_x / PIXEL2CM;
1514         z.p.y = (center_y - ROBOT_SEMI_INTERCOLUMN_SPACE) / PIXEL2CM ;
1515         z.h = h;
1516         z.valid = 1;
1517         ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
1518         IMG_NOTICE("test z1: %d", ret);
1519
1520         if (!ret)
1521                 return 0;
1522
1523         z.p.x = center_x / PIXEL2CM;
1524         z.p.y = (center_y  + ROBOT_SEMI_INTERCOLUMN_SPACE)/ PIXEL2CM;
1525         z.h = h;
1526         z.valid = 1;
1527         ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
1528         IMG_NOTICE("test z2: %d", ret);
1529         if (!ret)
1530                 return 0;
1531
1532
1533         /*
1534            if middle zone is less or egual to tested temple
1535          */
1536         z.p.x = center_x / PIXEL2CM;
1537         z.p.y = center_y / PIXEL2CM;
1538
1539         for (i = h; i > 0; i--){
1540                 z.h = i;
1541                 z.valid = 1;
1542                 ret = zone_has_enought_pixels(buffer, x_in, y_in, &z);
1543                 IMG_NOTICE("test z3:(h=%d) %d", i, ret);
1544
1545                 if (ret)
1546                         return 1;
1547         }
1548
1549         return 0;
1550
1551 }
1552
1553
1554 int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in,
1555                             uint8_t working_zone, int16_t center_x, int16_t center_y,
1556                             int16_t * dropzone_x, int16_t * dropzone_y)
1557 {
1558         uint8_t zone_num;
1559         int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE];
1560         int8_t z_n = -1;
1561         uint8_t i, j;
1562
1563
1564         /* find all drop zone */
1565         zone_num = filter_zones(buffer, x_in, y_in,
1566                                 g_all_zones, g_zone_num, MAX_ZONES,
1567                                 working_zone, center_x, center_y,
1568                                 -2);
1569
1570         /* precompute possible twin towers */
1571         find_twin_tower(zone_num, g_all_zones, sisters, center_x, center_y);
1572
1573
1574         for (i=0; i< zone_num; i++){
1575                 IMG_DEBUG("all sisters: %d", i);
1576                 for (j=0;j<MAX_SISTER_PER_ZONE;j++){
1577                         IMG_DEBUG("s: %d", sisters[i][j]);
1578                 }
1579         }
1580
1581         /* only use first sister of each zone */
1582         for (i=0; i< zone_num; i++){
1583
1584                 /* if zone doesn't have twin tower */
1585                 if (sisters[i][0] == -1)
1586                         continue;
1587
1588                 if (!g_all_zones[i].valid)
1589                         continue;
1590
1591                 if (z_n == -1){
1592                         z_n = i;
1593                         continue;
1594                 }
1595
1596                 /* if we found higher twin tower */
1597                 if (g_all_zones[z_n].h > g_all_zones[i].h)
1598                         continue;
1599
1600                 z_n = i;
1601         }
1602
1603         IMG_NOTICE("twin tower found :z_n=%d", z_n);
1604         if (z_n ==  -1)
1605                 return -1;
1606
1607         IMG_NOTICE("(%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")",
1608                    g_all_zones[z_n].p.x, g_all_zones[z_n].p.y,
1609                    g_all_zones[sisters[z_n][0]].p.x, g_all_zones[sisters[z_n][0]].p.y);
1610
1611
1612         *dropzone_x = ((double)(g_all_zones[z_n].p.x + g_all_zones[sisters[z_n][0]].p.x)*PIXEL2CM ) / 2;
1613         *dropzone_y = ((double)(g_all_zones[z_n].p.y + g_all_zones[sisters[z_n][0]].p.y)*PIXEL2CM ) / 2 + 30;
1614
1615
1616         return g_all_zones[z_n].h;
1617
1618 }
1619