/////////////////////////////////////////////////////////////////////////////////
//
//  Copyright (C) 2011-2012 Statoil ASA, Ceetron AS
//
//  ResInsight 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 3 of the License, or
//  (at your option) any later version.
//
//  ResInsight 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 at <http://www.gnu.org/licenses/gpl.html>
//  for more details.
//
/////////////////////////////////////////////////////////////////////////////////

#include "RigWellPath.h"

#include "cvfBoundingBox.h"
#include "cvfGeometryTools.h"
#include "cvfPlane.h"

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
RigWellPath::RigWellPath()
    : m_hasDatumElevation( false )
    , m_datumElevation( 0.0 )
{
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
void RigWellPath::setDatumElevation( double value )
{
    m_hasDatumElevation = true;
    m_datumElevation    = value;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
bool RigWellPath::hasDatumElevation() const
{
    return m_hasDatumElevation;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
double RigWellPath::datumElevation() const
{
    return m_datumElevation;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
double RigWellPath::rkbDiff() const
{
    if ( hasDatumElevation() )
    {
        return datumElevation();
    }

    // If measured depth is zero, use the z-value of the well path points
    if ( m_wellPathPoints.size() > 0 && m_measuredDepths.size() > 0 )
    {
        double epsilon = 1e-3;

        if ( cvf::Math::abs( m_measuredDepths[0] ) < epsilon )
        {
            double diff = m_measuredDepths[0] - ( -wellPathPoints()[0].z() );

            return diff;
        }
    }
    return HUGE_VAL;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
cvf::Vec3d RigWellPath::interpolatedVectorValuesAlongWellPath(
    const std::vector<cvf::Vec3d>& vectorValuesAlongWellPath,
    double                         measuredDepth,
    double*                        horizontalLengthAlongWellToStartClipPoint /*= nullptr*/ ) const
{
    CVF_ASSERT( vectorValuesAlongWellPath.size() == m_wellPathPoints.size() );
    cvf::Vec3d interpolatedVector = cvf::Vec3d::ZERO;

    if ( horizontalLengthAlongWellToStartClipPoint ) *horizontalLengthAlongWellToStartClipPoint = 0.0;

    size_t vxIdx = 0;
    while ( vxIdx < m_measuredDepths.size() && m_measuredDepths.at( vxIdx ) < measuredDepth )
    {
        if ( vxIdx > 0 && horizontalLengthAlongWellToStartClipPoint )
        {
            cvf::Vec3d segment = m_wellPathPoints[vxIdx] - m_wellPathPoints[vxIdx - 1];
            segment[2]         = 0.0;
            *horizontalLengthAlongWellToStartClipPoint += segment.length();
        }
        vxIdx++;
    }

    if ( m_measuredDepths.size() > vxIdx )
    {
        if ( vxIdx == 0 )
        {
            // For measuredDepth same or lower than first point, use this first point
            interpolatedVector = vectorValuesAlongWellPath.at( 0 );
        }
        else
        {
            // Do interpolation
            double segmentFraction = ( measuredDepth - m_measuredDepths.at( vxIdx - 1 ) ) /
                                     ( m_measuredDepths.at( vxIdx ) - m_measuredDepths.at( vxIdx - 1 ) );
            cvf::Vec3d segment = m_wellPathPoints[vxIdx] - m_wellPathPoints[vxIdx - 1];
            interpolatedVector = ( 1.0 - segmentFraction ) * vectorValuesAlongWellPath[vxIdx - 1] +
                                 segmentFraction * vectorValuesAlongWellPath[vxIdx];

            if ( horizontalLengthAlongWellToStartClipPoint )
            {
                segment[2] = 0.0;
                *horizontalLengthAlongWellToStartClipPoint += segment.length() * segmentFraction;
            }
        }
    }
    else
    {
        // Use endpoint if measuredDepth same or higher than last point
        interpolatedVector = vectorValuesAlongWellPath.at( vxIdx - 1 );
    }

    return interpolatedVector;
}

cvf::Vec3d
    RigWellPath::interpolatedPointAlongWellPath( double  measuredDepth,
                                                 double* horizontalLengthAlongWellToStartClipPoint /*= nullptr*/ ) const
{
    return interpolatedVectorValuesAlongWellPath( m_wellPathPoints,
                                                  measuredDepth,
                                                  horizontalLengthAlongWellToStartClipPoint );
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
double RigWellPath::wellPathAzimuthAngle( const cvf::Vec3d& position ) const
{
    size_t closestIndex    = cvf::UNDEFINED_SIZE_T;
    double closestDistance = cvf::UNDEFINED_DOUBLE;

    for ( size_t i = 1; i < m_wellPathPoints.size(); i++ )
    {
        cvf::Vec3d p1 = m_wellPathPoints[i - 1];
        cvf::Vec3d p2 = m_wellPathPoints[i - 0];

        double candidateDistance = cvf::GeometryTools::linePointSquareDist( p1, p2, position );
        if ( candidateDistance < closestDistance )
        {
            closestDistance = candidateDistance;
            closestIndex    = i;
        }
    }

    // For vertical well (x-component of direction = 0) returned angle will be 90.
    double azimuthAngleDegrees = 90.0;

    if ( closestIndex != cvf::UNDEFINED_DOUBLE )
    {
        cvf::Vec3d p1;
        cvf::Vec3d p2;

        if ( closestIndex > 0 )
        {
            p1 = m_wellPathPoints[closestIndex - 1];
            p2 = m_wellPathPoints[closestIndex - 0];
        }
        else
        {
            p1 = m_wellPathPoints[closestIndex + 1];
            p2 = m_wellPathPoints[closestIndex + 0];
        }

        cvf::Vec3d direction = p2 - p1;

        if ( fabs( direction.y() ) > 1e-5 )
        {
            double atanValue      = direction.x() / direction.y();
            double azimuthRadians = atan( atanValue );
            azimuthAngleDegrees   = cvf::Math::toDegrees( azimuthRadians );
        }
    }

    return azimuthAngleDegrees;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
void RigWellPath::twoClosestPoints( const cvf::Vec3d& position, cvf::Vec3d* p1, cvf::Vec3d* p2 ) const
{
    CVF_ASSERT( p1 && p2 );

    size_t closestIndex    = cvf::UNDEFINED_SIZE_T;
    double closestDistance = cvf::UNDEFINED_DOUBLE;

    for ( size_t i = 1; i < m_wellPathPoints.size(); i++ )
    {
        cvf::Vec3d point1 = m_wellPathPoints[i - 1];
        cvf::Vec3d point2 = m_wellPathPoints[i - 0];

        double candidateDistance = cvf::GeometryTools::linePointSquareDist( point1, point2, position );
        if ( candidateDistance < closestDistance )
        {
            closestDistance = candidateDistance;
            closestIndex    = i;
        }
    }

    if ( closestIndex != cvf::UNDEFINED_SIZE_T )
    {
        if ( closestIndex > 0 )
        {
            *p1 = m_wellPathPoints[closestIndex - 1];
            *p2 = m_wellPathPoints[closestIndex - 0];
        }
        else
        {
            *p1 = m_wellPathPoints[closestIndex + 1];
            *p2 = m_wellPathPoints[closestIndex + 0];
        }
    }
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
std::pair<std::vector<cvf::Vec3d>, std::vector<double>>
    RigWellPath::clippedPointSubset( double startMD, double endMD, double* horizontalLengthAlongWellToStartClipPoint ) const
{
    std::pair<std::vector<cvf::Vec3d>, std::vector<double>> pointsAndMDs;
    if ( m_measuredDepths.empty() ) return pointsAndMDs;
    if ( startMD > endMD ) return pointsAndMDs;

    pointsAndMDs.first.push_back( interpolatedPointAlongWellPath( startMD, horizontalLengthAlongWellToStartClipPoint ) );
    pointsAndMDs.second.push_back( startMD );

    for ( size_t i = 0; i < m_measuredDepths.size(); ++i )
    {
        double measuredDepth = m_measuredDepths[i];
        if ( measuredDepth > startMD && measuredDepth < endMD )
        {
            pointsAndMDs.first.push_back( m_wellPathPoints[i] );
            pointsAndMDs.second.push_back( measuredDepth );
        }
    }
    pointsAndMDs.first.push_back( interpolatedPointAlongWellPath( endMD ) );
    pointsAndMDs.second.push_back( endMD );

    return pointsAndMDs;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
std::vector<cvf::Vec3d>
    RigWellPath::wellPathPointsIncludingInterpolatedIntersectionPoint( double intersectionMeasuredDepth ) const
{
    std::vector<cvf::Vec3d> points;
    if ( m_measuredDepths.empty() ) return points;

    cvf::Vec3d interpolatedWellPathPoint = interpolatedPointAlongWellPath( intersectionMeasuredDepth );

    for ( size_t i = 0; i < m_measuredDepths.size() - 1; i++ )
    {
        if ( m_measuredDepths[i] == intersectionMeasuredDepth )
        {
            points.push_back( m_wellPathPoints[i] );
        }
        else if ( m_measuredDepths[i] < intersectionMeasuredDepth )
        {
            points.push_back( m_wellPathPoints[i] );
            if ( m_measuredDepths[i + 1] > intersectionMeasuredDepth )
            {
                points.push_back( interpolatedWellPathPoint );
            }
        }
        else if ( m_measuredDepths[i] > intersectionMeasuredDepth )
        {
            if ( i == 0 )
            {
                points.push_back( interpolatedWellPathPoint );
            }
            else
            {
                points.push_back( m_wellPathPoints[i] );
            }
        }
    }
    points.push_back( m_wellPathPoints.back() );

    return points;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
bool RigWellPath::isAnyPointInsideBoundingBox( const std::vector<cvf::Vec3d>& points, const cvf::BoundingBox& boundingBox )
{
    for ( const cvf::Vec3d& point : points )
    {
        if ( boundingBox.contains( point ) ) return true;
    }

    return false;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
std::vector<cvf::Vec3d> RigWellPath::clipPolylineStartAboveZ( const std::vector<cvf::Vec3d>& polyLine,
                                                              double                         maxZ,
                                                              double* horizontalLengthAlongWellToClipPoint,
                                                              size_t* indexToFirstVisibleSegment )
{
    CVF_ASSERT( horizontalLengthAlongWellToClipPoint );
    CVF_ASSERT( indexToFirstVisibleSegment );

    // Find first visible point, and accumulate distance along wellpath

    *horizontalLengthAlongWellToClipPoint = 0.0;
    *indexToFirstVisibleSegment           = cvf::UNDEFINED_SIZE_T;

    size_t firstVisiblePointIndex = cvf::UNDEFINED_SIZE_T;

    for ( size_t vxIdx = 0; vxIdx < polyLine.size(); ++vxIdx )
    {
        if ( polyLine[vxIdx].z() > maxZ )
        {
            if ( vxIdx > 0 )
            {
                cvf::Vec3d segment = polyLine[vxIdx] - polyLine[vxIdx - 1];
                segment[2]         = 0.0;
                *horizontalLengthAlongWellToClipPoint += segment.length();
            }
        }
        else
        {
            firstVisiblePointIndex = vxIdx;
            break;
        }
    }

    // Clip line, and add vx to the start of the clipped result

    std::vector<cvf::Vec3d> clippedPolyLine;

    if ( firstVisiblePointIndex == cvf::UNDEFINED_SIZE_T )
    {
        return clippedPolyLine;
    }

    if ( firstVisiblePointIndex > 0 )
    {
        cvf::Plane topPlane;
        topPlane.setFromPointAndNormal( {0.0, 0.0, maxZ}, cvf::Vec3d::Z_AXIS );
        cvf::Vec3d intersection;

        if ( topPlane.intersect( polyLine[firstVisiblePointIndex - 1], polyLine[firstVisiblePointIndex], &intersection ) )
        {
            cvf::Vec3d segment = intersection - polyLine[firstVisiblePointIndex - 1];
            segment[2]         = 0.0;
            *horizontalLengthAlongWellToClipPoint += segment.length();

            clippedPolyLine.push_back( intersection );
        }

        *indexToFirstVisibleSegment = firstVisiblePointIndex - 1;
    }
    else
    {
        *indexToFirstVisibleSegment = 0;
    }

    // Add the rest of the polyline

    for ( size_t vxIdx = firstVisiblePointIndex; vxIdx < polyLine.size(); ++vxIdx )
    {
        clippedPolyLine.push_back( polyLine[vxIdx] );
    }

    return clippedPolyLine;
}

const std::vector<cvf::Vec3d>& RigWellPath::wellPathPoints() const
{
    return m_wellPathPoints;
}

const std::vector<double>& RigWellPath::measureDepths() const
{
    return m_measuredDepths;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
std::vector<double> RigWellPath::trueVerticalDepths() const
{
    std::vector<double> tvds;
    for ( const cvf::Vec3d& point : m_wellPathPoints )
    {
        tvds.push_back( std::fabs( point.z() ) );
    }
    return tvds;
}