/**
    \file CSED.cpp
    Implementation of Ragnemalm's CSED distance transform algorithm which, 
    given and input binary image, calculates the corresponding distance 
    transform.

    \author George J. Grevera, Ph.D., ggrevera@sju.edu

    Copyright (C) 2002, George J. Grevera

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation.

    This program 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 program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
    USA or from http://www.gnu.org/licenses/gpl.txt.

    This General Public License does not permit incorporating this
    code into proprietary programs.  (So a hypothetical company such
    as GH (Generally Hectic) should NOT incorporate this code into
    their proprietary programs.)
 */
/*
     no OUT OF BOUNDS checks!
*/
#include <stdlib.h>
#include <vector>
#include "CSED.h"

using namespace std;

//define this symbol to produce the version of the algorithm as 
// described in the original paper (which may contains errors in
// the form of uninitialized distance transform values.
//#define ORIGINAL
//----------------------------------------------------------------------
/** \brief do the distance transform
 *
 *  Input : (C,I) - a 2D binary scene of domain size X by Y
 *  Output: (C,d) - a 2D grey scene of domain size X and Y representing
 *                  the distance scene
 */
void CSED::doTransform ( const unsigned char* const I ) {
    borderCheck(I);
    cleanUp();
    int  x, y;

    //'v' in Ragnemalm's paper is used as a _displacement_ to the parent
    // (rather than as the actual location of the parent)
    this->v = (P*)malloc( ySize*xSize*sizeof(P) );
    assert( this->v!=NULL );
    for (y=0; y<ySize; y++) {
        for (x=0; x<xSize; x++) {
            const int  i=sub(x,y);
            this->v[i].x = this->v[i].y = DistanceTransform::IntInfinity;
        }
    }

    //initialize immediate interior & exterior elements (i.e. border elements)

    //the following vectors contain lists of subscripts
    vector<P*>   v1;
    vector<P*>   v2;
    vector<P*>*  list1 = &v1;
    vector<P*>*  list2 = &v2;
    vector<P*>*  tmp   = NULL;

    int  count = 0;
    for (y=1; y<ySize-1; y++) {
        for (x=1; x<xSize-1; x++) {
            if ( I[sub(x-1,y)] != I[sub(x,y)] || I[sub(x+1,y)] != I[sub(x,y)] ||
                 I[sub(x,y-1)] != I[sub(x,y)] || I[sub(x,y+1)] != I[sub(x,y)] ) {
                    const int  i=sub(x,y);
                    this->v[i].x = this->v[i].y = 0;
                    ++count;
            }
        }
    }
    assert( count>0);

    for (y=1; y<ySize-1; y++) {
        for (x=1; x<xSize-1; x++) {
            if ( I[sub(x-1,y)] != I[sub(x,y)] || I[sub(x+1,y)] != I[sub(x,y)] ||
                 I[sub(x,y-1)] != I[sub(x,y)] || I[sub(x,y+1)] != I[sub(x,y)] ) {
                    const int  i=sub(x,y);
                    test(x, y,  0,  1, list2, i);
                    test(x, y,  1,  1, list2, i);
                    test(x, y,  1,  0, list2, i);
                    test(x, y,  1, -1, list2, i);
                    test(x, y,  0, -1, list2, i);
                    test(x, y, -1, -1, list2, i);
                    test(x, y, -1,  0, list2, i);
                    test(x, y, -1,  1, list2, i);
            }
        }
    }
    //cout << count << " border elements" << endl;

    //switch list1 and list2
    tmp   = list1;
    list1 = list2;
    list2 = tmp;

    int  d = 1;
    while (list1->size() > 0) {
        for (int i=0; i<(int)list1->size(); i++) {
            //get (x,y) from list1 entry
            const int  x = (*list1)[i]->x;
            const int  y = (*list1)[i]->y;
            //cout << "  element " << i << " " << x << "," << y << endl;
            const int  p = sub(x,y);
            if (D( v[p] ) > d) {
                //too far away for now.  put on list2 for later.
                P*  temp = new P(x,y);
                list2->push_back(temp);
            } else if (v[p].x==0) {
                //vertical - same column, previous or next row
                test(x, y, 0, sgn(v[p].y), list2, p);
            } else if (v[p].y==0) {
                //horizontal - same row, previous or next column
                test(x, y, sgn(v[p].x), 0, list2, p);
            } else if (abs(v[p].x) == abs(v[p].y)) {
                //diagonal
                test(x, y, sgn(v[p].x), sgn(v[p].y), list2, p);
#ifdef ORIGINAL
                if (d==1) {
#endif
                    test(x, y, sgn(v[p].x), 0,           list2, p);
                    test(x, y, 0,           sgn(v[p].y), list2, p);
#ifdef ORIGINAL
                }
#endif
            } else if (abs(v[p].x) > abs(v[p].y)) {
                test(x, y, sgn(v[p].x), sgn(v[p].y), list2, p);
                test(x, y, sgn(v[p].x), 0,           list2, p);
#ifdef ORIGINAL
            } else {  // abs(v[j].x) < abs(v[j].y)
                test(x, y, sgn(v[p].x), sgn(v[p].y), list2, p);
                test(x, y, 0,           sgn(v[p].y), list2, p);
            }
#else
        } else if (abs(v[p].x) < abs(v[p].y)) {  //not really necessary but a double check
                test(x, y, sgn(v[p].x), sgn(v[p].y), list2, p);
                test(x, y, 0,           sgn(v[p].y), list2, p);
            } else {
                assert(0);    //should never get here!
                test(x, y,  0,  1, list2, p);
                test(x, y,  1,  1, list2, p);
                test(x, y,  1,  0, list2, p);
                test(x, y,  1, -1, list2, p);
                test(x, y,  0, -1, list2, p);
                test(x, y, -1, -1, list2, p);
                test(x, y, -1,  0, list2, p);
                test(x, y, -1,  1, list2, p);
            }
#endif
        }
        list1->clear();
        ++d;
        //switch list1 and list2
        tmp   = list1;
        list1 = list2;
        list2 = tmp;

        //if (d>=10)  break;
    }

    //cout << "watching: (29,31): " << D(v[sub(29,31)]) << endl;
    //cout << "    v(29,31)=" << v[sub(29,31)].x << "," << v[sub(29,31)].y << endl;
    //cout << "    v(30,31)=" << v[sub(30,31)].x << "," << v[sub(30,31)].y << endl;
    //cout << "    v(30,32)=" << v[sub(30,32)].x << "," << v[sub(30,32)].y << endl;
    
    //initialize d
    double*  dmap = (double*)malloc( ySize*xSize*sizeof(double) );
    assert( dmap!=NULL );
    for (y=0; y<ySize; y++) {
        for (x=0; x<xSize; x++) {
            const int  i=sub(x,y);
            dmap[i] = D( v[i].x, v[i].y );
            if ( v[i].x >= DistanceTransform::IntInfinity ||
                 v[i].y >= DistanceTransform::IntInfinity ) {
                cout << "CSED error: never assigned at (" << x << ","
                     << y << ")." << endl;
            }
        }
    }

    finish(I, dmap);
}
//----------------------------------------------------------------------
/**
\param (x,y) is a pixel position, p
\param (ox,oy) is the displacement from (x,y) to a neighbor, o
\param list2 is a list
\param i is the subscript associated with (x,y)
<pre>
  subroutine test ( o ):
      if D(v(p+o)) > D(v(p)+o)
          v(p+o) = v(p) + o
          p+o is put in list2  //in both
</pre>
*/
void CSED::test ( const int x,  const int y,
                  const int ox, const int oy,
                  vector<P*>* list2, const int i )
{
    //calc the neighbor
    const int  nx = x+ox;
    const int  ny = y+oy;
    //is the neighbor in bounds?
    if (nx<0 || nx>=xSize)    return;
    if (ny<0 || ny>=ySize)    return;
    const int  ni = sub(nx, ny);  //subscript for neighbor

    //is the current assignment to the neighbor more than the D of the
    // center vector plus the offset to the neighbor?
    if ( D(v[ni]) > D(v[i].x+ox, v[i].y+oy) ) {
        v[ni].x = v[i].x+ox;
        v[ni].y = v[i].y+oy;
        //put the location (subscripts) of the neighbor in the list
        P*  tmp = new P(nx,ny);
        list2->push_back(tmp);  //"p+o is put in list2"
        //cout << "        added: " << nx << "," << ny << endl;
    }
}
//----------------------------------------------------------------------
