/*---------------------------------------------------------------------------*\
  =========                 |
  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
   \\    /   O peration     |
    \\  /    A nd           | Copyright (C) 1991-2007 OpenCFD Ltd.
     \\/     M anipulation  |
-------------------------------------------------------------------------------
License
    This file is part of OpenFOAM.

    OpenFOAM 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.

    OpenFOAM 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 OpenFOAM; if not, write to the Free Software Foundation,
    Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

\*---------------------------------------------------------------------------*/

#include "linearFitData.H"
#include "surfaceFields.H"
#include "volFields.H"
#include "SVD.H"
#include "syncTools.H"


// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //

namespace Foam
{
    defineTypeNameAndDebug(linearFitData, 0);
}


// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //

static int count = 0;

Foam::linearFitData::linearFitData
(
    const fvMesh& mesh,
    const scalar cWeight
)
:
    MeshObject<fvMesh, linearFitData>(mesh),
    centralWeight_(cWeight),
#   ifdef SPHERICAL_GEOMETRY
    dim_(2),
#   else
    dim_(mesh.nGeometricD()),
#   endif
    minSize_
    (
        dim_ == 1 ? 2 :
        dim_ == 2 ? 3 :
        dim_ == 3 ? 4 : 0
    ),
    stencil_(mesh),
    fit_(mesh.nInternalFaces())
{
    if (debug)
    {
        Info << "Contructing linearFitData" << endl;
    }

    // check input
    if (centralWeight_ < 1 - SMALL)
    {
        FatalErrorIn("linearFitData::linearFitData")
            << "centralWeight requested = " << centralWeight_
            << " should not be less than one"
            << exit(FatalError);
    }

    if (minSize_ == 0)
    {
        FatalErrorIn("linearFitSnGradData")
            << " dimension must be 1,2 or 3, not" << dim_ << exit(FatalError);
    }

    // store the polynomial size for each cell to write out
    surfaceScalarField interpPolySize
    (
        IOobject
        (
            "linearFitInterpPolySize",
            "constant",
            mesh,
            IOobject::NO_READ,
            IOobject::NO_WRITE
        ),
        mesh,
        dimensionedScalar("linearFitInterpPolySize", dimless, scalar(0))
    );

    // Get the cell/face centres in stencil order.
    // Centred face stencils no good for triangles of tets. Need bigger stencils
    List<List<point> > stencilPoints(stencil_.stencil().size());
    stencil_.collectData
    (
        mesh.C(),
        stencilPoints
    );

    // find the fit coefficients for every face in the mesh

    for(label faci = 0; faci < mesh.nInternalFaces(); faci++)
    {
        interpPolySize[faci] = calcFit(stencilPoints[faci], faci);
    }

    Pout<< "count = " << count << endl;

    if (debug)
    {
        Info<< "linearFitData::linearFitData() :"
            << "Finished constructing polynomialFit data"
            << endl;

        interpPolySize.write();
    }
}


// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //

void Foam::linearFitData::findFaceDirs
(
    vector& idir,        // value changed in return
    vector& jdir,        // value changed in return
    vector& kdir,        // value changed in return
    const fvMesh& mesh,
    const label faci
)
{
    idir = mesh.Sf()[faci];
    //idir = mesh.C()[mesh.neighbour()[faci]] - mesh.C()[mesh.owner()[faci]];
    idir /= mag(idir);

#   ifndef SPHERICAL_GEOMETRY
    if (mesh.nGeometricD() <= 2) // find the normal direcion
    {
        if (mesh.directions()[0] == -1)
        {
            kdir = vector(1, 0, 0);
        }
        else if (mesh.directions()[1] == -1)
        {
            kdir = vector(0, 1, 0);
        }
        else
        {
            kdir = vector(0, 0, 1);
        }
    }
    else // 3D so find a direction in the place of the face
    {
        const face& f = mesh.faces()[faci];
        kdir = mesh.points()[f[0]] - mesh.points()[f[1]];
    }
#   else
    // Spherical geometry so kdir is the radial direction
    kdir = mesh.Cf()[faci];
#   endif

    if (mesh.nGeometricD() == 3)
    {
        // Remove the idir component from kdir and normalise
        kdir -= (idir & kdir)*idir;

        scalar magk = mag(kdir);

        if (magk < SMALL)
        {
            FatalErrorIn("findFaceDirs") << " calculated kdir = zero"
                << exit(FatalError);
        }
        else
        {
            kdir /= magk;
        }
    }

    jdir = kdir ^ idir;
}


Foam::label Foam::linearFitData::calcFit
(
    const List<point>& C,
    const label faci
)
{
    vector idir(1,0,0);
    vector jdir(0,1,0);
    vector kdir(0,0,1);
    findFaceDirs(idir, jdir, kdir, mesh(), faci);

    scalarList wts(C.size(), scalar(1));
    wts[0] = centralWeight_;
    wts[1] = centralWeight_;

    point p0 = mesh().faceCentres()[faci];
    scalar scale = 0;

    // calculate the matrix of the polynomial components
    scalarRectangularMatrix B(C.size(), minSize_, scalar(0));

    for(label ip = 0; ip < C.size(); ip++)
    {
        const point& p = C[ip];

        scalar px = (p - p0)&idir;
        scalar py = (p - p0)&jdir;
#       ifndef SPHERICAL_GEOMETRY
        scalar pz = (p - p0)&kdir;
#       else
        scalar pz = mag(p) - mag(p0);
#       endif

        if (ip == 0)
        {
            scale = max(max(mag(px), mag(py)), mag(pz));
        }

        px /= scale;
        py /= scale;
        pz /= scale;

        label is = 0;

        B[ip][is++] = wts[0]*wts[ip];
        B[ip][is++] = wts[0]*wts[ip]*px;

        if (dim_ >= 2)
        {
            B[ip][is++] = wts[ip]*py;
        }
        if (dim_ == 3)
        {
            B[ip][is++] = wts[ip]*pz;
        }
    }

    // Set the fit
    label stencilSize = C.size();
    fit_[faci].setSize(stencilSize);
    scalarList singVals(minSize_);
    label nSVDzeros = 0;

    const GeometricField<scalar, fvsPatchField, surfaceMesh>& w =
        mesh().surfaceInterpolation::weights();

    bool goodFit = false;
    for(int iIt = 0; iIt < 10 && !goodFit; iIt++)
    {
        SVD svd(B, SMALL);

        scalar fit0 = wts[0]*wts[0]*svd.VSinvUt()[0][0];
        scalar fit1 = wts[0]*wts[1]*svd.VSinvUt()[0][1];

        //goodFit = (fit0 > 0 && fit1 > 0);

        goodFit =
            (mag(fit0 - w[faci])/w[faci] < 0.5)
         && (mag(fit1 - (1 - w[faci]))/(1 - w[faci]) < 0.5);

        //scalar w0Err = fit0/w[faci];
        //scalar w1Err = fit1/(1 - w[faci]);

        //goodFit =
        //    (w0Err > 0.5 && w0Err < 1.5)
        // && (w1Err > 0.5 && w1Err < 1.5);

        if (goodFit)
        {
            fit_[faci][0] = fit0;
            fit_[faci][1] = fit1;

            for(label i=2; i<stencilSize; i++)
            {
                fit_[faci][i] = wts[0]*wts[i]*svd.VSinvUt()[0][i];
            }

            singVals = svd.S();
            nSVDzeros = svd.nZeros();
        }
        else // (not good fit so increase weight in the centre and for linear)
        {
            wts[0] *= 10;
            wts[1] *= 10;

            for(label i = 0; i < B.n(); i++)
            {
                B[i][0] *= 10;
                B[i][1] *= 10;
            }

            for(label j = 0; j < B.m(); j++)
            {
                B[0][j] *= 10;
                B[1][j] *= 10;
            }
        }
    }

    // static const scalar L = 0.1;
    // static const scalar R = 0.2;

    // static const scalar beta = 1.0/(R - L);
    // static const scalar alpha = R*beta;

    if (goodFit)
    {
         if ((mag(fit_[faci][0] - w[faci])/w[faci] < 0.15)
         && (mag(fit_[faci][1] - (1 - w[faci]))/(1 - w[faci]) < 0.15))
         {
             count++;
             //Pout<< "fit " << mag(fit_[faci][0] - w[faci])/w[faci] << " " << mag(fit_[faci][1] - (1 - w[faci]))/(1 - w[faci]) << endl;
         }

        // scalar limiter =
        // max
        // (
        //     min
        //     (
        //         min(alpha - beta*mag(fit_[faci][0] - w[faci])/w[faci], 1),
        //         min(alpha - beta*mag(fit_[faci][1] - (1 - w[faci]))/(1 - w[faci]), 1)
        //     ), 0
        // );

        // Remove the uncorrected linear coefficients
        fit_[faci][0] -= w[faci];
        fit_[faci][1] -= 1 - w[faci];

        // if (limiter < 0.99)
        // {
        //     for(label i = 0; i < stencilSize; i++)
        //     {
        //         fit_[faci][i] *= limiter;
        //     }
        // }
    }
    else
    {
        Pout<< "Could not fit face " << faci
            << " " << fit_[faci][0] << " " << w[faci]
            << " " << fit_[faci][1] << " " << 1 - w[faci]<< endl;
        fit_[faci] = 0;
    }

    return minSize_ - nSVDzeros;
}


bool Foam::linearFitData::movePoints()
{
    notImplemented("linearFitData::movePoints()");

    return true;
}


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