// SPDX-FileCopyrightText: Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
// SPDX-FileCopyrightText: Copyright (c) 2015 Greg Fiumara
// SPDX-License-Identifier: BSD-3-Clause AND MIT
#include "vtkCIEDE2000.h"

#include <algorithm> // std::min, std::max
#include <array>
#include <deque>
#include <limits>
#include <set>
#include <utility> // std::pair, std::make_pair
#include <vtkMath.h>

namespace CIEDE2000
{
VTK_ABI_NAMESPACE_BEGIN

//------------------------------------------------------------------------------
static constexpr int COLORSPACE_SIZE_X = 17;
static constexpr int COLORSPACE_SIZE_Y = 17;
static constexpr int COLORSPACE_SIZE_Z = 17;

static constexpr int NEIGHBORHOOD_SIZE_X = 1;
static constexpr int NEIGHBORHOOD_SIZE_Y = 1;
static constexpr int NEIGHBORHOOD_SIZE_Z = 1;

typedef int PositionComponent;
typedef std::array<PositionComponent, 3> Position;
typedef double Distance;

//------------------------------------------------------------------------------
inline static void getPosition(const double rgb[3], Position& pos)
{
  pos[0] = static_cast<PositionComponent>(rgb[0] * (COLORSPACE_SIZE_X - 1) + 0.5);
  pos[1] = static_cast<PositionComponent>(rgb[1] * (COLORSPACE_SIZE_Y - 1) + 0.5);
  pos[2] = static_cast<PositionComponent>(rgb[2] * (COLORSPACE_SIZE_Z - 1) + 0.5);
}

//------------------------------------------------------------------------------
inline static void getRGBColor(const Position& pos, double rgb[3])
{
  rgb[0] = pos[0] / static_cast<double>(COLORSPACE_SIZE_X - 1);
  rgb[1] = pos[1] / static_cast<double>(COLORSPACE_SIZE_Y - 1);
  rgb[2] = pos[2] / static_cast<double>(COLORSPACE_SIZE_Z - 1);
}

//------------------------------------------------------------------------------
void MapColor(double rgb[3])
{
  Position pos;
  getPosition(rgb, pos);
  getRGBColor(pos, rgb);
}

//------------------------------------------------------------------------------
inline static void getLabColor(const Position& pos, double _lab[3])
{
  double rgb[3];
  getRGBColor(pos, rgb);

  vtkMath::RGBToLab(rgb, _lab);
}

//------------------------------------------------------------------------------
inline static int getIndex(const Position& pos)
{
  return pos[0] + COLORSPACE_SIZE_X * (pos[1] + COLORSPACE_SIZE_Y * pos[2]);
}

//------------------------------------------------------------------------------
double GetCIEDeltaE2000(const double lab1[3], const double lab2[3])
{
  // The three constants used in the CIEDE2000 measure
  static const double k_L = 1.0;
  static const double k_C = 1.0;
  static const double k_H = 1.0;

  // Calculate and return Delta E

  double C1 = std::sqrt((lab1[1] * lab1[1]) + (lab1[2] * lab1[2]));
  double C2 = std::sqrt((lab2[1] * lab2[1]) + (lab2[2] * lab2[2]));

  double barC = 0.5 * (C1 + C2);

  double G =
    0.5 * (1.0 - std::sqrt(std::pow(barC, 7.0) / (std::pow(barC, 7.0) + std::pow(25.0, 7.0))));

  double a1Prime = (1.0 + G) * lab1[1];
  double a2Prime = (1.0 + G) * lab2[1];

  double CPrime1 = std::sqrt((a1Prime * a1Prime) + (lab1[2] * lab1[2]));
  double CPrime2 = std::sqrt((a2Prime * a2Prime) + (lab2[2] * lab2[2]));

  double hPrime1;
  if ((lab1[2] == 0.0) && (a1Prime == 0.0))
  {
    hPrime1 = 0.0;
  }
  else
  {
    hPrime1 = std::atan2(lab1[2], a1Prime);
    if (hPrime1 < 0.0)
    {
      hPrime1 += 2.0 * vtkMath::Pi();
    }
  }

  double hPrime2;
  if ((lab2[2] == 0.0) && (a2Prime == 0.0))
  {
    hPrime2 = 0.0;
  }
  else
  {
    hPrime2 = std::atan2(lab2[2], a2Prime);
    if (hPrime2 < 0.0)
    {
      hPrime2 += 2.0 * vtkMath::Pi();
    }
  }

  double deltaLPrime = lab2[0] - lab1[0];

  double deltaCPrime = CPrime2 - CPrime1;

  double CPrimeProduct = CPrime1 * CPrime2;

  double deltahPrime;
  if (CPrimeProduct == 0.0)
  {
    deltahPrime = 0.0;
  }
  else
  {
    deltahPrime = hPrime2 - hPrime1;

    if (deltahPrime < -vtkMath::Pi())
    {
      deltahPrime += 2.0 * vtkMath::Pi();
    }
    else if (deltahPrime > vtkMath::Pi())
    {
      deltahPrime -= 2.0 * vtkMath::Pi();
    }
  }

  double deltaHPrime = 2.0 * std::sqrt(CPrimeProduct) * std::sin(0.5 * deltahPrime);

  double barLPrime = 0.5 * (lab1[0] + lab2[0]);

  double barCPrime = 0.5 * (CPrime1 + CPrime2);

  double hPrimeSum = hPrime1 + hPrime2;

  double barhPrime;
  if (CPrime1 * CPrime2 == 0.0)
  {
    barhPrime = hPrimeSum;
  }
  else
  {
    if (std::fabs(hPrime1 - hPrime2) <= vtkMath::Pi())
    {
      barhPrime = 0.5 * hPrimeSum;
    }
    else
    {
      if (hPrimeSum < 2.0 * vtkMath::Pi())
      {
        barhPrime = 0.5 * (hPrimeSum + 2.0 * vtkMath::Pi());
      }
      else
      {
        barhPrime = 0.5 * (hPrimeSum - 2.0 * vtkMath::Pi());
      }
    }
  }

  double T = 1.0 - 0.17 * std::cos(barhPrime - (vtkMath::Pi() * 30.0 / 180.0)) +
    0.24 * std::cos(2.0 * barhPrime) +
    0.32 * std::cos(3.0 * barhPrime + (vtkMath::Pi() * 6.0 / 180.0)) -
    0.20 * std::cos(4.0 * barhPrime - (vtkMath::Pi() * 63.0 / 180.0));

  double deltaTheta = (vtkMath::Pi() * 30.0 / 180.0) *
    std::exp(-std::pow(
      (barhPrime - (vtkMath::Pi() * 275.0 / 180.0)) / (vtkMath::Pi() * 25.0 / 180.0), 2.0));

  double R_C =
    2.0 * std::sqrt(std::pow(barCPrime, 7.0) / (std::pow(barCPrime, 7.0) + std::pow(25.0, 7.0)));

  double S_L =
    1.0 + (0.015 * pow(barLPrime - 50.0, 2.0) / std::sqrt(20.0 + std::pow(barLPrime - 50.0, 2.0)));

  double S_C = 1.0 + (0.045 * barCPrime);

  double S_H = 1.0 + (0.015 * barCPrime * T);

  double R_T = -std::sin(2.0 * deltaTheta) * R_C;

  double deltaE = std::sqrt(std::pow(deltaLPrime / (k_L * S_L), 2.0) +
    std::pow(deltaCPrime / (k_C * S_C), 2.0) + std::pow(deltaHPrime / (k_H * S_H), 2.0) +
    R_T * (deltaCPrime / (k_C * S_C)) * (deltaHPrime / (k_H * S_H)));

  return deltaE;
}

//------------------------------------------------------------------------------
double CorrectedDistance(std::vector<Node>& path)
{
  double distance = 0.0;

  for (std::size_t i = 1; i < path.size(); ++i)
  {
    double currentLabColor[3];
    vtkMath::RGBToLab(path.at(i).rgb, currentLabColor);

    double previousLabColor[3];
    vtkMath::RGBToLab(path.at(i - 1).rgb, previousLabColor);

    distance += GetCIEDeltaE2000(currentLabColor, previousLabColor);
    path.at(i).distance = distance;
  }

  return distance;
}

//------------------------------------------------------------------------------
double GetColorPath(
  const double rgb1[3], const double rgb2[3], std::vector<Node>& path, bool forceExactSupportColors)
{
  Position pos1, pos2;
  getPosition(rgb1, pos1);
  getPosition(rgb2, pos2);

  // Use Dijkstra's algorithm backwards to calculate the shortest distances from
  // the second color

  std::deque<Distance> distances(COLORSPACE_SIZE_X * COLORSPACE_SIZE_Y * COLORSPACE_SIZE_Z,
    std::numeric_limits<Distance>::infinity());
  std::deque<Position> predecessors(COLORSPACE_SIZE_X * COLORSPACE_SIZE_Y * COLORSPACE_SIZE_Z);

  // Use a set as the priority queue so we can update an entry in the queue by
  // deleting the old entry and re-inserting the new entry.
  // The set is sorted first by the distance from the seed node, so that the
  // first entry always is the node that can be reached shortest.
  std::set<std::pair<Distance, Position>> front;

  // Start backwards and use the second color as seed
  distances[getIndex(pos2)] = static_cast<Distance>(0);
  front.insert(std::make_pair(static_cast<Distance>(0), pos2));

  while (!front.empty())
  {
    Distance currentDist = front.begin()->first;
    Position currentPos = front.begin()->second;

    front.erase(front.begin());

    double currentLabColor[3];
    getLabColor(currentPos, currentLabColor);

    int minNeighborPosX = std::max(static_cast<int>(currentPos[0]) - NEIGHBORHOOD_SIZE_X, 0);
    int minNeighborPosY = std::max(static_cast<int>(currentPos[1]) - NEIGHBORHOOD_SIZE_Y, 0);
    int minNeighborPosZ = std::max(static_cast<int>(currentPos[2]) - NEIGHBORHOOD_SIZE_Z, 0);

    int maxNeighborPosX =
      std::min(static_cast<int>(currentPos[0]) + NEIGHBORHOOD_SIZE_X, COLORSPACE_SIZE_X - 1);
    int maxNeighborPosY =
      std::min(static_cast<int>(currentPos[1]) + NEIGHBORHOOD_SIZE_Y, COLORSPACE_SIZE_Y - 1);
    int maxNeighborPosZ =
      std::min(static_cast<int>(currentPos[2]) + NEIGHBORHOOD_SIZE_Z, COLORSPACE_SIZE_Z - 1);

    for (int neighborPosZ = minNeighborPosZ; neighborPosZ <= maxNeighborPosZ; ++neighborPosZ)
    {
      for (int neighborPosY = minNeighborPosY; neighborPosY <= maxNeighborPosY; ++neighborPosY)
      {
        for (int neighborPosX = minNeighborPosX; neighborPosX <= maxNeighborPosX; ++neighborPosX)
        {
          Position neighborPos;
          neighborPos[0] = neighborPosX;
          neighborPos[1] = neighborPosY;
          neighborPos[2] = neighborPosZ;

          if (neighborPos == currentPos)
          {
            continue;
          }

          double neighborLabColor[3];
          getLabColor(neighborPos, neighborLabColor);

          Distance deltaE =
            static_cast<Distance>(GetCIEDeltaE2000(currentLabColor, neighborLabColor));

          int neighborIdx = getIndex(neighborPos);

          Distance oldNeighborDist = distances[neighborIdx];
          Distance newNeighborDist = currentDist + deltaE;

          if (newNeighborDist < oldNeighborDist)
          {
            front.erase(std::make_pair(oldNeighborDist, neighborPos));

            distances[neighborIdx] = newNeighborDist;
            predecessors[neighborIdx] = currentPos;
            front.insert(std::make_pair(newNeighborDist, neighborPos));
          }
        }
      }
    }
  }

  // We started backwards from the second color, so the overall length of the
  // path is the distance value at the position of the first color
  Distance pathDistance = distances[getIndex(pos1)];

  // Start the path from the first color and follow each node's predecessor
  // until the second color is reached.
  // Since each node was reached shortest from its predecessor, this results in
  // a shortest path from the first to the second color.

  path.clear();

  Position currentPos = pos1;

  while (true)
  {
    int currentIdx = getIndex(currentPos);

    Node node;
    getRGBColor(currentPos, node.rgb);

    // The shortest distance from the first color to the node is the overall
    // shortest distance
    // from the first to the second color minus the shortest distance from the
    // second color to the node.
    node.distance = pathDistance - distances[currentIdx];

    path.push_back(node);

    if (currentPos == pos2)
    {
      break;
    }

    currentPos = predecessors[currentIdx];
  }

  // Force the first and the last node's color to be exact
  if (forceExactSupportColors)
  {
    for (int i = 0; i < 3; ++i)
    {
      path.front().rgb[i] = rgb1[i];
      path.back().rgb[i] = rgb2[i];
    }

    // Return the corrected overall length of the path. Necessary if forcing the
    return CorrectedDistance(path);
  }

  // Return the overall length of the path
  return pathDistance;
}
//------------------------------------------------------------------------------

VTK_ABI_NAMESPACE_END
} // namespace CIEDE2000