tcElevationOptimization::loadSnapShot: simplify reliability expression

[tecorrec.git] / geo / tcElevationOptimization.cpp

/***************************************************************************
 *   This file is part of Tecorrec.                                        *
 *   Copyright 2008 James Hogan <james@albanarts.com>                      *
 *                                                                         *
 *   Tecorrec 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.                                   *
 *                                                                         *
 *   Tecorrec 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 Tecorrec.  If not, write to the Free Software Foundation,  *
 *   Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.   *
 ***************************************************************************/

/**
 * @file tcElevationOptimization.cpp
 * @brief Optimized elevation.
 */

#include "tcElevationOptimization.h"
#include "tcChannelConfigWidget.h"
#include "tcSrtmModel.h"
#include "tcShadowClassifyingData.h"
#include <SplineDefinitions.h>

#include <qwt_plot.h>
#include <qwt_plot_curve.h>

#include <QObject>
#include <QWidget>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QGridLayout>
#include <QPushButton>
#include <QSpinBox>
#include <QLabel>
#include <QSlider>
#include <QMessageBox>
#include <QFileDialog>
#include <QTextStream>
#include <QThread>

/*
 * Private types
 */

/// Worker thread class.
class tcElevationOptimization::WorkerThread : public QThread
{
  public:
    /*
     * Constructors + destructor
     */

    /// Constructor.
    WorkerThread(tcElevationOptimization* self)
    : QThread()
    , m_this(self)
    {
    }

    /// Destructor.
    virtual ~WorkerThread()
    {
    }

  protected:
    /// Reimplemented.
    virtual void run()
    {
      m_this->optimize();

      m_this->m_stopProcessing = false;
      m_this->m_btnOptimize->setText(tr("Optimize Iteratively"));
      m_this->m_btnOptimize->update();
      m_this->m_processingMutex.unlock();
    }

  private:

    /*
     * Variables
     */

    /// Main optimization object.
    tcElevationOptimization* m_this;
};

/*
 * Constructors + destructor
 */

/// Primary constructor.
tcElevationOptimization::tcElevationOptimization(const QList<tcShadowClassifyingData*>& datasets, tcSrtmModel* dem, tcGeoImageData* imagery)
: tcChannelDem(dem, imagery,
               tr("Elevation Optimization"),
               tr("Optimizes elevation using image data."),
               false, true)
, m_numDatasets(datasets.size())
, m_datasets(new tcGeoImageData*[m_numDatasets])
, m_texToDatasetTex(new tcAffineTransform2<double>[m_numDatasets])
, m_shadowChannels(new tcChannel*[m_numDatasets])
, m_shadingChannels(new tcChannel*[m_numDatasets])
, m_elevationData(0)
, m_data(new tcTypedPixelData<PerDatasetPixelCache>*[m_numDatasets])
, m_datum(0)
, m_shadowData(new Reference<tcPixelData<float> >[m_numDatasets])
, m_shadingData(new Reference<tcPixelData<float> >[m_numDatasets])
, m_voidsOnly(false)
, m_loadingFromDem(false)
, m_thread(0)
, m_processingMutex()
, m_stopProcessing(false)
, m_btnOptimize(0)
, m_configWidget(0)
, m_spinIterations(0)
, m_plot(new QwtPlot())
, m_stdDevCurve(new QwtPlotCurve())
, m_stdDevVoidCurve(new QwtPlotCurve())
, m_stdDevNonVoidCurve(new QwtPlotCurve())
, m_snapshotFile()
, m_snapshotReadFile()
, m_snapshotReading(false)
, m_snapshotSlider(0)
, m_snapshotCounter(0)
{
  m_plot->setWindowFlags(Qt::Tool | Qt::WindowStaysOnTopHint);
  m_plot->setTitle("Standard Deviations");
  connect(this, SIGNAL(replotStdDeviations()), m_plot, SLOT(replot()), Qt::QueuedConnection);
  connect(this, SIGNAL(invalidateAndRedraw()), this, SLOT(invalidateAndRedrawSlot()), Qt::BlockingQueuedConnection);
  m_stdDevVoidCurve->setPen(QPen(Qt::black));
  m_stdDevVoidCurve->attach(m_plot);
  m_stdDevNonVoidCurve->setPen(QPen(Qt::green));
  m_stdDevNonVoidCurve->attach(m_plot);
  m_stdDevCurve->setPen(QPen(Qt::blue));
  m_stdDevCurve->attach(m_plot);
  for (int i = 0; i < m_numDatasets; ++i)
  {
    tcShadowClassifyingData* landsat = datasets[i];
    m_datasets[i] = landsat;
    m_texToDatasetTex[i] = imagery->texToGeo() * landsat->geoToTex();
    m_shadowChannels[i] = landsat->shadowClassification();
    if (0 != m_shadowChannels[i])
    {
      m_shadowChannels[i]->addDerivitive(this);
    }
    m_shadingChannels[i] = landsat->shading();
    if (0 != m_shadingChannels[i])
    {
      m_shadingChannels[i]->addDerivitive(this);
    }
    m_data[i] = 0;
  }
}

/// Destructor.
tcElevationOptimization::~tcElevationOptimization()
{
  if (!m_processingMutex.tryLock())
  {
    m_stopProcessing = true;
    m_thread->wait();
  }
  delete m_thread;
  delete m_plot;
  delete [] m_datasets;
  delete [] m_texToDatasetTex;
  for (int i = 0; i < m_numDatasets; ++i)
  {
    if (0 != m_shadowChannels[i])
    {
      m_shadowChannels[i]->removeDerivitive(this);
    }
    if (0 != m_shadingChannels[i])
    {
      m_shadingChannels[i]->removeDerivitive(this);
    }
    delete m_data[i];
  }
  delete [] m_shadowChannels;
  delete [] m_shadingChannels;
  delete m_elevationData;
  delete [] m_data;
  delete m_datum;
  delete [] m_shadowData;
  delete [] m_shadingData;
  delete m_configWidget;
}

/*
 * Main image interface
 */

tcChannelConfigWidget* tcElevationOptimization::configWidget()
{
  if (0 == m_configWidget)
  {
    m_configWidget = new tcChannelConfigWidget();
    m_configWidget->setWindowTitle(tr("%1 Configuration").arg(name()));

    QVBoxLayout* layout = new QVBoxLayout(m_configWidget);

    QLabel* stats = new QLabel(m_configWidget);
      layout->addWidget(stats);
      connect(this, SIGNAL(statistics(const QString&)), stats, SLOT(setText(const QString&)));

    // Row of optimisation controls
    QWidget* buttons1 = new QWidget(m_configWidget);
      layout->addWidget(buttons1);
      QHBoxLayout* buttons1Layout = new QHBoxLayout(buttons1);

      // reset DEM (sets all corrections to the original (interpolated) values
      QPushButton* btnResetDem = new QPushButton(tr("Reset DEM"), m_configWidget);
        buttons1Layout->addWidget(btnResetDem);
        btnResetDem->setToolTip(tr("Reset DEM corrected values to interpolated values"));
        connect(btnResetDem, SIGNAL(clicked(bool)), this, SLOT(resetDem()));

      // load base from DEM (loads DEM elevations into elevation buffer)
      QPushButton* btnLoadDem = new QPushButton(tr("Load DEM"), m_configWidget);
        buttons1Layout->addWidget(btnLoadDem);
        btnLoadDem->setToolTip(tr("Load corrected values from DEM into elevation field"));
        connect(btnLoadDem, SIGNAL(clicked(bool)), this, SLOT(loadFromDem()));

      // number of iterations (select number of iterations to perform)
      m_spinIterations = new QSpinBox(m_configWidget);
        buttons1Layout->addWidget(m_spinIterations);
        m_spinIterations->setToolTip(tr("Number of iterations of optimization to perform"));
        m_spinIterations->setRange(1, 1000);
        m_spinIterations->setValue(10);

      // optimize (iterates on elevation buffer and write back to DEM at intervals and end)
      m_btnOptimize = new QPushButton(tr("Optimize Iteratively"), m_configWidget);
        buttons1Layout->addWidget(m_btnOptimize);
        m_btnOptimize->setToolTip(tr("Iteratively optimize elevation field and write results back to DEM"));
        connect(m_btnOptimize, SIGNAL(clicked(bool)), this, SLOT(startStopOptimize()));

    // Another row of optimisation controls
    QWidget* buttons2 = new QWidget(m_configWidget);
      layout->addWidget(buttons2);
      QHBoxLayout* buttons2Layout = new QHBoxLayout(buttons2);

      // apply hard constraints, adjusting reliabilities
      QPushButton* btnHardConstrain = new QPushButton(tr("Hard Constrain"), m_configWidget);
        buttons2Layout->addWidget(btnHardConstrain);
        btnLoadDem->setToolTip(tr("Apply hard shadow constraints"));
        connect(btnHardConstrain, SIGNAL(clicked(bool)), this, SLOT(applyHardConstraints()));

      // bilinear interpolation of voids
      QPushButton* btnBilinear = new QPushButton(tr("Bilinear"), m_configWidget);
        buttons2Layout->addWidget(btnBilinear);
        btnLoadDem->setToolTip(tr("Void-fill using bilinear interpolation"));
        connect(btnBilinear, SIGNAL(clicked(bool)), this, SLOT(voidFillBilinear()));

      // bicubic interpolation of voids
      QPushButton* btnBicubic = new QPushButton(tr("Bicubic"), m_configWidget);
        buttons2Layout->addWidget(btnBicubic);
        btnLoadDem->setToolTip(tr("Void-fill using bicubic interpolation"));
        connect(btnBicubic, SIGNAL(clicked(bool)), this, SLOT(voidFillBicubic()));

    // Another row of controls, for graphs
    QWidget* buttons3 = new QWidget(m_configWidget);
      layout->addWidget(buttons3);
      QHBoxLayout* buttons3Layout = new QHBoxLayout(buttons3);

      // Show standard deviation curve
      QPushButton* btnStdDev = new QPushButton(tr("Std Dev"), m_configWidget);
        buttons3Layout->addWidget(btnStdDev);
        btnStdDev->setToolTip(tr("Show standard deviation graphs"));
        connect(btnStdDev, SIGNAL(clicked(bool)), m_plot, SLOT(show()));

      // Clear standard deviation
      QPushButton* btnStdDevClear = new QPushButton(tr("Clear"), m_configWidget);
        buttons3Layout->addWidget(btnStdDevClear);
        btnStdDevClear->setToolTip(tr("Clear standard deviation graphs"));
        connect(btnStdDevClear, SIGNAL(clicked(bool)), this, SLOT(clearStdDeviations()));

      // Sample standard deviation
      QPushButton* btnStdDevSample = new QPushButton(tr("Sample"), m_configWidget);
        buttons3Layout->addWidget(btnStdDevSample);
        btnStdDevSample->setToolTip(tr("Sample standard deviation"));
        connect(btnStdDevSample, SIGNAL(clicked(bool)), this, SLOT(sampleStdDeviations()));

      // Save standard deviation
      QPushButton* btnStdDevSave = new QPushButton(tr("Save"), m_configWidget);
        buttons3Layout->addWidget(btnStdDevSave);
        btnStdDevSave->setToolTip(tr("Save standard deviation samples to a text file for external processing"));
        connect(btnStdDevSave, SIGNAL(clicked(bool)), this, SLOT(saveStdDeviations()));

    // Another row of controls, for terrain snapshotting
    QWidget* buttons4 = new QWidget(m_configWidget);
      layout->addWidget(buttons4);
      QHBoxLayout* buttons4Layout = new QHBoxLayout(buttons4);

      // Initialise snapshotting
      QPushButton* btnInitSnapshotting = new QPushButton(tr("Start snapshotting"), m_configWidget);
        buttons4Layout->addWidget(btnInitSnapshotting);
        btnInitSnapshotting->setToolTip(tr("Start snapshotting the terrain at intervals for later analysis"));
        connect(btnInitSnapshotting, SIGNAL(clicked(bool)), this, SLOT(initSnapShotting()));

      // Load snapshots
      QPushButton* btnLoadSnapshots = new QPushButton(tr("Load snapshots"), m_configWidget);
        buttons4Layout->addWidget(btnLoadSnapshots);
        btnLoadSnapshots->setToolTip(tr("Load a sequence of snapshots for processing"));
        connect(btnLoadSnapshots, SIGNAL(clicked(bool)), this, SLOT(loadSnapShots()));

      // Samples resolution
      m_crossResolution = new QSpinBox(m_configWidget);
        buttons4Layout->addWidget(m_crossResolution);
        m_crossResolution->setToolTip(tr("Cross sectioning resolution"));
        m_crossResolution->setRange(64, 1024);
        m_crossResolution->setValue(1024);

      // Frequency of samples
      m_crossFrequency = new QSpinBox(m_configWidget);
        buttons4Layout->addWidget(m_crossFrequency);
        m_crossFrequency->setToolTip(tr("Cross sectioning frequency"));
        m_crossFrequency->setRange(1, 10);
        m_crossFrequency->setValue(1);

      // Button to save cross sectioning data
      QPushButton* btnCrossSectionSave = new QPushButton(tr("Save cross section data"), m_configWidget);
        buttons4Layout->addWidget(btnCrossSectionSave);
        connect(btnCrossSectionSave, SIGNAL(clicked(bool)), this, SLOT(saveCrossSections()));

      // Current frame
      m_snapshotCounter = new QLabel("0", m_configWidget);
        buttons4Layout->addWidget(m_snapshotCounter);

    // Snapshot slider
    m_snapshotSlider = new QSlider(Qt::Horizontal, m_configWidget);
      layout->addWidget(m_snapshotSlider);
      m_snapshotSlider->setEnabled(false);
      connect(m_snapshotSlider, SIGNAL(sliderMoved(int)), this, SLOT(loadSnapShot(int)));

    // Rows of weight sliders
    QString weightNames[WeightsMax] = {
      tr("Variance"),
      tr("Roughness"),
      tr("Steepness"),
      tr("Lit facing sun"),
      tr("Shape from shading"),
      tr("Photometric Stereo"),
      tr("Below shadow line"),
      tr("Transition elevation (top)"),
      tr("Transition elevation (bottom)"),
      tr("Transition slope"),
      tr("Transition curving down"),
      tr("Elevation Step"),
      tr("Range")
    };
    float defaults[WeightsMax][3] = {
      {0.0f, 10.0f, 1.0f}, // variance
      {0.0f, 5.0f, 0.1f}, // roughenss
      {0.0f, 5.0f, 0.1f}, // steepness
      {0.0f, 1000.0f, 255.0f}, // lit facing
      {0.0f, 100.0f, 0.0f}, // shading
      {0.0f, 100.0f, 0.0f}, // photometric stereo
      {0.0f, 10.0f, 1.0f}, // belowShadowLine
      {0.0f, 10.0f, 1.0f}, // transitionElevTop
      {0.0f, 10.0f, 1.0f}, // transitionElevBottom
      {0.0f, 1000.0f, 100.0f}, // transitionTangential
      {0.0f, 10.0f, 1.0f}, // transitionCurvingDown
      {0.0f, 20.0f, 4.0f}, // elevation step
      {0.0f, 4.0f, 0.0f}, // range 
    };
    QWidget* sliders = new QWidget(m_configWidget);
      layout->addWidget(sliders);
      QGridLayout* slidersLayout = new QGridLayout(sliders);

      int i = 0;
      for (; i < WeightsMax; ++i)
      {
        QLabel* label = new QLabel(weightNames[i], sliders);
          slidersLayout->addWidget(label, i, 0);

        int steps = 1000;
        m_weightLimits[0][i] = defaults[i][0];
        m_weightLimits[1][i] = (defaults[i][1] - defaults[i][0])/steps;
        int val = (int)((defaults[i][2] - m_weightLimits[0][i]) / m_weightLimits[1][i]);

        for (int sl = 0; sl < 2; ++sl)
        {
          m_weightSliders[sl][i] = new QSlider(Qt::Horizontal, sliders);
            slidersLayout->addWidget(m_weightSliders[sl][i], i, sl+1);
            m_weightSliders[sl][i]->setRange(0, steps);
            m_weightSliders[sl][i]->setValue(val);
          connect(m_weightSliders[sl][i], SIGNAL(sliderMoved(int)), this, SLOT(updateWeightLabels()));
          connect(m_weightSliders[sl][i], SIGNAL(valueChanged(int)), this, SLOT(updateWeightLabels()));
        }
        m_weightLabels[i] = new QLabel("-", sliders);
          slidersLayout->addWidget(m_weightLabels[i], i, 3);
      }
      updateWeightLabels();

    // Another row of controls, for weight saving, loading
    QWidget* buttons5 = new QWidget(m_configWidget);
      layout->addWidget(buttons5);
      QHBoxLayout* buttons5Layout = new QHBoxLayout(buttons5);

      // Save weights
      QPushButton* btnSaveWeights = new QPushButton(tr("Save weights"), m_configWidget);
        buttons5Layout->addWidget(btnSaveWeights);
        btnInitSnapshotting->setToolTip(tr("Save the weights to a file"));
        connect(btnSaveWeights, SIGNAL(clicked(bool)), this, SLOT(saveWeights()));

      // Load weights
      QPushButton* btnLoadWeights = new QPushButton(tr("Load weights"), m_configWidget);
        buttons5Layout->addWidget(btnLoadWeights);
        btnLoadWeights->setToolTip(tr("Load the weights from a file"));
        connect(btnLoadWeights, SIGNAL(clicked(bool)), this, SLOT(loadWeights()));
  }
  return m_configWidget;
}

/*
 * Slots
 */

/// Reset DEM.
void tcElevationOptimization::resetDem()
{
}

/// Load elevation data from DEM.
void tcElevationOptimization::loadFromDem()
{
  delete m_elevationData;
  m_elevationData = 0;

  delete m_datum;
  m_datum = 0;

  for (int i = 0; i < m_numDatasets; ++i)
  {
    delete m_data[i];
    m_data[i] = 0;
  }

  m_loadingFromDem = true;
  invalidate();
  m_configWidget->requestRedraw();
  m_loadingFromDem = false;
}

/// Apply hard constraints to elevation data.
void tcElevationOptimization::applyHardConstraints()
{
  if (0 != m_elevationData)
  {
    const int width = m_elevationData->width();
    const int height = m_elevationData->height();
    // Now start optimising
    startProcessing("Applying hard constraints");
    for (int j = 0; j <= height; ++j)
    {
      progress((float)j/(height-1));
      for (int i = 0; i <= width; ++i)
      {
        int index = j*width + i;

        // Only apply constraints to unreliable pixels
        PerPixelCache* cache = &m_datum->buffer()[index];
        if (cache->reliability != 0)
        {
          continue;
        }

        int elevationConstraintCount = 0;

        // Per dataset
        for (int ds = 0; ds < m_numDatasets; ++ds)
        {
          PerDatasetPixelCache* dsCache = &m_data[ds]->buffer()[index];

          bool isShadowed = (m_shadowData[ds]->sampleFloat((float)i/(width-1), (float)j/(height-1)) < 0.5f);
          bool isShadowEntrance = isShadowed &&
                                  dsCache->shadowTransition[TransitionEntrance][0] == i &&
                                  dsCache->shadowTransition[TransitionEntrance][1] == j;
          bool isShadowExit = isShadowed &&
                              dsCache->shadowTransition[TransitionExit][0] == i &&
                              dsCache->shadowTransition[TransitionExit][1] == j;

          if (isShadowed)
          {
            bool exitReliable = false;
            float exitElevation = 0.0f;
            float exitDistance = 0.0f;
            if (dsCache->shadowTransition[TransitionExit][0] >= 0 &&
                dsCache->shadowTransition[TransitionExit][0] < width &&
                dsCache->shadowTransition[TransitionExit][1] >= 0 &&
                dsCache->shadowTransition[TransitionExit][1] < height)
            {
              int exitIndex = width*dsCache->shadowTransition[TransitionExit][1] + dsCache->shadowTransition[TransitionExit][0];
              PerPixelCache* exitCache = &m_datum->buffer()[exitIndex];
              // Great, we can say for definite the elevation of this pixel because the corresponding pixel is reliable
              if (exitCache->reliability != 0)
              {
                exitReliable = true;
                exitElevation = m_elevationData->buffer()[exitIndex];
                exitDistance = dsCache->shadowTransitionDistance[TransitionExit];
                if (isShadowEntrance)
                {
                  float newElevation = exitElevation + exitDistance*dsCache->dlz_dlxy;
                  m_elevationData->buffer()[index] = m_elevationData->buffer()[index]*elevationConstraintCount + newElevation;
                  m_elevationData->buffer()[index] /= ++elevationConstraintCount;
                  cache->originalElevation = m_elevationData->buffer()[index];
                  cache->reliability = 1;
                }
              }
            }
            bool entranceReliable = false;
            float entranceElevation = 0.0f;
            float entranceDistance = 0.0f;
            if (dsCache->shadowTransition[TransitionEntrance][0] >= 0 &&
                dsCache->shadowTransition[TransitionEntrance][0] < width &&
                dsCache->shadowTransition[TransitionEntrance][1] >= 0 &&
                dsCache->shadowTransition[TransitionEntrance][1] < height)
            {
              int entranceIndex = width*dsCache->shadowTransition[TransitionEntrance][1] + dsCache->shadowTransition[TransitionEntrance][0];
              PerPixelCache* entranceCache = &m_datum->buffer()[entranceIndex];
              // Great, we can say for definite the elevation of this pixel because the corresponding pixel is reliable
              if (entranceCache->reliability != 0)
              {
                entranceReliable = true;
                entranceElevation = m_elevationData->buffer()[entranceIndex];
                entranceDistance = dsCache->shadowTransitionDistance[TransitionEntrance];
                if (isShadowExit)
                {
                  float newElevation = entranceElevation - entranceDistance*dsCache->dlz_dlxy;
                  m_elevationData->buffer()[index] = m_elevationData->buffer()[index]*elevationConstraintCount + newElevation;
                  m_elevationData->buffer()[index] /= ++elevationConstraintCount;
                  cache->originalElevation = m_elevationData->buffer()[index];
                  cache->reliability = 1;
                }
              }
            }

            if (exitReliable && entranceReliable)
            {
              // maximum elevation is between elevation at entrance and elevation at exit
              float maxElevation = (exitElevation*entranceDistance + entranceElevation*exitDistance) / (entranceDistance + exitDistance);
              if (m_elevationData->buffer()[index] > maxElevation)
              {
                m_elevationData->buffer()[index] = maxElevation;
              }
            }
          }
        }
      }
    }
    endProcessing();

    invalidate();
    m_configWidget->requestRedraw();
  }
  else
  {
    QMessageBox::warning(m_configWidget,
                         tr("Elevation field not initialised."),
                         tr("Please ensure the optimisation channel is displayed."));
  }
}

/// Void-fill bilinear.
void tcElevationOptimization::voidFillBilinear()
{
  if (0 != m_elevationData)
  {
    const int width = m_elevationData->width();
    const int height = m_elevationData->height();
    // Now start optimising
    startProcessing("Bilinear void filling");
    for (int j = 0; j < height; ++j)
    {
      progress(0.5f*j/(height-1));
      int lastNonVoid = -1;
      float lastNonVoidValue;
      for (int i = 0; i < width; ++i)
      {
        int index = j*width + i;
        if (m_datum->buffer()[index].reliability != 0)
        {
          float value = m_elevationData->buffer()[index];
          if (lastNonVoid != -1)
          {
            int gap = i - lastNonVoid;
            float startAlt = lastNonVoidValue;
            float dAlt = (value - lastNonVoidValue) / gap;
            // Lovely, between lastNonVoid and i is a void
            for (int ii = lastNonVoid+1; ii < i; ++ii)
            {
              int iIndex = j*width + ii;
              float alt = startAlt + dAlt * (ii - lastNonVoid);
              m_elevationData->buffer()[iIndex] = alt;
              // Keep track of the gap
              m_datum->buffer()[iIndex].temporary.i = gap;
            }
          }
          else
          {
            m_datum->buffer()[index].temporary.i = -1;
          }
          lastNonVoid = i;
          lastNonVoidValue = value;
        }
        else
        {
          m_datum->buffer()[index].temporary.i = -1;
        }
      }
    }
    for (int i = 0; i < width; ++i)
    {
      progress(0.5f + 0.5f*i/(width-1));
      int lastNonVoid = -1;
      float lastNonVoidValue;
      for (int j = 0; j < height; ++j)
      {
        int index = j*width + i;
        float value = m_elevationData->buffer()[index];
        if (m_datum->buffer()[index].reliability != 0)
        {
          if (lastNonVoid != -1)
          {
            int gap = j - lastNonVoid;
            float startAlt = lastNonVoidValue;
            float dAlt = (value - lastNonVoidValue) / gap;
            // Lovely, between lastNonVoid and j is a void
            for (int jj = lastNonVoid+1; jj < j; ++jj)
            {
              // Average with previous value if non zero, otherwise overwrite
              int iIndex = jj*width + i;
              float alt = startAlt + dAlt * (jj - lastNonVoid);
              int otherGap = m_datum->buffer()[iIndex].temporary.i;
              if (otherGap != -1)
              {
                float prevAlt = m_elevationData->buffer()[iIndex];
                // Prefer the value of the smaller gap
                alt = (alt * otherGap + prevAlt * gap) / (gap+otherGap);
              }
              m_elevationData->buffer()[iIndex] = alt;
            }
          }
          lastNonVoid = j;
          lastNonVoidValue = value;
        }
      }
    }
    endProcessing();

    invalidate();
    m_configWidget->requestRedraw();
  }
  else
  {
    QMessageBox::warning(m_configWidget,
                         tr("Elevation field not initialised."),
                         tr("Please ensure the optimisation channel is displayed."));
  }
}

/// Void-fill bicubic.
void tcElevationOptimization::voidFillBicubic()
{
  if (0 != m_elevationData)
  {
    const int width = m_elevationData->width();
    const int height = m_elevationData->height();
    // Now start optimising
    startProcessing("Bicubic void filling");
    for (int j = 0; j < height; ++j)
    {
      progress(0.5f*j/(height-1));
      int lastNonVoid = -1;
      float lastNonVoidValue;
      for (int i = 0; i < width; ++i)
      {
        int index = j*width + i;
        if (m_datum->buffer()[index].reliability != 0)
        {
          float value = m_elevationData->buffer()[index];
          if (lastNonVoid != -1)
          {
            int gap = i - lastNonVoid;
            // Obtain some control points for the splines
            // use the two samples either side of the gap as control points 1 and 2
            // use the ones before and after these samples, extended to the size of
            // the gap as control points 0 and 3
            float startAlt = lastNonVoidValue;
            float beforeStartAlt = startAlt;
            if (lastNonVoid > 0)
            {
              if (0 != m_datum->buffer()[j*width + lastNonVoid - 1].reliability)
              {
                beforeStartAlt = startAlt + (m_elevationData->buffer()[j*width + lastNonVoid - 1] - startAlt) * gap;
              }
            }
            float endAlt = value;
            float afterEndAlt = endAlt;
            if (i < width-1)
            {
              if (0 != m_datum->buffer()[j*width + i + 1].reliability)
              {
                afterEndAlt = endAlt + (m_elevationData->buffer()[j*width + i + 1] - endAlt) * gap;
              }
            }
            // Lovely, between lastNonVoid and i is a void
            for (int ii = lastNonVoid+1; ii < i; ++ii)
            {
              int iIndex = j*width + ii;
              float u = (float)(ii-lastNonVoid) / gap;
              float u2 = u*u;
              float u3 = u2*u;
              float alt = maths::catmullRomSpline.Spline(u, u2, u3, beforeStartAlt, startAlt, endAlt, afterEndAlt);
              m_elevationData->buffer()[iIndex] = alt;
              // Keep track of the gap
              m_datum->buffer()[iIndex].temporary.i = gap;
            }
          }
          else
          {
            m_datum->buffer()[index].temporary.i = -1;
          }
          lastNonVoid = i;
          lastNonVoidValue = value;
        }
        else
        {
          m_datum->buffer()[index].temporary.i = -1;
        }
      }
    }
    for (int i = 0; i < width; ++i)
    {
      progress(0.5f + 0.5f*i/(width-1));
      int lastNonVoid = -1;
      float lastNonVoidValue;
      for (int j = 0; j < height; ++j)
      {
        int index = j*width + i;
        float value = m_elevationData->buffer()[index];
        if (m_datum->buffer()[index].reliability != 0)
        {
          if (lastNonVoid != -1)
          {
            int gap = j - lastNonVoid;
            // Obtain some control points for the splines
            // use the two samples either side of the gap as control points 1 and 2
            // use the ones before and after these samples, extended to the size of
            // the gap as control points 0 and 3
            float startAlt = lastNonVoidValue;
            float beforeStartAlt = startAlt;
            if (lastNonVoid > 0)
            {
              if (0 != m_datum->buffer()[(lastNonVoid-1)*width + i].reliability)
              {
                beforeStartAlt = startAlt + (m_elevationData->buffer()[(lastNonVoid-1)*width + i] - startAlt) * gap;
              }
            }
            float endAlt = value;
            float afterEndAlt = endAlt;
            if (j < height-1)
            {
              if (0 != m_datum->buffer()[(j+1)*width + i].reliability)
              {
                afterEndAlt = endAlt + (m_elevationData->buffer()[(j+1)*width + i] - endAlt) * gap;
              }
            }
            // Lovely, between lastNonVoid and j is a void
            for (int jj = lastNonVoid+1; jj < j; ++jj)
            {
              // Average with previous value if non zero, otherwise overwrite
              int iIndex = jj*width + i;
              float u = (float)(jj-lastNonVoid) / gap;
              float u2 = u*u;
              float u3 = u2*u;
              float alt = maths::catmullRomSpline.Spline(u, u2, u3, beforeStartAlt, startAlt, endAlt, afterEndAlt);
              int otherGap = m_datum->buffer()[iIndex].temporary.i;
              if (otherGap != -1)
              {
                float prevAlt = m_elevationData->buffer()[iIndex];
                // Prefer the value of the smaller gap
                alt = (alt * otherGap + prevAlt * gap) / (gap+otherGap);
              }
              m_elevationData->buffer()[iIndex] = alt;
            }
          }
          lastNonVoid = j;
          lastNonVoidValue = value;
        }
      }
    }
    endProcessing();

    invalidate();
    m_configWidget->requestRedraw();
  }
  else
  {
    QMessageBox::warning(m_configWidget,
                         tr("Elevation field not initialised."),
                         tr("Please ensure the optimisation channel is displayed."));
  }
}

/// Start/stop optimization process.
void tcElevationOptimization::startStopOptimize()
{
  if (m_processingMutex.tryLock())
  {
    m_btnOptimize->setText(tr("Stop Optimization"));
    m_btnOptimize->update();
    if (!m_thread)
    {
      m_thread = new WorkerThread(this);
    }
    m_thread->start();
  }
  else
  {
    // Warn the worker thread that its time to stop.
    m_stopProcessing = true;
  }
}

/// Optimize a number of times.
void tcElevationOptimization::optimize()
{
  if (0 != m_elevationData)
  {
    const int width = m_elevationData->width();
    const int height = m_elevationData->height();
    // Now start optimising
    startProcessing("Optimizing elevation data");
    int passes = m_spinIterations->value();
    for (int i = 0; i < passes; ++i)
    {
      startProcessing("Optimizing elevation data");
      float passage = (float)i/(passes-1);
      float passageNeg = 1.0f - passageNeg;
      progress(passage);
      for (int w = 0; w < WeightsMax; ++w)
      {
        float start = m_weightLimits[0][w] + m_weightLimits[1][w]*m_weightSliders[0][w]->value();
        float end = m_weightLimits[0][w] + m_weightLimits[1][w]*m_weightSliders[1][w]->value();
        m_weights[w] = passageNeg*start + passage*end;
      }
      float deltaH = m_weights[ElevationStep];
      float range = m_weights[Range];
      optimizeElevations(0, 0, width-1, height-1, deltaH, range);
      if (m_stopProcessing)
      {
        emit invalidateAndRedraw();
        endProcessing();
        break;
      }

      if ((i+1) % 5 == 0 || i == passes-1)
      {
        emit invalidateAndRedraw();
        startProcessing("Optimizing elevation data");
        progress((float)i/passes);
      }

      sampleStdDeviations();
    }
    endProcessing();
  }
  else
  {
    QMessageBox::warning(m_configWidget,
                         tr("Elevation field not initialised."),
                         tr("Please ensure the optimisation channel is displayed."));
  }
}

/// Clear standard deviations.
void tcElevationOptimization::clearStdDeviations()
{
  m_plotX.clear();
  m_stdDevData.clear();
  m_stdDevCurve->setData(m_plotX, m_stdDevData);
  m_stdDevVoidData.clear();
  m_stdDevVoidCurve->setData(m_plotX, m_stdDevVoidData);
  m_stdDevNonVoidData.clear();
  m_stdDevNonVoidCurve->setData(m_plotX, m_stdDevNonVoidData);
  m_plot->replot();
  m_plot->update();
}

/// Sample standard deviations.
void tcElevationOptimization::sampleStdDeviations()
{
  int width = m_elevationData->width();
  int height = m_elevationData->height();
  float stdDevVoid = 0.0f;
  float stdDevNonVoid = 0.0f;
  float stdDevAll = 0.0f;
  int total = 0;
  int voids = 0;
  int marginx = height>>3;
  int marginy = width>>3;
  for (int j = marginy; j < height-marginy; ++j)
  {
    for (int i = marginx; i < width-marginx; ++i)
    {
      int index = j*width + i;
      float altitude = m_datum->buffer()[index].accurateElevation;

      float square = m_elevationData->buffer()[index] - altitude;
      square *= square;
      stdDevAll += square;
      if (m_datum->buffer()[index].reliability)
      {
        stdDevNonVoid += square;
      }
      else
      {
        stdDevVoid += square;
        ++voids;
      }
      ++total;
    }
  }
  stdDevNonVoid = sqrt(stdDevNonVoid / (total-voids));
  stdDevVoid = sqrt(stdDevVoid / voids);
  stdDevAll = sqrt(stdDevAll / total);
  emit statistics(tr("Overall std deviation: %1 m\n"
                     "Void std deviation: %2 (%4\%) m\n"
                     "Non-void std deviation: %3 (%5\%) m")
                  .arg(stdDevAll)
                  .arg(stdDevVoid).arg(stdDevNonVoid)
                  .arg(100.0f*voids/total).arg(100.0f*(total-voids)/total));
  //m_configWidget->update();

  if (m_snapshotReadFile.isEmpty() || m_snapshotReading)
  {
    m_plotX.append(m_plotX.size()+1);
    m_stdDevData.append(stdDevAll);
    m_stdDevCurve->setData(m_plotX, m_stdDevData);
    m_stdDevVoidData.append(stdDevVoid);
    m_stdDevVoidCurve->setData(m_plotX, m_stdDevVoidData);
    m_stdDevNonVoidData.append(stdDevNonVoid);
    m_stdDevNonVoidCurve->setData(m_plotX, m_stdDevNonVoidData);
    // this will get queued for event loop
    emit replotStdDeviations();

    // Take a snapshot
    if (!m_snapshotFile.isEmpty())
    {
      QString fname = QString("%1.%2").arg(m_snapshotFile).arg(m_plotX.size(), 4, 10, QLatin1Char('0'));
      QFile snap(fname);
      if (snap.open(QIODevice::WriteOnly))
      {
        QDataStream stream(&snap);
        stream.setByteOrder(QDataStream::LittleEndian);
        stream << width << height;
        int index = 0;
        for (int j = 0; j < height; ++j)
        {
          for (int i = 0; i < width; ++i)
          {
            uint16_t alt = floor(0.5f + m_elevationData->buffer()[index]);
            uint16_t orig = floor(0.5f + m_datum->buffer()[index].originalElevation);
            if (!m_datum->buffer()[index].reliability)
            {
              alt |= 0x8000;
            }
            stream << alt << orig;
            ++index;
          }
        }
      }
    }
  }
}

/// Sample standard deviations.
void tcElevationOptimization::saveStdDeviations()
{
  QString filename = QFileDialog::getSaveFileName(m_configWidget,
                        tr("Save standard deviation data"),
                        QString(),
                        tr("ASCII Data Files (*.adf)"));

  if (!filename.isEmpty())
  {
    // Open the file ready to write the data
    QFile file(filename);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
    {
      QMessageBox::warning(m_configWidget,
          tr("Open failed."),
          tr("Could not open '%1' for writing.").arg(filename));
      return;
    }

    QTextStream out(&file);

    for (int i = 0; i < m_plotX.size(); ++i)
    {
      out << m_plotX[i] << "\t"
          << m_stdDevData[i] << "\t"
          << m_stdDevVoidData[i] << "\t"
          << m_stdDevNonVoidData[i] << "\n";
    }
  }
}

/// Save cross section data.
void tcElevationOptimization::saveCrossSections()
{
  if (!m_crossSectioned)
  {
    QMessageBox::warning(m_configWidget,
        tr("Cross section save failed."),
        tr("You need to select a cross section with middle mouse button drag."));
    return;
  }

  QString filename = QFileDialog::getSaveFileName(m_configWidget,
                        tr("Save cross section data"),
                        QString(),
                        tr("Crosssection Files (*.xsec)"));

  if (!filename.isEmpty())
  {
    // Open the file ready to write the data
    QFile file(filename);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
    {
      QMessageBox::warning(m_configWidget,
          tr("Open failed."),
          tr("Could not open '%1' for writing.").arg(filename));
      return;
    }

    QTextStream out(&file);

    int resolution = m_crossResolution->value();
    int frequency  = m_crossFrequency->value();
    int snaps = m_snapshotSlider->maximum();
    tcGeo delta = (m_crossSection[1] - m_crossSection[0]) / (resolution-1);
    // save distances
    {
      float deltaDist = tcGeo::angleBetween(m_crossSection[1], m_crossSection[0])/(resolution-1)*6378.137e3;
      float distance = 0.0f;
      for (int p = 0; p < resolution; ++p)
      {
        out << distance << ((p == resolution-1) ? "\n" : "\t");
        distance += deltaDist;
      }
    }
    for (int snap = 1; snap <= snaps; snap += frequency)
    {
      // Load snapshot
      loadSnapShot(snap);

      tcGeo tmp = m_crossSection[0];
      for (int p = 0; p < resolution; ++p)
      {
        float elev = m_elevationData->elevationAt(tmp);
        out << elev << ((p == resolution-1) ? "\n" : "\t");
        tmp.setLon(tmp.lon() + delta.lon());
        tmp.setLat(tmp.lat() + delta.lat());
      }
    }
  }
}

/// Invalidate and redraw.
void tcElevationOptimization::invalidateAndRedrawSlot()
{
  invalidate();
  m_configWidget->requestRedraw();
}

/// Initialise snapshotting.
void tcElevationOptimization::initSnapShotting()
{
  m_snapshotReadFile = QString();
  m_snapshotSlider->setEnabled(false);
  m_snapshotFile = QFileDialog::getSaveFileName(m_configWidget,
                        tr("Save snapshotting config file"),
                        QString(),
                        tr("Snapshot Config Files (*.sdf)"));

  if (!m_snapshotFile.isEmpty())
  {
    // Open the file ready to write the data
    QFile file(m_snapshotFile);
    if (!file.open(QIODevice::WriteOnly))
    {
      QMessageBox::warning(m_configWidget,
          tr("Open failed."),
          tr("Could not open '%1' for writing.").arg(m_snapshotFile));
      return;
    }

    const double* portion = portionPosition();
    tcGeo p1(imagery()->texToGeo()*maths::Vector<2,double>(portion[0], portion[1]));
    tcGeo p2(imagery()->texToGeo()*maths::Vector<2,double>(portion[2], portion[3]));

    QDataStream stream(&file);
    stream.setByteOrder(QDataStream::LittleEndian);
    stream << p1.lon()*180.0/M_PI
           << p1.lat()*180.0/M_PI
           << p2.lon()*180.0/M_PI
           << p2.lat()*180.0/M_PI;
  }
}

#include <QtDebug>
/// Load a sequence of snapshots.
void tcElevationOptimization::loadSnapShots()
{
  m_snapshotFile = QString();
  m_snapshotReadFile = QFileDialog::getOpenFileName(m_configWidget,
                        tr("Open snapshotting config file"),
                        QString(),
                        tr("Snapshot Config Files (*.sdf)"));

  if (!m_snapshotReadFile.isEmpty())
  {
    // Open the file ready to read the data
    QFile file(m_snapshotReadFile);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
    {
      QMessageBox::warning(m_configWidget,
          tr("Open failed."),
          tr("Could not open '%1' for reading.").arg(m_snapshotReadFile));
      return;
    }

    QDataStream stream(&file);
    stream.setByteOrder(QDataStream::LittleEndian);

    double portion[2][2];
    stream >> portion[0][0]
           >> portion[0][1]
           >> portion[1][0]
           >> portion[1][1];
    tcGeo p1(portion[0][0]*M_PI/180.0, portion[0][1]*M_PI/180.0);
    tcGeo p2(portion[1][0]*M_PI/180.0, portion[1][1]*M_PI/180.0);

    // force update of region
    m_configWidget->requestSlice(p1, p2);

    // Find number of snaps
    int max = 0;
    bool exists = true;
    QFile snap;
    while (exists)
    {
      ++max;
      QString fname = QString("%1.%2").arg(m_snapshotReadFile).arg(max, 4, 10, QLatin1Char('0'));
      snap.setFileName(fname);
      exists = snap.exists();
    }
    --max;
    m_snapshotSlider->setRange(1, max);
    m_snapshotSlider->setEnabled(true);

    // load first sample
    clearStdDeviations();
    m_snapshotReading = true;
    for (int i = 1; i <= max; ++i)
    {
      loadSnapShot(i);
    }
    m_snapshotReading = false;
    loadSnapShot(max);
    m_snapshotSlider->setValue(max);
  }
  else
  {
    m_snapshotSlider->setEnabled(false);
  }
}

/// Update weight labels.
void tcElevationOptimization::updateWeightLabels()
{
  for (int i = 0; i < WeightsMax; ++i)
  {
    m_weightLabels[i]->setText(QString("%1 -> %2")
      .arg(m_weightLimits[0][i] + m_weightLimits[1][i]*m_weightSliders[0][i]->value())
      .arg(m_weightLimits[0][i] + m_weightLimits[1][i]*m_weightSliders[1][i]->value())
    );
  }
}

/// Save weights.
void tcElevationOptimization::saveWeights()
{
  QString filename = QFileDialog::getSaveFileName(m_configWidget,
                        tr("Save weights"),
                        QString(),
                        tr("Elevation optimization weights (*.eow)"));

  if (!filename.isEmpty())
  {
    // Open the file ready to write the data
    QFile file(filename);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
    {
      QMessageBox::warning(m_configWidget,
          tr("Open failed."),
          tr("Could not open '%1' for writing.").arg(filename));
      return;
    }

        QTextStream stream(&file);
        for (int w = 0; w < WeightsMax; ++w)
        {
          float start = m_weightLimits[0][w] + m_weightLimits[1][w]*m_weightSliders[0][w]->value();
          float end = m_weightLimits[0][w] + m_weightLimits[1][w]*m_weightSliders[1][w]->value();
          stream << start << "\t" << end << "\n";
    }
  }
}

/// Load weights.
void tcElevationOptimization::loadWeights()
{
  QString filename = QFileDialog::getOpenFileName(m_configWidget,
                        tr("Load weights"),
                        QString(),
                        tr("Elevation optimization weights (*.eow)"));

  if (!filename.isEmpty())
  {
    // Open the file ready to write the data
    QFile file(filename);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
    {
      QMessageBox::warning(m_configWidget,
          tr("Load failed."),
          tr("Could not open '%1' for reading.").arg(filename));
      return;
    }

    QTextStream stream(&file);
    float starts[WeightsMax], ends[WeightsMax];
    for (int w = 0; w < WeightsMax; ++w)
    {
      stream >> starts[w] >> ends[w];
      if (stream.atEnd())
      {
        if (w == WeightsMax-1)
        {
          // Go back to shading shifting the values
          while (w > PhotometricStereo)
          {
            starts[w] = starts[w-1];
            ends[w] = ends[w-1];
            --w;
          }
          starts[w] = ends[w] = 0.0f;
          break;
        }
        else
        {
          starts[w] = ends[w] = 0.0f;
          QMessageBox::warning(m_configWidget,
                               tr("Panic!"),
                               tr("Not enough weights"));
        }
      }
    }
    for (int w = 0; w < WeightsMax; ++w)
    {
      m_weightSliders[0][w]->setValue((starts[w] - m_weightLimits[0][w]) / m_weightLimits[1][w]);
      m_weightSliders[1][w]->setValue((ends[w] - m_weightLimits[0][w]) / m_weightLimits[1][w]);
    }
  }
}

/// Load a single snapshot.
void tcElevationOptimization::loadSnapShot(int id)
{
  QString counter = "-";
  // Load a snapshot
  if (!m_snapshotReadFile.isEmpty())
  {
    QString fname = QString("%1.%2").arg(m_snapshotReadFile).arg(id, 4, 10, QLatin1Char('0'));
    QFile snap(fname);
    if (snap.open(QIODevice::ReadOnly))
    {
      QDataStream stream(&snap);
      stream.setByteOrder(QDataStream::LittleEndian);
      int width, height;
      stream >> width >> height;
      if (m_elevationData->width() != width || m_elevationData->height() != height)
      {
        QMessageBox::warning(m_configWidget,
                             tr("Snapshot dimentions do not match with current."),
                             tr("Panic!"));
      }
      else
      {
        int index = 0;
        for (int j = 0; j < height; ++j)
        {
          for (int i = 0; i < width; ++i)
          {
            uint16_t alt;
            uint16_t orig;
            stream >> alt >> orig;
            m_datum->buffer()[index].reliability = !(alt & 0x8000);
            m_datum->buffer()[index].originalElevation = orig;
            alt &= 0x7FFF;
            m_elevationData->buffer()[index] = alt;
            ++index;
          }
        }
        sampleStdDeviations();
        if (!m_snapshotReading)
        {
          invalidate();
          m_configWidget->requestRedraw();
        }
        counter = QString("%1").arg(id);
      }
    }
  }
  m_snapshotCounter->setText(counter);
}

/*
 * Interface for derived class to implement
 */

void tcElevationOptimization::roundPortion(double* x1, double* y1, double* x2, double* y2)
{
  //m_shadowChannel->roundPortion(x1,y1,x2,y2);
}

tcAbstractPixelData* tcElevationOptimization::loadPortion(double x1, double y1, double x2, double y2, bool changed)
{
  int width = -1;
  int height = -1;
  
  for (int i = 0; i < m_numDatasets; ++i)
  {
    maths::Vector<2,double> xy1 = m_texToDatasetTex[i] * maths::Vector<2,double>(x1,y1);
    maths::Vector<2,double> xy2 = m_texToDatasetTex[i] * maths::Vector<2,double>(x2,y2);
    Reference<tcAbstractPixelData> channelData = m_shadowChannels[i]->portion(xy1[0], xy1[1], xy2[0], xy2[1]);
    if (i == 0)
    {
      width = channelData->width();
      height = channelData->height();
    }
    m_shadowData[i] = dynamicCast<tcPixelData<float>*>(channelData);
    if (0 == m_shadowData[i])
    {
      return 0;
    }
    if (0 != m_shadingChannels[i])
    {
      Reference<tcAbstractPixelData> shadingData = m_shadingChannels[i]->portion(xy1[0], xy1[1], xy2[0], xy2[1]);
      m_shadingData[i] = dynamicCast<tcPixelData<float>*>(shadingData);
    }
    else
    {
      m_shadingData[i] = 0;
    }
  }

  /// @todo What if its a different size because the portion has changed?
  if (0 == m_elevationData || 0 == m_datum || changed)
  {
    delete m_elevationData;
    for (int i = 0; i < m_numDatasets; ++i)
    {
      delete m_data[i];
    }
    delete m_datum;

    // Create a new pixel buffer, initialised to altitude

    m_elevationData = new tcElevationData(width, height);
    // Find transform of elevation data
    tcGeo geoBase = geoAt(maths::Vector<2,float>(x1, y1));
    tcGeo geoDiagonal = geoAt(maths::Vector<2,float>(x1 + (x2-x1)/(width-1), y1 + (y2-y1)/(height-1))) - geoBase;
    m_elevationData->setGeoToPixels(tcAffineTransform2<double>(geoBase, geoDiagonal).inverse());

    m_datum = new tcTypedPixelData<PerPixelCache>(width, height);
    for (int i = 0; i < m_numDatasets; ++i)
    {
      m_data[i] = new tcTypedPixelData<PerDatasetPixelCache>(width, height);
    }

    int maxWidthHeight = qMax(width, height);
    double minXY12 = qMin(x2-x1, y2-y1);

    startProcessing("Caching elevation characteristics");
    for (int j = 0; j < height; ++j)
    {
      progress((float)j/(height-1));
      for (int i = 0; i < width; ++i)
      {
        int index = j*width + i;
        // Transform coordinates
        maths::Vector<2,float> coord      (x1 + (x2-x1)*i / (width  - 1),
                                           y1 + (y2-y1)*j / (height - 1));
        tcGeo                  geoCoord = geoAt(coord);

        PerPixelCache* cache = &m_datum->buffer()[index];

        // Find resolution
        maths::Vector<2,float> coordx      (x1 + (x2-x1)*(i+1) / (width  - 1),
                                            y1 + (y2-y1)*j     / (height - 1));
        maths::Vector<2,float> coordy      (x1 + (x2-x1)*i     / (width  - 1),
                                            y1 + (y2-y1)*(j+1) / (height - 1));
        tcGeo                  geoCoordx = geoAt(coordx) - geoCoord;
        tcGeo                  geoCoordy = geoAt(coordy) - geoCoord;
        // Find approx
        float res = geoCoordx.angle();
        cache->resolution[0] = res*cos(geoCoord.lat());
        cache->resolution[1] = geoCoordy.angle();
        cache->resolution *= 6378.137e3;

        // Get some elevation data
        bool accurate;
        float altitude        = altitudeAt(geoCoord, true, &accurate);
        m_elevationData->buffer()[index] = altitude;
        cache->originalElevation = altitude;
        cache->reliability = accurate ? 1 : 0;
        cache->accurateElevation = dem()[1].altitudeAt(geoCoord, true, 0);

        // Per dataset cache
        for (int ds = 0; ds < m_numDatasets; ++ds)
        {
          tcGeoImageData* imagery = m_datasets[ds];
          maths::Vector<3,float> light = lightDirectionAt(geoCoord, imagery);

          PerDatasetPixelCache* dsCache = &m_data[ds]->buffer()[index];
          dsCache->light = light;
          dsCache->lxy = light.slice<0,2>().mag();
          dsCache->dlz_dlxy = light[2]/dsCache->lxy;

          tcGeo lightScaled(light[0]*res/sin(geoCoord.lat()),
                            light[1]*res);
          tcGeo offset(geoCoord + lightScaled);
          for (int depthDirection = 0; depthDirection < 2; ++depthDirection)
          {
            tcGeo pos = geoCoord;
            tcGeo delta;
            if (depthDirection == TransitionEntrance)
            {
              delta = offset - geoCoord;
            }
            else
            {
              delta = geoCoord - offset;
            }
            delta = delta / 2;
            pos = pos + delta;
            maths::Vector<2,float> tex = coord;
            int transitionI = i;
            int transitionJ = j;
            while (transitionI >= 0 && transitionI < width && transitionJ >= 0 && transitionJ < height)
            {
              if (m_shadowData[ds]->sampleFloat((tex[0]-x1)/(x2-x1), (tex[1]-y1)/(y2-y1)) > 0.5f)
              {
                break;
              }
              transitionI = 0.5f + (tex[0]-x1)/(x2-x1)*(width -1);
              transitionJ = 0.5f + (tex[1]-y1)/(y2-y1)*(height-1);
              pos = pos + delta;
              tex = textureAt(pos);
            }
            dsCache->shadowTransition[depthDirection][0] = transitionI;
            dsCache->shadowTransition[depthDirection][1] = transitionJ;
            dsCache->shadowTransitionDistance[depthDirection] = tcGeo::angleBetween(pos,geoCoord)*6378.137e3;
          }
        }

        {
          /*
           * matrix L={Light row vec of shading channel i;..}
           * invert L
           * multiply by shading values for shading channels
           * should be mostly about the same magnitude
           * normalize
           */
          // Estimate a normal from the shading channels
          maths::Vector<3,float> totalN(0.0f, 0.0f, 0.01f);

          int ds[3];
          for (ds[0] = 0; ds[0] < m_numDatasets; ++ds[0])
          {
            if (0 == m_shadingData[ds[0]])
            {
              continue;
            }
            for (ds[1] = ds[0]+1; ds[1] < m_numDatasets; ++ds[1])
            {
              if (0 == m_shadingData[ds[1]])
              {
                continue;
              }
              for (ds[2] = ds[1]+1; ds[2] < m_numDatasets; ++ds[2])
              {
                if (0 == m_shadingData[ds[2]])
                {
                  continue;
                }
                maths::Matrix<3,float> L(m_data[ds[0]]->buffer()[index].light,
                                         m_data[ds[1]]->buffer()[index].light,
                                         m_data[ds[2]]->buffer()[index].light);
                maths::Vector<3,float> S;
                bool singular = false;
                L.invert(&singular);
                if (singular)
                {
                  continue;
                }
                for (int dsi = 0; dsi < 3; ++dsi)
                {
                  S[dsi] = m_shadingData[ds[dsi]]->sampleFloat((float)i/(width-1), (float)j/(height-1));
                }
                maths::Vector<3,float> N = S * L;
                N.normalize();
                totalN += N;
              }
            }
          }
          cache->normalEstimate = totalN;
          cache->normalEstimate.normalize();
        }
      }
    }
  }

  if (!m_loadingFromDem)
  {
    // Update elevation model
    startProcessing("Updating elevation model");
    dem()->updateFromElevationData(m_elevationData);
  }

  // Don't bother outputting anything
  //return new tcTypedPixelData<unsigned char>(1,1);

  // Downscale the elevation for viewing
  startProcessing("Downscaling for viewing");
  tcPixelData<GLubyte>* result = new tcPixelData<GLubyte>(width, height);
  //tcTypedPixelData<maths::Vector<3,float> >* result = new tcTypedPixelData<maths::Vector<3,float> >(width, height);
  Q_ASSERT(0 != result);

  int index = 0;
  for (int j = 0; j < height; ++j)
  {
    progress((float)j/(height-1));
    for (int i = 0; i < width; ++i)
    {
    //*
      PerPixelCache* cache = &m_datum->buffer()[index];

      result->buffer()[index] = 255 * cache->reliability;
      ++index;
      continue;

#if 0
      if (0)
      {
        float dh_dx = 0.0f;
        float dh_dy = 0.0f;
        if (i > 0 && i < width-1)
        {
          float hp1 = m_elevationData->buffer()[index+1];
          float hm1 = m_elevationData->buffer()[index-1];
          dh_dx = (hp1-hm1)/2 / cache->resolution[0];
        }
        if (j > 0 && j < height-1)
        {
          float hp1 = m_elevationData->buffer()[index+width];
          float hm1 = m_elevationData->buffer()[index-width];
          dh_dy = (hp1-hm1)/2 / cache->resolution[1];
        }
        // (0.0f, -1.0f, dh_dy) x (1.0f, 0.0f, dh_dx)
        maths::Vector<3,float> norm(-dh_dx, dh_dy, 1.0f);
        norm.normalize();
        result->buffer()[index] = norm;
      }
#endif

#if 0
      for (int ds = 0; ds < m_numDatasets; ++ds)
      {
        result->buffer()[index] = 0;
        PerDatasetPixelCache* dsCache = &m_data[ds]->buffer()[index];

        float hwm1 = m_elevationData->sampleFloat(
          ((float)i + dsCache->light[0]/cache->resolution[0])/(width-1),
          ((float)j - dsCache->light[1]/cache->resolution[1])/(height-1)
        );
        float hwp1 = m_elevationData->sampleFloat(
          ((float)i - dsCache->light[0]/cache->resolution[0])/(width-1),
          ((float)j + dsCache->light[1]/cache->resolution[1])/(height-1)
        );
        float dh_dw = (hwm1 - hwp1)/(2*dsCache->lxy);
        float facingness = dsCache->dlz_dlxy-dh_dw;
//         if (facingness < 0.0f)
//           result->buffer()[index] = 128.0f;

        result->buffer()[index] = 128.0f * facingness*facingness;

result->buffer()[index] = 0;

        bool isShadowed = (m_shadowData[ds]->sampleFloat((float)i/(width-1), (float)j/(height-1)) < 0.5f);
        bool isShadowEntrance = isShadowed &&
                                dsCache->shadowTransition[TransitionEntrance][0] == i &&
                                dsCache->shadowTransition[TransitionEntrance][1] == j;
        bool isShadowExit = isShadowed &&
                            dsCache->shadowTransition[TransitionExit][0] == i &&
                            dsCache->shadowTransition[TransitionExit][1] == j;
        float elevation = m_elevationData->buffer()[index];
        result->buffer()[index] = (elevation - 2500.0f)/1500.0f*255.0f;

        if (false && isShadowed)
        {
          // maximum elevation is between elevation at entrance and elevation at exit
          if (m_weights[BelowShadowLine] != 0.0f &&
              dsCache->shadowTransition[TransitionExit][0] >= 0 &&
              dsCache->shadowTransition[TransitionExit][0] < width &&
              dsCache->shadowTransition[TransitionExit][1] >= 0 &&
              dsCache->shadowTransition[TransitionExit][1] < height &&
              dsCache->shadowTransition[TransitionEntrance][0] >= 0 &&
              dsCache->shadowTransition[TransitionEntrance][0] < width &&
              dsCache->shadowTransition[TransitionEntrance][1] >= 0 &&
              dsCache->shadowTransition[TransitionEntrance][1] < height)
          {
            int exitIndex = width*dsCache->shadowTransition[TransitionExit][1] + dsCache->shadowTransition[TransitionExit][0];
            int entranceIndex = width*dsCache->shadowTransition[TransitionEntrance][1] + dsCache->shadowTransition[TransitionEntrance][0];
            float exitElevation = m_elevationData->buffer()[exitIndex];
            float entranceElevation = m_elevationData->buffer()[entranceIndex];
    
            result->buffer()[index] = qMax(0.0f, elevation - (exitElevation     * dsCache->shadowTransitionDistance[TransitionEntrance]
                                                             +entranceElevation * dsCache->shadowTransitionDistance[TransitionExit])
                                                           / (dsCache->shadowTransitionDistance[TransitionEntrance]
                                                             +dsCache->shadowTransitionDistance[TransitionExit]));
          }
        }

        result->buffer()[index] = 128.0f*m_shadingData[ds]->sampleFloat((float)i/(width-1), (float)j/(height-1));

#if 0
        int xdist = abs(dsCache->shadowTransition[TransitionEntrance][0] - i);
        int ydist = abs(dsCache->shadowTransition[TransitionEntrance][1] - j);
        bool bothIn = isShadowed && (dsCache->shadowTransition[TransitionExit][0] >= 0 &&
                dsCache->shadowTransition[TransitionExit][0] < width &&
                dsCache->shadowTransition[TransitionExit][1] >= 0 &&
                dsCache->shadowTransition[TransitionExit][1] < height &&
                dsCache->shadowTransition[TransitionEntrance][0] >= 0 &&
                dsCache->shadowTransition[TransitionEntrance][0] < width &&
                dsCache->shadowTransition[TransitionEntrance][1] >= 0 &&
                dsCache->shadowTransition[TransitionEntrance][1] < height);

        if (isShadowEntrance && (dsCache->shadowTransition[TransitionExit][0] >= 0 &&
                dsCache->shadowTransition[TransitionExit][0] < width &&
                dsCache->shadowTransition[TransitionExit][1] >= 0 &&
                dsCache->shadowTransition[TransitionExit][1] < height))
        {
          int exitIndex = width*dsCache->shadowTransition[TransitionExit][1] + dsCache->shadowTransition[TransitionExit][0];

          float exitElevation = m_elevationData->buffer()[exitIndex];
          float diff = exitElevation + dsCache->shadowTransitionDistance[TransitionExit]*dsCache->dlz_dlxy - elevation;
          result->buffer()[index] += diff/5000.0f*255.0f;
        }
        else
        {
          result->buffer()[index] = 0;
        }
#endif
      }
      // */

      //result->buffer()[index] = isShadowed ? qMin(255, 100 * (xdist + ydist)) : 255;
      /*result->buffer()[index] = (isShadowed && (dsCache->shadowTransition[TransitionExit][0] >= 0 &&
              dsCache->shadowTransition[TransitionExit][0] < width &&
              dsCache->shadowTransition[TransitionExit][1] >= 0 &&
              dsCache->shadowTransition[TransitionExit][1] < height))?255:0;*/
      //float mycostChange = 100000.0f*255.0f*costChange(i, j, +4.0f, 0);
      //result->buffer()[index] = 128.0f-mycostChange;
      //result->buffer()[index] = m_datum->buffer()[index].reliability*255;

#else

#if 0
      result->buffer()[index] = cache->normalEstimate;
#endif

#endif

      ++index;
    }
  }

  endProcessing();

  return result;
}

template <typename T>
T MySqr(T t)
{
  return t*t;
}

/*
 * Private functions
 */

#include <QtDebug>
/// Calculate the cost for a set of pixels.
float tcElevationOptimization::cost(int x1, int y1, int x2, int y2) const
{
  int width = m_elevationData->width();
  int height = m_elevationData->height();
  if (x1 < 0)
  {
    x1 = 0;
  }
  if (x2 >= width)
  {
    x2 = width-1;
  }
  if (y1 < 0)
  {
    y1 = 0;
  }
  if (y2 >= height)
  {
    y2 = height-1;
  }

  int maxWidthHeight = qMax(width, height);
  //double minXY12 = qMin(m_window.x2-m_window.x1, m_window.y2-m_window.y1);

  float costVariance = 0.0f;
  int countVariance = 0;
  float costRoughness = 0.0f;
  int countRoughness = 0;
  float costSteepness = 0.0f;
  int countSteepness = 0;
  float costLitFacing = 0.0f;
  int countLitFacing = 0;
  float costShading = 0.0f;
  int countShading = 0;
  float costStereo = 0.0f;
  int countStereo = 0;
  float costBelowShadowLine = 0.0f;
  int countBelowShadowLine = 0;
  float costTransitionElevTop = 0.0f;
  int countTransitionElevTop = 0;
  float costTransitionElevBottom = 0.0f;
  int countTransitionElevBottom = 0;
  float costTransitionTangential = 0.0f;
  int countTransitionTangential = 0;
  float costTransitionCurvingDown = 0.0f;
  int countTransitionCurvingDown = 0;
  for (int j = y1; j <= y2; ++j)
  {
    for (int i = x1; i <= x2; ++i)
    {
      float* elevationPtr = pixelElevation(i, j);
      if (0 == elevationPtr)
      {
        continue;
      }

      int index = j*width + i;

      // Basic data retrieval
      float elevation = *elevationPtr;
      PerPixelCache* cache = &m_datum->buffer()[index];
      if (m_voidsOnly && cache->reliability == 0)
      {
        continue;
      }

      // Gradient
      // Derivitive of gradient
      float dh_dx = 0.0f;
      float dh_dy = 0.0f;
      float d2h_dx2 = 0.0f;
      float d2h_dy2 = 0.0f;
      if (i > 0 && i < width-1)
      {
        float hp1 = m_elevationData->buffer()[index+1];
        float hm1 = m_elevationData->buffer()[index-1];
        d2h_dx2 = (hp1+hm1)/2 - elevation;
        dh_dx = (hp1-hm1)/2 / cache->resolution[0];
      }
      if (j > 0 && j < height-1)
      {
        float hp1 = m_elevationData->buffer()[index+width];
        float hm1 = m_elevationData->buffer()[index-width];
        d2h_dy2 = (hp1+hm1)/2 - elevation;
        dh_dy = (hp1-hm1)/2 / cache->resolution[1];
      }

      // Minimise variance from original value
      if (m_weights[Variance] != 0.0f)
      {
        // treat borderline pixels as reliable
        float reliability = cache->reliability;
        if (i < 5 || i >= width-5 || j < 5 || j >= height-5)
        {
          reliability = 10.0f;
        }
        costVariance += reliability * (elevation-cache->originalElevation)*(elevation-cache->originalElevation);
        ++countVariance;
      }

      // Minimise roughness
      if (m_weights[Roughness] != 0.0f)
      {
        costRoughness += (d2h_dx2*d2h_dx2 + d2h_dy2*d2h_dy2);
        ++countRoughness;
      }

      // Also minimise slope
      // This works really well at getting a nice slope
      if (m_weights[Steepness] != 0.0f)
      {
        costSteepness += (dh_dx*dh_dx + dh_dy*dh_dy);
        ++countSteepness;
      }

      // Photometric stereo
      if (cache->normalEstimate[2] > 0.2f && m_weights[PhotometricStereo] != 0.0f)
      {
        // (0.0f, -1.0f, dh_dy) x (1.0f, 0.0f, dh_dx)
        maths::Vector<3,float> norm(-dh_dx, dh_dy, 1.0f);
        norm.normalize();
        costStereo += MySqr(acos(cache->normalEstimate*norm));
        //costStereo += MySqr(dh_dx - cache->normalEstimate[0]/cache->normalEstimate[2]);
        //costStereo += MySqr(dh_dy - cache->normalEstimate[1]/cache->normalEstimate[2]);
        ++countStereo;
      }

      // Per dataset
      for (int ds = 0; ds < m_numDatasets; ++ds)
      {
        PerDatasetPixelCache* dsCache = &m_data[ds]->buffer()[index];

        bool isShadowed = (m_shadowData[ds]->sampleFloat((float)i/(width-1), (float)j/(height-1)) < 0.5f);
        bool isShadowEntrance = isShadowed &&
                                dsCache->shadowTransition[TransitionEntrance][0] == i &&
                                dsCache->shadowTransition[TransitionEntrance][1] == j;
        bool isShadowExit = isShadowed &&
                            dsCache->shadowTransition[TransitionExit][0] == i &&
                            dsCache->shadowTransition[TransitionExit][1] == j;

        // Calculate measures relating to the light direction
        // Gradient
        // Two sample points are 2*dsCache->lxy apart
        float hwm1 = m_elevationData->sampleFloat(
          ((float)i + dsCache->light[0]/cache->resolution[0])/(width-1),
          ((float)j - dsCache->light[1]/cache->resolution[1])/(height-1)
        );
        float hwp1 = m_elevationData->sampleFloat(
          ((float)i - dsCache->light[0]/cache->resolution[0])/(width-1),
          ((float)j + dsCache->light[1]/cache->resolution[1])/(height-1)
        );
        float dh_dw = (hwm1 - hwp1)/(2*dsCache->lxy);
        float d2h_dw2 = (hwm1+hwp1)/2 - elevation;
        float facingness = dsCache->dlz_dlxy-dh_dw;

        if (isShadowed)
        {
          // maximum elevation is between elevation at entrance and elevation at exit
          if (m_weights[BelowShadowLine] != 0.0f &&
              dsCache->shadowTransition[TransitionExit][0] >= 0 &&
              dsCache->shadowTransition[TransitionExit][0] < width &&
              dsCache->shadowTransition[TransitionExit][1] >= 0 &&
              dsCache->shadowTransition[TransitionExit][1] < height &&
              dsCache->shadowTransition[TransitionEntrance][0] >= 0 &&
              dsCache->shadowTransition[TransitionEntrance][0] < width &&
              dsCache->shadowTransition[TransitionEntrance][1] >= 0 &&
              dsCache->shadowTransition[TransitionEntrance][1] < height)
          {
            int exitIndex = width*dsCache->shadowTransition[TransitionExit][1] + dsCache->shadowTransition[TransitionExit][0];
            int entranceIndex = width*dsCache->shadowTransition[TransitionEntrance][1] + dsCache->shadowTransition[TransitionEntrance][0];
            float exitElevation = m_elevationData->buffer()[exitIndex];
            float entranceElevation = m_elevationData->buffer()[entranceIndex];

            costBelowShadowLine += MySqr(qMax(0.0f, elevation - (exitElevation     * dsCache->shadowTransitionDistance[TransitionEntrance]
                                                                +entranceElevation * dsCache->shadowTransitionDistance[TransitionExit])
                                                              / (dsCache->shadowTransitionDistance[TransitionEntrance]
                                                                +dsCache->shadowTransitionDistance[TransitionExit])));
            ++countBelowShadowLine;
          }
        }
        else
        {
          if (facingness < 0.0f && m_weights[LitFacing] != 0.0f)
          {
            // Lit pixels must face light
            costLitFacing += facingness*facingness;
            ++countLitFacing;
          }
          // Simple shape from shading
          if (0 != m_shadingData[ds] && m_weights[Shading] != 0.0f)
          {
            float intensity = m_shadingData[ds]->sampleFloat((float)i/(width-1), (float)j/(height-1));
            // intensity = cos(theta) = L.N
            // (0.0f, -1.0f, dh_dy) x (1.0f, 0.0f, dh_dx)
            maths::Vector<3,float> norm(-dh_dx, dh_dy, 1.0f);
            norm.normalize();
            costShading += MySqr(acos(qMin(1.0f, intensity)) - acos(qMin(1.0f, dsCache->light*norm)));
            ++countShading;
          }
        }
        if (isShadowEntrance)
        {
          if (m_weights[TransitionElevTop] != 0.0f &&
              dsCache->shadowTransition[TransitionExit][0] >= 0 &&
              dsCache->shadowTransition[TransitionExit][0] < width &&
              dsCache->shadowTransition[TransitionExit][1] >= 0 &&
              dsCache->shadowTransition[TransitionExit][1] < height)
          {
            int exitIndex = width*dsCache->shadowTransition[TransitionExit][1] + dsCache->shadowTransition[TransitionExit][0];
            float exitElevation = m_elevationData->buffer()[exitIndex];
            //if (dsCache->shadowTransitionDistance[TransitionExit]*dsCache->dlz_dlxy > 100.0f)
            {
              costTransitionElevTop += MySqr(exitElevation + dsCache->shadowTransitionDistance[TransitionExit]*dsCache->dlz_dlxy - elevation);
              ++countTransitionElevTop;
            }
          }
          // gradient tangential to sun direction
          if (m_weights[TransitionTangential] != 0.0f)
          {
            costTransitionTangential += MySqr(facingness);
            ++countTransitionTangential;
          }
          // second derivitive should be negative at entrance
          if (m_weights[TransitionCurvingDown] != 0.0f)
          {
            costTransitionCurvingDown += MySqr(qMax(0.0f, d2h_dw2));
            ++countTransitionCurvingDown;
          }
        }
        if (isShadowExit)
        {
          if (m_weights[TransitionElevBottom] != 0.0f &&
              dsCache->shadowTransition[TransitionEntrance][0] >= 0 &&
              dsCache->shadowTransition[TransitionEntrance][0] < width &&
              dsCache->shadowTransition[TransitionEntrance][1] >= 0 &&
              dsCache->shadowTransition[TransitionEntrance][1] < height)
          {
            int entranceIndex = width*dsCache->shadowTransition[TransitionEntrance][1] + dsCache->shadowTransition[TransitionEntrance][0];
            float entranceElevation = m_elevationData->buffer()[entranceIndex];
            // this has less effect, its usually "more" right
            /// @todo Smooth high reliability into low reliability
            costTransitionElevBottom += 1.0f * MySqr(entranceElevation - dsCache->shadowTransitionDistance[TransitionEntrance]*dsCache->dlz_dlxy - elevation);
            ++countTransitionElevBottom;
          }
        }
      }
    }
  }

  return m_weights[Variance]*costVariance//countVariance
       + m_weights[Roughness]*costRoughness//countRoughness
       + m_weights[Steepness]*costSteepness//countSteepness
       + m_weights[LitFacing]*costLitFacing//countLitFacing
       + m_weights[Shading]*costShading//countShading
       + m_weights[PhotometricStereo]*costStereo//countStereo
       + m_weights[BelowShadowLine]*costBelowShadowLine//countBelowShadowLine
       + m_weights[TransitionElevTop]*costTransitionElevTop//countTransitionElevTop
       + m_weights[TransitionElevBottom]*costTransitionElevBottom//countTransitionElevBottom
       + m_weights[TransitionTangential]*costTransitionTangential//countTransitionTangential
       + m_weights[TransitionCurvingDown]*costTransitionCurvingDown//countTransitionCurvingDown
       ;
}

/*
 * f(0) = 1
 * f(range) = small
 * f(x) = exp[ - (2*x/range)^2 ]
 */
inline float elevationFalloffFunction(float xsq)
{
  return qMax(0.0f, 1.0f - xsq);
  //return exp(-4*xsq);
}
/*
 */

/// Calculate the relative cost of changing a pixel's elevation.
float tcElevationOptimization::costChange(int x, int y, float dh, int range)
{
  // Override range for now
#define ELEV_RANGE 4
  range = qMin(range, ELEV_RANGE);
  static const int effectiveRange = 1 + range;
  float oldElevations[1+2*ELEV_RANGE][1+2*ELEV_RANGE];

  // Assume a single change in elevation only affects cost of pixels to effectiveRange from it.
  // Now technically this doesn't hold for shadow edges as all pixels across the shadow can be affected.
  // If the pixel is a shadow edge, take into account the cost of extra pixels.
  float currentCost = cost(x-effectiveRange, y-effectiveRange, x+effectiveRange, y+effectiveRange);
  for (int i = 0; i < 1+range*2; ++i)
  {
    int di = i - range;
    for (int j = 0; j < 1+range*2; ++j)
    {
      int dj = j - range;
      float* elev = pixelElevation(x+di, y+dj);
      if (0 != elev && (!m_voidsOnly || 0 != m_datum->buffer()[j*m_datum->width() + i].reliability))
      {
        oldElevations[i][j] = *elev;
        *elev += dh*elevationFalloffFunction(float(di*di + dj*dj) / ((range+1)*(range+1)));
      }
    }
  }
  float newCost = cost(x-effectiveRange, y-effectiveRange, x+effectiveRange, y+effectiveRange);
  for (int i = 0; i < 1+range*2; ++i)
  {
    int di = i - range;
    for (int j = 0; j < 1+range*2; ++j)
    {
      int dj = j - range;
      float* elev = pixelElevation(x+di, y+dj);
      if (0 != elev && (!m_voidsOnly || 0 != m_datum->buffer()[j*m_datum->width() + i].reliability))
      {
        *elev = oldElevations[i][j];
      }
    }
  }
  return newCost - currentCost;
}

/// Optimize a whole range of elevations.
void tcElevationOptimization::optimizeElevations(int x1, int y1, int x2, int y2, float dh, int range)
{
  for (int j = y1; j <= y2; ++j)
  {
    for (int i = x1; i <= x2; ++i)
    {
      if (m_stopProcessing)
      {
        return;
      }
      int index = j*m_datum->width() + i;
      //while (true)
      if (!m_voidsOnly || 0 != m_datum->buffer()[j*m_datum->width() + i].reliability)
      {
        // Find the cost of adjusting the elevation by dh in each direction
        float costInc = costChange(i,j, dh, range);
        float costDec = costChange(i,j,-dh, range);
        if (costInc < 0.0f && costInc < costDec)
        {
          adjustElevation(i,j,dh, range);
        }
        else if (costDec < 0.0f)
        {
          adjustElevation(i,j,-dh, range);
        }
        else
        {
          //break;
        }
      }
    }
  }
}

/// Get pointer to elevation at a pixel.
float* tcElevationOptimization::pixelElevation(int x, int y) const
{
  if (x < 0 || x >= m_elevationData->width() ||
      y < 0 || y >= m_elevationData->height())
  {
    return 0;
  }
  int index = y*m_elevationData->width() + x;
  return &(m_elevationData->buffer()[index]);
}

/// Adjust elevation at a pixel.
void tcElevationOptimization::adjustElevation(int x, int y, float dh, int range)
{
  range = qMin(range, ELEV_RANGE);
  for (int i = 0; i < 1+range*2; ++i)
  {
    int di = i - range;
    for (int j = 0; j < 1+range*2; ++j)
    {
      int dj = j - range;
      float* elev = pixelElevation(x+di, y+dj);
      if (0 != elev)
      {
        *elev += dh*elevationFalloffFunction(float(di*di + dj*dj) / ((range+1)*(range+1)));
      }
    }
  }
}