/////////////////////////////////////////////////////////////////////////////////
//
//  Copyright (C) 2018-     Equinor ASA
//
//  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 "RiaArcCurveCalculator.h"

#include "RiaOffshoreSphericalCoords.h"
#include "cvfGeometryTools.h"

//--------------------------------------------------------------------------------------------------
///                + p1
///           t1 //
///              |      + C
///               \
///                + p2
//--------------------------------------------------------------------------------------------------
RiaArcCurveCalculator::RiaArcCurveCalculator( cvf::Vec3d p1, cvf::Vec3d t1, cvf::Vec3d p2 )
    : m_radius( std::numeric_limits<double>::infinity() )
    , m_arcCS( cvf::Mat4d::ZERO )
    , m_endAzi( 0 )
    , m_endInc( 0 )
    , m_curveStatus( OK )
{
    bool isOk = t1.normalize();
    if ( !isOk )
    {
        // No tangent. Bail out
        m_curveStatus = FAILED_INPUT_OVERLAP;
        return;
    }

    cvf::Vec3d p1p2 = p2 - p1;
    cvf::Vec3d t12  = p1p2.getNormalized( &isOk );
    if ( !isOk )
    {
        // p1 and p2 in the same place.
        m_curveStatus = FAILED_INPUT_OVERLAP;

        return;
    }

    cvf::Vec3d N = ( t1 ^ t12 ).getNormalized( &isOk );
    if ( !isOk )
    {
        // P2 is on the p1 + k*t1 line. We have a straight line
        m_curveStatus = OK_STRAIGHT_LINE;

        RiaOffshoreSphericalCoords endTangent( t1 );
        m_endTangent = t1;
        m_endAzi     = endTangent.azi();
        m_endInc     = endTangent.inc();
        m_radius     = std::numeric_limits<double>::infinity();
        m_arcAngle   = 0;
        m_arcLength  = p1p2.length();
        return;
    }

    cvf::Vec3d tr1 = ( N ^ t1 ).getNormalized();

    m_radius = 0.5 * p1p2.length() / ( tr1.dot( t12 ) );

    cvf::Vec3d C = p1 + m_radius * tr1;

    cvf::Vec3d nTr1 = -tr1;
    m_arcCS         = cvf::Mat4d::fromCoordSystemAxes( &nTr1, &t1, &N );

    m_arcCS.setTranslation( C );

    m_arcAngle = cvf::GeometryTools::getAngle( N, p1 - C, p2 - C );

    m_arcLength = m_radius * m_arcAngle;

    m_endTangent = N ^ ( p2 - C ).getNormalized();

    RiaOffshoreSphericalCoords endTangent( m_endTangent );
    m_endAzi = endTangent.azi();
    m_endInc = endTangent.inc();
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
RiaArcCurveCalculator::RiaArcCurveCalculator( cvf::Vec3d p1, double azi1, double inc1, cvf::Vec3d p2 )
{
    cvf::Vec3d t1( RiaOffshoreSphericalCoords::unitVectorFromAziInc( azi1, inc1 ) );

    ( *this ) = RiaArcCurveCalculator( p1, t1, p2 );
}