ossimSarSensorModel.cpp

/*
 * Copyright (C) 2005-2017 by Centre National d'Etudes Spatiales (CNES)
 *
 * This file is licensed under MIT license:
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */


#include "ossim/ossimSarSensorModel.h"
#include "ossim/ossimKeyWordListUtilities.h"
#include "ossim/ossimTraceHelpers.h"
#include "ossim/ossimRangeUtilities.h"
#include "ossim/ossimSarSensorModelPathsAndKeys.h"
#include <ossim/base/ossimRegExp.h>
#include <ossim/base/ossimLsrSpace.h>
#include <ossim/base/ossimKeywordNames.h>
#include <boost/static_assert.hpp>
#include <iostream>
#include <vector>
#include <algorithm>

#if defined(USE_BOOST_TIME)
using boost::posix_time::microseconds;
using boost::posix_time::seconds;
#else
using ossimplugins::time::microseconds;
using ossimplugins::time::seconds;
#endif

namespace {// Anonymous namespace
   const bool         k_verbose = false; // global verbose constant; TODO: use an option

   // Sometimes, we don't need to compare the actual distance, its square value is
   // more than enough.
   inline double squareDistance(ossimDpt const& lhs, ossimDpt const& rhs) {
      const double dx = lhs.x - rhs.x;
      const double dy = lhs.y - rhs.y;
      return dx*dx + dy*dy;
   }

   inline double squareDistance(ossimGpt const& lhs, ossimGpt const& rhs) {
      const ossimEcefPoint l(lhs);
      const ossimEcefPoint r(rhs);
      return (l-r).norm2();
   }

   template <typename Container>
      inline void unzip(Container const& in, Container& out_even, Container& out_odd)
      {
         typedef typename Container::const_iterator const_iterator;
         typedef typename Container::size_type      size_type;

         const size_type size                         = in.size();
         const bool      has_a_odd_number_of_elements = size % 2 == 1;

         out_even.reserve(size/2+1);
         out_odd.reserve(size/2);

         const_iterator end = in.end();
         if (has_a_odd_number_of_elements)
         {
            std::advance(end, -1);
         }
         for (const_iterator it=in.begin(); it != end ; )
         {
            out_even.push_back(*it++);
            out_odd.push_back(*it++);
         }
         if (has_a_odd_number_of_elements)
         {
            assert(end != in.end());
            out_even.push_back(*end);
         }
         assert(out_even.size() >= out_odd.size());
         assert(out_odd.capacity() == size/2); // The correct number of element have been reserved
         assert(out_even.capacity() == size/2+1); // The correct number of element have been reserved
         assert(out_odd.size() + out_even.size() == size);
      }

   ossimTrace traceExec  ("ossimSarSensorModel:exec");
   ossimTrace traceDebug ("ossimSarSensorModel:debug");

   typedef char const* const* strings_iterator;
   static char const* const PRODUCTTYPE_STRINGS[] = { "SLC", "GRD", "MGD", "GEC", "EEC" };
}// Anonymous namespace

namespace ossimplugins
{
   RTTI_DEF1(ossimSarSensorModel, "ossimSarSensorModel", ossimSensorModel);

   const double ossimSarSensorModel::C = 299792458;

   const unsigned int ossimSarSensorModel::thePluginVersion = 2;

   ossimSarSensorModel::ProductType::ProductType(string_view const& s)
   {
      using ossimplugins::begin;
      using ossimplugins::end;
      strings_iterator const ProductType_it = std::find(begin(::PRODUCTTYPE_STRINGS),end(::PRODUCTTYPE_STRINGS), s);
      if (ProductType_it == end(::PRODUCTTYPE_STRINGS))  {
         throw std::runtime_error("Invalid Sar Sensor Product type: `"+s+"'");
      }
      m_value = Type(std::distance(begin(::PRODUCTTYPE_STRINGS), ProductType_it));
      assert(m_value < MAX__);
   }

   string_view ossimSarSensorModel::ProductType::ToString() const
   {
      BOOST_STATIC_ASSERT((MAX__ == array_size(::PRODUCTTYPE_STRINGS)));
      assert(m_value != UNDEFINED__); // Yes, I know UNDEFINED__ > MAX__
      assert(m_value < MAX__);
      return PRODUCTTYPE_STRINGS[m_value];
   }

   ossimSarSensorModel::ossimSarSensorModel()
      : theRadarFrequency(0.),
      theAzimuthTimeInterval(seconds(0)),
      theNearRangeTime(0.),
      theRangeSamplingRate(0.),
      theRangeResolution(0.),
      theBistaticCorrectionNeeded(false),
      theAzimuthTimeOffset(seconds(0)),
      theRangeTimeOffset(0.)
      {}

   ossimSarSensorModel::GCPRecordType const&
      ossimSarSensorModel::findClosestGCP(ossimDpt const& imPt) const
      {
         assert(!theGCPRecords.empty()&&"theGCPRecords is empty.");

         // Find the closest GCP
         double distance2 = squareDistance(imPt, theGCPRecords.front().imPt);

         std::vector<GCPRecordType>::const_iterator refGcp = theGCPRecords.begin();
         std::vector<GCPRecordType>::const_iterator gcpIt  = theGCPRecords.begin();
         std::vector<GCPRecordType>::const_iterator gcpEnd = theGCPRecords.end();
         for(++gcpIt ; gcpIt!=gcpEnd ; ++gcpIt)
         {
            const double currentDistance2 = squareDistance(imPt, gcpIt->imPt);

            if(currentDistance2 < distance2)
            {
               distance2 = currentDistance2;
               refGcp = gcpIt;
            }
         }

         assert(refGcp != theGCPRecords.end() && "A GCP record shall have been found!");
         return *refGcp;
      }

   void ossimSarSensorModel::lineSampleHeightToWorld(const ossimDpt& imPt, const double & heightAboveEllipsoid, ossimGpt& worldPt) const
   {
      // std::clog << "ossimSarSensorModel::lineSampleHeightToWorld()\n";
      assert(!theGCPRecords.empty()&&"theGCPRecords is empty.");

      GCPRecordType const& refGcp = findClosestGCP(imPt);

      // Set the height reference
      ossim_float64 hgtSet;
      if ( ossim::isnan(heightAboveEllipsoid) )
      {
         hgtSet = refGcp.worldPt.height();
      }
      else
      {
         hgtSet = heightAboveEllipsoid;
      }
      const ossimHgtRef hgtRef(AT_HGT, hgtSet);

      ossimEcefPoint ellPt;

      // Simple iterative inversion of inverse model starting at closest gcp
      projToSurface(refGcp,imPt,hgtRef,ellPt);

      worldPt = ossimGpt(ellPt);
   }

   void ossimSarSensorModel::lineSampleToWorld(ossimDpt const& imPt, ossimGpt& worldPt) const
   {
      // std::clog << "ossimSarSensorModel::lineSampleToWorld()\n";
      assert(!theGCPRecords.empty()&&"theGCPRecords is empty.");

      GCPRecordType const& refGcp = findClosestGCP(imPt);
      ossimGpt      const& refPt = refGcp.worldPt;

      const ossimHgtRef hgtRef(AT_DEM);

      ossimEcefPoint ellPt;

      // Simple iterative inversion of inverse model starting at closest gcp
      projToSurface(refGcp,imPt,hgtRef,ellPt);

      worldPt = ossimGpt(ellPt);
   }

   void ossimSarSensorModel::worldToLineSample(const ossimGpt& worldPt, ossimDpt & imPt) const
   {
      // std::clog << "ossimSarSensorModel::worldToLineSample()\n";
      assert(theRangeResolution>0&&"theRangeResolution is null.");

      // First compute azimuth and range time
      TimeType azimuthTime;
      double rangeTime;

      const bool success = worldToAzimuthRangeTime(worldPt, azimuthTime, rangeTime);

      if(!success)
      {
         imPt.makeNan();
         return;
      }
      // std::clog << "AzimuthTime: " << azimuthTime << "\n";
      // std::clog << "RangeTime: " << rangeTime << "\n";
      // std::clog << "GRD: " << isGRD() << "\n";

      // Convert azimuth time to line
      azimuthTimeToLine(azimuthTime,imPt.y);

      if(isGRD())
      {
         // GRD case
         double groundRange(0);
         slantRangeToGroundRange(rangeTime*C/2,azimuthTime,groundRange);
         // std::clog << "GroundRange: " << groundRange << "\n";
         // std::clog << "TheRangeResolution: " << theRangeResolution << "\n";

         // Eq 32 p. 31
         // TODO: possible micro-optimization: precompute 1/theRangeResolution, and
         // use *
         imPt.x = groundRange/theRangeResolution;
      }
      else
      {
         // std::clog << "TheNearRangeTime: " << theNearRangeTime << "\n";
         // std::clog << "TheRangeSamplingRate: " << theRangeSamplingRate << "\n";
         // SLC case
         // Eq 23 and 24 p. 28
         imPt.x = (rangeTime - theNearRangeTime)*theRangeSamplingRate;
      }
   }

   bool ossimSarSensorModel::worldToAzimuthRangeTime(const ossimGpt& worldPt, TimeType & azimuthTime, double & rangeTime) const
   {
      // std::clog << "ossimSarSensorModel::worldToAzimuthRangeTime()\n";
      // First convert lat/lon to ECEF
      ossimEcefPoint inputPt(worldPt);

      // Compute zero doppler time
      TimeType interpTime;
      ossimEcefPoint interpSensorPos;
      ossimEcefVector interpSensorVel;

      const bool success = zeroDopplerLookup(inputPt,azimuthTime,interpSensorPos,interpSensorVel);

      if(!success)
      {
         // TODO: check whether we could throw instead
         return false;
      }

      if(theBistaticCorrectionNeeded)
      {
         // Compute bistatic correction if needed
         DurationType bistaticCorrection;
         computeBistaticCorrection(inputPt,interpSensorPos,bistaticCorrection);

         // Update interpolated azimuth time
         azimuthTime += bistaticCorrection;

         // Update sensor position and velocity
         interpolateSensorPosVel(interpTime,interpSensorPos,interpSensorVel);
      }

      // rangeTime is the round-tripping time to target
      const double rangeDistance = (interpSensorPos-inputPt).magnitude();
      rangeTime = theRangeTimeOffset + 2*rangeDistance/C;

      return true;
   }

   void ossimSarSensorModel::lineSampleToAzimuthRangeTime(const ossimDpt & imPt, TimeType & azimuthTime, double & rangeTime) const
   {
      // std::clog << "ossimSarSensorModel::lineSampleToAzimuthRangeTime()\n";
      // First compute azimuth time here
      lineToAzimuthTime(imPt.y,azimuthTime);

      // Then compute range time
      if(isGRD())
      {
         // Handle grd case here
         double slantRange;
         groundRangeToSlantRange(imPt.x*theRangeResolution,azimuthTime, slantRange);
         rangeTime = theRangeTimeOffset + 2*slantRange/C;
      }
      else
      {
         rangeTime = theRangeTimeOffset + theNearRangeTime + imPt.x*(1/theRangeSamplingRate);
      }
   }

   void ossimSarSensorModel::computeRangeDoppler(const ossimEcefPoint & inputPt, const ossimEcefPoint & sensorPos, const ossimEcefVector sensorVel, double & range, double & doppler) const
   {
      // std::clog << "ossimSarSensorModel::computeRangeDoppler()\n";
      assert(theRadarFrequency>0&&"theRadarFrequency is null");

      // eq. 19, p. 25
      const ossimEcefVector s2gVec = inputPt - sensorPos;

      range = s2gVec.magnitude();

      const double coef = -2*C/(theRadarFrequency*range);

      doppler = coef * sensorVel.dot(s2gVec);
   }

   void ossimSarSensorModel::interpolateSensorPosVel(const TimeType & azimuthTime, ossimEcefPoint& sensorPos, ossimEcefVector& sensorVel, unsigned int deg) const
   {
      assert(!theOrbitRecords.empty()&&"The orbit records vector is empty");

      // Lagrangian interpolation of sensor position and velocity

      unsigned int nBegin(0), nEnd(0);

      sensorPos[0] = 0;
      sensorPos[1] = 0;
      sensorPos[2] = 0;

      sensorVel[0] = 0;
      sensorVel[1] = 0;
      sensorVel[2] = 0;

      // First, we search for the correct set of record to use during
      // interpolation

      // If there are less records than degrees, use them all
      if(theOrbitRecords.size()<deg)
      {
         nEnd = theOrbitRecords.size()-1;
      }
      else
      {
         // Search for the deg number of records around the azimuth time
         unsigned int t_min_idx = 0;
         DurationType t_min = abs(azimuthTime - theOrbitRecords.front().azimuthTime);

         unsigned int count = 0;

         for(std::vector<OrbitRecordType>::const_iterator it = theOrbitRecords.begin();it!=theOrbitRecords.end();++it,++count)
         {
            const DurationType current_time = abs(azimuthTime-it->azimuthTime);

            if(t_min > current_time)
            {
               t_min_idx = count;
               t_min = current_time;
            }
         }
         // TODO: see if these expressions can be simplified
         nBegin = std::max((int)t_min_idx-(int)deg/2+1,(int)0);
         nEnd = std::min(nBegin+deg-1,(unsigned int)theOrbitRecords.size());
         nBegin = nEnd<theOrbitRecords.size()-1 ? nBegin : nEnd-deg+1;
      }

      // Compute lagrangian interpolation using records from nBegin to nEnd
      for(unsigned int i = nBegin; i < nEnd; ++i)
      {
         double w = 1.;

         unsigned int j = nBegin;
         for( ; j != i ; ++j)
         {
            const DurationType td1 = azimuthTime                    - theOrbitRecords[j].azimuthTime;
            const DurationType td2 = theOrbitRecords[i].azimuthTime - theOrbitRecords[j].azimuthTime;
            const double f = td1 / td2;
            w *= f;
         }
         ++j;
         for( ; j < nEnd; ++j)
         {
            const DurationType td1 = azimuthTime                    - theOrbitRecords[j].azimuthTime;
            const DurationType td2 = theOrbitRecords[i].azimuthTime - theOrbitRecords[j].azimuthTime;
            const double f = td1 / td2;
            w *= f;
         }

         sensorPos[0]+=w*theOrbitRecords[i].position[0];
         sensorPos[1]+=w*theOrbitRecords[i].position[1];
         sensorPos[2]+=w*theOrbitRecords[i].position[2];

         sensorVel[0]+=w*theOrbitRecords[i].velocity[0];
         sensorVel[1]+=w*theOrbitRecords[i].velocity[1];
         sensorVel[2]+=w*theOrbitRecords[i].velocity[2];
      }
   }

   void ossimSarSensorModel::slantRangeToGroundRange(const double & slantRange, const TimeType & azimuthTime, double & groundRange) const
   {
      applyCoordinateConversion(slantRange,azimuthTime,theSlantRangeToGroundRangeRecords,groundRange);
   }

   void ossimSarSensorModel::groundRangeToSlantRange(const double & groundRange, const TimeType & azimuthTime, double & slantRange) const
   {
      applyCoordinateConversion(groundRange,azimuthTime,theGroundRangeToSlantRangeRecords,slantRange);
   }

   void ossimSarSensorModel::applyCoordinateConversion(const double & in, const TimeType& azimuthTime, const std::vector<CoordinateConversionRecordType> & records, double & out) const
   {
      assert(!records.empty()&&"The records vector is empty.");
      // std::clog << "conv coord(" << in << ", az="<<azimuthTime<<")\n";

      // First, we need to find the correct pair of records for interpolation
      std::vector<CoordinateConversionRecordType>::const_iterator it = records.begin();

      CoordinateConversionRecordType srgrRecord;

      std::vector<CoordinateConversionRecordType>::const_iterator  previousRecord = it;
      ++it;

      std::vector<CoordinateConversionRecordType>::const_iterator nextRecord = it;

      // Look for the correct record
      // std::clog << "Looking for " << azimuthTime << " within records:\n";
      while(it!=records.end())
      {
         // std::clog << "- record: " << it->azimuthTime << "...";
         // nextRecord = it;

         if(azimuthTime >= previousRecord->azimuthTime
               && azimuthTime < nextRecord->azimuthTime)
         {
            // std::clog << " found!\n";
            break;
         }
         else
         {
            previousRecord = nextRecord;
            ++it;
            nextRecord = it;
            // std::clog << " NOT found => next!\n";
         }
      }
      assert(nextRecord == it);
      if(it == records.end())
      {
         if(azimuthTime < records.front().azimuthTime)
         {
            srgrRecord = records.front();
            // std::clog << "Not found, but before first => srgrRecord: " << srgrRecord.azimuthTime << "\n";
         }
         else if(azimuthTime >= records.back().azimuthTime)
         {
            srgrRecord = records.back();
            // std::clog << "Not found, but after last => srgrRecord: " << srgrRecord.azimuthTime << "\n";
         }
      }
      else
      {
         assert(nextRecord != records.end());
         assert(!previousRecord->coefs.empty()&&"previousRecord coefficients vector is empty.");
         assert(!nextRecord->coefs.empty()&&"nextRecord coefficients vector is empty.");

         // If azimuth time is between 2 records, interpolate
         const double interp
            = DurationType(azimuthTime             - previousRecord->azimuthTime)
            / (nextRecord->azimuthTime - previousRecord->azimuthTime)
            ;
         // std::clog << "interp: " << interp << " ="
         // << " (" << azimuthTime             << " - " << previousRecord->azimuthTime << " (="<< (azimuthTime             - previousRecord->azimuthTime)<< ") )"
         // << "/(" << nextRecord->azimuthTime << " - " << previousRecord->azimuthTime << " (="<< (nextRecord->azimuthTime - previousRecord->azimuthTime)<< ") )"
         // << "\n";

         srgrRecord.rg0 = (1-interp) * previousRecord->rg0 + interp*nextRecord->rg0;

         srgrRecord.coefs.clear();
         std::vector<double>::const_iterator pIt = previousRecord->coefs.begin();
         std::vector<double>::const_iterator nIt = nextRecord->coefs.begin();

         for(;pIt != previousRecord->coefs.end() && nIt != nextRecord->coefs.end();++pIt,++nIt)
         {
            srgrRecord.coefs.push_back(interp*(*nIt)+(1-interp)*(*pIt));
         }

         assert(!srgrRecord.coefs.empty()&&"Slant range to ground range interpolated coefficients vector is empty.");
      }

      // Now that we have the interpolated coefs, compute ground range
      // from slant range
      const double sr_minus_sr0 =  in-srgrRecord.rg0;

      assert(!srgrRecord.coefs.empty()&&"Slant range to ground range coefficients vector is empty.");

      out = 0;

      for(std::vector<double>::const_reverse_iterator cIt = srgrRecord.coefs.rbegin();cIt!=srgrRecord.coefs.rend();++cIt)
      {
         out = *cIt + sr_minus_sr0*out;
      }
   }


   bool ossimSarSensorModel::zeroDopplerLookup(const ossimEcefPoint & inputPt, TimeType & interpAzimuthTime, ossimEcefPoint & interpSensorPos, ossimEcefVector & interpSensorVel) const
   {
      assert((theOrbitRecords.size()>=2) && "Orbit records vector contains less than 2 elements");

      std::vector<OrbitRecordType>::const_iterator it = theOrbitRecords.begin();

      double doppler2(0.);

      // Compute range and doppler of first record
      // NOTE: here we only use the scalar product with vel and discard
      // the constant coef as it has no impact on doppler sign

      double doppler1 = (inputPt-it->position).dot(it->velocity);


      bool dopplerSign1 = doppler1 < 0;

      ++it; // -> it != begin

      // Look for the consecutive records where doppler freq changes sign
      // Note: implementing a bisection algorithm here might be faster
      for ( ; it!=theOrbitRecords.end() ; ++it)
      {
         // compute range and doppler of current record
         doppler2 = (inputPt-it->position).dot(it->velocity);

         const bool dopplerSign2 = doppler2 <0;

         // If a change of sign is detected
         if(dopplerSign1 != dopplerSign2)
         {
            break;
         }
         else
         {
            doppler1 = doppler2;
         }
      }

      // In this case, we need to extrapolate
      if(it == theOrbitRecords.end())
      {
         std::vector<OrbitRecordType>::const_iterator record1 = theOrbitRecords.begin();
         std::vector<OrbitRecordType>::const_iterator record2 = record1 + theOrbitRecords.size()-1;
         doppler1 = (inputPt-record1->position).dot(record1->velocity);
         doppler2 = (inputPt-record2->position).dot(record2->velocity);
         const DurationType delta_td = record2->azimuthTime - record1->azimuthTime;
         interpAzimuthTime = record1->azimuthTime - doppler1 / (doppler2 - doppler1) * delta_td;
      }
      else
      {
         assert(it != theOrbitRecords.begin());
         assert(it != theOrbitRecords.end());
         std::vector<OrbitRecordType>::const_iterator record2 = it;
         std::vector<OrbitRecordType>::const_iterator record1 = --it;
         // now interpolate time and sensor position
         const double abs_doppler1 = std::abs(doppler1);
         const double interpDenom = abs_doppler1+std::abs(doppler2);

         assert(interpDenom>0&&"Both doppler frequency are null in interpolation weight computation");

         const double interp = abs_doppler1/interpDenom;
         // std::clog << "interp: " << interp << "\n";

         const DurationType delta_td = record2->azimuthTime - record1->azimuthTime;
         // std::clog << "delta_td: " << delta_td << " = " << record2->azimuthTime <<" - " <<record1->azimuthTime<< "\n";

         // Compute interpolated time offset wrt record1
         // (No need for that many computations (day-frac -> ms -> day frac))
         const DurationType td     = delta_td * interp;
         // std::clog << "td: " << td  << "(" << td.total_microseconds() << "us)\n";
         // Compute interpolated azimuth time
         interpAzimuthTime = record1->azimuthTime + td + theAzimuthTimeOffset;
      }

      // std::clog << "interpAzimuthTime: " << interpAzimuthTime << "\n";

      // Interpolate sensor position and velocity
      interpolateSensorPosVel(interpAzimuthTime,interpSensorPos, interpSensorVel);

      return true;
   }

   void ossimSarSensorModel::computeBistaticCorrection(const ossimEcefPoint & inputPt, const ossimEcefPoint & sensorPos, DurationType & bistaticCorrection) const
   {
      // Bistatic correction (eq 25, p 28)
      double halftrange = 1000000. * (sensorPos-inputPt).magnitude()/C;
      bistaticCorrection= microseconds(static_cast<unsigned long>(floor(halftrange+0.5)));
   }


   void ossimSarSensorModel::azimuthTimeToLine(const TimeType & azimuthTime, double & line) const
   {
      assert(!theBurstRecords.empty()&&"Burst records are empty (at least one burst should be available)");

      std::vector<BurstRecordType>::const_iterator currentBurst = theBurstRecords.begin();

      // Look for the correct burst. In most cases the number of burst
      // records will be 1 (except for TOPSAR Sentinel1 products)
      std::vector<BurstRecordType>::const_iterator it = theBurstRecords.begin();
      std::vector<BurstRecordType>::const_iterator itend = theBurstRecords.end();
      for(; it!= itend ; ++it)
      {
         if(azimuthTime >= it->azimuthStartTime
               && azimuthTime < it->azimuthStopTime)
         {
            currentBurst = it;
            break;
         }
      }

      // If no burst is found, we will use the first (resp. last burst to
      // extrapolate line
      if(it == itend)
      {
         if(! theBurstRecords.empty())
         {
            if(azimuthTime < theBurstRecords.front().azimuthStartTime)
            {
               currentBurst = theBurstRecords.begin();
            }
            else if (azimuthTime > theBurstRecords.back().azimuthStopTime)
            {
               currentBurst = theBurstRecords.end()-1;
            }
         }
         else
         {
            // Fall back to the only record
            currentBurst = theBurstRecords.begin();
         }
      }

      const DurationType timeSinceStart = azimuthTime - currentBurst->azimuthStartTime;

      // Eq 22 p 27
      line = (timeSinceStart/theAzimuthTimeInterval) + currentBurst->startLine;
      // std::clog << "line = " << line << " <- " << timeSinceStart << "/" << theAzimuthTimeInterval << "+" << currentBurst->startLine << "\n";
   }

   void ossimSarSensorModel::lineToAzimuthTime(const double & line, TimeType & azimuthTime) const
   {
      assert(!theBurstRecords.empty()&&"Burst records are empty (at least one burst should be available)");

      std::vector<BurstRecordType>::const_iterator currentBurst = theBurstRecords.begin();

      if(theBurstRecords.size() != 1)
      {
         // Look for the correct burst. In most cases the number of burst
         // records will be 1 (except for TOPSAR Sentinel1 products)
         std::vector<BurstRecordType>::const_iterator it = theBurstRecords.begin();
         std::vector<BurstRecordType>::const_iterator itend = theBurstRecords.end();
         for( ; it!= itend; ++it)
         {
            if(line >= it->startLine && line < it->endLine)
            {
               currentBurst = it;
               break;
            }
         }

         if(it == itend)
         {
            if(line < theBurstRecords.front().startLine)
            {
               currentBurst = theBurstRecords.begin();
            }
            else if (line >= theBurstRecords.back().endLine)
            {
               currentBurst = theBurstRecords.end()-1;
            }
         }

      }

      const DurationType timeSinceStart = (line - currentBurst->startLine)*theAzimuthTimeInterval;
      // std::clog << "timeSinceStart: " << timeSinceStart.total_microseconds() << "us\n";

      // Eq 22 p 27
      azimuthTime = currentBurst->azimuthStartTime + timeSinceStart + theAzimuthTimeOffset;
      // std::clog << "offset: "         << theAzimuthTimeOffset << "\n";
      // std::clog << "->azimuthTime: "  << azimuthTime << "\n";
   }



   bool ossimSarSensorModel::projToSurface(const GCPRecordType & initGcp, const ossimDpt & target, const ossimHgtRef & hgtRef, ossimEcefPoint & ellPt) const
   {
     
      // Initialize current estimation
      ossimEcefPoint currentEstimation(initGcp.worldPt);

      // Compute corresponding image position
      // std::clog << "initGCP: " << initGcp.imPt << "\n";
      ossimDpt currentImPoint(initGcp.imPt);

      ossim_float64 currentImSquareResidual = squareDistance(target,currentImPoint);
      double currentHeightResidual = initGcp.worldPt.height() - hgtRef.getRefHeight(initGcp.worldPt);

      bool init = true;

      unsigned int iter = 0;

      // TODO: Every time the function is called, an allocation (+a free) is done.
      // This is not efficient. => Find a static matrix of 3x3 elements
      // Moreover, NEWMAT implies a lot of objet creations, hence allocations
      NEWMAT::SymmetricMatrix BtB(3);
      NEWMAT::ColumnVector BtF(3);
      NEWMAT::ColumnVector F(3);
      NEWMAT::ColumnVector dR(3);

      // Stop condition: img residual < 1e-2 pixels, height residual² <
      // 0.01² m, nb iter < 50. init ensure that loop runs at least once.
      while((init || (currentImSquareResidual > (0.01*0.01) || std::abs(currentHeightResidual) > 0.01))  && iter < 50)
      {
         if(init)
            init =false;

         // std::clog<<"Iter: "<<iter<<", Res: im="<<currentImSquareResidual<<", hgt="<<currentHeightResidual<<'\n';

         // compute residuals
         F(1) = target.x - currentImPoint.x;
         F(2) = target.y - currentImPoint.y;
         F(3) = currentHeightResidual;

         // std::clog<<"F("<<iter<<")="<<F<<'\n';

         // Delta use for partial derivatives estimation (in meters)
         const double d = 10.;

         // Compute partial derivatives
         ossimEcefVector p_fx, p_fy, p_fh,dx(d,0,0),dy(0,d,0),dz(0,0,d);
         ossimDpt tmpImPt;

         ossim_float64 rdx(0.0),rdy(0.0),rdz(0.0), fdx(0.0),fdy(0.0),fdz(0.0);

         ossimGpt currentEstimationWorld(currentEstimation);
         ossimGpt tmpGpt = ossimGpt(currentEstimation+dx);
         worldToLineSample(tmpGpt,tmpImPt);
         // std::clog << "currentEstimationWorld: " << currentEstimationWorld << "\n";
         // std::clog << "currentEstimation: " << currentEstimation << "\n";
         // std::clog << "tmpGpt: " << tmpGpt << "\n";
         // std::clog << "tmpImPt: " << tmpImPt << "\n";
         p_fx[0] = (currentImPoint.x-tmpImPt.x)/d;
         p_fy[0] = (currentImPoint.y-tmpImPt.y)/d;
         p_fh[0] = (currentEstimationWorld.height()-tmpGpt.height())/d;

         tmpGpt = ossimGpt(currentEstimation+dy);
         worldToLineSample(tmpGpt,tmpImPt);
         p_fx[1] = (currentImPoint.x-tmpImPt.x)/d;
         p_fy[1] = (currentImPoint.y-tmpImPt.y)/d;
         p_fh[1] = (currentEstimationWorld.height()-tmpGpt.height())/d;

         tmpGpt = ossimGpt(currentEstimation+dz);
         worldToLineSample(tmpGpt,tmpImPt);
         p_fx[2] = (currentImPoint.x-tmpImPt.x)/d;
         p_fy[2] = (currentImPoint.y-tmpImPt.y)/d;
         p_fh[2] = (currentEstimationWorld.height()-tmpGpt.height())/d;

         // Form B-matrix
         NEWMAT::Matrix B = ossimMatrix3x3::create(p_fx[0], p_fx[1], p_fx[2],
               p_fy[0], p_fy[1], p_fy[2],
               p_fh[0], p_fh[1], p_fh[2]);

         // std::clog<<"B: "<<B<<'\n';

         // Invert system
         try {
            dR = B.i() * F;
         } catch (NEWMAT::SingularException const& e) {
         ellPt = currentEstimation;
         
         ossimNotify(ossimNotifyLevel_WARN) <<"ossim::SarSensorModel::projToSurface(): singular matrix can not be inverted. Returning best estimation so far ("<<ellPt<<") for output point ("<<target<<")\n";
         std::clog << "initGCP: " << initGcp.imPt <<", "<<initGcp.worldPt<< "\n";
         
         return true;
         }

         // Update estimate
         for (ossim_int32 k=0; k<3; k++)
         {
            currentEstimation[k] -= dR(k+1);
         }

         // std::clog<<"dR: "<<dR<<'\n';

         currentEstimationWorld=ossimGpt(currentEstimation);

         // Update residuals
         const ossim_float64 atHgt = hgtRef.getRefHeight(currentEstimationWorld);
         currentHeightResidual = atHgt - currentEstimationWorld.height();

         worldToLineSample(currentEstimationWorld,currentImPoint);

         // std::clog<<currentImPoint<<'\n';

         currentImSquareResidual = squareDistance(currentImPoint,target);

         ++iter;
      }

      // std::clog<<"Iter: "<<iter<<", Res: im="<<currentImSquareResidual<<", hgt="<<currentHeightResidual<<'\n';

      ellPt = currentEstimation;
      return true;
   }



   //*************************************************************************************************
   // Infamous DUP
   //*************************************************************************************************
   ossimObject* ossimSarSensorModel::dup() const
   {
      return new ossimSarSensorModel(*this);
   }
   bool ossimSarSensorModel::useForward() const
   {
      return false;
   }

   bool ossimSarSensorModel::autovalidateInverseModelFromGCPs(const double & xtol, const double & ytol, const double azTimeTol, const double & rangeTimeTol) const
   {
      std::clog << "ossimSarSensorModel::autovalidateInverseModelFromGCPs()\n";
      if(theGCPRecords.empty())
      {
         return false;
      }

      bool success = true;

      unsigned int gcpId = 1;

      std::clog << theGCPRecords.size() << " GCPS\n";