/*
   Copyright 2005 The University of New South Wales (UNSW) and National  
   ICT Australia (NICTA).

   This file is part of the 2005 team rUNSWift RoboCup entry.  You may  
   redistribute it and/or modify it under the terms of the GNU General  
   Public License as published by the Free Software Foundation; either  
   version 2 of the License, or (at your option) any later version as  
   modified below.  As the original licensors, we add the following  
   conditions to that license:

   In paragraph 2.b), the phrase "distribute or publish" should be  
   interpreted to include entry into a competition, and hence the source  
   of any derived work entered into a competition must be made available  
   to all parties involved in that competition under the terms of this  
   license.

   In addition, if the authors of a derived work publish any conference  
   proceedings, journal articles or other academic papers describing that  
   derived work, then appropriate academic citations to the original work  
   should be included in that publication.

   This rUNSWift source is distributed in the hope that it will be useful,  
   but WITHOUT ANY WARRANTY; without even the implied warranty of  
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU  
   General Public License for more details.

   You should have received a copy of the GNU General Public License along  
   with this source code; if not, write to the Free Software Foundation,  
   Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

 */


/* SubObject.cc
 * Subsampling visual object recognition
 */

#include <iostream>

#include "SubObject.h"
#include "SanityChecks.h"
#include "Profile.h"

#ifndef OFFLINE
#include "Robolink.h"
#endif

//#define SUBOBJECT_DEBUG
//#define SUBOBJECT_WALL_DEBUG

#ifndef OFFLINE
#undef SUBOBJECT_DEBUG
#undef SUBOBJECT_WALL_DEBUG
#endif

using namespace std;

enum {
    // minimum number of ball features before we try chordball/projection
    MIN_BALLFEATURES_PROJECTION = 5,
    // minimum number of chords to attempt chord ball
    MIN_CHORDBALL_CHORDS = 3,
    // minimum bisector intersections to use chord ball
    MIN_CHORDBALL_INTERSECTS = 3,
    // minimum number of ball features to try repeated median ball estimation
    MIN_BALLFEATURES_MEDIANBALL = 4,
    // max ballfeatures we want. Using too many takes too long
    MAX_BALLFEATURES = 17, // this could probably be reduced further
    // minumum number of radii (features) before we average the middle ones
    MIN_RADII_AVERAGE = 7,
    // max non-orange pixels tolerated for ball blobs to be merged
    BALL_FLOOD_TOLERANCE = 0, // not yet implemented
    // search radius when looking for blue/yellow other half-beacon
    BEACON_COLOUR_SEARCH_RADIUS = 2, // gives 5*5 search: 17 pix
    // min blue/yellow to find in above radius for beacon
    BEACON_MIN_COLOUR = 6,
    // search radius when looking for colour in goal centre
    GOAL_COLOUR_SEARCH_RADIUS = 2, // gives 5*5 search
};

// Tolerance to distance of outlying features to the average distance
static const double POS_TOLERANCE = 1.5;


// Static list of beacon candidates (groups of features) used in findBeacons,
// and goal candidates for findGoals
static list<list<int> > beaconGroups;
static list<list<int> > goalGroups;
static list<list<int> > pinkGroups;

// number of features found. This tells us where the new list of features
// start if we search for more ball features later
static int nFeatures = 0;

void SubObject::setParameters(bool _hzExists,
                              double apan, 
                              double atilt, 
                              double acrane,
                              double roll,
                              double sinEroll, 
                              double cosEroll) { 
    hzExists  = _hzExists;
    pan = apan; 
    tilt = atilt;
    crane = acrane;
    mRoll     = roll;                       
    mSinEroll = sinEroll; 
    mCosEroll = cosEroll;
}

bool SubObject::isWallExist() {
    return wallExists;
}

/* Searches through the VisualFeatures generated by SubVision and
 * attempts to recognise objects among them. Information about these objects
 * is placed in the SubObject::v* fields. The items filled in are:
 * cx, cy, x, y, width, height, radius, cf
 *
 * Other fields (e.g. heading, var, projections) should be calculated from
 * these by the caller (VisualCortex). Returns the number of objects
 * recognised.
 */
int SubObject::findVisualNonBallObjects() {
    int nObjects = 0;
    mNoPinkBeacon = 0;
    features = subvision.getFeatures();
    
    #ifdef SUBOBJECT_DEBUG
    cout << __func__ << " : start" << endl;
    #endif    
    ballFeatures.clear();
    beaconFeatures.clear();
    goalFeatures.clear();
    wallFeatures.clear();
    fieldLineFeatures.clear();
    
    vBall->reset();
    vBeacons[svPinkOnBlue].reset();
    vBeacons[svBlueOnPink].reset();
    vBeacons[svPinkOnYellow].reset();
    vBeacons[svYellowOnPink].reset();
    vGoals[svBlueGoal].reset();
    vGoals[svYellowGoal].reset();
    wallExists = false;

    Profile::start(PR_SO_PRUNE);
    nFeatures = pruneFeatures();
    Profile::stop(PR_SO_PRUNE);
    
    #ifdef SUBOBJECT_DEBUG
    cout << __func__ << " : finding walls" << endl;
    #endif 
    Profile::start(PR_SO_WALL);
    findWalls();
    Profile::stop(PR_SO_WALL);
    
    #ifdef SUBOBJECT_DEBUG
    cout << __func__ << " : finding beacons" << endl;
    #endif 
    Profile::start(PR_SO_BEACONS);
    nObjects += findBeacons();
    Profile::stop(PR_SO_BEACONS);
    
    #ifdef SUBOBJECT_DEBUG
    cout << __func__ << " : finding goals" << endl;
    #endif 
    Profile::start(PR_SO_GOALS);
    nObjects += findGoals();
    Profile::stop(PR_SO_GOALS);
    
    Profile::start(PR_SO_INSANEFEATURES);
    pruneInsaneBallFeatures();
    pruneInsaneObstacleFeatures();
    pruneInsaneLineFeatures();        
    Profile::stop(PR_SO_INSANEFEATURES);

    // TODO: discard field line features that end up inside visual objects (like
    // the ball, robots)

    return nObjects;
}

/* Searches for a visual ball object.
 */ 
int SubObject::findVisualBallObject() { 
    pruneFeatures(nFeatures); // make sure extra ball features found after
                              // other objects are included
    int ret = findBall();
    return ret;
}


/* Sends the visual objects recognised this frame to robolink */
void SubObject::sendObjects(void) {
#ifndef OFFLINE
    struct rlnk_vobs {
        VisualObject vBall;
        VisualObject vBlueOnPink;
        VisualObject vPinkOnBlue;
        VisualObject vYellowOnPink;
        VisualObject vPinkOnYellow;
        VisualObject vBlueGoal;
        VisualObject vYellowGoal;
    };

    struct rlnk_vobs vobs;
    vobs.vBall = *this->vBall;
    vobs.vBlueOnPink = this->vBeacons[svBlueOnPink];
    vobs.vPinkOnBlue = this->vBeacons[svPinkOnBlue];
    vobs.vYellowOnPink = this->vBeacons[svYellowOnPink];
    vobs.vPinkOnYellow = this->vBeacons[svPinkOnYellow];
    vobs.vBlueGoal = this->vGoals[svBlueGoal];
    vobs.vYellowGoal = this->vGoals[svYellowGoal];    

//    cerr << "SubObject::sendObjects sending " << sizeof(vobs)
//        << " bytes = 7 vobs at " << sizeof(VisualObject) << " bytes each"
//        << endl;
    Robolink::sendToBase((byte*)&vobs, RLNK_SUBOBJECT, sizeof(vobs));
//    cerr << "SubObject: objects sent" << endl;
#endif
}

/* Iterates over the features detected by subvision and (a) copies them to
 * the lists of ballFeatures, beaconFeatures etc and (b) prunes out any
 * unlikely features based on surrounding pixels etc. Returns the size of
 * this->features.
 */
int SubObject::pruneFeatures(int start) {
    for (int i = start; i < (int)features.size(); ++i) {
        if (features[i].type == VF_BALL) {
            //int x = (int)features[i].x;
            //int y = (int)features[i].y;
            //cout << "ball feature at " << x << ", " << y << endl;
            
            // adjacent ball features (caused by a feature in both a horizontal
            // and vertical scan line) screw with the object recognition, so 
            // prune them here.
            for (int j = 0; j < i; ++j) {
                if (features[j].type == VF_BALL
                        && adjacent(features[i], features[j])) {

                    #ifdef OFFLINE
                    //cout << __func__ << " : pruned a point at (" <<
                    //    features[i].x << ", " << features[i].y << 
                    //    ") and (" << features[j].x << ", " << features[j].y <<
                    //    ") by adjacent." << endl; 
                    #endif
                    if (features[i].dsum > features[j].dsum) {
                        features[j].type = VF_NONE;
                        ballFeatures.remove(j);
                    } else {
                        features[i].type = VF_NONE;
                        break;
                    }
                }
            }
            if (features[i].type == VF_BALL
                    && ballFeatures.size() < MAX_BALLFEATURES)
                ballFeatures.push_back(i);
        } else if (features[i].type == VF_PINK) {
            beaconFeatures.push_back(i);
        } else if (features[i].type == VF_BLUE
                || features[i].type == VF_YELLOW) {
            goalFeatures.push_back(i);
        } else if (features[i].type == VF_WALL) {             
            wallFeatures.push_back(i);
        } else if (features[i].type == VF_FIELDLINE) {
            fieldLineFeatures.push_back(i);
        }
    }
    
    #ifdef OFFLINE
    for (int i = 0; i < (int)features.size(); i++) { 
        if (features[i].type == VF_BALL) {
            //cout << __func__ << " : left over ball point (" << features[i].x <<
            //", " << features[i].y << ") dir " << features[i].dir << endl; 
        }    
    }    
    #endif

    //cout << "After pruning: " << ballFeatures.size() << " ballFeatures" << endl;
    return features.size();
}


/*
 *
 */
void SubObject::pruneInsaneBallFeatures() { 
    // prune all the ball feature points which are above the near top 
    // of goals or the beacons.
    int goalIndex = -1;
    double goalElev = 0;
    if (vGoals[svBlueGoal].cf > 0 || vGoals[svYellowGoal].cf > 0) {

        if (vGoals[svBlueGoal].cf < vGoals[svYellowGoal].cf) {
            goalIndex = svYellowGoal;
        } else {
            goalIndex = svBlueGoal;
        }        
        point rc = rotatePoint((int)vGoals[goalIndex].cx,(int)vGoals[goalIndex].cy,
                                CPLANE_WIDTH/2,CPLANE_HEIGHT/2,mRoll);
        point top = rotatePoint((int)rc.first,int(rc.second-vGoals[goalIndex].height/3),
                                CPLANE_WIDTH/2,CPLANE_HEIGHT/2,-mRoll);
                        
        goalElev = pointToElevation(top.first,top.second,mSinEroll,mCosEroll);        
    }

    int beaconIndex = -1;
    double beaconElev = 0; 
    if (vBeacons[svBlueOnPink].cf > 0) {        
        beaconIndex = svBlueOnPink;
    } else if (vBeacons[svPinkOnBlue].cf > 0) {        
        beaconIndex = svPinkOnBlue;       
    } else if (vBeacons[svYellowOnPink].cf > 0) {        
        beaconIndex = svYellowOnPink;       
    } else if (vBeacons[svPinkOnYellow].cf > 0) {        
        beaconIndex = svPinkOnYellow;       
    }
    if (beaconIndex != -1) {    
        beaconElev = pointToElevation(vBeacons[beaconIndex].cx,vBeacons[beaconIndex].cy,mSinEroll,mCosEroll);
    }

    
    
    double fElev;
    list<int>::iterator itr;
    for (itr = ballFeatures.begin(); itr != ballFeatures.end();) { 
        VisualFeature &f = features[*itr];

        if (wallExists) {                           
            if (isAboveWall(f.x,f.y)) {                
                #ifdef OFFLINE
                //cout << __func__ << " : pruned a point at (" << f.x << ", " << f.y << ") by wall." << endl; 
                #endif
                f.type = VF_NONE;                
                itr = ballFeatures.erase(itr);
                continue;
            }
        } 
        
        fElev = pointToElevation(f.x,f.y,mSinEroll,mCosEroll);
        
        if ((goalIndex != -1 && fElev > goalElev)
            || (beaconIndex != -1 && fElev > beaconElev)) {
            #ifdef OFFLINE
            //cout << __func__ << " : pruned a point at (" << f.x << ", " << f.y << ") by goal and beacon." << endl; 
            #endif
            f.type = VF_NONE;            
            itr = ballFeatures.erase(itr);              
            continue;
        } 
        ++itr;           
    } 
}


void SubObject::pruneInsaneObstacleFeatures() { 
    bool removeFeatures = true;
    
    if (wallExists) { 
    
        vector<VisualFeature>::iterator itr; 
        for (itr = obstacleFeatures.begin(); itr != obstacleFeatures.end();) { 
            
            VisualFeature &f = *itr;
                                      
            if (isAboveWall(f.x,f.y)) {                
                f.type = VF_NONE;
                if (removeFeatures) {  
                    f = obstacleFeatures.back();
                    obstacleFeatures.pop_back();
                    continue;
                }                                                                
            }
            ++itr;               
        }        
    }
}

void SubObject::pruneInsaneLineFeatures() { 
    bool removeFeatures = true;

    if (wallExists) { 
        vector<int>::iterator itr; 
        
        for (itr = fieldLineFeatures.begin(); itr != fieldLineFeatures.end();) { 
            VisualFeature &f = features[*itr];

            if (isAboveWall(f.x,f.y)) {              
                f.type = VF_NONE;
                if (removeFeatures) { 
                    *itr = fieldLineFeatures.back();
                    fieldLineFeatures.pop_back();
                    continue;                    
                }                                                                
            }
            ++itr;               
        }        
    }
}

bool SubObject::isAboveWall(double x, double y) { 
    if (fabs(wall_m) < 2) {
        double v1 = y;
        double v2 = wall_m * x + wall_b; 
        if ((pruneAboveWall && v1 < v2)
            || (!pruneAboveWall && v1 > v2)) {
            return true;    
        }
    } else { 
        double v1 = x; 
        double v2 = (y - wall_b) / wall_m;
        if ((pruneAboveWall && v1 < v2)
            || (!pruneAboveWall && v1 > v2)) { 
            return true;
        }
    }  
    return false;
}

// Nobuyuki has to comment this function.. please remind HIM!!!
int SubObject::findWalls() {
   
    if (ransacWalls()) {
        vWall.type = VF_WALL_OBJECT;
        vWall.x = wall_m;
        vWall.y = wall_b;
        features.push_back(vWall);
            
        pruneAboveWall = true;
        if (wallExists && hzExists) {
            VisualFeature f = features[wallFeatures[0]];
            point p = subvision.getHorizonPoint(); 
            double img_hz_m = subvision.getHorizonGradient();
            double img_hz_c = p.second - img_hz_m * p.first;
            
            if (fabs(wall_m) < 2) { 
                double v = img_hz_m * f.x + img_hz_c; 
                #ifdef SUBOBJECT_WALL_DEBUG
                cout << __func__ << " : compare y " << endl; 
                cout << __func__ << " : hz_m " << img_hz_m << ", hz_c " << img_hz_c << endl; 
                cout << __func__ << " : f.x " << f.x << ", f.y" << f.y << ", v " << v << endl;
                #endif 
                if (f.y < v) {
                    pruneAboveWall = false;
                    #ifdef SUBOBJECT_WALL_DEBUG    
                    cout << __func__ << " : pruneAboveWall false" << endl;
                    #endif
                }            
            } else { 
                double v = (f.y - img_hz_c) / img_hz_m;
                #ifdef SUBOBJECT_WALL_DEBUG
                cout << __func__ << " : compare x " << endl; 
                cout << __func__ << " : hz_m " << img_hz_m << ", hz_c " << img_hz_c << endl; 
                cout << __func__ << " : f.x " << f.x << ", f.y" << f.y << ",   v " << v << endl;
                #endif
                if (f.x < v) { 
                    pruneAboveWall = false;
                    #ifdef SUBOBJECT_WALL_DEBUG
                    cout << __func__ << " : pruneAboveWall false" << endl;
                    #endif
                }
            
            } 
            
        }
        
        return true;     
    }
    return false;
   
}

bool SubObject::ransacWalls() {
    uint L = 5;
    uint N = 10; 
    double K1 = 0.9;
    double K2 = 0.8;
    double S = 0.5;
    
    double bestK = 0; 
    
    srand(time(NULL));

    #ifdef SUBOBJECT_WALL_DEBUG
    FCOUT << " : -----------------------" << endl;
    #endif
    
    if (wallFeatures.size() < N) {         
        return false;
    }
    
    N = MIN(uint(wallFeatures.size()*S),N);

    for (uint l = 0; l < L; l++) {
        vector<int> sample; 
        sample.assign(wallFeatures.begin(),wallFeatures.end());   
        while (sample.size() > N) { 
            int index = rand() % sample.size();
            sample[index] = sample.back();
            sample.pop_back();    
        } 


        double sx = 0; 
        double sy = 0;
        double ssx = 0;
        double ssy = 0;
        double sxy = 0;    
   

        vector<int>::iterator itr; 
        
        for (itr = sample.begin(); itr != sample.end(); itr++) { 
            VisualFeature f = features[*itr];
            double x = f.x; 
            double y = f.y; 

            sx += x; 
            sy += y;         
            ssx += x * x; 
            ssy += y * y;
            sxy += x * y;

        }
        
        double mx = sx / N; 
        double my = sy / N; 
        double ssxx = ssx - N * mx * mx; 
        double ssyy = ssy - N * my * my; 
        double ssxy = sxy - N * mx * my;
        double b = ssxy / ssxx; 
        double a = my - b * mx;     
                
        double r = (ssxy * ssxy) / (ssxx * ssyy);

        #ifdef SUBOBJECT_WALL_DEBUG
        cout << __func__ << " : a possible wall found" << endl;
        cout << __func__ << " : a " << a << ", b " << b << endl;
        cout << __func__ << " : r " << r << endl;
        #endif 
        
        if (r > K1) {
            wallExists = true; 
            wall_m = b; 
            wall_b = a;
            return true;
        } else if (r > bestK) {
            bestK = r; 
            wall_m = b; 
            wall_b = a;
        }
    }
    
    if (bestK > K2) { 
        wallExists = true; 
        return true;
    }

    return false;
}

bool SubObject::medianWalls() { 
    char LIMIT = 10; 
    double m = 0, b = 0;
    vector< pair<double,double> > ms;
    
    int n = wallFeatures.size();
    for (int i1 = 0; i1 < LIMIT && i1 < n; i1++) {        
        for (int i2 = i1 + 1; i2 < LIMIT && i2 < n; i2++) { 
            for (int i3 = i2 + 1; i3 < LIMIT && i3 < n; i3++) { 
                
                VisualFeature 
                    &f1 = features[wallFeatures[i1]], 
                    &f2 = features[wallFeatures[i2]], 
                    &f3 = features[wallFeatures[i3]];               
                                                
                // check points if they are colinear
                m = (f2.y - f1.y) / (f2.x - f1.x);
                b = f1.y - m * f1.x;
                if (fabs((m * f3.x + b) - f3.y) < 0.5) {               

                    ms.push_back(pair<double,double>(m,b));
                    wallExists = true;                                
                }                                          
            }              
        }
    }

    if (wallExists) {
        sort(ms.begin(),ms.end()); 
        pair<double,double> p = ms[ms.size()/2];
        wall_m = p.first;        
        wall_b = p.second;
        return true;
    } else { 
        return false;
    } 
}

bool SubObject::medianWalls2() { 
    uchar ANGLE_MAX = 90; // ANGLE_MAX * ANGLE_DIV = 180
    uchar ANGLE_DIV = 2;
    uchar MAX_COUNT = 4;
    uchar LIMIT = 10; 
    double m = 0, b = 0;
    vector< pair<double,double> > ms;
    
        
    char counts[ANGLE_MAX];
    int n = wallFeatures.size();
    for (uchar i1 = 0; i1 < LIMIT && i1 < n; i1 += 2) {    
        VisualFeature &f1 = features[wallFeatures[i1]];
        
        // Initialising count table
        for (uchar j = 0; j < ANGLE_MAX; j++) { 
            counts[j] = 0;        
        }
        
        // Finding gradients to a point selected in the outer loop.
        for (uchar i2 = 0; i2 < LIMIT && i2 < n; i2++) { 
            
            if (i1 == i2) {
                continue;
            } 
             
            VisualFeature &f2 = features[wallFeatures[i2]];   
                        
            m = (f2.y - f1.y) / (f2.x - f1.x);
            int angle = int(NormalizeAngle_180(RAD2DEG(atan(m))));            
            if (angle < 0) {
                angle = 180 + angle;
            }
            if (abs(angle) >= 180) { 
                angle = 0;
            }
            angle = angle / ANGLE_DIV;
            
            counts[angle]++;
            
            if (counts[angle] >= MAX_COUNT) { 
                b = f1.y - m * f1.x;
                ms.push_back(pair<double,double>(m,b));
                wallExists = true;
                
                counts[angle] = 0;
            }                                                       
        }
    }

    if (wallExists) {
        sort(ms.begin(),ms.end()); 
        pair<double,double> p = ms[ms.size()/2];
        wall_m = p.first;        
        wall_b = p.second;
        return true;
    } else { 
        return false;
    }  
}


/* Searches for the ball among the features and expands around it to gather
 * image location information. Returns 1 if ball was found, else 0.
 */
int SubObject::findBall() {
    Profile::start(PR_SO_BALL);

    bool result = false;

    result = medianBall();
    if (! result) {
        #ifdef SUBOBJECT_DEBUG
        cout << __func__ << " : failed" << endl;
        #endif 
        vBall->reset();
    }

    
    // fill in other vob information from cx, cy, radius
    if (result) {
        vBall->x = vBall->cx - vBall->radius;
        vBall->y = vBall->cy - vBall->radius;
        vBall->height = vBall->width = 2 * vBall->radius;
    }
    
    #ifdef SUBOBJECT_DEBUG
    cout << __func__ << " : running sanity" << endl;
    #endif 
    if (result) {
        if (SanityChecks::checkBallSanity(*vBall) != SANE) {
            vBall->reset();
            result = false;
        } else { 
            // Get the direction of the ball from the centroid
            findBallDirection();
        
        }
    } 
    
    #ifdef SUBOBJECT_DEBUG
    cout << __func__ << " : done" << endl;
    #endif 
    
    Profile::stop(PR_SO_BALL);
    return result ? 1 : 0;
}

// This function finds in which direction the ball appears on the image. 
int SubObject::findBallDirection() { 

    XYZ_Coord vec1(0,0,1);
    vec1.rotateYZ(tilt); 
    vec1.rotateXY(pan); 
    vec1.rotateYZ(crane);     
    XYZ_Coord vec2(vec1.y,0,vec1.x);
    vec2.normalize(); 

    point h1 = subvision.getHorizonPoint();
    point h2 = subvision.getOtherHorizonPoint();
    double hx = h2.first - h1.first; 
    double hy = h2.second - h1.second;
    XYZ_Coord hzPt(-hy,0,hx);
    hzPt.normalize();
    
    double bx = vBall->cx - CPLANE_WIDTH / 2;
    double by = vBall->cy - CPLANE_HEIGHT / 2;
    XYZ_Coord ballPt(-by,0,bx);    
    ballPt.normalize();
    

    double dot_product_vt = vec2.x * ballPt.x + vec2.z * ballPt.z;         
    double dot_product_hz = hzPt.x * ballPt.x + hzPt.z * ballPt.z; 
    
    double cos_theta_vt = dot_product_vt;
    double theta_vt = RAD2DEG(acos(cos_theta_vt));
    
    double cos_theta_hz = dot_product_hz; 
    double theta_hz = RAD2DEG(acos(cos_theta_hz));

    //cout << __func__ << " : theta_vt " << theta_vt << endl;
    //cout << __func__ << " : theta_hz " << theta_hz << endl;
    if (0 <= theta_vt && theta_vt <= 90) { 
        if (0 <= theta_hz && theta_hz <= 90) { 
            //cout << " -- top right" << endl;
            vBall->imgDir = 1;
            return 1; // top right        
        } else { 
            vBall->imgDir = 4;
            //cout << " -- top left" << endl;
            return 4; // top left 
        }
    } else { 
        if (0 <= theta_hz && theta_hz <= 90) { 
            //cout << " -- bottom right" << endl;
            vBall->imgDir = 2;
            return 2; // bottom right       
        } else { 
            //cout << " -- bottom left" << endl;
            vBall->imgDir = 3;
            return 3; // bottom left   
        }    
    } 
    return 0;
}


/* Searches for pink patches 
 * Returns the pink patches recognised
 */
/*
int SubObject::findPinkPatch(){
    pinkGroups.clear();

    if (beaconFeatures.size() <= 0)
        return 0;
//    cerr << "recognising beacons: " << beaconFeatures.size() << " features... ";
 
    // group the features
    groupFeatures(beaconFeatures, pinkGroups);

    // classify the groups by searching for blue/yellow around them and work
    // out the size/orientation of the beacons
    list<list<int> >::iterator groupItr;
    int bignum = 0;
    for (groupItr = beaconGroups.begin();
            groupItr != beaconGroups.end();
            ++groupItr) {
        if (++bignum > 10000) {
            cerr << __func__ << " INFINITE LOOP" << endl;
            die(__func__);
        }
        classifyPinkBeacon(groupItr);
    }
    int nBeacons = 0;
    
    return nBeacons;
}
*/

/* Searches for beacons among the features.
 * Returns the number of beacons recognised
 */
int SubObject::findBeacons() {
    beaconGroups.clear();

    // no beacons when grabbed
    if (subvision.getIsGrabbed()) return 0;

    if (beaconFeatures.size() <= 0)
        return 0;
//    cerr << "recognising beacons: " << beaconFeatures.size() << " features... ";
 

    // group the features
    groupFeatures(beaconFeatures, beaconGroups);

    // classify the groups by searching for blue/yellow around them and work
    // out the size/orientation of the beacons
    list<list<int> >::iterator groupItr;
    //int temp3 = 0;
    for (groupItr = beaconGroups.begin();
            groupItr != beaconGroups.end();
            ++groupItr) {
//        if (++temp3 > 10000) {
//            cerr << "INFINITE LOOP AT SUBOBJECT::FINDBEACONS "
//                <<" temp3 ~line 220" << endl;
//            die("FINDBEACONS");
//        }
        if (!classifyBeacon(groupItr) && flag_LOC_CHALLENGE)
        //Localisation challenge if can not classify by beacon, classify by pink beacon
            classifyPinkBeacon(groupItr);
    }

    int nBeacons = 0;
    if (vBeacons[svBlueOnPink].cf > 0)
        ++nBeacons;
    if (vBeacons[svPinkOnBlue].cf > 0)
        ++nBeacons;
    if (vBeacons[svYellowOnPink].cf > 0)
        ++nBeacons;
    if (vBeacons[svPinkOnYellow].cf > 0)
        ++nBeacons;

//    cerr << "done" << endl;
    return nBeacons;
}

/* Searches for goals among the features. Returns the number of goal
 * objects recognised
 */
int SubObject::findGoals() {
    goalGroups.clear();
    if (goalFeatures.size() <= 0)
        return 0;
  //  cerr << "recognising goals: " << goalFeatures.size() << " features... ";


    // group the features. We allow goal features to be further apart 
    // than for beacons
    groupFeatures(goalFeatures, goalGroups,
                   /* subvision.getIsGrabbed()? 4 :*/ 2, true);

//    cout << "Goal features resolved to " << goalGroups.size()
//        << " groups" << endl;
    
    // Determine the the size/orientation of the goal(s)
    list<list<int> >::iterator groupItr;
    list<int>::iterator memberItr;
    //int temp4 = 0;
    for (groupItr = goalGroups.begin();
            groupItr != goalGroups.end();
            ++groupItr) {
//        if (++temp4 > 10000) {
//            cerr << "INFINITE LOOP AT SUBOBJECT::FINDGOALS "
//                <<" temp4 ~line 260" << endl;
//            die("FINDGOALS");
//        }
        // initally approximate the goal width by the length of the longest 
        // feature and height by the largest hight over all features
        double cx, cy;
        double xmin = VERY_LARGE_DOUBLE, xmax = 0,
               ymin = VERY_LARGE_DOUBLE, ymax = 0;
        double width = 0, height = 0;
        int lowestGapScanline = -1; // bottom scanline index of best gap
        int NUM_BOTTOM_SCANLINE = 4;
        if (subvision.getIsGrabbed()) NUM_BOTTOM_SCANLINE = 8;
        int bestGapScanline = -1, gap2Scanline = -1;
        double gapLeft = 0, gapRight = 0, gap2Left = 0, gap2Right = 0;
        Color col = cNONE;

        for (memberItr = groupItr->begin();
                memberItr != groupItr->end();
                ++memberItr) {
//            if (++temp4 > 10000)  {
//                cerr << "INFINITE LOOP AT SUBOBJECT::FINDGOALS "
//                    <<" temp4 ~line 275" << endl;
//                die("FINDGOALS2");
//            }
            if (features[*memberItr].type == VF_BLUE)
                col = cBEACON_BLUE;
            else
                col = cBEACON_YELLOW;
                   
            double x = features[*memberItr].x,
                y = features[*memberItr].y,
                endx = features[*memberItr].endx,
                endy = features[*memberItr].endy;
            if (x < xmin)
                xmin = x;
            if (x > xmax)
                xmax = x;
            if (endx < xmin)
                xmin = endx;
            if (endx > xmax)
                xmax = endx;
            if (y < ymin)
                ymin = y;
            if (y > ymax)
                ymax = y;
            if (endy < ymin)
                ymin = endy;
            if (endy > ymax)
                ymax = endy;
            double len;
            //if (len > width)
            //    width = len;
            // seach up and down the blob to determine height
            point top = subvision.searchLine((int)(x+endx)/2, (int)(y+endy)/2,
                                                col, true, true);
            point bot = subvision.searchLine((int)(x+endx)/2, (int)(y+endy)/2,
                                                col, false, true);
            len = DISTANCE(top.first, top.second, bot.first, bot.second);
            if (len > height) {
                height = len;
            }
            if (top.first < xmin)
                xmin = top.first;
            if (top.first > xmax)
                xmax = top.first;
            if (top.second < ymin)
                ymin = top.second;
            if (top.second > ymax)
                ymax = top.second;
            if (bot.first < xmin)
                xmin = bot.first;
            if (bot.first > xmax)
                xmax = bot.first;
            if (bot.second < ymin)
                ymin = bot.second;
            if (bot.second > ymax)
                ymax = bot.second;

#if 0
            // best gap calculation. We take the longest feature in the
            // bottom scanlines in the object. This calculates up to
            // *two* best gaps, one each side of the goal.
            // This assumes that the image is pretty much upright - a likely
            // assumption when taking a shot on goal
            int currentScanline = features[*memberItr].scanline;
            cerr << "feature at scanline " << features[*memberItr].scanline << endl;
            if (gap2Scanline >= currentScanline + NUM_BOTTOM_SCANLINE) {
                gap2Scanline = -1;
                gap2Left = gap2Right = 0;
            }
            if (lowestGapScanline == -1) { // first time round
                lowestGapScanline = bestGapScanline = currentScanline;
                gapLeft = x;
                gapRight = endx;
                cerr << "first time gap x = " << gapLeft << " to " << gapRight
                    << " scanline " << currentScanline << endl;
            } else if (currentScanline < lowestGapScanline+NUM_BOTTOM_SCANLINE){
                if (currentScanline < lowestGapScanline)
                    lowestGapScanline = currentScanline;

                if (endx - x > gapRight - gapLeft || 
                        bestGapScanline >= lowestGapScanline
                                            + NUM_BOTTOM_SCANLINE) {
                    bestGapScanline = currentScanline;
                    // if the new gap does not overlap the old one our new
                    // 2nd best is the old best
                    if (endx < gapLeft || x > gapRight) {
                        gap2Scanline = bestGapScanline;
                        gap2Left = gapLeft;
                        gap2Right = gapRight;
                    }
                    gapLeft = x;
                    gapRight = endx;
                    cerr << "New best gap x = " << gapLeft << " to " << gapRight
                        << " scanline " << currentScanline << endl;
                } else if ((endx - x > gap2Right - gap2Left
                                || gap2Scanline >= lowestGapScanline
                                                + NUM_BOTTOM_SCANLINE)
                            && (endx < gapLeft || x > gapRight)) {
                    gap2Scanline = currentScanline;
                    gap2Left = x;
                    gap2Right = endx;
                    cerr << "New 2nd gap x = " << gap2Left << " to "
                        << gap2Right << endl;
                }
            }
#endif
        } // loop over features

        // iterate over the features from the bottom up looking for the best
        // and second best gap
        list<int>::reverse_iterator gapItr;
        int minScanLine = -1;   // lowest scan line
        for (gapItr = groupItr->rbegin();
                gapItr != groupItr->rend();
                ++gapItr) {
            if (minScanLine == -1) {
                minScanLine = features[*gapItr].scanline;
            }
            if (features[*gapItr].scanline >= minScanLine + NUM_BOTTOM_SCANLINE) {
                break;
            }
//            cerr << "feature on line " << features[*gapItr].scanline << endl;
            double x = features[*gapItr].x,
                y = features[*gapItr].y,
                endx = features[*gapItr].endx,
                endy = features[*gapItr].endy;
            // new best gap
            if (endx - x > gapRight - gapLeft) {
                // if the new gap does not overlap the old one our new
                // 2nd best is the old best
                if (endx < gapLeft || x > gapRight) {
                    gap2Left = gapLeft;
                    gap2Right = gapRight;
                }
                gapLeft = x;
                gapRight = endx;
//                cerr << "New best gap x = " << gapLeft << " to " << gapRight
//                    << " scanline " << features[*gapItr].scanline << endl;

            } else if ((endx - x > gap2Right - gap2Left) 
                        && (endx < gapLeft || x > gapRight)) {
                gap2Left = x;
                gap2Right = endx;
//                cerr << "New 2nd gap x = " << gap2Left << " to "
//                    << gap2Right << endl;
            }
        }

//        cerr << "gaps " << gapLeft << ", " << gapRight << "  "
//            << gap2Left << ", " << gap2Right << endl;
        if (col == cNONE)
            continue;
        width = xmax - xmin;
        cx = (xmax + xmin)/2;
        cy = (ymax + ymin)/2;


        // Fill in the information about the goal
        VisualObject goal;

        goal.cf = groupItr->size(); // cf for goal is number of features
        goal.cx = cx;
        goal.cy = cy;
        goal.width = width;
        goal.height = height;
        goal.x = cx - width/2;
        goal.y = cy - height/2;
        if (gapLeft < gap2Left) { // best gap is the left one
            goal.leftGapMin = gapLeft;
            goal.leftGapMax = gapRight;
            goal.rightGapMin = gap2Left;
            goal.rightGapMax = gap2Right;
        } else {
            goal.leftGapMin = gap2Left;
            goal.leftGapMax = gap2Right;
            goal.rightGapMin = gapLeft;
            goal.rightGapMax = gapRight;
        }
        goal.nFeatures = groupItr->size();
        if (SanityChecks::checkGoalSanity(goal) != SANE) {
            continue;
        }

//        cerr << "Goal centred at " << cx << ", " << cy
//            << " width " << width << ", height " << height 
//            << " with " << groupItr->size() << " features" << endl;
        
        VisualObject* whichGoal;
        if (col == cBEACON_BLUE)
            whichGoal = &vGoals[svBlueGoal];
        else
            whichGoal = &vGoals[svYellowGoal];
        // Only overwrite an existing goal vob if this is better
        if (goal.cf > whichGoal->cf)
            *whichGoal = goal;
    }
    
    int nGoals = 0;
    if (vGoals[svBlueGoal].cf > 0)
        ++nGoals;
    if (vGoals[svYellowGoal].cf > 0)
        ++nGoals;

    // Of course we can only see one goal at a time, but that filtering is
    // left to the sanity checks

 //   cerr << "done" << endl;
    return nGoals;
}


/* Attempts to determine the ball's centre and radius by fitting
 * a circle to the detected ball edge points. This uses a generalisation of
 * Siegel's repeated median (RM) line fitting algorithm (Siegel82, Mount97)
 * to fit the circle and is claimed to be robust to up to 50% of the points
 * being outliers. There are some modifications though, since the original
 * assumes that the points that aren't noise acutally lie on the circle,
 * whereas we get occluded, blurred and ovoid balls.
 * Returns true if the procedure succeeeds.
 */
bool SubObject::medianBall(void) {
#ifdef SUBOBJECT_DEBUG
    cerr << "Medianball, " << ballFeatures.size() << " features" << endl;
#endif
    if (ballFeatures.size() < MIN_BALLFEATURES_MEDIANBALL)
        return false;

    // Naiive O(n^3) implementation - a las vegas O(n^2 log n) exists
    list<int>::iterator itr1;
    list<int>::iterator itr2;
    list<int>::iterator itr3;

    vector<double> outerX;
    vector<double> outerY;
    vector<double> middleX;
    vector<double> middleY;
    vector<double> innerX;
    vector<double> innerY;

    outerX.reserve(ballFeatures.size());
    outerY.reserve(ballFeatures.size());
    middleX.reserve(ballFeatures.size());
    middleY.reserve(ballFeatures.size());
    innerX.reserve(ballFeatures.size());
    innerY.reserve(ballFeatures.size());

    for (itr1 = ballFeatures.begin(); itr1 != ballFeatures.end(); ++itr1) {
        middleX.resize(0);
        middleY.resize(0);
        
        // bug out if this has taken too long
        if (Profile::timeSoFar(PR_SO_BALL) >= 3.0)
        {
            //FCOUT << " has taken " << Profile::timeSoFar(PR_SO_BALL)
            //    << " for " << outerX.size() << " outer iterations from "
            //    << ballFeatures.size() << " features. Quitting."
            //    << endl;
            break;
        } 
                   
        for (itr2 = ballFeatures.begin(); itr2 != ballFeatures.end(); ++itr2) {
            if (itr1 == itr2)
                continue;
            innerX.resize(0);
            innerY.resize(0);
            for (itr3 = ballFeatures.begin();itr3 != ballFeatures.end();++itr3){
                // check if three features are the same one. 
                if (itr1 == itr3 || itr2 == itr3) {
                    continue;
                }

                VisualFeature
                    &f1 = features[*itr1],
                    &f2 = features[*itr2],
                    &f3 = features[*itr3];

                //#ifdef SUBOBJECT_DEBUG
                //cout << __func__ << ": triplet " << f1.x << "," << f1.y << " "
                //    << f2.x << "," << f2.y << " "
                //    << f3.x << "," << f3.y << endl;
                //#endif                                 
#ifdef SUBOBJECT_DEBUG
                //cerr << "triplet ";
#endif
                // check points are not colinear
                double m = (f2.y - f1.y)/(f2.x - f1.x);
                if (solveForX(make_pair(f1.x, f1.y), m, f3.x) == f3.y) {
#ifdef SUBOBJECT_DEBUG
                    cout << __func__ << " : colinear ball features"
                        << f1.x << "," << f1.y
                        << " " << f2.x << "," << f2.y << " " << f3.x 
                        << "," << f3.y << endl;
#endif 
                    continue;
                }                             
#ifdef SUBOBJECT_DEBUG
                //cerr << "ok" << endl;
#endif

                // circle equations from hell due to
                // http://mcraefamily.com/MathHelp/GeometryConicSectionCircleEquationGivenThreePoints.htm
                double centreX = ((SQUARE(f1.x)+SQUARE(f1.y))*(f3.y-f2.y)
                        + (SQUARE(f2.x)+SQUARE(f2.y))*(f1.y-f3.y)
                        + (SQUARE(f3.x)+SQUARE(f3.y))*(f2.y-f1.y))
                    / (2*(f1.x*(f3.y-f2.y)+f2.x*(f1.y-f3.y)+f3.x*(f2.y-f1.y)));
                double centreY =  ((SQUARE(f1.x)+SQUARE(f1.y))*(f3.x-f2.x)
                        + (SQUARE(f2.x)+SQUARE(f2.y))*(f1.x-f3.x)
                        + (SQUARE(f3.x)+SQUARE(f3.y))*(f2.x-f1.x))
                    / (2*(f1.y*(f3.x-f2.x)+f2.y*(f1.x-f3.x)+f3.y*(f2.x-f1.x))); 
                
                if (isnan(centreX) || isinf(centreX)
                        || isnan(centreY) || isinf(centreY))
                    continue;

                //#ifdef SUBOBJECT_DEBUG
                //cout << __func__ << " : intersection at " << centreX 
                //    << "," << centreY
                //    << " radius " << radius << endl;
                //#endif

                // Sanity check. Confirm that the calculated centre of the
                // ball is on the right side of each feature. 
                // VisualFeature.dir indicates on which side the orange should
                // be. At steep slopes the directionality no longer reflects
                // where the centroid should be so we skip this check.
                // This test looks expensive but because it stops us inserting
                // things in the candidate lists it's probably worth it.
                if (fabs(subvision.getHorizonGradient()) < 2) {
#ifdef SUBOBJECT_DEBUG
                    //cerr << "Checking ball centre...";
#endif
                    if (f1.dir == DIR_UP && centreY > f1.y) {
                        continue;
                    } else if (f1.dir == DIR_DOWN && centreY < f1.y) {
                        continue;
                    } else if (f1.dir == DIR_LEFT && centreX > f1.x) {
                        continue;
                    } else if (f1.dir == DIR_RIGHT && centreX < f1.x) {
                        continue;
                    }
                    if (f2.dir == DIR_UP && centreY > f2.y) {
                        continue;
                    } else if (f2.dir == DIR_DOWN && centreY < f2.y) {
                        continue;
                    } else if (f2.dir == DIR_LEFT && centreX > f2.x) {
                        continue;
                    } else if (f2.dir == DIR_RIGHT && centreX < f2.x) {
                        continue;
                    }
                    if (f3.dir == DIR_UP && centreY > f3.y) {
                        continue;
                    } else if (f3.dir == DIR_DOWN && centreY < f3.y) {
                        continue;
                    } else if (f3.dir == DIR_LEFT && centreX > f3.x) {
                        continue;
                    } else if (f3.dir == DIR_RIGHT && centreX < f3.x) {
                        continue;
                    }
#ifdef SUBOBJECT_DEBUG
                    //cerr << " ok" << endl;
#endif
                }

                innerX.push_back(centreX);
                innerY.push_back(centreY);
            }
            // extract the medians from the inner loop and insert into the
            // middle loop data
            if (innerX.size() <= 0)
                continue;
            sort(innerX.begin(), innerX.end());
            sort(innerY.begin(), innerY.end());

            middleX.push_back(innerX[innerX.size()/2]);
            middleY.push_back(innerY[innerY.size()/2]);
#ifdef SUBOBJECT_DEBUG
            cout << "inner median " << middleX.back() << "," << middleY.back()
                << endl;
#endif
        }
        // extract the medians from the middle loop and insert into outer loop
        // data
        if (middleX.size() <= 0)
            continue;

        sort(middleX.begin(), middleX.end());
        sort(middleY.begin(), middleY.end());

        outerX.push_back(middleX[middleX.size()/2]);
        outerY.push_back(middleY[middleY.size()/2]);
#ifdef SUBOBJECT_DEBUG
        cout << "middle median " << outerX.back() << "," << outerY.back()
            << endl;
#endif
    }
    // extract the medians from the outer loop, which form our estimate
    // TODO: due to the accuracy inherent in the pixellated image we might do
    // better averaging a couple of samples around the median (and above too)
    // TODO: we could perform a fixed number of gradient descent steps to
    // minimise the error of each point since the algo assumes the (non-noise)
    // points are circular, not always the case.
    if (outerX.size() <= 0) {
        #ifdef SUBOBJECT_DEBUG
        cout << __func__ << " : no ball found. " << endl;
        #endif
        return false;
    }

    sort(outerX.begin(), outerX.end());
    sort(outerY.begin(), outerY.end());

    vBall->cx = outerX[outerX.size()/2];
    vBall->cy = outerY[outerY.size()/2];
    vBall->cf = 500; // FIXME

    // The radius is calculated seperately, *after* the correct centroid has
    // been found. We average the middle few solutions if these are all close
    // together to reduce jitter.
    // N.B. calculate using squares to avoid the sqrts in DISTANCE
    vector<double> radii_sqr;
    
    for (itr1 = ballFeatures.begin(); itr1 != ballFeatures.end(); ++itr1) {
        radii_sqr.push_back(DISTANCE_SQR(features[*itr1].x, features[*itr1].y,
                                    vBall->cx, vBall->cy));
    }

    sort(radii_sqr.begin(), radii_sqr.end());
    if (radii_sqr.size() > MIN_RADII_AVERAGE) {
        // average the middle 3 and use that if they are similar
        double medianDist = sqrt(radii_sqr[radii_sqr.size()/2]);
        double avgDist = sqrt(radii_sqr[(int)radii_sqr.size()/2 - 1]) // radii_sqr.size() is unsigned
                        + sqrt(radii_sqr[radii_sqr.size()/2])
                        + sqrt(radii_sqr[radii_sqr.size()/2 + 1]);
        avgDist = avgDist/3;
        if (avgDist/medianDist < 1.15 || avgDist/medianDist > 0.85) {
            #ifdef SUBOBJECT_DEBUG
            cerr << "Using average median of three distances" << endl;
            #endif
            vBall->radius = medianDist;
        } else {
            #ifdef SUBOBJECT_DEBUG
            cerr << "Using median distance only" << endl;
            #endif
            vBall->radius = avgDist;
        }
    } else {
        // just the median
        vBall->radius = sqrt(radii_sqr[radii_sqr.size()/2]);
    }

    #ifdef SUBOBJECT_DEBUG
    cerr << __func__ << " : ball at " << vBall->cx << ", " << vBall->cy
        << " radius " << vBall->radius << endl;
    #endif

    return true;
}


/* Groups beacon or goal features (which are horizontal lines) from feat
 * into sets of features that are probably part of the same object. Features
 * that are vertically above each other are probably in the same blob. The
 * features must be on scanlines that are adjacent by at most nScanLines. 
 * If groupHoriz is true then the groups are then merged if they are
 * close horizontally, i.e. have features on the same scan line. The
 * complete groups are put in the groups collection. Returns the number of
 * groups groups found.
 */
int SubObject::groupFeatures(list<int>& feat, list<list<int> >& groups, 
                                int nScanLines, bool groupHoriz) {
    list<int>::iterator itr;

    // Begin by placing each feature in a group on it's own, and then merging
    // overlapping groups
    for (itr = feat.begin(); itr != feat.end(); ++itr) {
        groups.push_back(list<int>(1, *itr));
    }

    list<list<int> >::iterator groupItr;
    list<list<int> >::iterator otherGroupItr;
    list<int>::iterator memberItr;
    list<int>::iterator otherMemberItr;
    for (groupItr = groups.begin();
            groupItr != groups.end();
            ++groupItr) {
        //cout << "next top level group, size " << groupItr->size() << endl;
        bool merged = false;
        for (otherGroupItr = groups.begin();
                otherGroupItr != groups.end();
                ++otherGroupItr) {
            // after a merge we have to send the second group iterator
            // back to the start since the relationships have changed
            if (merged)
                otherGroupItr = groups.begin();
            merged = false;
            if (otherGroupItr == groupItr)
                continue;
            for (memberItr = groupItr->begin(); 
                    memberItr != groupItr->end() && ! merged;
                    ++memberItr) {
                for (otherMemberItr = otherGroupItr->begin(); 
                        otherMemberItr != otherGroupItr->end();
                        ++otherMemberItr) {
                    if (features[*memberItr].type !=
                                features[*otherMemberItr].type)
                        continue;
                    if (lineAbove(features[*memberItr],
                                features[*otherMemberItr], nScanLines)) {
                        //cout << feat[*memberItr].x << ","
                        //    << feat[*memberItr].y << " in line with "
                        //    << feat[*otherMemberItr].x << ","
                        //    << feat[*otherMemberItr].y
                        //    << " ... merging groups" << endl;
                        groupItr->splice(groupItr->begin(), *otherGroupItr);
                        merged = true;
                        break;
                    } else if (groupHoriz && features[*memberItr].scanline
                                    == features[*otherMemberItr].scanline) {
                        groupItr->splice(groupItr->begin(), *otherGroupItr);
                        merged = true;
                        break;
                    }
                    //cout << feat[*memberItr].x << ","
                    //    << feat[*memberItr].y << " not in line with "
                    //    << feat[*otherMemberItr].x << ","
                    //    << feat[*otherMemberItr].y << endl;
                }
            }
        }
    }
    
    // erase groups made empty by splicing
    for (groupItr = groups.begin(); groupItr != groups.end(); ++groupItr) {
        while (groupItr != groups.end() && groupItr->empty()) {
            groupItr = groups.erase(groupItr);
        }
    }

    // debug output
    //for (groupItr = groups.begin();
    //        groupItr != groups.end();
    //        ++groupItr) {
    //    cout << "Beacon group: ";
    //    for (memberItr = groupItr->begin(); 
    //            memberItr != groupItr->end();
    //            ++memberItr) {
    //        cout << feat[*memberItr].x << ","
    //            << feat[*memberItr].y << " ";
    //    }
    //    cout << endl;
    //}
    return groups.size();
}


/*
 * Get the number of pink blob that are calculated by classifyPinkBeacon
 * Localisation Challenge
 */
int SubObject::getNoPinkBlob()
{
    return mNoPinkBeacon;
} 

/* Calssify the pink object
 */
 
bool SubObject::classifyPinkBeacon(list<list<int> >::iterator groupItr) {
    list<int>::iterator memberItr;
    if (vChallengePinkBeacon == NULL || mNoPinkBeacon >= mMaxChallengePinkBeacon)
    {        
        if (mNoPinkBeacon >= mMaxChallengePinkBeacon)
            cout << __func__ << " MAX PINK OBJECT!" <<endl;
        return false;
    }
    
    // approximate the centre of pink by the average of feature line centres
    double pinkcx = 0, pinkcy = 0;
    double width = 0;
    double height = 0;          // of pink
    double beaconHeight = 0;    //
    double len; 
    double topline = 999;
    double bottom = 0;
    point beaconTop, beaconBottom; // including white part
//    double xmin = VERY_LARGE_DOUBLE, xmax = 0,
//           ymin = VERY_LARGE_DOUBLE, ymax = 0;
    for (memberItr = groupItr->begin();
            memberItr != groupItr->end();
            ++memberItr) {
        double x = features[*memberItr].x,
            y = features[*memberItr].y,
            endx = features[*memberItr].endx,
            endy = features[*memberItr].endy;
//        cerr << "New feature at " << x << "," << y << endl;

        // FIXME: estimate centroid by centre of bounding box rather
        // than average (if no outliers...)
        pinkcx += (x + endx)/2;
        pinkcy += (y + endy)/2;

        // estimate width by longest run
        len = DISTANCE(x, y, endx, endy);
        if (len > width) width = len;
        
        // seach up and down the pink blob to determine height
        point top = subvision.searchLine((int)(x+endx)/2, (int)(y+endy)/2,
                                            cBEACON_PINK, true, true);
        point bot = subvision.searchLine((int)(x+endx)/2, (int)(y+endy)/2,
                                            cBEACON_PINK, false, true);
        len = DISTANCE(top.first, top.second, bot.first, bot.second);
#ifdef OFFLINE        
        cout << __func__ << " top1:"<<top.first << " top2:"<< top.second <<" bot1:" << bot.first <<" bot2:"<< bot.second <<endl;
#endif        
        if (topline > top.second)
            topline = top.second;
        if (bottom < bot.second)
            bottom = bot.second;
        if (len > beaconHeight) {
            height = len;
            beaconHeight = len;
            //beaconTop = top;    // estimate top of beacon with this too
        }
    }
    height = bottom - topline;
    pinkcx /= groupItr->size();
    pinkcy /= groupItr->size();


    //double size = MAX(width, height);
//    pinkcx = (xmin + xmax)/2;
//    pinkcy = (ymin + ymax)/2;
//    width = size;
//    height = size;
    VisualObject *pinkPatch = &vChallengePinkBeacon[mNoPinkBeacon];
    pinkPatch->reset();
    pinkPatch->cx = pinkcx;
    pinkPatch->cy = pinkcy;
    pinkPatch->width = width;
    pinkPatch->height = height;
    pinkPatch->cf = groupItr->size(); // beacon cf is number of features
    ++mNoPinkBeacon;   
    
#ifdef OFFLINE 
    //cerr << __func__ << " pink centroid at " << pinkcx << "," << pinkcy << endl;
    //cerr << "width " << width << ", height " << height << endl;
#endif
    return true;
}


/* Searches around the given beacon group (pointed to by itr) for the boundaries
 * of the pink blob and for blue or yellow above or below the beacon. If the 
 * group is recognised as a valid beacon then it's positional information is
 * copied into the relevant SubObject vob field.
 */
bool SubObject::classifyBeacon(list<list<int> >::iterator groupItr) {
    list<int>::iterator memberItr;

    // approximate the centre of pink by the average of feature line centres
    double pinkcx = 0, pinkcy = 0;
    double width = 0;
    double height = 0;          // of pink
    double beaconHeight = 0;    // of the whole thing, approx 4*height
    double len; 
    point beaconTop, beaconBottom; // including white part
//    double xmin = VERY_LARGE_DOUBLE, xmax = 0,
//           ymin = VERY_LARGE_DOUBLE, ymax = 0;
    for (memberItr = groupItr->begin();
            memberItr != groupItr->end();
            ++memberItr) {
        double x = features[*memberItr].x,
            y = features[*memberItr].y,
            endx = features[*memberItr].endx,
            endy = features[*memberItr].endy;
//        cerr << "New feature at " << x << "," << y << endl;

        // FIXME: estimate centroid by centre of bounding box rather
        // than average (if no outliers...)
        pinkcx += (x + endx)/2;
        pinkcy += (y + endy)/2;

        // estimate width by longest run
        len = DISTANCE(x, y, endx, endy);
        if (len > width) width = len;
        
        // seach up and down the pink blob to determine height
        point top = subvision.searchLine((int)(x+endx)/2, (int)(y+endy)/2,
                                            cBEACON_PINK, true, true);
        point bot = subvision.searchLine((int)(x+endx)/2, (int)(y+endy)/2,
                                            cBEACON_PINK, false, true);
        len = DISTANCE(top.first, top.second, bot.first, bot.second);
        if (len > beaconHeight) {
            height = len;
            beaconHeight = 4*len;
            beaconTop = top;    // estimate top of beacon with this too
        }
#if 0
        if (features[*memberItr].x < xmin)
            xmin = features[*memberItr].x;
        if (features[*memberItr].x > xmax)
            xmax = features[*memberItr].x;
        if (features[*memberItr].y < ymin)
            ymin = features[*memberItr].y;
        if (features[*memberItr].y > ymax)
            ymax = features[*memberItr].y;
        if (features[*memberItr].endx < xmin)
            xmin = features[*memberItr].endx;
        if (features[*memberItr].endx > xmax)
            xmax = features[*memberItr].endx;
        if (features[*memberItr].endy < ymin)
            ymin = features[*memberItr].endy;
        if (features[*memberItr].endy > ymax)
            ymax = features[*memberItr].endy;
#endif
    }
    pinkcx /= groupItr->size();
    pinkcy /= groupItr->size();
#ifdef OFFLINE
#ifdef LOCALISATION_CHALLENGE
    cout << __func__ << " Pink coordinate cx:" << pinkcx << " cy:" << pinkcy << endl;
#endif    
#endif
    // Width is also a good estimate for height, so catch underestimates here
    if (width > 0.9 * height) {
        beaconHeight = 4*width;
    }
#if 0
    if (top.first < xmin)
        xmin = top.first;
    if (top.first > xmax)
        xmax = top.first;
    if (top.second < ymin)
        ymin = top.second;
    if (top.second > ymax)
        ymax = top.second;
    if (bot.first < xmin)
        xmin = bot.first;
    if (bot.first > xmax)
        xmax = bot.first;
    if (bot.second < ymin)
        ymin = bot.second;
    if (bot.second > ymax)
        ymax = bot.second;
#endif

    double size = MAX(width, height);
//    pinkcx = (xmin + xmax)/2;
//    pinkcy = (ymin + ymax)/2;
//    width = size;
//    height = size;
//    cerr << "pink centroid at " << pinkcx << "," << pinkcy << endl;
//    cerr << "width " << width << ", height " << height << endl;

    // estimate centre of colour blob below and above pink
    double horizonAngle = atan(subvision.getHorizonGradient());
    int flip = subvision.getHorizonUpsideDown() ? -1 : 1;
    double lowcx, lowcy;
    lowcx = pinkcx - sin(horizonAngle) * size * flip;
    lowcy = pinkcy + cos(horizonAngle) * size * flip;
    double highcx, highcy;
    highcx = pinkcx + sin(horizonAngle) * size * flip;
    highcy = pinkcy - cos(horizonAngle) * size * flip;
    if (lowcx < 0) lowcx = 0;
    if (lowcx > CPLANE_WIDTH - 1) lowcx = CPLANE_WIDTH - 1;
    if (lowcy < 0) lowcy = 0;
    if (lowcy > CPLANE_HEIGHT - 1) lowcy = CPLANE_HEIGHT - 1;
    if (highcx < 0) highcx = 0;
    if (highcx > CPLANE_WIDTH - 1) highcx = CPLANE_WIDTH - 1;
    if (highcy < 0) highcy = 0;
    if (highcy > CPLANE_HEIGHT - 1) highcy = CPLANE_HEIGHT - 1;

#ifdef OFFLINE    
#ifdef LOCALISATION_CHALLENGE
    cout << "check below at " << lowcx << "," << lowcy << endl;
    cout << "check above at " << highcx << "," << highcy << endl;
#endif    
#endif

    // Check for colour above and below. Look below first since
    // whatever is above may well be noise.
    // FIXME: use most likely candidate when multiple match
    VisualObject beacon;
    VisualObject* candidate = 0;
    Color col; // colour of the other part
    bool pinkAbove;  // true if pink above col
    if (subvision.nearColour((int)lowcx, (int)lowcy, cBEACON_BLUE,
                BEACON_COLOUR_SEARCH_RADIUS) >= BEACON_MIN_COLOUR) {
        candidate = &vBeacons[svPinkOnBlue];
        col = cBEACON_BLUE;
        pinkAbove = true;
    } else if (subvision.nearColour((int)lowcx, (int)lowcy, cBEACON_YELLOW,
                BEACON_COLOUR_SEARCH_RADIUS) >= BEACON_MIN_COLOUR) {
        candidate = &vBeacons[svPinkOnYellow];
        col = cBEACON_YELLOW;
        pinkAbove = true;
    } else if (subvision.nearColour((int)highcx, (int)highcy, cBEACON_BLUE,
                BEACON_COLOUR_SEARCH_RADIUS) >= BEACON_MIN_COLOUR) {
        candidate = &vBeacons[svBlueOnPink];
        col = cBEACON_BLUE;
        pinkAbove = false;
    } else if (subvision.nearColour((int)highcx, (int)highcy, cBEACON_YELLOW,
                BEACON_COLOUR_SEARCH_RADIUS) >= BEACON_MIN_COLOUR) {
        candidate = &vBeacons[svYellowOnPink];
        col = cBEACON_YELLOW;
        pinkAbove = false;
    } else {
#ifdef OFFLINE
#ifdef LOCALISATION_CHALLENGE
        cout << __func__ << " pink is not normal beacon" <<endl;
#endif        
#endif    
        if (! flag_LOC_CHALLENGE) {
            // not a beacon candidate if we couldn't find the colour below.
            // Set feature types to none and clear features from this beaconGroup
            for (memberItr = groupItr->begin();
                    memberItr != groupItr->end();
                    ++memberItr) {
                features[*memberItr].type = VF_NONE;
            }
            groupItr->clear();
    //        cerr << "Not a beacon" << endl;
        }
        return false;
    }

    if (candidate == 0)
        cout << "ERROR - beacon candidate NULL" << endl;

    // estimate height of the whole coloured component
    if (pinkAbove) {
        // beaconTop is already right
        point bot = subvision.searchLine((int)lowcx, (int)lowcy,
                                            col, false, true);
        len = DISTANCE(beaconTop.first, beaconTop.second,
                        bot.first, bot.second);
    } else {
        beaconTop = subvision.searchLine((int)highcx, (int)highcy,
                                            col, true, true);
        point bot = subvision.searchLine((int)pinkcx, (int)pinkcy,
                                            cBEACON_PINK, false, true);
        len = DISTANCE(beaconTop.first, beaconTop.second,
                        bot.first, bot.second);
    }
    // we have already estimated beaconHeight as 4*height. If 2*len looks
    // sensible it's probably a better estimate
    len *= 2;
    if (len > 0.75 * beaconHeight && len < 1.5 * beaconHeight) {
        beaconHeight = len;
    }
    double lowlowcx, lowlowcy; // bottom quarter of the beacon (white)
    lowlowcx = pinkcx - sin(horizonAngle) * (pinkAbove ? 3 : 2) * size * flip;
    lowlowcy = pinkcy + cos(horizonAngle) * (pinkAbove ? 3 : 2) * size * flip;
    if (lowlowcx >= 0 && lowlowcx < CPLANE_WIDTH
            && lowlowcy >= 0 && lowlowcy < CPLANE_HEIGHT) {
        // FIXME: reduce CF if not nearColour white
        beaconBottom = subvision.searchLine((int)lowlowcx, (int)lowlowcy,
                                            cWHITE, false, true);
        len = DISTANCE(beaconTop.first, beaconTop.second,
                        beaconBottom.first, beaconBottom.second);

        // and again if the full visual length of the beacon is a sensible
        // estimate it's probably better.
        if (len > 0.75 * beaconHeight && len < 1.5 * beaconHeight) {
            beaconHeight = len;
        }
    }
    // fill in vob information. The information is all based on the
    // pink square.
    if (pinkAbove) {
        beacon.cx = (pinkcx + lowcx)/2; // cx, cy is the line between
        beacon.cy = (pinkcy + lowcy)/2; // colours to match old vision
    } else {
        beacon.cx = (pinkcx + highcx)/2;
        beacon.cy = (pinkcy + highcy)/2;
    }
    beacon.x = beacon.cx - width/2;
    beacon.y = beacon.cy - beaconHeight/4;
    beacon.width = width;
    beacon.height = beaconHeight;
    beacon.cf = groupItr->size(); // beacon cf is number of features
#ifdef OFFLINE    
    //cout << "cx:" << beacon.cx << " cy:" << beacon.cy << " x:" << beacon.x << " y:" << beacon.y << " beacon.height:" << beacon.height << " beacon.cf:" << beacon.cf << endl;
#endif
    if (SanityChecks::checkBeaconSanity(beacon) != SANE) {
        return false;
    } else if (beacon.cf < candidate->cf) {
        return false;
    }
    // only overwrite if this beacon is better
    *candidate = beacon;
    return true;

}


/* Returns true if some part of the first Visual feature (which must be a line,
 * with endx and endy defined) is directly above or below (with reference to
 * the horizon) some part of the second line; i.e. a vertical bisector would
 * intersect both lines. The two features must be on adjacent scan lines.
 */
bool SubObject::lineAbove(const VisualFeature& vf1, const VisualFeature& vf2,
                            int nScanLines) {
    double x1, endx1, x2, endx2;
    double hGradient = subvision.getHorizonGradient();
    if (abs(vf1.scanline - vf2.scanline) > nScanLines) {
        return false;
    }
    // We draw lines from the feature endpoints perpendicular to the horizon
    // and calculate the (x coord of the) point of intersection of this
    // line with the horizon. We can then determine if the x coords of these
    // lines lie "within" on another.
    // The equation of the horizon is y = mx. Then the x coord of the point 
    // of intersection is given by
    // x = (m*vf.y + vf.x)/(1 - m^2)
    x1 = (hGradient * vf1.y + vf1.x)/(1 - SQUARE(hGradient));
    endx1 = (hGradient * vf1.endy + vf1.endx)/ (1 - SQUARE(hGradient));
    x2 = (hGradient * vf2.y + vf2.x)/(1 - SQUARE(hGradient));
    endx2 = (hGradient * vf2.endy + vf2.endx)/ (1 - SQUARE(hGradient));

    if (x1 > endx1)
        swap(x1, endx1);
    if (x2 > endx2)
        swap(x2, endx2);

    if (x2 >= x1 && x2 <= endx1)
        return true;
    if (endx2 >= x1 && endx2 <= endx1)
        return true;
    if (x1 >= x2 && x1 <= endx2)
        return true;
    if (endx1 >= x2 && endx1 <= endx2)
        return true;
    
    //cout << x1 << " - " << endx1 << " not lined with " << x2 << " - "
    //    << endx2 << endl;
    return false;
}
// 
// Below here is disabled code for reference
//
#if 0
/* Attempts to determine the ball's centre and radius by contructing 
 * perpendicular bisectors to chords formed from pairs of ball features
 * and averaging these to find a centroid. (This used to be called
 * fireball). Returns true if the procedure succeeded.
 */
bool SubObject::chordBall(void) {
    if (ballFeatures.size() < MIN_BALLFEATURES_PROJECTION) {
        return false;
    }
    pair<double, double> centroid(0,0);
    double radius = 0; // average distance from centroid
    // Draw chords from pairs of edge points, construct perpendicular
    // bisectors and estimate their average intersection.
    list<int>::iterator itr;
    list<int>::iterator itr2;
    vector<pair<pair<double, double>, double> > bisectors; // ((x, y), grad)
    // first construct the bisectors
    for (itr = ballFeatures.begin(); itr != ballFeatures.end(); ++itr) {
        for (itr2 = ballFeatures.begin(); itr2 != ballFeatures.end(); ++itr2) {
            if (*itr == *itr2)
                continue;
            VisualFeature& f1 = features[*itr];
            VisualFeature& f2 = features[*itr2];
            // calculate line middle
            pair<double, double> bisect((f1.x + f2.x)/2, (f1.y + f2.y)/2);
            double gradient = (f2.y - f1.y)/(f2.x - f1.x);
            gradient = -1/gradient; // perpendicular
            bisectors.push_back(make_pair(bisect, gradient));
            //cout << "bisector (" << bisect.first << "," << bisect.second
            //    << ") m = " << gradient << endl;
        }
    }
    if (bisectors.size() < MIN_CHORDBALL_CHORDS) {
        //cout << "chordBall: too few chords (" << bisectors.size() << ")"
        //    << endl;
        return false;
    }

    // now for each pair of bisectors, calculate their point of intersection
    // and average it
    //vector<pair<double, double> > intersections;
    int numIntersect = 0;
    centroid.first = centroid.second = 0.0;
    vector<pair<pair<double, double>, double> >::iterator bitr, bitr2;
    for (bitr = bisectors.begin(); bitr != bisectors.end(); ++bitr) {
        for (bitr2 = bisectors.begin(); bitr2 != bisectors.end(); ++bitr2) {
            if (bitr == bitr2)
                continue;
            // convert lines to y = mx + b
            double b1 = solveForX(bitr->first, bitr->second, 0.0);
            double b2 = solveForX(bitr2->first, bitr2->second, 0.0);
            // now x = (b2-b1)/(m1-m2)
            pair<double, double> intersect;
            intersect.first = (b2 - b1)/(bitr->second - bitr2->second);
            intersect.second = solveForX(bitr->first, bitr->second,
                                        intersect.first);
            if (isnan(intersect.first) || isinf(intersect.first)
                    || isnan(intersect.second) || isinf(intersect.second))
                continue;
            //intersections.push_back(intersect);
            ++numIntersect;
            centroid.first += intersect.first;
            centroid.second += intersect.second;
//            if (! (intersect.first > 0 && intersect.first < 208))
//                cout << "intersection at (" << intersect.first << "," 
//                << intersect.second << ")" << endl;
        }
    }
    if (numIntersect < MIN_CHORDBALL_INTERSECTS) {
        //cout << "chordBall: too few intersections (" << numIntersect
        //    << ")" << endl;
        return false;
    }

    centroid.first /= (numIntersect);
    centroid.second /= (numIntersect);
    // recalculate the radius from this estimated centre
    for (itr = ballFeatures.begin(); itr != ballFeatures.end(); ++itr) {
        radius += DISTANCE(features[*itr].x, features[*itr].y,
                            centroid.first, centroid.second);
    }
    radius /= ballFeatures.size();

    vBall->cx = centroid.first;
    vBall->cy = centroid.second;
    vBall->radius = radius;
    vBall->cf = 500; // FIXME set a proper value here
    return true;
}
#endif

