/*
 * 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 otbSARCartesianMeanFunctor_h
#define otbSARCartesianMeanFunctor_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 SARCartesianMeanFunctor
* \brief Catesian mean estimation.
*
* This functor is an implementation of SARPolygonsFunctor. It estimates the Cartesian coordonates mean image.
*
* The characteristics are the following :
* _ Output Geometry : ML (defined by ML factors)
* _ Seven required components : L, C, Y, Z, XCart, YCart and ZCart
* _ Four estimated components : Mean of XCart, YCart, ZCart and an isData mask
* _ Optionnal image : No
*
* \ingroup DiapOTBModule
*/
  template <class TInputPixel, class TOutputPixel>
  class SARCartesianMeanFunctor : public SARPolygonsFunctor<TInputPixel, TOutputPixel>
{
public:
  /** Standard class typedefs */
  typedef SARCartesianMeanFunctor        Self;
  typedef itk::Object                   Superclass;
  typedef itk::SmartPointer<Self>       Pointer;
  typedef itk::SmartPointer<const Self> ConstPointer;
 
  /** Runtime information */
  itkTypeMacro(SARCartesianMeanFunctor, 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;
	  }
	else if (it->first == "XCart")
	  {
	    m_indXCart = it->second;
	  }
	else if (it->first == "YCart")
	  {
	    m_indYCart = it->second;
	  }
	else if (it->first == "ZCart")
	  {
	    m_indZCart = it->second;
	  }
	++it;
      }
  }

  /**
   * Method initialize (override)
   */
  void initalize(int nbElt, TOutputPixel * outValue, int threadId=1) override
  {
    int nbComponentsMax = this->GetNumberOfEstimatedComponents();

    if (m_CountPolygons[threadId] == 0)
      {
	m_CountPolygons[threadId] = new int[nbElt];
      }

    // Reserve Components into outValue and Initalize
    for(int ind = 0; ind < nbElt; ind++)
      {
	m_CountPolygons[threadId][ind] = 0;

	if (outValue[ind].GetSize() == 0)
	  {
	    outValue[ind].Reserve(nbComponentsMax);
	  }

	for (int iC = 0; iC < nbComponentsMax; iC++)
	  {
	    outValue[ind][iC] = 0;
	  }
      }
  }

  /**
   * Method estimate (override)
   */
  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 * outValue,
			bool isFirstMaille, double & tgElevationMaxForCurrentLine,int threadId=1) override
  {
    // ML factor in range (to get SAR geometry (= Projeted DEM Pixel/Bands Geometry))
    firstCol_into_outputRegion = m_originC + (firstCol_into_outputRegion - m_originC)*m_MLran;
    nbCol_into_outputRegion *= m_MLran;

    if (static_cast<unsigned int>(nbCol_into_outputRegion+firstCol_into_outputRegion) > 
	(this->GetNbColSAR() + m_originC))
      {
	nbCol_into_outputRegion = this->GetNbColSAR() - firstCol_into_outputRegion + m_originC;
      }

    double cE, cS;
    double dc;
    int col1, col2;
    // Elevation angle (for shadow)
    double tgElevationMax = 0.;
    double tgElevation = 0.;

    double zE, yE, zS, yS, xCartE, xCartS, yCartE, yCartS, zCartE, zCartS;
    double a, b, Z, Y, XCart, YCart, ZCart ;
 
    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]);
	
	xCartE = b1*(CLZY_InSideUp[m_indXCart]) + 
	  a1*(CLZY_InSideDown[m_indXCart]);
	yCartE = b1*(CLZY_InSideUp[m_indYCart]) + 
	  a1*(CLZY_InSideDown[m_indYCart]);
	zCartE = b1*(CLZY_InSideUp[m_indZCart]) + 
	  a1*(CLZY_InSideDown[m_indZCart]);

	zS = b2*(CLZY_OutSideUp[m_indZ]) + 
	  a2*(CLZY_OutSideDown[m_indZ]);
	yS = b2*(CLZY_OutSideUp[m_indY]) + 
	  a2*(CLZY_OutSideDown[m_indY]);
	
	xCartS = b2*(CLZY_OutSideUp[m_indXCart]) + 
	  a2*(CLZY_OutSideDown[m_indXCart]);
	yCartS = b2*(CLZY_OutSideUp[m_indYCart]) + 
	  a2*(CLZY_OutSideDown[m_indYCart]);
	zCartS = b2*(CLZY_OutSideUp[m_indZCart]) + 
	  a2*(CLZY_OutSideDown[m_indZCart]);

	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]);

	xCartE = b2*(CLZY_OutSideUp[m_indXCart]) + 
	  a2*(CLZY_OutSideDown[m_indXCart]);
	yCartE = b2*(CLZY_OutSideUp[m_indYCart]) + 
	  a2*(CLZY_OutSideDown[m_indYCart]);
	zCartE = b2*(CLZY_OutSideUp[m_indZCart]) + 
	  a2*(CLZY_OutSideDown[m_indZCart]);

	zS = b1*(CLZY_InSideUp[m_indZ]) + 
	  a1*(CLZY_InSideDown[m_indZ]);
	yS = b1*(CLZY_InSideUp[m_indY]) + 
	  a1*(CLZY_InSideDown[m_indY]);
	
	xCartS = b1*(CLZY_InSideUp[m_indXCart]) + 
	  a1*(CLZY_InSideDown[m_indXCart]);
	yCartS = b1*(CLZY_InSideUp[m_indYCart]) + 
	  a1*(CLZY_InSideDown[m_indYCart]);
	zCartS = b1*(CLZY_InSideUp[m_indZCart]) + 
	  a1*(CLZY_InSideDown[m_indZCart]);
 
	cE = c2;
	cS = c1;
      }
    
    dc = cS - cE;
	      
    // 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;
	XCart = a*xCartE + b*xCartS;
	YCart = a*yCartE + b*yCartS;
	ZCart = a*zCartE + b*zCartS;
	 
	tgElevation = (Z > 0.0) ? Y / Z : sign (1e20, Y);     
	 
	// If no into shadows
	if (tgElevation > tgElevationMax)
	  {                                                                         
	    tgElevationMax = tgElevation;
	     	     	     
	    // Contribution for this maille
	    int ind_for_out = k - firstCol_into_outputRegion;

	    // Into ML Geo if m_MLRan > 1
	    ind_for_out /= m_MLran; // Integer division
	 
	    // Accumulate X, Y and Z Cartesian
	    outValue[ind_for_out][0] += XCart;
	    outValue[ind_for_out][1] += YCart;
	    outValue[ind_for_out][2] += ZCart;

	    // Set isData at 1
	    outValue[ind_for_out][3] = 1; 
	    
	    // Increment the counter
	    m_CountPolygons[threadId][ind_for_out] += 1;
	  }
      }

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

  /**
   * Method synthetize (override)
   */
  // Synthetize to apply on XCart, YCart and Zcart the counter of polygons
  void synthetize(int nbElt, TOutputPixel * outValue, int threadId=1) override
  {
    for(int ind = 0; ind < nbElt; ind++)
      {
	if (m_CountPolygons[threadId][ind] !=0)
	  {
	    outValue[ind][0] /= m_CountPolygons[threadId][ind];
	    outValue[ind][1] /= m_CountPolygons[threadId][ind];
	    outValue[ind][2] /= m_CountPolygons[threadId][ind];
	  }
      }
  }
  
  // default constructor
  SARCartesianMeanFunctor()
  { 
  }
  

  /** Constructor */ // 
  SARCartesianMeanFunctor(int nbThreads, unsigned int mlran = 1, unsigned int mlazi = 1, 
			  unsigned int originC = 0)
  {    
    // Specific argument
    m_NbThreads = nbThreads;
    
    // Allocate the count buffer and the current line indicator (one per each thread)
    m_CountPolygons = new int*[m_NbThreads];

    // Allocate the buffer of polygons for each thread 
    for (int i = 0; i < m_NbThreads; i++)
      {
	m_CountPolygons[i] = 0;
      }

    m_MLran = mlran;
    m_MLazi = mlazi;

    m_originC = originC;

    // Global argument (from PolygonsFunctor)
    this->SetNumberOfExpectedComponents(7); // expected components in input = 7
    this->SetNumberOfEstimatedComponents(4); // estimated components in output = 3 
    
    if (m_MLran == 1 && m_MLazi == 1)
      {
	this->SetOutputGeometry("SAR");
      }
    else 
      {
	this->SetOutputGeometry("ML");
      }

    // 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");
    vecComponents.push_back("XCart");
    vecComponents.push_back("YCart");
    vecComponents.push_back("ZCart");
    this->SetRequiredComponents(vecComponents);    

    std::vector<std::string> estimatedComponents;
    estimatedComponents.push_back("XCart");
    estimatedComponents.push_back("YCart");
    estimatedComponents.push_back("ZCart");
    estimatedComponents.push_back("isData");
    this->SetEstimatedComponents(estimatedComponents);    
  }

  
  /**
   * Method GetOutputGeometry (override)
   */
  void GetOutputGeometry(std::string & outputGeo, unsigned int & mlran, unsigned int & mlazi) override
  {
    if (m_MLran == 1 && m_MLazi == 1)
      {
	outputGeo = "SAR";
      }
    else 
      {
	outputGeo = "ML";
      }

    mlran = m_MLran;
    mlazi = m_MLazi;
  }

  // Redefinition to use construction with NumberOfThreads and ML factor (Functor used in Multi-Threading)
  static Pointer New(int nbThreads, unsigned int mlran = 1, unsigned int mlazi = 1, unsigned int originC = 0) 
  {
  Pointer smartPtr = ::itk::ObjectFactory< Self >::Create();    
    if ( smartPtr == nullptr )				     
       {                                                
         smartPtr = new Self(nbThreads, mlran, mlazi, originC);
       }
    smartPtr->UnRegister();
    return smartPtr;
  }

  /** Destructor */
  ~SARCartesianMeanFunctor() override 
  {
    // free Memory
    for (int i = 0; i < m_NbThreads; i++)
      {
	delete m_CountPolygons[i];
      } 

    delete m_CountPolygons;
  }
  
  
protected :
  int m_NbThreads;
  int ** m_CountPolygons; // Counter of contribution for each thread
  
  // Index for each components
  int m_indL;
  int m_indC;
  int m_indZ;
  int m_indY;
  int m_indXCart;
  int m_indYCart;
  int m_indZCart;

  // ML factors
  unsigned int m_MLran;
  unsigned int m_MLazi;

  // Origin for range dimension
  unsigned int m_originC;
  };

}
}

#endif