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

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));
}

/* Construct list of stars in image 1 and sorts them with respect of distance from the star i1. */
static void Init1(int n1, const double *x1, const double *y1, int i1, struct _CmpackMatchObject *idr1)
{
    int i;
    double xc, yc, dx, dy;

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

/* Construct list of stars in image 2 and sorts them with respect of distance from the star j1. */
static void Init2(int n2, const double *x2, const double *y2, int j1, struct _CmpackMatchObject *idr2)
{
    int j;
    double xc, yc, dx, dy;

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

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

	A(0,0) = n*c;
	A(1,0) = 0.0;
	A(2,0) = c*axi;
	A(3,0) = c*ayi;
	A(0,1) = 0.0;
	A(1,1) = A(0,0);
	A(2,1) = mirror*c*ayi;
	A(3,1) = -mirror*c*axi;
	A(0,2) = A(2,0);
	A(1,2) = A(2,1);
	A(2,2) = c*a2;
	A(3,2) = 0.0;
	A(0,3) = A(3,0);
	A(1,3) = A(3,1);
	A(2,3) = 0.0;
	A(3,3) = A(2,2);
	A(4,0) = c*b[0];
	A(4,1) = c*b[1];
	A(4,2) = c*b[2];
	A(4,3) = c*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;

	M(0,0) = A(4,2);
    M(0,1) = mirror*A(4,3);
    M(0,2) = A(4,0);
    M(1,0) = -mirror*A(4,3);
    M(1,1) = A(4,2);
    M(1,2) = A(4,1);
    M(2,0) = 0.0;
    M(2,1) = 0.0;
    M(2,2) = 1.0;
	return 0;
}

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 *sumsq, int *mstar, double *m)
/* Find coefficients of transformation */
{
    int    i,j,j0,nn,id1,id2;
    double s,axi,ayi,a2,xx1,yy1,xx2,yy2,xx,yy,tol2,dr,rmin,p[4];
    double zoom,dx,dy,mirror;
	int    i1,i2,i3,k,l,rep;
	double dx1,dx2,dy1,dy2,maxdev, sig;
	double xmed,xsig,xdev,xarc,prum;

    /* Set output to defaults */
    mirror = 1;
	memset(m, 0, 9*sizeof(double));
	M(0,0) = M(1,1) = M(2,2) = 1.0;
	*mstar = 0;
	*sumsq = 0.0;
	  
    /* We need at least one stars */
    if (ns<1)
        return 0;		
         
	zoom = 1.0;
	if (ns>=3) {
		/* Calculation of ratio of similarity (scale) 
		 * We need at least two stars */
		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) {
			/* Standard deviation is too great */
	    	return 0;   				
		}
		if (fabs(prum-xmed)>=sig) {
			/* Test for equality of median and mean */
	    	return 0; 					
		}
	    /* Compute zoom estimation */
    	if (rep) 
			zoom = 1.0/xmed;
        else 
			zoom = xmed;
    	/* Test on the same arcs of N-polygon */
    	/* We need at least three stars */
	    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)) {
						/* devation onto 1.0 is too great */
				    	return 0; 
					}
				    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) {
			/* Mean deviation is too great */
	    	return 0;           
		}
		if (fabs(xarc-1.0) > 3.0*(xdev+MACHEPS)) {
			/* Median must be equal 1.0 */
	    	return 0; 			
		}
    }
      
    /* Zero-iteration */
    axi = ayi = 0.0;
    for (i=0; i<ns; i++) {
	    id1  = idl1[i];
	    id2  = idl2[i];
	    dx   = x2[id2] - x1[id1];
	    dy   = y2[id2] - y1[id1];
	    if ((dx*dx+dy*dy)>cfg->maxoffset*cfg->maxoffset)
			return 0;
	    axi += dx;
	    ayi += dy;
    }
	M(0,2) = axi/ns;
	M(1,2) = ayi/ns;

	/* Only one star */
	if (ns==1) {
		*mstar = 1;
		*sumsq = 0;
		return 1;
	}
      
    /* 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)+MACHEPS);
	if (tol2>cfg->maxoffset*cfg->maxoffset)
		return 0;

    /* Reziduals for all stars */
    nn = 0; j0 = -1;
    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;
        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; }
        }
        j = j0;
        if (rmin<=tol2) {
	        /* Star was found */
	        s   += rmin;
    	    xx1  = x1[i];
	        yy1  = y1[i];
	        xx2  = x2[j];
	        yy2  = y2[j];
	        axi += xx1;
	        ayi += yy1;
	        a2  += xx1*xx1+yy1*yy1;
	        p[0]+= xx2;
	        p[1]+= yy2;
	        p[2]+= xx1*xx2+mirror*yy1*yy2;
	        p[3]+= yy1*xx2-mirror*xx1*yy2;
	        nn++;
        }
    }
      
    /* We need at least one star */
    if (nn<1)
	    return 0;
	      
	/* Find improved transformation matrix */
    if (Resrov(nn,zoom,mirror,axi,ayi,a2,p,m)!=0)
	    return 0;
	      
	/* Compute improved tolerance */
    tol2 = fmax(0.0001, 3.0*cfg->clip*cfg->clip*(s/nn)+MACHEPS);
	if (tol2>cfg->maxoffset*cfg->maxoffset)
		return 0;

    /* Test the transformation, comute final tolerance and number of identified stars */
    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<1) 
		return 0;
      
	/* Normal return, transformation is OK */
	*mstar = nn;          
	*sumsq = s;
	return 1;
}

static int _simple(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, i3, j1, j3, i4, j4;
	double 	dx, dy, xx, yy, dr, rr, rl, rg, rmax, dif, u1, u2, v1, v2;
	double	sumsq, tol2, 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 stars on reference frame -> i1 */
	for (i1=0;i1<n1;i1++) {
		Init1(n1, lc->g_x1, lc->g_y1, i1, idr1);
	    id1[0] = idr1[0].i;
		/* For all stars on input frame -> j1 */
		for (j1=0;j1<n2;j1++) {
			dx = (lc->g_x2[j1] - lc->g_x1[i1]);
			dy = (lc->g_y2[j1] - lc->g_y1[i1]);
			
			/* Check max. offset */
			if ((dx*dx + dy*dy)>cfg->maxoffset*cfg->maxoffset) 
				continue;
			
			Init2(n2, lc->g_x2, lc->g_y2, j1, idr2);
		    id2[0] = idr2[0].i;
		    ni = 1;
			/* Find transformation matrix */
			if (FindTrafo(cfg,n1,id1,lc->g_x1,lc->g_y1,n2,id2,lc->g_x2,lc->g_y2,dev,k2,1,&sumsq,&mstar,m)) {
				/* Add transformation to the stack */
			    StAdd(stack,id1,id2,1,sumsq,mstar,m);
			}
			for (i3=1; i3<n1; i3++) {
                /* Find the star on input frame with the distance most close to i3 -> j3 */
                rr   = idr1[i3].r;
            	rg   = 1.3*rr;
                rmax = MACHMAX;
                j3  = -1;
                for (j=2; j<n2 && idr2[j].r<rg; j++) {
                    if (idr2[j].i!=id2[0]) {
                    	dif = fabs(idr2[j].r-rr);
                		if (dif<rmax) {
                    		rmax = dif; 
                    		j3 = j; 
                    	}
                	}
                }
                if (j3<0) 
                	continue; /* Next i3 */

                /* Add i3 and j3 to the polygon */
                id1[1] = idr1[i3].i;
                id2[1] = idr2[j3].i;
				/* Find transformation matrix */
				if (FindTrafo(cfg,n1,id1,lc->g_x1,lc->g_y1,n2,id2,lc->g_x2,lc->g_y2,dev,k2,2,&sumsq,&mstar,m)) {
					/* Add transformation to the stack */
				    StAdd(stack,id1,id2,2,sumsq,mstar,m);
				}
                
				ni = 2;
				for (i4=i3+1; i4<n1 && ni<cfg->maxstar; i4++) {
	                /* Find the star on input frame with the distance most close to i3 -> j3 */
	                rr   = idr1[i4].r;
	                rl   = rr/1.3;
	            	rg   = rr*1.3;
	                rmax = MACHMAX;
	                j4  = -1;
	                for (j=2; j<n2 && idr2[j].r<rg; j++) {
	                    if (idr2[j].r>rl && idr2[j].i!=id2[0] && idr2[j].i!=id2[1]) {
	                    	dif = fabs(idr2[j].r-rr);
	                		if (dif<rmax) { 
	                    		rmax = dif; 
	                    		j4 = j; 
	                    	}
	                	}
	                }
	                if (j4<0) 
	                	continue; /* Next i3 */
	
	                /* Calculation of the triangle coordinates (u1,v1) in the reference file */
	                UV(id1[ni-2],id1[ni-1],idr1[i4].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[j4].i,lc->g_x2,lc->g_y2,&u2,&v2);
	                if (((u1-u2)*(u1-u2)+(v1-v2)*(v1-v2))<0.0005) {
						/* Add i4and j4 to the polygon */
						id1[ni] = idr1[i4].i;
						id2[ni] = idr2[j4].i;
						ni++;
						/* 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,&sumsq,&mstar,m)) {
							/* Add transformation to the stack */
						    StAdd(stack,id1,id2,ni,sumsq,mstar,m);
						}
	                    if (ni>=cfg->nstar) {
							/* 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;
	}
    
	tol2 = fmax(0.0001, 3.0*cfg->clip*cfg->clip*(sumsq/mstar)+MACHEPS);
    
    /* Matching OK */
	sprintf(msg,"No. of stars used for matching   : %d", mstar);
	printout(cfg->con,1, msg);
	sprintf(msg,"Sum of squares in the best case  : %.4f", sumsq);
	printout(cfg->con,1, msg);
	sprintf(msg,"Tolerance                        : %.2f", sqrt(tol2));
	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<tol2)) {
				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 Simple(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               : Sparse fields");

	/* 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;
	}
	if (cfg->maxoffset<=0) {
		printout(cfg->con, 0,"Max. position offset muse be greater than zero");
		return CMPACK_ERR_INVALID_PAR;
	}

	/* Check data */
	if (cfg->c1<1) {
		printout(cfg->con, 0, "Too few stars in the reference file!");
		return CMPACK_ERR_FEW_POINTS_SRC;
	}
	if (cfg->c2<1) {
		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 = _simple(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;
}
