/*

Calculate Ditigally Reconstructed Topogram from a CT dataset using a modified
version of incremental ray-tracing algorithm

gfattori 08.11.2021

*/



/*=========================================================================
 *
 *  Copyright NumFOCUS
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
/*=========================================================================
The algorithm was initially proposed by Robert Siddon and improved by
Filip Jacobs etc.

-------------------------------------------------------------------------
References:

R. L. Siddon, "Fast calculation of the exact radiological path for a
threedimensional CT array," Medical Physics 12, 252-55 (1985).

F. Jacobs, E. Sundermann, B. De Sutter, M. Christiaens, and I. Lemahieu,
"A fast algorithm to calculate the exact radiological path through a pixel
or voxel space," Journal of Computing and Information Technology ?
CIT 6, 89-94 (1998).

=========================================================================*/

#ifndef itkgSiddonJacobsRayCastInterpolateImageFunction_hxx
#define itkgSiddonJacobsRayCastInterpolateImageFunction_hxx

#include "itkgSiddonJacobsRayCastInterpolateImageFunction.h"

#include "itkMath.h"
#include <cstdlib>
#include <unistd.h>

namespace itk
{

template <typename TInputImage, typename TCoordRep>
const double gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::TOL =
    1e-9;

template <typename TInputImage, typename TCoordRep>
const double gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::EPSILON =
        0.00001;

template <typename TInputImage, typename TCoordRep>
gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::gSiddonJacobsRayCastInterpolateImageFunction()
{
    m_FocalPointToIsocenterDistance = 1000.; // Focal point to isocenter distance in mm.
    m_ProjectionAngle = 0.;                  // Angle in radians betweeen projection central axis and reference axis
    m_Threshold = 0.;                        // Intensity threshold, below which is ignored.
    m_PanelOffset = 0.;

    m_SourcePoint[0] = 0.;
    m_SourcePoint[1] = 0.;
    m_SourcePoint[2] = 0.;

    m_InverseTransform = TransformType::New();
    m_InverseTransform->SetComputeZYX(true);

    m_ComposedTransform = TransformType::New();
    m_ComposedTransform->SetComputeZYX(true);

    m_GantryRotTransform = TransformType::New();
    m_GantryRotTransform->SetComputeZYX(true);
    m_GantryRotTransform->SetIdentity();

    m_CamShiftTransform = TransformType::New();
    m_CamShiftTransform->SetComputeZYX(true);
    m_CamShiftTransform->SetIdentity();

    m_CamRotTransform = TransformType::New();
    m_CamRotTransform->SetComputeZYX(true);
    m_CamRotTransform->SetIdentity();
    // constant for converting degrees into radians
    const float dtr = (atan(1.0) * 4.0) / 180.0;
    m_CamRotTransform->SetRotation(dtr * (-90.0), 0.0, 0.0);

    m_Threshold = 0;
}


template <typename TInputImage, typename TCoordRep>
void
gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::PrintSelf(std::ostream & os, Indent indent) const
{
    this->Superclass::PrintSelf(os, indent);

    os << indent << "Threshold: " << m_Threshold << std::endl;
    os << indent << "Transform: " << m_Transform.GetPointer() << std::endl;
}


template <typename TInputImage, typename TCoordRep>
typename gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::OutputType
gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::Evaluate(const PointType & point) const
{
    // --- Geometry / Siddon working variables use double ---
    double    rayVector[3];
    IndexType cIndex;

    PointType  drrPixelWorld; // Coordinate of a DRR pixel in the world coordinate system
    OutputType pixval;

    double firstIntersection[3];
    double alphaX1, alphaXN, alphaXmin, alphaXmax;
    double alphaY1, alphaYN, alphaYmin, alphaYmax;
    double alphaZ1, alphaZN, alphaZmin, alphaZmax;
    double alphaMin, alphaMax;
    double alphaX, alphaY, alphaZ;
    double alphaUx, alphaUy, alphaUz;
    double alphaIntersectionUp[3], alphaIntersectionDown[3];
    double d12, value;
    double firstIntersectionIndex[3];
    int    firstIntersectionIndexUp[3], firstIntersectionIndexDown[3];
    int    iU, jU, kU;

    // Min/max values of the output pixel type AND these values
    // represented as the output type of the interpolator
    const OutputType minOutputValue = itk::NumericTraits<OutputType>::NonpositiveMin();
    const OutputType maxOutputValue = itk::NumericTraits<OutputType>::max();

    // If the volume was shifted, recalculate the overall inverse transform
    unsigned long int interpMTime     = this->GetMTime();
    unsigned long int vTransformMTime = m_Transform->GetMTime();

    if (interpMTime < vTransformMTime)
    {
        this->ComputeInverseTransform();
    }

    PointType PointReq = point;
    PointReq[0] += m_PanelOffset;

    drrPixelWorld = m_InverseTransform->TransformPoint(PointReq);

    // Get the input pointers
    InputImageConstPointer inputPtr = this->GetInputImage();

    typename InputImageType::SizeType    sizeCT;
    typename InputImageType::RegionType  regionCT;
    typename InputImageType::SpacingType ctPixelSpacing;
    typename InputImageType::PointType   ctOrigin;

    ctPixelSpacing = inputPtr->GetSpacing();
    ctOrigin       = inputPtr->GetOrigin();
    regionCT       = inputPtr->GetLargestPossibleRegion();
    sizeCT         = regionCT.GetSize();

    PointType SlidingSourcePoint = m_SourcePoint;
    SlidingSourcePoint[0]        = 0.;
    // Simple sliding source model, the source follows the y of the pixel
    // requested, which sets the Z position to look in the CT volume.
    SlidingSourcePoint[1] = point[1];
    SlidingSourcePoint[2] = 0.;

    PointType SourceWorld = m_InverseTransform->TransformPoint(SlidingSourcePoint);

    PointType O(3);
    O[0] = -ctPixelSpacing[0] / 2.;
    O[1] = -ctPixelSpacing[1] / 2.;
    O[2] = -ctPixelSpacing[2] / 2.;

    // be sure that drr pixel position is inside the volume
    double xSizeCT = O[0] + (sizeCT[0]) * ctPixelSpacing[0];
    double ySizeCT = O[1] + (sizeCT[1]) * ctPixelSpacing[1];
    double zSizeCT = O[2] + (sizeCT[2]) * ctPixelSpacing[2];
    double xDrrPix = drrPixelWorld[0];
    double yDrrPix = drrPixelWorld[1];
    double zDrrPix = drrPixelWorld[2];

    if (xDrrPix <= O[0] || xDrrPix >= xSizeCT ||
        yDrrPix <= O[1] || yDrrPix >= ySizeCT ||
        zDrrPix <= O[2] || zDrrPix >= zSizeCT)
    {
        pixval = static_cast<OutputType>(0.0);
        return pixval;
    }

    // calculate the detector position for this pixel center by moving
    // 2*m_FocalPointToIsocenterDistance from the source in the pixel
    // directions
    double p1[3], p2[3];

    p1[0] = SourceWorld[0];
    p1[1] = SourceWorld[1];
    p1[2] = SourceWorld[2];

    p2[0] = drrPixelWorld[0];
    p2[1] = drrPixelWorld[1];
    p2[2] = drrPixelWorld[2];

    double P12D = std::sqrt(
        (p2[0] - p1[0]) * (p2[0] - p1[0]) +
        (p2[1] - p1[1]) * (p2[1] - p1[1]) +
        (p2[2] - p1[2]) * (p2[2] - p1[2]));

    double p12V[3];
    p12V[0] = p2[0] - p1[0];
    p12V[1] = p2[1] - p1[1];
    p12V[2] = p2[2] - p1[2];

    double p12v[3];
    p12v[0] = p12V[0] / P12D;
    p12v[1] = p12V[1] / P12D;
    p12v[2] = p12V[2] / P12D;

    double pDest[3];
    pDest[0] = p1[0] + m_FocalPointToIsocenterDistance * 2.0 * p12v[0];
    pDest[1] = p1[1] + m_FocalPointToIsocenterDistance * 2.0 * p12v[1];
    pDest[2] = p1[2] + m_FocalPointToIsocenterDistance * 2.0 * p12v[2];

    // The following is the Siddon-Jacob fast ray-tracing algorithm
    // Compute ray from source to "detector" point pDest
    rayVector[0] = pDest[0] - SourceWorld[0];
    rayVector[1] = pDest[1] - SourceWorld[1];
    rayVector[2] = pDest[2] - SourceWorld[2];

    const double rayLength = std::sqrt(rayVector[0]*rayVector[0] +
                                       rayVector[1]*rayVector[1] +
                                       rayVector[2]*rayVector[2]);

    // just in case (should never really be zero)
    if (rayLength <= EPSILON)
    {
        pixval = static_cast<OutputType>(0.0);
        return pixval;
    }
    const double INF = std::numeric_limits<double>::infinity();

    // Convenience bounds for the CT volume in world coords
    double xMin = O[0];
    double xMax = O[0] + sizeCT[0] * ctPixelSpacing[0];
    double yMin = O[1];
    double yMax = O[1] + sizeCT[1] * ctPixelSpacing[1];
    double zMin = O[2];
    double zMax = O[2] + sizeCT[2] * ctPixelSpacing[2];

    /* Calculate the parametric values of the first and last
       intersection points of the ray with the X, Y, and Z-planes
       that define the CT volume. */

    // ---- X slab ----
    if (std::fabs(rayVector[0]) > EPSILON)
    {
        alphaX1 = (xMin - SourceWorld[0]) / rayVector[0];
        alphaXN = (xMax - SourceWorld[0]) / rayVector[0];
        alphaXmin = std::min(alphaX1, alphaXN);
        alphaXmax = std::max(alphaX1, alphaXN);
    }
    else
    {
        // Ray is parallel to X planes: must lie within [xMin, xMax] or no intersection
        if (SourceWorld[0] < xMin || SourceWorld[0] > xMax)
        {
            pixval = static_cast<OutputType>(0.0);
            return pixval;
        }
        alphaXmin = -INF;
        alphaXmax =  INF;
    }

    // ---- Y slab ----
    if (std::fabs(rayVector[1]) > EPSILON)
    {
        alphaY1 = (yMin - SourceWorld[1]) / rayVector[1];
        alphaYN = (yMax - SourceWorld[1]) / rayVector[1];
        alphaYmin = std::min(alphaY1, alphaYN);
        alphaYmax = std::max(alphaY1, alphaYN);
    }
    else
    {
        if (SourceWorld[1] < yMin || SourceWorld[1] > yMax)
        {
            pixval = static_cast<OutputType>(0.0);
            return pixval;
        }
        alphaYmin = -INF;
        alphaYmax =  INF;
    }

    // ---- Z slab ----
    if (std::fabs(rayVector[2]) > EPSILON)
    {
        alphaZ1 = (zMin - SourceWorld[2]) / rayVector[2];
        alphaZN = (zMax - SourceWorld[2]) / rayVector[2];
        alphaZmin = std::min(alphaZ1, alphaZN);
        alphaZmax = std::max(alphaZ1, alphaZN);
    }
    else
    {
        if (SourceWorld[2] < zMin || SourceWorld[2] > zMax)
        {
            pixval = static_cast<OutputType>(0.0);
            return pixval;
        }
        alphaZmin = -INF;
        alphaZmax =  INF;
    }

    /* Get the very first and the last alpha values when the ray
       intersects with the CT volume. */
    alphaMin = std::max(std::max(alphaXmin, alphaYmin), alphaZmin);
    alphaMax = std::min(std::min(alphaXmax, alphaYmax), alphaZmax);

    // No intersection at all
    if (alphaMax <= alphaMin)
    {
        pixval = static_cast<OutputType>(0.0);
        return pixval;
    }

    // Restrict to the [0,1] segment (from SourceWorld to pDest)
    alphaMax = std::min(alphaMax, 1.0);
    alphaMin = std::max(alphaMin, 0.0);

    if (alphaMax <= alphaMin)
    {
        pixval = static_cast<OutputType>(0.0);
        return pixval;
    }

    /* Calculate the parametric values of the first intersection point
       after the ray entered the CT volume. */
    firstIntersection[0] = SourceWorld[0] + alphaMin * rayVector[0] - O[0];
    firstIntersection[1] = SourceWorld[1] + alphaMin * rayVector[1] - O[1];
    firstIntersection[2] = SourceWorld[2] + alphaMin * rayVector[2] - O[2];

    /* Transform world coordinates to continuous indices of the CT volume */
    firstIntersectionIndex[0] = firstIntersection[0] / ctPixelSpacing[0];
    firstIntersectionIndex[1] = firstIntersection[1] / ctPixelSpacing[1];
    firstIntersectionIndex[2] = firstIntersection[2] / ctPixelSpacing[2];

    // Helper for equality with tolerance (for on-grid cases)
    auto nearlyEqual = [](double a, double b)
    {
        return std::fabs(a - b) < TOL;
    };

    // "Correct" the indices for special entry cases
    if (nearlyEqual(alphaMin, alphaXmin)) // i is special
    {
        if (nearlyEqual(alphaXmin, alphaX1))
            firstIntersectionIndex[0] = 0.0;            // continuous index
        else
            firstIntersectionIndex[0] = sizeCT[0] - 1.; // continuous index
    }

    if (nearlyEqual(alphaMin, alphaYmin)) // j is special
    {
        if (nearlyEqual(alphaYmin, alphaY1))
            firstIntersectionIndex[1] = 0.0;
        else
            firstIntersectionIndex[1] = sizeCT[1] - 1.;
    }

    if (nearlyEqual(alphaMin, alphaZmin)) // k is special
    {
        if (nearlyEqual(alphaZmin, alphaZ1))
            firstIntersectionIndex[2] = 0.0;
        else
            firstIntersectionIndex[2] = sizeCT[2] - 1.;
    }

    firstIntersectionIndexUp[0]   = static_cast<int>(std::ceil(firstIntersectionIndex[0]));
    firstIntersectionIndexUp[1]   = static_cast<int>(std::ceil(firstIntersectionIndex[1]));
    firstIntersectionIndexUp[2]   = static_cast<int>(std::ceil(firstIntersectionIndex[2]));
    firstIntersectionIndexDown[0] = static_cast<int>(std::floor(firstIntersectionIndex[0]));
    firstIntersectionIndexDown[1] = static_cast<int>(std::floor(firstIntersectionIndex[1]));
    firstIntersectionIndexDown[2] = static_cast<int>(std::floor(firstIntersectionIndex[2]));

    // ---- Compute first plane hits and increments ----

    // X
    if (std::fabs(rayVector[0]) <= EPSILON)
    {
        alphaX  = INF;
        alphaUx = INF;
    }
    else
    {
        alphaIntersectionUp[0] =
            (O[0] + firstIntersectionIndexUp[0]   * ctPixelSpacing[0] - SourceWorld[0]) / rayVector[0];
        alphaIntersectionDown[0] =
            (O[0] + firstIntersectionIndexDown[0] * ctPixelSpacing[0] - SourceWorld[0]) / rayVector[0];
        alphaX  = std::max(alphaIntersectionUp[0], alphaIntersectionDown[0]);
        alphaUx = ctPixelSpacing[0] / std::fabs(rayVector[0]);
    }

    // Y
    if (std::fabs(rayVector[1]) <= EPSILON)
    {
        alphaY  = INF;
        alphaUy = INF;
    }
    else
    {
        alphaIntersectionUp[1] =
            (O[1] + firstIntersectionIndexUp[1]   * ctPixelSpacing[1] - SourceWorld[1]) / rayVector[1];
        alphaIntersectionDown[1] =
            (O[1] + firstIntersectionIndexDown[1] * ctPixelSpacing[1] - SourceWorld[1]) / rayVector[1];
        alphaY  = std::max(alphaIntersectionUp[1], alphaIntersectionDown[1]);
        alphaUy = ctPixelSpacing[1] / std::fabs(rayVector[1]);
    }

    // Z
    if (std::fabs(rayVector[2]) <= EPSILON)
    {
        alphaZ  = INF;
        alphaUz = INF;
    }
    else
    {
        alphaIntersectionUp[2] =
            (O[2] + firstIntersectionIndexUp[2]   * ctPixelSpacing[2] - SourceWorld[2]) / rayVector[2];
        alphaIntersectionDown[2] =
            (O[2] + firstIntersectionIndexDown[2] * ctPixelSpacing[2] - SourceWorld[2]) / rayVector[2];
        alphaZ  = std::max(alphaIntersectionUp[2], alphaIntersectionDown[2]);
        alphaUz = ctPixelSpacing[2] / std::fabs(rayVector[2]);
    }

    // Make sure the first plane alphas are not before entry

    // Only advance if this axis is not parallel (alphaUx finite)
    if (!std::isinf(alphaUx))
    {
        while (alphaX <= alphaMin + TOL)
            alphaX += alphaUx;
    }
    if (!std::isinf(alphaUy))
    {
        while (alphaY <= alphaMin + TOL)
            alphaY += alphaUy;
    }
    if (!std::isinf(alphaUz))
    {
        while (alphaZ <= alphaMin + TOL)
            alphaZ += alphaUz;
    }

    /* Calculate voxel index incremental values along the ray path. */
    // Use the direction of the ray
    iU = (rayVector[0] > 0.0) ? 1 : -1;
    jU = (rayVector[1] > 0.0) ? 1 : -1;
    kU = (rayVector[2] > 0.0) ? 1 : -1;

    // Initialize line integral
    d12 = 0.0;

    /* Initialize the current voxel index with the voxel entered at alphaMin. */
    cIndex[0] = firstIntersectionIndexDown[0];
    cIndex[1] = firstIntersectionIndexDown[1];
    cIndex[2] = firstIntersectionIndexDown[2];

    // Current parametric position and previous one
    double alpha     = alphaMin;
    double alphaPrev = alphaMin;

    // Siddon traversal
    while (alpha < alphaMax)
    {
        // Next plane intersection along the ray
        double alphaNext = std::min(std::min(alphaX, alphaY), alphaZ);
        if (alphaNext > alphaMax)
            alphaNext = alphaMax;

        double segmentLength = alphaNext - alphaPrev;
        // don’t break immediately on a single zero-length segment. This would cause black pixels!
        // if (segmentLength <= 0.0)
        //  break; // FP guard, especially for grid-aligned rays
        if (segmentLength <= 0.0)
        {
            // advance to the next plane and try again
            if (nearlyEqual(alphaNext, alphaX))
            {
                cIndex[0] += iU;
                alphaX    += alphaUx;
            }
            else if (nearlyEqual(alphaNext, alphaY))
            {
                cIndex[1] += jU;
                alphaY    += alphaUy;
            }
            else
            {
                cIndex[2] += kU;
                alphaZ    += alphaUz;
            }

            alphaPrev = alphaNext;
            alpha     = alphaNext;

            if (alpha >= alphaMax)
                break;

            continue;
        }

        // Accumulate contribution if inside the volume
        if (cIndex[0] >= 0 && cIndex[0] < static_cast<IndexValueType>(sizeCT[0]) &&
            cIndex[1] >= 0 && cIndex[1] < static_cast<IndexValueType>(sizeCT[1]) &&
            cIndex[2] >= 0 && cIndex[2] < static_cast<IndexValueType>(sizeCT[2]))
        {
            value = static_cast<float>(inputPtr->GetPixel(cIndex));

            if (value > m_Threshold)
            {
                // physical path length = segmentLength (in α) * |rayVector|
                d12 += segmentLength * rayLength * (value - m_Threshold);
            }
        }

        if (alphaNext >= alphaMax)
            break;

        alphaPrev = alphaNext;
        alpha     = alphaNext;

        // Advance along the axis whose plane we actually hit
        if (nearlyEqual(alphaNext, alphaX))
        {
            cIndex[0] += iU;
            alphaX    += alphaUx;
        }
        else if (nearlyEqual(alphaNext, alphaY))
        {
            cIndex[1] += jU;
            alphaY    += alphaUy;
        }
        else // must be z
        {
            cIndex[2] += kU;
            alphaZ    += alphaUz;
        }
    }

    // Clamp output value
    if (d12 < minOutputValue)
    {
        pixval = minOutputValue;
    }
    else if (d12 > maxOutputValue)
    {
        pixval = maxOutputValue;
    }
    else
    {
        pixval = static_cast<OutputType>(d12);
    }
    return pixval;
}


template <typename TInputImage, typename TCoordRep>
typename gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::OutputType
gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::EvaluateAtContinuousIndex(
        const ContinuousIndexType & index) const
{
    OutputPointType point;
    this->m_Image->TransformContinuousIndexToPhysicalPoint(index, point);

    return this->Evaluate(point);
}


template <typename TInputImage, typename TCoordRep>
void
gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::ComputeInverseTransform() const
{
    m_ComposedTransform->SetIdentity();
    m_ComposedTransform->Compose(m_Transform, 0);

    typename TransformType::InputPointType isocenter;
    isocenter = m_Transform->GetCenter();

//   //This is problably not necessary since we correct the ray.... to be checked..
//    InputImageConstPointer inputPtr = this->GetInputImage();

//    std::cout<<inputPtr<<std::endl;
//      if(inputPtr != NULL)
//    {
//      isocenter[0] -= inputPtr->GetSpacing()[0]/2.;
//      isocenter[1] -= inputPtr->GetSpacing()[1]/2.;
//      isocenter[2] -= inputPtr->GetSpacing()[2]/2.;
//      std::cout<<inputPtr->GetSpacing()<<std::endl;
//    } else {
//          isocenter[0] += 0.5;
//          isocenter[1] += 0.5;
//          isocenter[2] += 1;
//      }
    // An Euler 3D transform is used to rotate the volume to simulate the roation of the linac gantry.
    // The rotation is about z-axis. After the transform, a AP projection geometry (projecting
    // towards positive y direction) is established.
    m_GantryRotTransform->SetRotation(0.0, 0.0, -m_ProjectionAngle);
    m_GantryRotTransform->SetCenter(isocenter);
    m_ComposedTransform->Compose(m_GantryRotTransform, 0);

    // An Euler 3D transfrom is used to shift the source to the origin.
    typename TransformType::OutputVectorType focalpointtranslation;
    focalpointtranslation[0] = -isocenter[0];
    focalpointtranslation[1] = m_FocalPointToIsocenterDistance - isocenter[1];
    focalpointtranslation[2] = -isocenter[2];
    m_CamShiftTransform->SetTranslation(focalpointtranslation);
    m_ComposedTransform->Compose(m_CamShiftTransform, 0);

    // A Euler 3D transform is used to establish the standard negative z-axis projection geometry. (By
    // default, the camera is situated at the origin, points down the negative z-axis, and has an up-
    // vector of (0, 1, 0).)

    m_ComposedTransform->Compose(m_CamRotTransform, 0);

    // The overall inverse transform is computed. The inverse transform will be used by the interpolation
    // procedure.
    m_ComposedTransform->GetInverse(m_InverseTransform);
    this->Modified();
}


template <typename TInputImage, typename TCoordRep>
void
gSiddonJacobsRayCastInterpolateImageFunction<TInputImage, TCoordRep>::Initialize()
{
    this->ComputeInverseTransform();
    m_SourceWorld = m_InverseTransform->TransformPoint(m_SourcePoint);
}

} // namespace itk

#endif