/*
    Theseus - maximum likelihood superpositioning of macromolecular structures

    Copyright (C) 2004-2015 Douglas L. Theobald

    This file is part of THESEUS.

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

    THESEUS 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 THESEUS in the file 'COPYING'. If not, see
    <http://www.gnu.org/licenses/>.

    -/_|:|_|_\-
*/

#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <ctype.h>
#include <math.h>
#include <float.h>
#include "DLTmath.h"
#include "FragCds.h"


FragCds
*FragCdsAlloc(int fraglen)
{
    int            i;
    FragCds       *frag = NULL;

    frag = (FragCds *) malloc(sizeof(FragCds));

    frag->fraglen = fraglen;

    frag->x   = (double *) calloc((size_t) fraglen, sizeof(double));
    frag->y   = (double *) calloc((size_t) fraglen, sizeof(double));
    frag->z   = (double *) calloc((size_t) fraglen, sizeof(double));
    frag->w   = (double *) calloc((size_t) fraglen, sizeof(double));
    frag->var = (double *) calloc((size_t) fraglen, sizeof(double));

    if (frag->x   == NULL ||
        frag->y   == NULL ||
        frag->z   == NULL ||
        frag->var == NULL ||
        frag->w   == NULL)
    {
        perror("\n\n ERROR");
        fprintf(stderr, "\n ERROR51: could not allocate memory in function FragCdsAlloc(). \n\n");
        exit(EXIT_FAILURE);
    }

    for(i = 0; i < fraglen; ++i)
        frag->var[i] = frag->w[i] = 1.0;

    return(frag);
}


void
FragCdsFree(FragCds **frag_ptr)
{
    FragCds *frag = *frag_ptr;

    free(frag->x);
    free(frag->y);
    free(frag->z);
    free(frag->var);
    free(frag->w);
    free(frag);

    *frag_ptr = NULL;
}


void
CenterFrag(FragCds *frag)
{
    int             i;
    double          x, y, z, len;

    len = (double) frag->fraglen;
    x = y = z = 0.0;

    for (i = 0; i < frag->fraglen; ++i)
    {
        x += frag->x[i];
        y += frag->y[i];
        z += frag->z[i];
    }

    frag->center[0] = x / len;
    frag->center[1] = y / len;
    frag->center[2] = z / len;

    for (i = 0; i < frag->fraglen; ++i)
    {
        frag->x[i] -= frag->center[0];
        frag->y[i] -= frag->center[1];
        frag->z[i] -= frag->center[2];
    }
}


void
CenterFragCA(FragCds *frag)
{
    int             i, center;
    double          cx, cy, cz;
    double         *x = frag->x;
    double         *y = frag->y;
    double         *z = frag->z;

    center = (frag->fraglen - 1) / 2;

//     frag->center[0] = frag->x[center];
//     frag->center[1] = frag->y[center];
//     frag->center[2] = frag->z[center];

    cx = x[center];
    cy = y[center];
    cz = z[center];

    for (i = 0; i < frag->fraglen; ++i)
    {
//         x[i] -= frag->center[0];
//         y[i] -= frag->center[1];
//         z[i] -= frag->center[2];
        x[i] -= cx;
        y[i] -= cy;
        z[i] -= cz;
    }
}


double
DiffDist(FragCds *frag1, FragCds *frag2, double **distmat1, double **distmat2)
{
    int             i, j;
    int             len;
    double          sum;

    len = frag1->fraglen;

    for (i = 0; i < len; ++i)
        for (j = 0; j < len; ++j)
            distmat1[i][j] = sqrt(SqrFragCdsDist(frag1, i, frag1, j));

    for (i = 0; i < len; ++i)
        for (j = 0; j < len; ++j)
            distmat2[i][j] = sqrt(SqrFragCdsDist(frag2, i, frag2, j));

    sum = 0.0;
    for (i = 0; i < len; ++i)
        for (j = 0; j < len; ++j)
            sum += mysquare(distmat2[i][j] - distmat1[i][j]);

    /* sum = sqrt(sum);*/
    /* sum /= (double) len * len; */
    return(sum);
}


double
SqrFragCdsDist(FragCds *cds1, int atom1, FragCds *cds2, int atom2)
{
    double          sqrdist;
    double          xdist, ydist, zdist;
    double          xx, yy, zz;

    xdist = cds2->x[atom2] - cds1->x[atom1];
    ydist = cds2->y[atom2] - cds1->y[atom1];
    zdist = cds2->z[atom2] - cds1->z[atom1];

    xx = mysquare(xdist);
    yy = mysquare(ydist);
    zz = mysquare(zdist);

    sqrdist = xx + yy + zz;

    return(sqrdist);
}


double
RadGyrSqrFrag(const FragCds *frag)
{
    int             i;
    double          sum, tmpx, tmpy, tmpz;
    const double   *x = frag->x, *y = frag->y, *z = frag->z;

    sum = 0.0;
    for (i = 0; i < frag->fraglen; ++i)
    {
        tmpx = x[i];
        tmpy = y[i];
        tmpz = z[i];
        sum += (tmpx * tmpx + tmpy * tmpy + tmpz * tmpz);
    }

    return(sum);
}


void
PrintFragCds(FragCds *cds)
{
    int             i;

    fprintf(stderr, "\n fraglen = %d \n", cds->fraglen);

    for (i = 0; i < cds->fraglen; ++i)
    {
        fprintf(stderr,
                " %4d %8.3f %8.3f %8.3f\n",
                i+1,
                cds->x[i],
                cds->y[i],
                cds->z[i]);
    }
    fprintf(stderr, " END \n\n");
}