/**********************************************************************
 
	Copyright (C) 2003 Hirohisa MORI <joshua@nichibun.ac.jp>
 
	This program is free software; you can redistribute it 
	and/or modify it under the terms of the GLOBALBASE 
	Library General Public License (G-LGPL) as published by 

	http://www.globalbase.org/
 
	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.

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


#include	"xlerror.h"
#include	"xl.h"
#include	"netmapper.h"
#include	"equation.h"

extern void gc_gb_sexp();
XL_SEXP * xl_AffenFitter();
int loading_target;

void
init_AffenFitter(XLISP_ENV * env)
{
	set_env(env,l_string(std_cm,"AffenFitter"),
		get_func_prim(xl_AffenFitter,FO_APPLICATIVE,0,2,2));
}

XL_SEXP *
xl_AffenFitter(XLISP_ENV * env,XL_SEXP * s)
{
XL_SEXP * ptr, * co;
XL_SEXP * src,* dest;
int i,j;
double m1[E_DIM][E_DIM];
double m2[E_DIM][E_DIM];
double v1[E_DIM];
double v2[E_DIM];
double r1[E_DIM],r2[E_DIM];
double src_x,src_y,dest_x,dest_y;
XL_SEXP * xx;
char * e_param;

	for ( i = 0 ; i < E_DIM ; i ++ ) {
		for ( j = 0 ; j < E_DIM ; j ++ ) {
			m1[i][j] = 0;
		}
		v1[i] = v2[i] = 0;
	}
	ptr = get_el(s,1);
	if ( get_type(ptr) != XLT_PAIR ) {
		e_param = "point pair list";
		goto type_missmatch;
	}
	for ( ; get_type(ptr) == XLT_PAIR ; ptr = cdr(ptr) ) {
		co = car(ptr);
		if ( get_type(co) != XLT_PAIR ) {
			e_param = "point pair";
			goto type_missmatch;
		}
		if ( list_length(co) != 2 ) {
			e_param = "point pair (not equal 2 points)";
			goto type_missmatch;
		}
		src = get_el(co,0);
		dest = get_el(co,1);
		if ( list_length(src) != 2 ) {
			e_param = "not 2d point (src)";
			goto type_missmatch;
		}
		if ( list_length(dest) != 2 ) {
			e_param = "not 2d point (dest)";
			goto type_missmatch;
		}
		xx = get_el(src,0);
		switch ( get_type(xx) ) {
		case XLT_INTEGER:
			src_x = xx->integer.data;
			break;
		case XLT_FLOAT:
			src_x = xx->floating.data;
			break;
		default:
			e_param = "src(x) element type";
			goto type_missmatch;
		}
		xx = get_el(src,1);
		switch ( get_type(xx) ) {
		case XLT_INTEGER:
			src_y = xx->integer.data;
			break;
		case XLT_FLOAT:
			src_y = xx->floating.data;
			break;
		default:
			e_param = "src(y) element type";
			goto type_missmatch;
		}

		xx = get_el(dest,0);
		switch ( get_type(xx) ) {
		case XLT_INTEGER:
			dest_x = xx->integer.data;
			break;
		case XLT_FLOAT:
			dest_x = xx->floating.data;
			break;
		default:
			e_param = "dest(x) element type";
			goto type_missmatch;
		}
		xx = get_el(dest,1);
		switch ( get_type(xx) ) {
		case XLT_INTEGER:
			dest_y = xx->integer.data;
			break;
		case XLT_FLOAT:
			dest_y = xx->floating.data;
			break;
		default:
			e_param = "dest(y) element type";
			goto type_missmatch;
		}

		m1[0][0] += src_x*src_x;
		m1[0][1] += src_x*src_y;
		m1[0][2] += src_x;
		m1[1][1] += src_y*src_y;
		m1[1][2] += src_y;
		m1[2][2] ++;

		v1[0] += dest_x*src_x;
		v1[1] += dest_x*src_y;
		v1[2] += dest_x;

		v2[0] += dest_y*src_x;
		v2[1] += dest_y*src_y;
		v2[2] += dest_y;
	}
	m1[1][0] = m1[0][1];
	m1[2][0] = m1[0][2];
	m1[2][1] = m1[1][2];
	for ( i = 0 ; i < E_DIM ; i ++ )
		for ( j = 0 ; j < E_DIM ; j ++ )
			m2[i][j] = m1[i][j];

	if ( equation(r1,m1,v1) < 0 )
		goto unexec;
	if ( equation(r2,m2,v2) < 0 )
		goto unexec;

	return List(
		List(
			List(	get_floating(r1[0],0),
				get_floating(r1[1],0),
				-1),
			List(	get_floating(r2[0],0),
				get_floating(r2[1],0),
				-1),
			-1),
		List(
			get_floating(r1[2],0),
			get_floating(r2[2],0),
			-1),
		-1);
unexec:
	return get_error(
		s->h.file,
		s->h.line,
		XLE_PROTO_UNDEF_RESOURCE,
		l_string(std_cm,"AffenFitter"),
		List(n_get_string("cannot get result"),
			-1));
type_missmatch:
	return get_error(
		s->h.file,
		s->h.line,
		XLE_SEMANTICS_TYPE_MISSMATCH,
		l_string(std_cm,"AffenFitter"),
		List(n_get_string("type_missmatch"),
			n_get_string(e_param),
			-1));
}


