#include <limits.h>

//#define DEBUG_CHESSBOARD_EVERY_CORNER

static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count, cv::Ptr<IplImage> );

    cv::Ptr<IplImage> dbg3_img;

    int expected_quad_num = ((pattern_size.width+1)*(pattern_size.height+1)+1)/2;

    dbg3_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 );

                PRINTF("EX quad count: %d/%d\n", quad_count, expected_quad_num);

                PRINTF("Quad count: %d/%d\n", quad_count, expected_quad_num);

#ifdef DEBUG_CHESSBOARD

            icvFindQuadNeighbors( quads, quad_count, dbg_img );

#else

            icvFindQuadNeighbors( quads, quad_count, NULL );

#endif

#ifdef DEBUG_CHESSBOARD

			cvCopy(dbg_img, dbg3_img);

			cvNamedWindow("quads_neighbors", 1);

			for (i = 0; i < quad_count; i++) {

				for (int k=0; k<4; k++)

				{

					CvPoint2D32f pt1, pt2;

					CvScalar color = CV_RGB(30,255,30);

					pt1 = quads[i].corners[k]->pt;

					pt2 = quads[i].corners[(k+1)%4]->pt;

					if (k>0)

						color = CV_RGB(200,200,0);

					cvLine( dbg3_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 2, 8);

					if (quads[i].neighbors[k] != NULL) {

						cvCircle(dbg3_img, cvPointFrom32f(pt1), 4, CV_RGB(255,0,0), 1, 8, 0);

					}

				}

			}

			cvShowImage("quads_neighbors", (IplImage*)dbg3_img);

			cvWaitKey();

#endif

typedef struct quadList {

    CvCBQuad *quad;

    struct quadList *next;

    int length;

} quadList;

static quadList *newQuadList(CvCBQuad *quad) {

    quadList *list = (quadList *)cvAlloc(sizeof(quadList));

    list->quad = quad;

    list->next = NULL;

    list->length = 1;

    return list;

}

static void deleteQuadList(quadList *list) {

    quadList *next;

    while (list != NULL) {

        next = list->next;

        cvFree(&list);

        list = next;

    }

}

static void addToQuadList(quadList **list, CvCBQuad *quad) {

    quadList *newList = newQuadList(quad);

    if (*list != NULL) {

        newList->next = *list;

        newList->length += (*list)->length;

    }

    *list = newList;

}

static bool inQuadList(quadList *list, CvCBQuad *quad) {

    while (list != NULL) {

        if (list->quad == quad) {

            return true;

        }

        list = list->next;

    }

    return false;

}

static double itMean(double oldValue, double newValue, int it) {

	// it is zero-based

	if (it == 0) {

		return newValue;

	}

	else {

		return oldValue + (newValue - oldValue) / (it + 1);

	}

}

static double edgeMeanRec(double mean, CvCBQuad *quad, quadList **list) {

    if (inQuadList(*list, quad)) {

        return mean;

    }

    int i;

    double dx, dy;

    int length = *list == NULL ? 0 : (*list)->length;

    for (i = 0; i < 4; i++) {

        dx = quad->corners[i]->pt.x - quad->corners[(i+1)%4]->pt.x;

        dy = quad->corners[i]->pt.y - quad->corners[(i+1)%4]->pt.y;

        mean = itMean(mean, dx * dx + dy * dy, 4 * length + i);

    }

    addToQuadList(list, quad);

    for (i = 0; i < 4; i++) {

        if (quad->neighbors[i] != NULL) {

            mean = edgeMeanRec(mean, quad->neighbors[i], list);

        }

    }

    return mean;

}

static double edgeMean(CvCBQuad *cur_quad, CvCBQuad *other_quad) {

    double mean = 0.0;

    quadList *list = NULL;

    mean = edgeMeanRec(mean, cur_quad, &list);

    mean = edgeMeanRec(mean, other_quad, &list);

    deleteQuadList(list);

    return mean;

}

static int minNumerOfEdgesBetweenCorners(CvCBQuad *cur_quad, int cur_corner, CvCBCorner *target_corner, int max_limit) {

    if (cur_quad->corners[cur_corner] == target_corner) {

        return 0;

    }

    if (max_limit == 0) {

        return INT_MAX - 1; // Infinity, not reachable in zero edges, -1 to not overflow later on

    }

    int cur_edges;

    int min_edges = max_limit;

    cur_edges = minNumerOfEdgesBetweenCorners(cur_quad, (cur_corner+1)%4, target_corner, max_limit-1) + 1;

    if (cur_edges < min_edges)

        min_edges = cur_edges;

    cur_edges = minNumerOfEdgesBetweenCorners(cur_quad, (cur_corner-1+4)%4, target_corner, max_limit-1) + 1;

    if (cur_edges < min_edges)

        min_edges = cur_edges;

    CvCBQuad *neighbor_quad = cur_quad->neighbors[cur_corner];

    if (neighbor_quad != NULL) {

        int i;

        for (i = 0; i < 4; i++) {

            if (neighbor_quad->neighbors[i] == cur_quad)

                break;

        }

        CV_Assert( i < 4 );

        cur_edges = minNumerOfEdgesBetweenCorners(neighbor_quad, (i+1)%4, target_corner, max_limit-1) + 1;

        if (cur_edges < min_edges)

            min_edges = cur_edges;

        cur_edges = minNumerOfEdgesBetweenCorners(neighbor_quad, (i-1+4)%4, target_corner, max_limit-1) + 1;

        if (cur_edges < min_edges)

            min_edges = cur_edges;

    }

    return min_edges;

}

static void icvFindQuadNeighbors( CvCBQuad *quads, int quad_count, cv::Ptr<IplImage> img )

{

    const double max_thresh_scale = 2.0; // Ratio of max diagonal in a x 2a quad, squared (max for all quads is 4.0, 2.0^2)

    const double thresh_scale_step = 0.1;

    double thresh_scale;

    int idx, i, k, j;

    float dx, dy, dist;

    bool quad_added;

    int check_other_closest;

#ifdef DEBUG_CHESSBOARD

    cv::Ptr<IplImage> dbg_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3 );

#endif

    check_other_closest = 1;

    do {

        for (thresh_scale = thresh_scale_step; thresh_scale <= max_thresh_scale; thresh_scale += thresh_scale_step) {

            do {

                quad_added = false;

                // find quad neighbors

                for( idx = 0; idx < quad_count; idx++ )

                    CvCBQuad* cur_quad = &quads[idx];

                    // choose the points of the current quadrangle that are close to

                    // some points of the other quadrangles

                    // (it can happen for split corners (due to dilation) of the

                    // checker board). Search only in other quadrangles!

                    // for each corner of this quadrangle

                    for( i = 0; i < 4; i++ )

                    {

                        CvPoint2D32f pt;

                        float min_dist = FLT_MAX;

                        int closest_corner_idx = -1;

                        CvCBQuad *closest_quad = 0;

                        CvCBCorner *closest_corner = 0;

                        if( cur_quad->neighbors[i] )

                            continue;

                        pt = cur_quad->corners[i]->pt;

                        // find the closest corner in all other quadrangles

                        for( k = 0; k < quad_count; k++ )

                            if( k == idx )

                                continue;

                            for( j = 0; j < 4; j++ )

                            {

                                if( quads[k].neighbors[j] )

                                    continue;

                                dx = pt.x - quads[k].corners[j]->pt.x;

                                dy = pt.y - quads[k].corners[j]->pt.y;

                                dist = dx * dx + dy * dy;

                                if( dist < min_dist &&

                                    dist <= edgeMean(cur_quad, &(quads[k]))*thresh_scale )

                                {

                                    // check edge lengths, make sure they're compatible

                                    // edges that are different by more than 1:4 are rejected

                                    float ediff = cur_quad->edge_len - quads[k].edge_len;

                                    if (ediff > 32*cur_quad->edge_len ||

                                        ediff > 32*quads[k].edge_len)

                                    {

                                        PRINTF("Incompatible edge lengths\n");

                                        continue;

                                    }

                                    closest_corner_idx = j;

                                    closest_quad = &quads[k];

                                    min_dist = dist;

                                }

                            }

                        // we found a matching corner point?

                        if( closest_corner_idx >= 0 && min_dist < FLT_MAX )

                        {

                            // If another point from our current quad is closer to the found corner

                            // than the current one, then we don't count this one after all.

                            // This is necessary to support small squares where otherwise the wrong

                            // corner will get matched to closest_quad;

                            closest_corner = closest_quad->corners[closest_corner_idx];

            #ifdef DEBUG_CHESSBOARD

            #ifdef DEBUG_CHESSBOARD_EVERY_CORNER

                            cvCopy(img,dbg_img);

                            cvNamedWindow("closest_corner", 1);

                            cvCircle(dbg_img, cvPointFrom32f(cur_quad->corners[i]->pt), 4, CV_RGB(0,255,0), 1, 8, 0);

                            cvCircle(dbg_img, cvPointFrom32f(closest_corner->pt), 4, CV_RGB(255,0,0), 1, 8, 0);

                            cvShowImage("closest_corner", (IplImage*)dbg_img);

                            cvWaitKey();

            #endif

            #endif

                            for( j = 0; j < 4; j++ )

                            {

                                if( cur_quad->neighbors[j] == closest_quad )

                                    break;

                                dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x;

                                dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y;

                                if( dx * dx + dy * dy < min_dist )

                                    break;

                            }

                            if( j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4 ) {

                                PRINTF("Another corner of the current quad is closer.\n");

                                continue;

                            }

                            // Check that each corner is a neighbor of different quads

                            for( j = 0; j < closest_quad->count; j++ )

                            {

                                if( closest_quad->neighbors[j] == cur_quad )

                                    break;

                            }

                            if( j < closest_quad->count ) {

                                PRINTF("Closest quad has already for neighbor current quad.\n");

                                continue;

                            }

                            // Check if merging corners will make a triangle

                            // This happens if two edges from opposite quads are merged together, then space (white quad) between

                            // them becomes a triangle

                            // As we increase thresh_scale incrementally we can be sure that better (closer) corners were already

                            // merged if this was possible, so this one is probably not the right one to merge

                            if (minNumerOfEdgesBetweenCorners(cur_quad, i, closest_corner, 4) < 4) {

                                PRINTF("Merging corners would make a triangle.\n");

                                continue;

                            }

                            // First time we check if there is some other quad which is closer to selected closest corner

                            // Second time all useful corners in other quads are already used so we can use selected closest corner

                            // even if there is some possible (but obviously not really useful) other quad closer

                            if (check_other_closest > 0) {

                                // check whether the closest corner to closest_corner

                                // is different from cur_quad->corners[i]->pt

                                for( k = 0; k < quad_count; k++ )

                                {

                                    CvCBQuad* q = &quads[k];

                                    if( k == idx || q == closest_quad )

                                        continue;

                                    for( j = 0; j < 4; j++ )

                                        if( !q->neighbors[j] )

                                        {

                                            dx = closest_corner->pt.x - q->corners[j]->pt.x;

                                            dy = closest_corner->pt.y - q->corners[j]->pt.y;

                                            dist = dx*dx + dy*dy;

                                            if( dist < min_dist )

                                                break;

                                        }

                                    if( j < 4 )

                                        break;

                                }

                                if( k < quad_count ) {            

                                    PRINTF("Another corner is closer than our closest one.\n");

                                    continue;

                                }

                            }

                            PRINTF("Found closest corner.\n");

                            closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;

                            closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;

                            // We've found one more corner - remember it

                            cur_quad->count++;

                            cur_quad->neighbors[i] = closest_quad;

                            cur_quad->corners[i] = closest_corner;

                            closest_quad->count++;

                            closest_quad->neighbors[closest_corner_idx] = cur_quad;

                            quad_added = true;

                        }

                    }

            } while (quad_added);

        check_other_closest--;

    } while (check_other_closest >= 0);