/***************************************************************************
**
**  This file is part of ArrayCore.
**
**  ArrayCore 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.
**
**  ArrayCore 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 Foobar.  If not, see <http://www.gnu.org/licenses/>
**
**  See http://www.geopsy.org for more information.
**
**  Created: 2018-03-29
**  Copyright: 2018-2019
**    Marc Wathelet (ISTerre, Grenoble, France)
**
***************************************************************************/

#include "HRFKDirectRayleigh.h"
#include "FKCrossSpectrum.h"
#include "FKGridSearch.h"
#include "FKGradientSearch.h"
#include "HRFKRayleighEllipticity.h"
#include "FKPower.h"
#include "FKSteeringThreeComponentRayleighRadial.h"

namespace ArrayCore {

  /*!
    \class HRFKDirectRayleigh HRFKDirectRayleigh.h
    \brief Rayleigh high resolution FK power with direct steering of a 3-component cross spectrum

    Rayleigh high resolution FK power versus wavenumber (k) function.
    The cross-spectrum is made of raw component E, N and Z.
    Projection on the radial direction is included in the steering matrix.
  */

  HRFKDirectRayleigh::HRFKDirectRayleigh(FKCache * gridCache)
    : HRFKDirectRadial(gridCache)
  {
    delete _steering;
    _steering=new FKSteeringThreeComponentRayleighRadial;
    _steering->setArray(gridCache->array());
    static_cast<FKSteeringThreeComponentRayleighRadial *>(_steering)->resize();

    _RoverN=0.0;
    _ellipticity=0.0;

    _countGrid=0;
    _countFree=0;
  }

  HRFKDirectRayleigh::~HRFKDirectRayleigh()
  {
    App::log(tr("%1 grid values, %2 free values\n").arg(_countGrid).arg(_countFree));
  }

  void HRFKDirectRayleigh::addCrossSpectrum()
  {
    _crossSpectrum->addThreeComponent();
  }

  /*void HRFKDirectRayleigh::setGrid(FKGradientSearch * g, double cacheStep, double step, double size)
  {
    g->setScales(Point(step, step, 0.1*M_PI));
    g->setGrid(Point(size, size, 0.5*M_PI));
  }*/

  void HRFKDirectRayleigh::setGrid(FKGridSearch * g, double step, double size)
  {
    // Classical kx, ky and ell
    g->setGrid(-size, size, step, -size, size, step, -0.5*M_PI, 0.5*M_PI, 0.05*M_PI);
  }

  bool HRFKDirectRayleigh::initGrid(int n)
  {
    if(_gridCache->isEmpty(FKCache::ThreeComponentRayleigh)) {
      App::log(tr("Caching steering vectors for 3-component Rayleigh ... (%1 values)\n").arg(n) );
      _gridCache->resize(FKCache::ThreeComponentRayleigh, n);
      return true;
    } else {
      return false;
    }
  }

  void HRFKDirectRayleigh::initGrid(const Point& kell, int index)
  {
    FKSteeringThreeComponentRayleighRadial::init(_gridCache, index , kell);
  }

  double HRFKDirectRayleigh::value(const Point& kell, int index) const
  {
    if(isInsideLimits(kell)) {
      _countGrid++;
      FKPower p(&FKSteeringThreeComponentRayleighRadial::cache(_gridCache, index));
      p.setThreeComponentValue(_crossSpectrum->rayleigh());
      return p.value();
    } else {
      return -1.0;
    }
  }

#if 0
  void HRFKDirectRayleigh::setGradient(FKPower& p) const
  {
    _countGrid++;
    p.setThreeComponentValue(_crossSpectrum->rayleigh());
    p.setThreeComponentGradient();
  }

  void HRFKDirectRayleigh::setHessian(FKPower& p) const
  {
    _countGrid+=4;
    p.setThreeComponentHessian(_crossSpectrum->rayleigh());
  }
#endif

  double HRFKDirectRayleigh::value(const Point& kell) const
  {
    if(isInsideLimits(kell)) {
      _countFree++;
      FKSteeringThreeComponentRayleighRadial * s=static_cast<FKSteeringThreeComponentRayleighRadial *>(_steering);
      s->initValue(kell, kell.z());
      FKPower p(s);
      p.setThreeComponentValue(_crossSpectrum->rayleigh());
      return p.value();
    } else {
      return -1.0;
    }
  }

#if 0
  void HRFKDirectRayleigh::setGradient(const Point& kell, FKPower& p) const
  {
    _countFree++;
    FKSteeringThreeComponentRayleighRadial * s=static_cast<FKSteeringThreeComponentRayleighRadial *>(_steering);
    s->initValue(kell, kell.z());
    s->initGradient();
    p.setThreeComponentValue(_crossSpectrum->rayleigh());
    p.setThreeComponentGradient();
  }

  void HRFKDirectRayleigh::setHessian(const Point&, FKPower& p) const
  {
    _countFree+=4;
    FKSteeringThreeComponentRayleighRadial * s=static_cast<FKSteeringThreeComponentRayleighRadial *>(_steering);
    s->initHessian();
    p.setThreeComponentHessian(_crossSpectrum->rayleigh());
  }
#endif

  double HRFKDirectRayleigh::ellipticity(const Point& kell)
  {
    FKGridSearch grid;
    double ellh, ellz;
    HRFKDirectRayleighRadialEllipticity * fh;
    fh=new HRFKDirectRayleighRadialEllipticity(_gridCache);
    grid.setFunction(fh);
    fh->setK(kell);
    grid.setGrid(kell.z()-M_PI/20, kell.z()+M_PI/20, M_PI/40);
    grid.globalMax(0.0);
    ellh=::tan(grid.pos().x());

    HRFKDirectRayleighVerticalEllipticity *fz;
    fz=new HRFKDirectRayleighVerticalEllipticity(_gridCache);
    grid.setFunction(fz);
    fz->setK(kell);
    grid.setGrid(kell.z()-M_PI/20, kell.z()+M_PI/20, M_PI/40);
    grid.globalMax(0.0);
    ellz=::tan(grid.pos().x());

    double ellR=ellipticityHelper(ellh, ellz);
    // Check that the two ways of getting ellipticity provides the same results
    if(fabs(kell.z()-::atan(ellR))>1.0/180.0*M_PI) {
      _ellipticity=0.0;
    } else {
      _ellipticity=kell.z();
    }
    return _ellipticity;
  }

  double HRFKDirectRayleigh::ellipticityHelper(double ellh, double ellz)
  {
    double ellRatio=ellz/ellh;
    if(ellRatio>=1.0) {
      _RoverN=0.5*(::sqrt(1+8.0*ellRatio)-3.0);
      ellz/=1.0+0.5*_RoverN;
      ellh*=1.0+_RoverN;
      if(fabs(ellz-ellh)>1e-5) {
        APP_LOG(1, tr("Difference between ellh (%1) and ellz (%2)=%3\n").arg(ellh).arg(ellz).arg(ellz-ellh));
      }
      return ellz;
    } else {
      _RoverN=std::numeric_limits<double>::infinity();
      return 0.0;
    }
  }

  double HRFKDirectRayleigh::noise(const Point&) const
  {
    return _crossSpectrum->array().count()*_RoverN;
  }

  double HRFKDirectRayleigh::power(double optimumPower) const
  {
    if(_ellipticity!=0.0) {
      return ::pow(optimumPower/(_ellipticity*_ellipticity), 1.0/1.5);
    } else {
      return 0.0;
    }
  }

} // namespace ArrayCore

