# include "util.h"
# include "pdb.h"

#define COORD_C
#include "utypes.h"
#include "errors.h"
#include "umath.h"
#include "vectors.h"
#include "coord.h"

# include "fit.h"

CoordType * FileSelectToHinds(file_records *f, int * npts)
{
  int ii;
  CoordType 
    * a = (CoordType *) malloc(f->atomnum * sizeof(CoordType)),
    * na ;

  *npts = 0;
  
  for (ii = 0;ii<f->atomnum; ii++) {
    atom_record * atom = f->atoms[ii];
    if (SELECTED_ATOM_P(atom)) {
      a[*npts].x = atom->x ;
      a[*npts].y = atom->y ;
      a[*npts].z = atom->z ;
      (*npts)++ ;
    }
  }
  if (*npts == 0) 
    STDERR("*npts == 0");
  na = (CoordType *) realloc(a, *npts * sizeof(CoordType)) ;
  return na;
}


double MyFit
(double ** RotMat, double * TVect, file_records *Moving, file_records *Fixed)
{
  int nMove=0, nFix =0 ;
  CoordType * HindsMove = FileSelectToHinds(Moving,&nMove);
  CoordType * HindsFix  = FileSelectToHinds(Fixed,&nFix);
  rotate_t u = {0};
  xlate_t t = {0};
  double rms = bestfit(u, t, HindsMove, HindsFix, nMove) ;
  int i, j;
  if (nMove != nFix ) {
    fprintf(stderr,"MyFit(): %6d = nFix != nMove = %6d\n",nMove,nFix);
  }

#   ifdef OFF
    printf("    [ %7.4f  %7.4f  %7.4f ]        [ %9.4f ]\n",
	   u[0][0],u[0][1],u[0][2],t[0]);
    printf("R = [ %7.4f  %7.4f  %7.4f ]    T = [ %9.4f ]\n",
	   u[1][0],u[1][1],u[1][2],t[1]);
    printf("    [ %7.4f  %7.4f  %7.4f ]        [ %9.4f ]\n",
	   u[2][0],u[2][1],u[2][2],t[2]);
    printf("\nRMS = %7.4f\n",rms);
#   endif


  for (i=0; i<3; i++) {
    TVect[i+1] = (double) t[i];
    for (j=0; j<3; j++)
      RotMat[i+1][j+1] = (double) u[i][j];
  }
  return rms ;
}