/* $Id: fors_pattern.c,v 1.4 2010-09-14 07:49:30 cizzo Exp $
 *
 * This file is part of the FORS Library
 * Copyright (C) 2002-2010 European Southern Observatory
 *
 * 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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
 */

/*
 * $Author: cizzo $
 * $Date: 2010-09-14 07:49:30 $
 * $Revision: 1.4 $
 * $Name: not supported by cvs2svn $
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <fors_pattern.h>

#include <fors_double.h>
#include <fors_utils.h>

#include <cpl.h>

#include <math.h>

struct _fors_pattern 
{
    double ratsq;  /* (rmin/Rmax)^2 */
    double dratsq; /* error */

    double theta;  /* angle min - angle max  in [0; 2pi[ */
    double dtheta; /* error */
    const fors_point *ref, *min, *max; 
    /* Reference, nearest, farest points used to build this pattern */
};

#undef cleanup
#define cleanup

/**
 * @brief   Constructor
 * @param   r1                distance to 1st point
 * @param   r2                distance to 2nd point
 * @param   t1                angle to 1st point
 * @param   t2                angle to 2nd point
 * @param   sigma             error of point x- and y-positions. This is
 *                            used to determine the uncertainty of the pattern.
 *                            The same value is used for all input points.
 * @return  Newly allocated pattern
 *
 * Note: Covariance between the two parameters is not computed/propagated.
 *
 */
fors_pattern *fors_pattern_new(const fors_point *ref,
                               const fors_point *p1,
                               const fors_point *p2,
                               double sigma)
{
    fors_pattern *p = cpl_malloc(sizeof(*p));

    assure( ref != NULL, return p, NULL );
    assure(  p1 != NULL, return p, NULL );
    assure(  p2 != NULL, return p, NULL );
    assure(  sigma >= 0, return p, NULL );

    p->ref = ref;
    {
        double r1 = fors_point_distsq(ref, p1);
        double r2 = fors_point_distsq(ref, p2);
        double dr1 = sqrt(8*sigma*sigma*r1);
        double dr2 = sqrt(8*sigma*sigma*r2);

        double dt1, dt2;
        double t1 = double_atan2(ref->y - p1->y, sqrt(2)*sigma,
                                 ref->x - p1->x, sqrt(2)*sigma,
                                 &dt1);
        
        double t2 = double_atan2(ref->y - p2->y, sqrt(2)*sigma,
                                 ref->x - p2->x, sqrt(2)*sigma,
                                 &dt2);
        
        if (r1 < r2) {
            p->ratsq = double_divide(r1, dr1,
                                     r2, dr2,
                                     &p->dratsq);

            p->theta = double_subtract(t1, dt1,
                                       t2, dt2,
                                       &p->dtheta);
            p->min   = p1;
            p->max   = p2;
        }
        else {
            p->ratsq = double_divide(r2, dr2,
                                     r1, dr1,
                                     &p->dratsq);
            p->theta = double_subtract(t2, dt2,
                                       t1, dt1,
                                       &p->dtheta);
            p->min   = p2;
            p->max   = p1;
        }
        
        while (p->theta <  0     ) p->theta += 2*M_PI; /* Error does not change */
        while (p->theta >= 2*M_PI) p->theta -= 2*M_PI;
    }
    
    return p;
}

#undef cleanup
#define cleanup
/**
 * @brief   Create patterns from list of points
 * @param   points           points
 * @param   tolerance        minimum distance between two points
 *                           in a pattern
 * @param   sigma            uncertainty of every point,
 *                           used for calculating pattern uncertainty
 * @return  possibly empty list of patterns
 */
fors_pattern_list *
fors_pattern_new_from_points(fors_point_list *points,
                             double tolerance,
                             double sigma)
{
    fors_pattern_list *patterns = fors_pattern_list_new();
    double tol_sq = tolerance * tolerance;
    fors_point *ref, *p1, *p2;

    assure( points != NULL, return NULL, NULL );

    for (ref = fors_point_list_first(points);
         ref != NULL;
         ref = fors_point_list_next(points)) {

        for (fors_point_list_first_pair(points, &p1, &p2);
             p1 != NULL;
             fors_point_list_next_pair(points, &p1, &p2)) {
                
            if (fors_point_distsq(ref, p1) > tol_sq &&
                fors_point_distsq(ref, p2) > tol_sq &&
                fors_point_distsq(p1 , p2) > tol_sq) {
                
                fors_pattern_list_insert(patterns,
                                         fors_pattern_new(
                                             ref, p1, p2, sigma));
            }
        }
    }

    cpl_msg_debug(cpl_func,
                  "Created %d pattern(s)", fors_pattern_list_size(patterns));
    
    return patterns;
}

#undef cleanup
#define cleanup
/**
 * @brief   Copy constructor
 * @param   p             pattern to duplicate
 * @return  Newly allocated pattern
 */
#if 0  /* check before enabling! */
static fors_pattern *
fors_pattern_duplicate(const fors_pattern *p)
{
    fors_pattern *d = NULL;

    assure( p != NULL, return NULL, NULL );
    
    d = cpl_malloc(sizeof(*d));

    d->ratsq = p->ratsq;
    d->theta = p->theta;

    return d;
}
#endif

/**
 * @brief   Deallocate and set pointer to NULL
 * @param  p    pattern to delete
 */
void fors_pattern_delete(fors_pattern **p)
{
    if (p && *p) {
        cpl_free(*p); *p = NULL;
    }
    return;
}

/**
 * @brief   Get reference star
 * @param   p       pattern
 * @return  pattern reference star
 */
const fors_point *
fors_pattern_get_ref(const fors_pattern *p)
{
    assure( p != NULL, return NULL, NULL );

    return p->ref;
}

/**
 * @brief   Get pattern uncertainty
 * @param   p       pattern
 * @return  dr2     (output) error in (Rmin/Rmax)^2
 * @return  dtheta  (output) error in theta
 *
 * The errors are calculated in normalized parameter space [0;1]x[0;1]
 */
void fors_pattern_error(const fors_pattern *p,
			double *dr2,
			double *dtheta)
{
    assure( p      != NULL, return, NULL );
    assure( dr2    != NULL, return, NULL );
    assure( dtheta != NULL, return, NULL );

    *dr2    = p->dratsq;
    *dtheta = p->dtheta / (2*M_PI);

    return;
}


/**
 * @brief   Metric
 * @param   p       1st pattern
 * @param   q       2nd pattern
 * @return  squared distance between patterns
 *
 * The distance is calculated in normalized parameter space [0;1]x[0;1]
 */
double fors_pattern_distsq(const fors_pattern *p,
                           const fors_pattern *q)
{
    assure( p != NULL, return -1, NULL );
    assure( q != NULL, return -1, NULL );
    
    double dtheta = fors_angle_diff(&p->theta, &q->theta); /* in [0;pi] */
    
    /* Return distance in normalized parameter space [0;1[ x [0;1[.
       This is to give equal weight to differences in radii
       and differences in theta.
    */
    return 
        (p->ratsq - q->ratsq) * (p->ratsq - q->ratsq) / (1.0 * 1.0) +
        (dtheta * dtheta) / (M_PI*M_PI);
}

/**
 * @brief   Get distance between patterns, divided by statistical error
 * @param   p       1st pattern
 * @param   q       2nd pattern
 * @return  normalized distance
 */
double fors_pattern_dist_per_error(const fors_pattern *p,
                                   const fors_pattern *q)
{
    double dtheta = fors_angle_diff(&p->theta, &q->theta);
    
    double p_error_r;
    double p_error_t;
    
    fors_pattern_error(p, 
                       &p_error_r,
                       &p_error_t);

    double q_error_r;
    double q_error_t;

    fors_pattern_error(q, 
                       &q_error_r,
                       &q_error_t);
 
    /* variance of difference */
    double rr = p_error_r*p_error_r + q_error_r*q_error_r;
    double tt = p_error_t*p_error_t + q_error_t*q_error_t;

    return sqrt(
        (p->ratsq - q->ratsq) * (p->ratsq - q->ratsq) / ((1.0*1.0) * rr) + 
        (dtheta * dtheta) / ((M_PI*M_PI) * tt)
        );
}




/**
 * @brief   Print pattern
 * @param   p       to print
 */
void fors_pattern_print(const fors_pattern *p)
{
    if (p == NULL) {
        cpl_msg_info(cpl_func, "NULL pattern");
    }
    else {
        cpl_msg_info(cpl_func, "Rmin^2/Rmax^2 = %f ; theta = %f",
                     p->ratsq, p->theta);
    }
    return;
}

/**
 * @brief   Get scale ratio of matching patterns
 * @param   p       1st pattern
 * @param   q       2nd pattern
 * @return ratio of pattern scales, assuming that the two patterns match
 */

double fors_pattern_get_scale(const fors_pattern *p,
                              const fors_pattern *q)
{
    assure( p != NULL, return 0, NULL );
    assure( q != NULL, return 0, NULL );
    
    double s1 = sqrt(fors_point_distsq(p->ref, p->max));
    double s2 = sqrt(fors_point_distsq(q->ref, q->max));

    return (s2 == 0) ? 0 : s1/s2;
}

/**
 * @brief   Get angle of orientation between matching patterns
 * @param   p       1st pattern
 * @param   q       2nd pattern
 * @return rotation angle in [0;2pi[ from p to q
 */

double fors_pattern_get_angle(const fors_pattern *p,
                              const fors_pattern *q)
{
    assure( p != NULL, return -1, NULL );
    assure( q != NULL, return -1, NULL );
    
    double t1 = atan2(p->ref->y - p->max->y,
               p->ref->x - p->max->x);
    double t2 = atan2(q->ref->y - q->max->y,
               q->ref->x - q->max->x);

    double t = t1 - t2;

    while (t >= 2*M_PI) t -= 2*M_PI;
    while (t  < 0     ) t += 2*M_PI;
    
    return t;
}


#define LIST_DEFINE
#undef LIST_ELEM
#define LIST_ELEM fors_pattern
#include <list.h>

/**@}*/
