/*
 * Copyright (C) 2005-2018 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef otbSARAmplitudeEstimationFunctor_h
#define otbSARAmplitudeEstimationFunctor_h
#include <cassert>

#include "itkVariableLengthVector.h"
#include "itkRGBPixel.h"
#include "itkRGBAPixel.h"
#include "itkObject.h"
#include "itkObjectFactory.h"

#include "otbSARPolygonsFunctor.h"

namespace otb
{
namespace Function
{
/** \class SARAmplitudeEstimationFunctor
* \brief Amplitude Estimation.
*
* This functor is an implementation of SARPolygonsFunctor. It estimates the simulated amplitude image.
*
* The characteristics are the following :
* _ Output Geometry : SAR
* _ Four required components : L, C, Y and Z
* _ One estimated component : Amplitude
* _ Optionnal image : No
*
* \ingroup DiapOTBModule
*/
  template <class TInputPixel, class TOutputPixel>
  class SARAmplitudeEstimationFunctor : public SARPolygonsFunctor<TInputPixel, TOutputPixel>
{
public:
  /** Standard class typedefs */
  typedef SARAmplitudeEstimationFunctor        Self;
  typedef itk::Object                   Superclass;
  typedef itk::SmartPointer<Self>       Pointer;
  typedef itk::SmartPointer<const Self> ConstPointer;
  
  /** Runtime information */
  itkTypeMacro(SARAmplitudeEstimationFunctor, itk::Object);
  
  /**
   * Method SetRequiredComponentsToInd (override)
   */
  void SetRequiredComponentsToInd(std::map<std::string, int> mapForInd) override
  {
    std::map<std::string, int>::const_iterator it = mapForInd.begin();
    while(it!=mapForInd.end())
      {
	if (it->first == "L")
	  {
	    m_indL = it->second;
	  }
	else if (it->first == "C")
	  {
	    m_indC = it->second;
	  }
	else if (it->first == "Y")
	  {
	    m_indY = it->second;
	  }
	else if (it->first == "Z")
	  {
	    m_indZ = it->second;
	  }
	++it;
      }
  }

  /**
   * Method estimate (override)
   */
  virtual void estimate(const int ind_LineSAR, TInputPixel * CLZY_InSideUpPtr, 
			TInputPixel * CLZY_InSideDownPtr, 
			TInputPixel * CLZY_OutSideUpPtr, 
			TInputPixel * CLZY_OutSideDownPtr, 
			int firstCol_into_outputRegion, 
			int nbCol_into_outputRegion, TOutputPixel * outValueAmplitude,
			bool isFirstMaille, double & tgElevationMaxForCurrentLine, int /*threadId=1*/) override
  {
    double cE, cS;
    double dy, dz, dc;
    int col1, col2;
    // Elevation angle (for shadow)
    double tgElevationMax = 0.;
    double tgElevation = 0.;

    double zE, yE, zS, yS;
    double a, b, Z, Y;
    double cotgIncidence;
    double aux;

    double a1, b1, c1, a2, b2, c2;
    
    TInputPixel CLZY_InSideUp = *CLZY_InSideUpPtr; 
    TInputPixel CLZY_InSideDown = *CLZY_InSideDownPtr; 
    TInputPixel CLZY_OutSideUp =  *CLZY_OutSideUpPtr;
    TInputPixel CLZY_OutSideDown = *CLZY_OutSideDownPtr;

    // Define the coef a and b
    a1 =  CLZY_InSideUp[m_indL] - ind_LineSAR;
    a1 /= CLZY_InSideUp[m_indL] - CLZY_InSideDown[m_indL];

    b1 = 1-a1;

    c1 = b1*(CLZY_InSideUp[m_indC]) + a1*(CLZY_InSideDown[m_indC]);

    a2 = CLZY_OutSideUp[m_indL] - ind_LineSAR;
    a2 /= (CLZY_OutSideUp[m_indL] - CLZY_OutSideDown[m_indL]);

    b2 = 1-a2;

    c2 = b2*(CLZY_OutSideUp[m_indC]) + a2*(CLZY_OutSideDown[m_indC]);
     
    // Caculate zE, yE, zS and yS
    // Select input and output side for the current maille with c1 and c2 comparaison
    if (c2 >= c1)
      {	
	zE = b1*(CLZY_InSideUp[m_indZ]) + 
	  a1*(CLZY_InSideDown[m_indZ]);
	yE = b1*(CLZY_InSideUp[m_indY]) + 
	  a1*(CLZY_InSideDown[m_indY]);
     
	zS = b2*(CLZY_OutSideUp[m_indZ]) + 
	  a2*(CLZY_OutSideDown[m_indZ]);
	yS = b2*(CLZY_OutSideUp[m_indY]) + 
	  a2*(CLZY_OutSideDown[m_indY]);

	cE = c1;
	cS = c2;
      }
    else
      {
	zE = b2*(CLZY_OutSideUp[m_indZ]) + 
	  a2*(CLZY_OutSideDown[m_indZ]);
	yE = b2*(CLZY_OutSideUp[m_indY]) + 
	  a2*(CLZY_OutSideDown[m_indY]);

	zS = b1*(CLZY_InSideUp[m_indZ]) + 
	  a1*(CLZY_InSideDown[m_indZ]);
	yS = b1*(CLZY_InSideUp[m_indY]) + 
	  a1*(CLZY_InSideDown[m_indY]);
	 
	cE = c2;
	cS = c1;
      }
      
    dc = cS - cE;
    dy = yS - yE;
    dz = zS - zE;
	      
    // Colunm into // TODO: he current maille
    col1 = (int) (std::min (cE, cS))+1;        // +1 ?                                                                                
    col2 = (int) (std::max (cE, cS));
     
    // tgElevationMax per mailles (not sure)
    if (isFirstMaille)
      {
	tgElevationMax = (zE > 0.0) ?  yE / zE : sign (1e20, yE);  
	if (tgElevationMax < 0.) tgElevationMax = 0.;
      }
    else
      {
	tgElevationMax = tgElevationMaxForCurrentLine;
      }
    
    // Loop on colunm 
    for (int k = std::max(col1, firstCol_into_outputRegion); k<= std::min(col2, nbCol_into_outputRegion+firstCol_into_outputRegion-1); k++)
      {
	a = (cS - k) / dc;                                                                                    
	b = 1.0 - a;       	  
	Z = a * zE + b * zS;
	Y = a * yE + b * yS;

	 
	tgElevation = (Z > 0.0) ? Y / Z : sign (1e20, Y);     
	 
	// If no into shadows
	if (tgElevation > tgElevationMax)
	  {                                                                         
	    tgElevationMax = tgElevation;
	     	     
	    aux = fabs(Y * dy + Z * dz); 
	     
	    cotgIncidence =  (aux > 0.0) ? (Z*dy - Y*dz) /aux :  1E20;          
	     
	    // Contribution for this maille
	    int ind_for_out = k - firstCol_into_outputRegion;
	    outValueAmplitude[ind_for_out] +=  m_Gain * cotgIncidence;
	  }
      }

    // Store the tgElevationMax (for the next mailles)
    tgElevationMaxForCurrentLine = tgElevationMax; 
  }

  /** Constructor */ // 
  SARAmplitudeEstimationFunctor(double gain)
  {    
    // Specific argument
    m_Gain = gain;
    
    // Global argument (from PolygonsFunctor)
    this->SetNumberOfExpectedComponents(4);
    this->SetNumberOfEstimatedComponents(1);
    this->SetOutputGeometry("SAR");

    // Set the vector of Required Components
    std::vector<std::string> vecComponents;
    vecComponents.push_back("C");
    vecComponents.push_back("L");
    vecComponents.push_back("Z");
    vecComponents.push_back("Y");
    this->SetRequiredComponents(vecComponents);    
  }

  // Redefinition to use construction with gain 
  static Pointer New(double gain)
  {
    Pointer smartPtr = ::itk::ObjectFactory< Self >::Create();    
    if ( smartPtr == nullptr )				     
       {                                                
         smartPtr = new Self(gain);                                
        }                                                
       smartPtr->UnRegister();                            
       return smartPtr;
  }

  /** Destructor */
  ~SARAmplitudeEstimationFunctor() override {}
  
  
private :
  double m_Gain;
  int m_indL;
  int m_indC;
  int m_indZ;
  int m_indY;
  };

}
}

#endif