/**************************************************************

matsolve.c (C-Munipack project)
Copyright (C) 2003 David Motl, dmotl@volny.cz

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; either version 2
of the License, or (at your option) any later version.

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.

**************************************************************/
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "comfun.h"
#include "console.h"
#include "cholesky.h"
#include "robmean.h"
#include "cmpack_common.h"
#include "cmpack_match.h"
#include "matfun.h"
#include "matsolve.h"
#include "matstack.h"
#include "match.h"

/* Transformation matrix mapping */
#define M(x,y) m[(x)+(y)*3]
        
/* Mapping for matrix a */
#define A(x,y) a[(x)+(y)*5]
        
/* machine epsilon real number (appropriately) */
#define MACHEPS 1.0E-15
        
/* machine maximum real number (appropriately) */
#define MACHMAX 1.0E10

/* Compare two objects by radius */
static int compare_fn(const void *a, const void *b)
{
	double ra = ((struct _CmpackMatchObject*)a)->r, rb = ((struct _CmpackMatchObject*)b)->r;
	return (ra < rb ? -1 : (ra > rb ? 1 : 0));
}

static void Init1(int n1, const double *x1, const double *y1, int i1, int i2, struct _CmpackMatchObject *idr1)
/* Construct list of stars in image 1 and sorts them with respect of distance from the center of the abscissa
   given by stars i1 and i2. Adds i1 and i2 to the polygon id1. Returns squared distance of i1 and i2 as z1. */
{
    double xc, yc, dx, dy, z1;
    int j;

    xc = 0.5*(x1[i1]+x1[i2]);
    yc = 0.5*(y1[i1]+y1[i2]);
    dx = x1[i1]-x1[i2];
    dy = y1[i1]-y1[i2];
    z1 = 1.0/((dx*dx)+(dy*dy));
    for (j=0;j<n1;j++) {
	    idr1[j].i = j;
	    if (j==i1) 
	        idr1[j].r = -2.0; 
	    else if (j==i2) 
	        idr1[j].r = -1.0; 
	    else {
		    dx = x1[j]-xc;
		    dy = y1[j]-yc;
	        idr1[j].r = z1*((dx*dx)+(dy*dy));
        }
    }
	qsort(idr1,n1,sizeof(struct _CmpackMatchObject),compare_fn);
}

static void Init2(int n2, const double *x2, const double *y2, int j1, int j2, struct _CmpackMatchObject *idr2)
/* Construct list of stars in image 2 and sorts them with respect of distance from the center of the abscissa
   given by stars i1 and i2. Adds i1 and i2 to the polygon id1. Returns squared distance of i1 and i2 as z1. */
{
    double xc, yc, dx, dy, z2;
    int j;

    xc = 0.5*(x2[j1]+x2[j2]);
    yc = 0.5*(y2[j1]+y2[j2]);
    dx = x2[j1]-x2[j2];
    dy = y2[j1]-y2[j2];
    z2 = 1.0/((dx*dx)+(dy*dy));
    for (j=0;j<n2;j++) {
	    idr2[j].i = j;
	    if (j==j1) 
	        idr2[j].r = -2.0; 
	    else if (j==j2) 
	        idr2[j].r = -1.0; 
	    else {
		    dx = x2[j]-xc;
		    dy = y2[j]-yc;
	        idr2[j].r = z2*((dx*dx)+(dy*dy));
        }
    }
	qsort(idr2,n2,sizeof(struct _CmpackMatchObject),compare_fn);
}

static int Resrov(int n, double axi, double ayi, double a2, double *b, double *y)
/* Resi soustavu rovnic a vypocte vyslednou transformacni matici M */
{
	double a[5*4];

	A(0,0) = n;
	A(1,0) = 0.0;
	A(2,0) = axi;
	A(3,0) = ayi;
	A(0,1) = 0.0;
	A(1,1) = n;
	A(2,1) = ayi;
	A(3,1) = -axi;
	A(0,2) = axi;
	A(1,2) = ayi;
	A(2,2) = a2;
	A(3,2) = 0.0;
	A(0,3) = ayi;
	A(1,3) = -axi;
	A(2,3) = 0.0;
	A(3,3) = a2;
	A(4,0) = b[0];
	A(4,1) = b[1];
	A(4,2) = b[2];
	A(4,3) = b[3];
	if (cholesky(4,a)) 
	    return 1;
	if (fabs(A(4,2)*A(4,2)+A(4,3)*A(4,3)-1.0)>0.2) 
	    return 1;

	y[0] = A(4,2);
	y[1] = A(4,3);
	y[2] = A(4,0);
	y[3] = A(4,1);
	return 0;
}

/* Find coefficients of transformation */
static int FindTrafo(CmpackMatch *cfg, int n1, const int *idl1, const double *x1, const double *y1, 
	int n2, const int *idl2, const double *x2, const double *y2, double *dev, double *k2, 
	int ns, double mirror, int *mstar, double *sumsq, double *m)
{
	int    i,j,j0,nn,id1,id2,i1,i2,i3,k,l,rep;
	double zoom,s,axi,ayi,a2,xx1,yy1,xx2,yy2,xx,yy,tol2,dr,rmin,p[4],y[4];
	double maxdev, sig, dx1, dx2, dy1, dy2;
	double xmed, xsig, xdev, xarc, prum;

	/* We need at least three stars */
	if (ns<3)
		return 0;				

	/* Calculation of ratio of similarity (scale). */
	l = 0; rep = 0;
	for (i=0;i<ns;i++) {
		for (j=i+1;j<ns;j++) {
			i1  = idl1[i];
			i2  = idl1[j];
			dx1 = x1[i2]-x1[i1];
			dy1 = y1[i2]-y1[i1];
            i1  = idl2[i];
            i2  = idl2[j];
            dx2 = x2[i2]-x2[i1];
            dy2 = y2[i2]-y2[i1];
            k2[l] = sqrt((dx2*dx2+dy2*dy2)/(dx1*dx1+dy1*dy1));
            if ((l==0)&&(k2[l]<0.9)) rep = 1;
            if (rep) k2[l] = 1.0/k2[l];
			l++;
		}
	}

	/* Calculation of mean and median */
	cmpack_robustmean(l,k2,&xmed,&xsig);

    /* Calculation of absolute deviations */
    for (i=0;i<l;i++) {
	    dev[i] = fabs(k2[i]-xmed);
		if (dev[i] > 0.2*xmed) {
			/* deviation is too great (because it isn't from the true series) */
	    	return 0;   
		}
    }

	/* Calculation of the median deviation */
	cmpack_robustmean(l,dev,&xdev,&xsig);

	if (xdev > 0.05*xmed) {
		/* Mean deviation is too great */
		return 0;       
	}

    /* Calculation of the aritmetical mean */
    s = 0.0;
    for (i=0;i<l;i++) 
    	s += k2[i];
    prum = s/l;
    /* Find the greatest deviation into mean */
    j = 0; maxdev = -1.0;
    for (i=0;i<l;i++) {
	    dev[i] = (k2[i]-prum)*(k2[i]-prum);
        if (dev[i]>maxdev) {
	        maxdev = dev[i];
	        j = i;
        }
    }
    /* Calculation of the new mean */
    s = 0.0;
    for (i=0;i<l;i++) 
    	if (i!=j) s += k2[i];
    prum = s/(l-1);
    /* Estimation of the standard deviation */
    s = 0.0;
    for (i=0;i<l;i++) {
	    dev[i] = (k2[i]-prum);
        if (i!=j) s += dev[i]*dev[i];
    }
    sig = sqrt(s/(l-2))+MACHEPS;
    if (sig > 0.05*prum) 
    	return 0;   /* Standard deviation is too great */
    if (fabs(prum-xmed)>=sig) 
    	return 0; /* Test for equality of median and mean */

    /* Test on the same arcs of N-polygon */
    l = 0;
	for (i=0;i<ns;i++) {
		for (j=i+1;j<ns;j++) {
		    for (k=j+1;k<ns;k++) {
				double a1[2], a2[2], b1[2], b2[2];
			    i1 = idl1[i];
			    i2 = idl1[j];
			    i3 = idl1[k];
			    a1[0] = x1[i2]-x1[i1];
			    a1[1] = y1[i2]-y1[i1];
			    b1[0] = x1[i3]-x1[i1];
			    b1[1] = y1[i3]-y1[i1];
			    i1 = idl2[i];
			    i2 = idl2[j];
			    i3 = idl2[k];
			    a2[0] = x2[i2]-x2[i1];
			    a2[1] = y2[i2]-y2[i1];
			    b2[0] = x2[i3]-x2[i1];
			    b2[1] = y2[i3]-y2[i1];
			    k2[l] = Uhel(a1,b1)/Uhel(a2,b2);
			    if ((k2[l]>2.0)||(k2[l]<0.5)) 
			    	return 0; /* devation onto 1.0 is too great */
			    l++;
		    }
	    }
    }

	/* Calculation of mean and median of arcs */
	cmpack_robustmean(l,k2,&xarc,&xsig);

    /* Calculation of the deviation into median */
    for (i=0;i<l;i++) {
	    dev[i] = fabs(k2[i]-xarc);
        if (dev[i] > 0.2*xarc) 
        	return 0;
    }
	cmpack_robustmean(l,dev,&xdev,&xsig);

    if (xdev+MACHEPS > 0.2*xarc) 
    	return 0;           /* Mean deviation is too great */
    if (fabs(xarc-1.0) > (3.0*(xdev+MACHEPS))) 
    	return 0; 			/* Median must be equal 1.0 */

	/* Calculate scale factor */
    zoom = (rep ? 1.0/xmed : xmed);

    /* Zero-iteration */
    p[0] = p[1] = p[2] = p[3] = axi = ayi = a2 = 0.0;
    for (i=0; i<ns; i++) {
	    id1  = idl1[i];
	    xx1  = x1[id1];
	    yy1  = y1[id1];
	    id2  = idl2[i];
	    xx2  = x2[id2];
	    yy2  = y2[id2];
	    axi += zoom*xx1;
	    ayi += zoom*mirror*yy1;
	    a2  += zoom*zoom*(xx1*xx1+yy1*yy1);
	    p[0]+= xx2;
	    p[1]+= yy2;
	    p[2]+= zoom*(xx1*xx2+mirror*yy1*yy2);
	    p[3]+= zoom*(mirror*yy1*xx2-xx1*yy2);
    }
      
    /* Find estimation of transformation matrix */
    if (Resrov(ns,axi,ayi,a2,p,y)!=0)
	    return 0;
	M(0,0) = zoom*y[0];
    M(0,1) = zoom*mirror*y[1];
    M(0,2) = y[2];
    M(1,0) = -zoom*y[1];
    M(1,1) = zoom*mirror*y[0];
    M(1,2) = y[3];
    M(2,0) = 0.0;
    M(2,1) = 0.0;
    M(2,2) = 1.0;

    /* Residuals */
    s = 0.0;
    for (i=0;i<ns;i++) {
	    xx1 = x1[idl1[i]];
	    yy1 = y1[idl1[i]];
	    Trafo(m,xx1,yy1,&xx,&yy);
	    xx2 = x2[idl2[i]];
	    yy2 = y2[idl2[i]];
	    s  += (xx-xx2)*(xx-xx2)+(yy-yy2)*(yy-yy2);
    }
    tol2 = fmax(0.0001, 3.0*cfg->clip*cfg->clip*s/(ns-2.0)+MACHEPS);

    /* Residuals for all stars */
    nn = 0;
    p[0] = p[1] = p[2] = p[3] = axi = ayi = a2 = s = 0.0;
    for (i=0;i<n1;i++) {
	    Trafo(m,x1[i],y1[i],&xx,&yy);
        /* Find star with the smallest residues */
        rmin = MACHMAX;
		j0 = -1;
        for (j=0;j<n2;j++) {
	        dr =(xx-x2[j])*(xx-x2[j])+(yy-y2[j])*(yy-y2[j]);
	        if (dr<rmin) { rmin = dr; j0 = j; }
        }
        if (rmin<=tol2) {
	        /* Star was found */
	        s   += rmin;
    	    xx1  = x1[i];
	        yy1  = y1[i];
	        xx2  = x2[j0];
	        yy2  = y2[j0];
	        axi += zoom*xx1;
	        ayi += zoom*mirror*yy1;
	        a2  += zoom*zoom*(xx1*xx1+yy1*yy1);
	        p[0]+= xx2;
	        p[1]+= yy2;
	        p[2]+= zoom*(xx1*xx2+mirror*yy1*yy2);
	        p[3]+= zoom*(mirror*yy1*xx2-xx1*yy2);
	        nn++;
        }
    }
      
    /* We need at least three stars */
    if (nn<3)
	    return 0;
	      
	/* Find improved transformation matrix */
    if (Resrov(nn,axi,ayi,a2,p,y)!=0)
	    return 0;
	M(0,0) = zoom*y[0];
    M(0,1) = zoom*mirror*y[1];
    M(0,2) = y[2];
    M(1,0) = -zoom*y[1];
    M(1,1) = zoom*mirror*y[0];
    M(1,2) = y[3];
    M(2,0) = 0.0;
    M(2,1) = 0.0;
    M(2,2) = 1.0;
	      
	/* Compute improved tolerance */
    tol2 = fmax(0.0001, 3.0*cfg->clip*cfg->clip*s/(nn-2.0)+MACHEPS);

    /* Compute number of fitted stars nn and sum of residuals s */
    nn=0; s=0.0;
    for (i=0;i<n1;i++) {
	    Trafo(m,x1[i],y1[i],&xx,&yy);
        /* Find star with the smallest residues */
        rmin = MACHMAX;
        for (j=0;j<n2;j++) {
	        dr =(xx-x2[j])*(xx-x2[j])+(yy-y2[j])*(yy-y2[j]);
	        if (dr<rmin) { rmin = dr; }
        }
        if (rmin<tol2) {
	        s += rmin;
	        nn++;
        }
	}
	
	/* Transformation is OK, if there are at least three stars identified */
	if (nn<3) 
		return 0;
      
	/* Normal return, transformation is OK */
	*mstar = nn;          
	*sumsq = s;

	return 1;
}

int _solve(CmpackMatch *cfg, CmpackMatchFrame *lc, int *id1, struct _CmpackMatchObject *idr1,
	int *id2, struct _CmpackMatchObject *idr2, double *dev, double *k2, CmpackMatchStack *stack)
{
	int 	mstar, nstar, n1, n2, i, j, j0, ni, i1, i2, i3, j1, j2, j3;
	double 	dr, rr, rl, rg, rmax, dif, u1, u2, v1, v2;
	double 	xx, yy, dx, dy, sumsq, tol, m[9];
	char 	msg[256];

	/* Number of stars used for matching */
	n1 = 0;
	for (i1=0; i1<cfg->c1 && n1<cfg->maxstar; i1++) {
		if (cfg->i1[i1] >= 0 && cfg->i1[i1] != cfg->ignore[0]) {
			lc->g_x1[n1] = cfg->x1[i1];
			lc->g_y1[n1] = cfg->y1[i1];
			n1++;
		}
	}
	n2 = 0;
	for (j1=0; j1<cfg->c2 && n2<cfg->maxstar; j1++) {
		if (cfg->i2[j1] >= 0 && cfg->i2[j1] != cfg->ignore[1]) {
			lc->g_x2[n2] = cfg->x2[j1];
			lc->g_y2[n2] = cfg->y2[j1];
			n2++;
		}
	}

	/* For all doubles of stars on reference frame -> i1, i2 */
	for (i1=0;i1<n1;i1++) {
		for (i2=i1+1;i2<n1;i2++) {
			Init1(n1,lc->g_x1,lc->g_y1,i1,i2,idr1);
		    id1[0] = idr1[0].i;
		    id1[1] = idr1[1].i;
			/* For all doubles of stars on input frame -> j1, j2 */
			for (j1=0;j1<n2;j1++) {
				for (j2=j1+1;j2<n2;j2++) {
					Init2(n2,lc->g_x2,lc->g_y2,j1,j2,idr2);
				    id2[0] = idr2[0].i;
				    id2[1] = idr2[1].i;
  				    /* For all stars on reference frame except i1 and i2 -> i3 */
					ni = 2;
					for (i3=2; i3<n1 && ni<cfg->maxstar; i3++) {
                        /* Find the star on input frame with the distance most close to i3 -> j3 */
                        rr   = idr1[i3].r;
                        rl   = rr/1.5;
	                	rg   = rr*1.5;
                        rmax = MACHMAX;
                        j3  = -1;
                        for (j=2; j<n2 && idr2[j].r<rg; j++) {
                            if (idr2[j].r>rl && idr2[j].i!=id2[ni-1] && idr2[j].i!=id2[ni-2]) {
                            	dif = fabs(idr2[j].r-rr);
	                    		if (dif<rmax) { 
		                    		rmax = dif; 
		                    		j3 = j; 
		                    	}
	                    	}
                        }
                        if (j3<0) {
	                        /* Not found, try next i3 */
                        	continue; 
                    	}
                        /* Calculation of the triangle coordinates (u1,v1) in the reference file */
                        UV(id1[ni-2],id1[ni-1],idr1[i3].i,lc->g_x1,lc->g_y1,&u1,&v1);
                        /* Calculation of the triangle coordinates (u2,v2) in the input file */
                        UV(id2[ni-2],id2[ni-1],idr2[j3].i,lc->g_x2,lc->g_y2,&u2,&v2);
                        if (((u1-u2)*(u1-u2)+(v1-v2)*(v1-v2))<0.0005) {
							/* Add i3 and j3 to the polygon */
							id1[ni] = idr1[i3].i;
							id2[ni] = idr2[j3].i;
	                        ni++;
                            if (ni>=cfg->nstar) {
	                            /* We have enough vertices in the polygon, find transformation matrix */
								if (FindTrafo(cfg,n1,id1,lc->g_x1,lc->g_y1,n2,id2,lc->g_x2,lc->g_y2,dev,k2,ni,1.0,&mstar,&sumsq,m)) {
									/* Add transformation to the stack */
								    StAdd(stack,id1,id2,ni,sumsq,mstar,m);
								}
								if (FindTrafo(cfg,n1,id1,lc->g_x1,lc->g_y1,n2,id2,lc->g_x2,lc->g_y2,dev,k2,ni,-1.0,&mstar,&sumsq,m)) {
									/* Add transformation to the stack */
								    StAdd(stack,id1,id2,ni,sumsq,mstar,m);
								}
								/* Next j2 */
	                            break;	
                            }
					    }
                    }
                }
            }
        }
    }

	/* Select from stack the best case */
    StSelect(stack,&mstar,&sumsq,m);
    if (mstar==0) {
		printout(cfg->con,1,"Coincidences not found!");
	    return CMPACK_ERR_MATCH_NOT_FOUND;
	}

	tol = fmax(0.0001, 3.0*cfg->clip*cfg->clip*sumsq/(mstar-2.0)+MACHEPS);

    /* Matching OK */
	sprintf(msg,"Sum of squares in the best case  : %.4f", sumsq);
	printout(cfg->con,1, msg);
	sprintf(msg,"Tolerance                        : %.2f", sqrt(tol));
	printout(cfg->con,1, msg);
	sprintf(msg,"Transformation matrix            : ");
	printout(cfg->con,1, msg);
	sprintf(msg,"   %15.3f %15.3f %15.3f",M(0,0),M(0,1),M(0,2));
	printout(cfg->con,1, msg);
	sprintf(msg,"   %15.3f %15.3f %15.3f",M(1,0),M(1,1),M(1,2));
	printout(cfg->con,1, msg);
	sprintf(msg,"   %15.3f %15.3f %15.3f",M(2,0),M(2,1),M(2,2));
	printout(cfg->con,1, msg);
	
	/* Set cross-references for all stars on input frame */
	nstar = 0;
	if (cfg->setref[0]>0 && cfg->setref[1]>0) {
		/* Client defined match for a minor body object */
		for (j=0; j<cfg->c2; j++) {
			if (cfg->i2[j] == cfg->setref[1]) {
				cfg->xref[j] = cfg->setref[0];
				nstar++;
				break;
			}
		}
	}
    for (i=0; i<cfg->c1; i++) {
		if (cfg->i1[i] >= 0 && cfg->i1[i] != cfg->setref[0]) {		
			/* Compute estimated destination coordinates */
			if (cfg->i1[i] != cfg->setpos_ref_id) 
				Trafo(m,cfg->x1[i],cfg->y1[i],&xx,&yy);
			else
				Trafo(m,cfg->setpos_xy[0],cfg->setpos_xy[1],&xx,&yy);
			/* Select the nearest star */
			rr = MACHMAX; j0 = -1;
			for (j=0; j<cfg->c2; j++) {
				if (cfg->i2[j] >= 0 && cfg->xref[j] < 0) {		
					dx = xx-cfg->x2[j];
					dy = yy-cfg->y2[j];
					dr = (dx*dx+dy*dy);
					if (dr<rr) { rr = dr; j0 = j; }
				}
			}
			if ((j0>=0)&&(rr<tol)) {
				cfg->xref[j0] = cfg->i1[i];
				nstar++;
			}
		}
    }
	cfg->matched_stars = nstar;
	cmpack_matrix_init(&cfg->matrix, M(0,0), M(1,0), M(0,1), M(1,1), M(0,2), M(1,2));
	return 0;
}

int Solve(CmpackMatch *cfg)
{
	int max1, max2, retval, *id1, *id2;
	struct _CmpackMatchObject *idr1, *idr2;
	double *dev, *k2;
	CmpackMatchStack stack;
	CmpackMatchFrame frame;

	match_frame_reset(cfg);

	printout(cfg->con,1,"Matching algorithm               : Standard");
	
	/* Check parameters */
	if (cfg->nstar<=2) {
		printout(cfg->con, 0,"Number of identification stars muse be greater than 2");
		return CMPACK_ERR_INVALID_PAR;
	}
	if (cfg->nstar>=20) {
		printout(cfg->con, 0,"Number of identification stars muse be less than 20");
		return CMPACK_ERR_INVALID_PAR;
	}	  
	if (cfg->maxstar<cfg->nstar) {
		printout(cfg->con, 0,"Number of stars used muse be greater or equal to number of identification stars");
		return CMPACK_ERR_INVALID_PAR;
	}
	if (cfg->maxstar>=1000) {
		printout(cfg->con, 0,"Number of stars used for matching muse be less than 1000");
		return CMPACK_ERR_INVALID_PAR;
	}
	if (cfg->clip<=0) {
		printout(cfg->con, 0,"Clipping factor must be greater than zero");
		return CMPACK_ERR_INVALID_PAR;
	}

	/* Check data */
	if (cfg->c1<cfg->nstar) {
		printout(cfg->con, 0, "Too few stars in the reference file!");
		return CMPACK_ERR_FEW_POINTS_SRC;
	}
	if (cfg->c2<cfg->nstar) {
		printout(cfg->con, 0, "Too few stars in the source file!");
		return CMPACK_ERR_FEW_POINTS_SRC;
	}

	/* Alloc buffers */
	max1 = cfg->maxstar;
	idr1 = (struct _CmpackMatchObject*) cmpack_malloc(max1*sizeof(struct _CmpackMatchObject));
	id1 = (int*) cmpack_malloc(max1*sizeof(int));
	frame.g_x1 = (double*) cmpack_malloc(max1*sizeof(double));
	frame.g_y1 = (double*) cmpack_malloc(max1*sizeof(double));
	idr2 = (struct _CmpackMatchObject*) cmpack_malloc(max1*sizeof(struct _CmpackMatchObject));
	id2 = (int*) cmpack_malloc(max1*sizeof(int));
	frame.g_x2 = (double*) cmpack_malloc(max1*sizeof(double));
	frame.g_y2 = (double*) cmpack_malloc(max1*sizeof(double));
	
	max2 = (cfg->nstar*(cfg->nstar-1)*(cfg->nstar-2))/3+1;
	dev = (double*)cmpack_malloc(max2*sizeof(double));
	k2 = (double*)cmpack_malloc(max2*sizeof(double));

	StInit(&stack);

	retval = _solve(cfg, &frame, id1, idr1, id2, idr2, dev, k2, &stack);

	StClear(&stack);

	cmpack_free(idr1);
	cmpack_free(idr2);
	cmpack_free(id1);
	cmpack_free(id2);
	cmpack_free(k2);
	cmpack_free(dev);

	cmpack_free(frame.g_x1);
	cmpack_free(frame.g_y1);
	cmpack_free(frame.g_x2);
	cmpack_free(frame.g_y2);

	return retval;
}
