// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void AlignSectionsMutualInformation::find_shifts(std::vector<int64_t>& xshifts, std::vector<int64_t>& yshifts)
{
  DataContainer::Pointer m = getDataContainerArray()->getDataContainer(getDataContainerName());

  int64_t totalPoints = m->getAttributeMatrix(getCellAttributeMatrixName())->getNumTuples();
  Int32ArrayType::Pointer p = Int32ArrayType::CreateArray((totalPoints * 1), "_INTERNAL_USE_ONLY_MIFeatureIds");
  m_FeatureIds = p->getPointer(0);

  std::ofstream outFile;
  if (getWriteAlignmentShifts() == true)
  {
    outFile.open(getAlignmentShiftFileName().toLatin1().data());
  }

  size_t udims[3] = { 0, 0, 0 };
  m->getGeometryAs<ImageGeom>()->getDimensions(udims);
#if (CMP_SIZEOF_SIZE_T == 4)
  typedef int32_t DimType;
#else
  typedef int64_t DimType;
#endif
  DimType dims[3] =
  {
    static_cast<DimType>(udims[0]),
    static_cast<DimType>(udims[1]),
    static_cast<DimType>(udims[2]),
  };

  float disorientation = 0.0f;
  float mindisorientation = std::numeric_limits<float>::max();
  float** mutualinfo12 = NULL;
  float* mutualinfo1 = NULL;
  float* mutualinfo2 = NULL;
  int32_t featurecount1 = 0, featurecount2 = 0;
  int64_t newxshift = 0;
  int64_t newyshift = 0;
  int64_t oldxshift = 0;
  int64_t oldyshift = 0;
  float count = 0.0f;
  DimType slice = 0;

  int32_t refgnum = 0, curgnum = 0;
  DimType refposition = 0;
  DimType curposition = 0;

  form_features_sections();

  std::vector<std::vector<float> >  misorients;
  misorients.resize(dims[0]);
  for (DimType a = 0; a < dims[0]; a++)
  {
    misorients[a].assign(dims[1], 0.0f);
  }

  for (DimType iter = 1; iter < dims[2]; iter++)
  {
    QString ss = QObject::tr("Aligning Sections || Determining Shifts || %1% Complete").arg(((float)iter / dims[2]) * 100);
    notifyStatusMessage(getMessagePrefix(), getHumanLabel(), ss);
    mindisorientation = std::numeric_limits<float>::max();
    slice = (dims[2] - 1) - iter;
    featurecount1 = featurecounts[slice];
    featurecount2 = featurecounts[slice + 1];
    mutualinfo12 = new float *[featurecount1];
    mutualinfo1 = new float[featurecount1];
    mutualinfo2 = new float[featurecount2];

    for (int32_t a = 0; a < featurecount1; a++)
    {
      mutualinfo1[a] = 0.0f;
      mutualinfo12[a] = new float[featurecount2];
      for (int32_t b = 0; b < featurecount2; b++)
      {
        mutualinfo12[a][b] = 0.0f;
        mutualinfo2[b] = 0.0f;
      }
    }
    oldxshift = -1;
    oldyshift = -1;
    newxshift = 0;
    newyshift = 0;
    for (DimType a = 0; a < dims[0]; a++)
    {
      for (DimType b = 0; b < dims[1]; b++)
      {
        misorients[a][b] = 0;
      }
    }
    while (newxshift != oldxshift || newyshift != oldyshift)
    {
      oldxshift = newxshift;
      oldyshift = newyshift;
      for (int32_t j = -3; j < 4; j++)
      {
        for (int32_t k = -3; k < 4; k++)
        {
          disorientation = 0;
          count = 0;
          if (misorients[k + oldxshift + dims[0] / 2][j + oldyshift + dims[1] / 2] == 0 && abs(k + oldxshift) < (dims[0] / 2)
              && (j + oldyshift) < (dims[1] / 2))
          {
            for (DimType l = 0; l < dims[1]; l = l + 4)
            {
              for (DimType n = 0; n < dims[0]; n = n + 4)
              {
                if ((l + j + oldyshift) >= 0 && (l + j + oldyshift) < dims[1] && (n + k + oldxshift) >= 0 && (n + k + oldxshift) < dims[0])
                {
                  refposition = ((slice + 1) * dims[0] * dims[1]) + (l * dims[0]) + n;
                  curposition = (slice * dims[0] * dims[1]) + ((l + j + oldyshift) * dims[0]) + (n + k + oldxshift);
                  refgnum = m_FeatureIds[refposition];
                  curgnum = m_FeatureIds[curposition];
                  if (curgnum >= 0 && refgnum >= 0)
                  {
                    mutualinfo12[curgnum][refgnum]++;
                    mutualinfo1[curgnum]++;
                    mutualinfo2[refgnum]++;
                    count++;
                  }
                }
                else
                {
                  mutualinfo12[0][0]++;
                  mutualinfo1[0]++;
                  mutualinfo2[0]++;
                }
              }
            }
            float ha = 0.0f;
            float hb = 0.0f;
            float hab = 0.0f;
            for (int32_t b = 0; b < featurecount1; b++)
            {
              mutualinfo1[b] = mutualinfo1[b] / count;
              if (mutualinfo1[b] != 0) { ha = ha + mutualinfo1[b] * logf(mutualinfo1[b]); }
            }
            for (int32_t c = 0; c < featurecount2; c++)
            {
              mutualinfo2[c] = mutualinfo2[c] / float(count);
              if (mutualinfo2[c] != 0) { hb = hb + mutualinfo2[c] * logf(mutualinfo2[c]); }
            }
            for (int32_t b = 0; b < featurecount1; b++)
            {
              for (int32_t c = 0; c < featurecount2; c++)
              {
                mutualinfo12[b][c] = mutualinfo12[b][c] / count;
                if (mutualinfo12[b][c] != 0) { hab = hab + mutualinfo12[b][c] * logf(mutualinfo12[b][c]); }
                float value = 0.0f;
                if (mutualinfo1[b] > 0 && mutualinfo2[c] > 0) { value = (mutualinfo12[b][c] / (mutualinfo1[b] * mutualinfo2[c])); }
                if (value != 0) { disorientation = disorientation + (mutualinfo12[b][c] * logf(value)); }
              }
            }
            for (int32_t b = 0; b < featurecount1; b++)
            {
              for (int32_t c = 0; c < featurecount2; c++)
              {
                mutualinfo12[b][c] = 0.0f;
                mutualinfo1[b] = 0.0f;
                mutualinfo2[c] = 0.0f;
              }
            }
            disorientation = 1.0f / disorientation;
            misorients[k + oldxshift + dims[0] / 2][j + oldyshift + dims[1] / 2] = disorientation;
            if (disorientation < mindisorientation)
            {
              newxshift = k + oldxshift;
              newyshift = j + oldyshift;
              mindisorientation = disorientation;
            }
          }
        }
      }
    }
    xshifts[iter] = xshifts[iter - 1] + newxshift;
    yshifts[iter] = yshifts[iter - 1] + newyshift;
    if (getWriteAlignmentShifts() == true)
    {
      outFile << slice << "	" << slice + 1 << "	" << newxshift << "	" << newyshift << "	" << xshifts[iter] << "	" << yshifts[iter] << "\n";
    }
    delete[] mutualinfo1;
    delete[] mutualinfo2;
    for (int32_t i = 0; i < featurecount1; i++)
    {
      delete mutualinfo12[i];
    }
    delete[] mutualinfo12;
    mutualinfo1 = NULL;
    mutualinfo2 = NULL;
    mutualinfo12 = NULL;
  }

  m->getAttributeMatrix(getCellAttributeMatrixName())->removeAttributeArray(DREAM3D::CellData::FeatureIds);

  if (getWriteAlignmentShifts() == true)
  {
    outFile.close();
  }
}
// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void AdaptiveAlignmentMutualInformation::find_shifts(std::vector<int64_t>& xshifts, std::vector<int64_t>& yshifts, std::vector<float>& xneedshifts, std::vector<float>& yneedshifts)
{
	DataContainer::Pointer m = getDataContainerArray()->getDataContainer(getDataContainerName());

	int64_t totalPoints = m->getAttributeMatrix(getCellAttributeMatrixName())->getNumTuples();
	m_MIFeaturesPtr = Int32ArrayType::CreateArray((totalPoints * 1), "_INTERNAL_USE_ONLY_MIFeatureIds");
	m_MIFeaturesPtr->initializeWithZeros();
	int32_t* miFeatureIds = m_MIFeaturesPtr->getPointer(0);

	size_t udims[3] = { 0, 0, 0 };
	m->getGeometryAs<ImageGeom>()->getDimensions(udims);

	uint64_t dims[3] =
	{
		static_cast<uint64_t>(udims[0]),
		static_cast<uint64_t>(udims[1]),
		static_cast<uint64_t>(udims[2]),
	};

	uint64_t maxstoredshifts = 1;
	if (xneedshifts.size() > 0) maxstoredshifts = 20;

	float disorientation = 0.0f;

	std::vector<std::vector<int64_t>>  newxshift(dims[2]);
	std::vector<std::vector<int64_t>>  newyshift(dims[2]);
	std::vector<std::vector<float>>  mindisorientation(dims[2]);
	for (uint64_t a = 1; a < dims[2]; a++)
	{
		newxshift[a].resize(maxstoredshifts, 0);
		newyshift[a].resize(maxstoredshifts, 0);
		mindisorientation[a].resize(maxstoredshifts, std::numeric_limits<float>::max());
	}

	float** mutualinfo12 = NULL;
	float* mutualinfo1 = NULL;
	float* mutualinfo2 = NULL;
	int32_t featurecount1 = 0, featurecount2 = 0;
	int64_t oldxshift = 0;
	int64_t oldyshift = 0;
	float count = 0.0f;
	uint64_t slice = 0;

	int32_t refgnum = 0, curgnum = 0;
	uint64_t refposition = 0;
	uint64_t curposition = 0;

	form_features_sections();

	// Allocate a 2D Array which will be reused from slice to slice
	// second dimension is assigned in each cycle separately
	std::vector<std::vector<bool> >  misorients(dims[0]);

	const uint64_t halfDim0 = static_cast<uint64_t>(dims[0] * 0.5f);
	const uint64_t halfDim1 = static_cast<uint64_t>(dims[1] * 0.5f);
	uint64_t progInt = 0;

	for (uint64_t iter = 1; iter < dims[2]; iter++)
	{
		progInt = static_cast<uint64_t>(iter * 100 / static_cast<float>(dims[2]));
		QString ss = QObject::tr("Aligning Anisotropic Sections || Determining Shifts || %1% Complete").arg(progInt);
		notifyStatusMessage(getMessagePrefix(), getHumanLabel(), ss);

		slice = (dims[2] - 1) - iter;
		featurecount1 = featurecounts[slice];
		featurecount2 = featurecounts[slice + 1];
		mutualinfo12 = new float *[featurecount1];
		mutualinfo1 = new float[featurecount1];
		mutualinfo2 = new float[featurecount2];

		for (int32_t a = 0; a < featurecount1; a++)
		{
			mutualinfo1[a] = 0.0f;
			mutualinfo12[a] = new float[featurecount2];
			for (int32_t b = 0; b < featurecount2; b++)
			{
				mutualinfo12[a][b] = 0.0f;
				mutualinfo2[b] = 0.0f;
			}
		}
		oldxshift = -1;
		oldyshift = -1;

		for (uint64_t i = 0; i < dims[0]; i++)
		{
			misorients[i].assign(dims[1], false);
		}

		while (newxshift[iter][0] != oldxshift || newyshift[iter][0] != oldyshift)
		{
			oldxshift = newxshift[iter][0];
			oldyshift = newyshift[iter][0];

			for (int32_t j = -3; j < 4; j++)
			{
				for (int32_t k = -3; k < 4; k++)
				{
					disorientation = 0;
					count = 0;
					if (llabs(k + oldxshift) < halfDim0 && llabs(j + oldyshift) < halfDim1 && misorients[k + oldxshift + halfDim0][j + oldyshift + halfDim1] == false)
					{
						for (uint64_t l = 0; l < dims[1]; l = l + 4)
						{
							for (uint64_t n = 0; n < dims[0]; n = n + 4)
							{
								if ((l + j + oldyshift) >= 0 && (l + j + oldyshift) < dims[1] && (n + k + oldxshift) >= 0 && (n + k + oldxshift) < dims[0])
								{
									refposition = ((slice + 1) * dims[0] * dims[1]) + (l * dims[0]) + n;
									curposition = (slice * dims[0] * dims[1]) + ((l + j + oldyshift) * dims[0]) + (n + k + oldxshift);
									refgnum = miFeatureIds[refposition];
									curgnum = miFeatureIds[curposition];
									if (curgnum >= 0 && refgnum >= 0)
									{
										mutualinfo12[curgnum][refgnum]++;
										mutualinfo1[curgnum]++;
										mutualinfo2[refgnum]++;
										count++;
									}
								}
								else
								{
									mutualinfo12[0][0]++;
									mutualinfo1[0]++;
									mutualinfo2[0]++;
								}
							}
						}
						float ha = 0.0f;
						float hb = 0.0f;
						float hab = 0.0f;
						for (int32_t b = 0; b < featurecount1; b++)
						{
							mutualinfo1[b] = mutualinfo1[b] / count;
							if (mutualinfo1[b] != 0) { ha = ha + mutualinfo1[b] * logf(mutualinfo1[b]); }
						}
						for (int32_t c = 0; c < featurecount2; c++)
						{
							mutualinfo2[c] = mutualinfo2[c] / float(count);
							if (mutualinfo2[c] != 0) { hb = hb + mutualinfo2[c] * logf(mutualinfo2[c]); }
						}
						for (int32_t b = 0; b < featurecount1; b++)
						{
							for (int32_t c = 0; c < featurecount2; c++)
							{
								mutualinfo12[b][c] = mutualinfo12[b][c] / count;
								if (mutualinfo12[b][c] != 0) { hab = hab + mutualinfo12[b][c] * logf(mutualinfo12[b][c]); }
								float value = 0.0f;
								if (mutualinfo1[b] > 0 && mutualinfo2[c] > 0) { value = (mutualinfo12[b][c] / (mutualinfo1[b] * mutualinfo2[c])); }
								if (value != 0) { disorientation = disorientation + (mutualinfo12[b][c] * logf(value)); }
							}
						}
						for (int32_t b = 0; b < featurecount1; b++)
						{
							for (int32_t c = 0; c < featurecount2; c++)
							{
								mutualinfo12[b][c] = 0.0f;
								mutualinfo1[b] = 0.0f;
								mutualinfo2[c] = 0.0f;
							}
						}
						disorientation = 1.0f / disorientation;
						misorients[k + oldxshift + halfDim0][j + oldyshift + halfDim1] = true;

						// compare the new shift with currently stored ones
						int64_t s = maxstoredshifts;
						while (s - 1 >= 0 && disorientation < mindisorientation[iter][s - 1])
						{
							s--;
						}

						// new shift is stored with index 's' in the arrays
						if (s < maxstoredshifts)
						{
							// lag the shifts already stored
							for (int64_t t = maxstoredshifts - 1; t > s; t--)
							{
								newxshift[iter][t] = newxshift[iter][t - 1];
								newyshift[iter][t] = newyshift[iter][t - 1];
								mindisorientation[iter][t] = mindisorientation[iter][t - 1];
							}
							// store the new shift
							newxshift[iter][s] = k + oldxshift;
							newyshift[iter][s] = j + oldyshift;
							mindisorientation[iter][s] = disorientation;
						}
					}
				}
			}
		}
		xshifts[iter] = xshifts[iter - 1] + newxshift[iter][0];
		yshifts[iter] = yshifts[iter - 1] + newyshift[iter][0];

		delete[] mutualinfo1;
		delete[] mutualinfo2;
		for (int32_t i = 0; i < featurecount1; i++)
		{
			delete mutualinfo12[i];
		}
		delete[] mutualinfo12;
		mutualinfo1 = NULL;
		mutualinfo2 = NULL;
		mutualinfo12 = NULL;
	}

	std::vector<uint64_t> curindex(dims[2], 0);

	// find corrected shifts
	if (xneedshifts.size() > 0)
	{
		QString ss = QObject::tr("Aligning Anisotropic Sections || Correcting shifts");
		notifyStatusMessage(getMessagePrefix(), getHumanLabel(), ss);

		std::vector<float> changedisorientation(dims[2], 0);
		std::vector<uint64_t> changeindex(dims[2], 0);
		std::vector<float> changeerror(dims[2], 0);

		std::vector<float> xshiftsest;	// cumulative x-shifts estimated from SEM images
		std::vector<float> yshiftsest;	// cumulative y-shifts estimated from SEM images

		float curerror = 0;
		float tolerance = 1.0f / static_cast<float>(dims[2]);

		// evaluate error between current shifts and desired shifts
		if (xneedshifts.size() == 1)      // error is computed as misagreement between slopes
		{
			curerror = compute_error1(dims[2], 0, xneedshifts[0], yneedshifts[0], newxshift, newyshift, curindex);
		}
		else if (xneedshifts.size() > 1)  // error is computed as misagreement with shifts estimated from SEM images
		{
			xshiftsest.resize(dims[2], 0);
			yshiftsest.resize(dims[2], 0);
			for (uint64_t iter = 1; iter < dims[2]; iter++)
			{
				xshiftsest[iter] = xshiftsest[iter - 1] + xneedshifts[iter - 1];
				yshiftsest[iter] = yshiftsest[iter - 1] + yneedshifts[iter - 1];
			}
			curerror = compute_error2(dims[2], 0, xshiftsest, yshiftsest, newxshift, newyshift, curindex);
		}

		// iterative selection of a candidate shift, recomputing of current candidates, evaluation of error
		if (curerror > tolerance)
		{
			float minchangedisorientation = 0;
			float minchangeerror = 0;
			int64_t minchangeindex = 0;
			int64_t minchangeiter = 0;
			float olderror = 0;
			float newerror = 0;
			uint64_t progInt = 0;

			do
			{
				QString ss = QObject::tr("Aligning Anisotropic Sections || Correcting Shifts || Iteration %1").arg(++progInt);;
				notifyStatusMessage(getMessagePrefix(), getHumanLabel(), ss);
				if (getCancel() == true)
				{
					return;
				}

				olderror = curerror;
				for (uint64_t iter = 1; iter < dims[2]; iter++)
				{
					float newminerror = std::numeric_limits<float>::max();
					float newmindisorientation = std::numeric_limits<float>::max();
					uint64_t newminindex = 0;
					for (uint64_t index = curindex[iter] + 1; index < maxstoredshifts; index++)
					{
						// recompute error for the configuration with this candidate changed
						if (xneedshifts.size() == 1)
						{
							newerror = compute_error1(iter, index, xneedshifts[0], yneedshifts[0], newxshift, newyshift, curindex);
						}
						else if (xneedshifts.size() > 1)
						{
							newerror = compute_error2(iter, index, xshiftsest, yshiftsest, newxshift, newyshift, curindex);
						}

						// compare the new error with the best current error
						if (newerror < curerror &&
							mindisorientation[iter][index] / mindisorientation[iter][0] < newmindisorientation)
						{
							newminerror = newerror;
							newminindex = index;
							newmindisorientation = mindisorientation[iter][index] / mindisorientation[iter][0];
						}
					}
					// assign best error, corresponding index and disorientation value for this slice
					changeerror[iter] = newminerror;
					changeindex[iter] = newminindex;
					changedisorientation[iter] = newmindisorientation;
				}

				// among all slices, find the best candidate (with minimum disorientation change)
				minchangedisorientation = std::numeric_limits<float>::max() - 1;
				minchangeerror = std::numeric_limits<float>::max();
				minchangeindex = 0;
				minchangeiter = 0;
				for (uint64_t iter = 1; iter < dims[2]; iter++)
				{
					if (changeerror[iter] < curerror &&
						(changedisorientation[iter] < minchangedisorientation ||
						(changedisorientation[iter] == minchangedisorientation &&
						llabs(newxshift[iter][changeindex[iter]]) + llabs(newyshift[iter][changeindex[iter]]) < llabs(newxshift[iter][minchangeindex]) + llabs(newyshift[iter][minchangeindex]))))
					{
						minchangeiter = iter;
						minchangeindex = changeindex[iter];
						minchangedisorientation = changedisorientation[iter];
						minchangeerror = changeerror[iter];
					}
				}

				if (minchangeerror < curerror && minchangeerror >= tolerance)
				{
					// assign the best candidate
					changedisorientation[minchangeiter] = minchangedisorientation;
					curindex[minchangeiter] = minchangeindex;
					// reassign current error
					curerror = minchangeerror;
				}

			} while (minchangedisorientation < std::numeric_limits<float>::max() - 1 && curerror < olderror && curerror > tolerance);
		}
	}


	if (getWriteAlignmentShifts() == true)
	{
		std::ofstream outFile;
		outFile.open(getAlignmentShiftFileName().toLatin1().data());
		for (uint64_t iter = 1; iter < dims[2]; iter++)
		{
			slice = (dims[2] - 1) - iter;
			xshifts[iter] = xshifts[iter - 1] + newxshift[iter][curindex[iter]];
			yshifts[iter] = yshifts[iter - 1] + newyshift[iter][curindex[iter]];
			outFile << slice << "	" << slice + 1 << "	" << newxshift[iter][curindex[iter]] << "	" << newyshift[iter][curindex[iter]] << "	" << xshifts[iter] << "	" << yshifts[iter] << "\n";
		}
		outFile.close();
	}

	m->getAttributeMatrix(getCellAttributeMatrixName())->removeAttributeArray(DREAM3D::CellData::FeatureIds);

}