/////////////////////////////////////////////////////////////////////////////////
//
//  Copyright (C) 2018-     Statoil 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 "RiaJCurveCalculator.h"
#include "RiaOffshoreSphericalCoords.h"
#include "cvfMatrix3.h"
#include "RiaArcCurveCalculator.h"

//--------------------------------------------------------------------------------------------------
/// 
//--------------------------------------------------------------------------------------------------
RiaJCurveCalculator::RiaJCurveCalculator(cvf::Vec3d p1, double azi1, double inc1, double r1, 
                                         cvf::Vec3d p2)
    : m_c1( cvf::Vec3d::UNDEFINED)
    , m_n1( cvf::Vec3d::UNDEFINED)
    , m_radius( std::numeric_limits<double>::infinity())
    , m_curveStatus(OK)
{
    cvf::Vec3d t1 (RiaOffshoreSphericalCoords::unitVectorFromAziInc(azi1, inc1));

    cvf::Vec3d p1p2 = p2 - p1;
    bool isOk = true;

    cvf::Vec3d tr1 = p1p2 - (p1p2.dot(t1)) * t1;
    double tr1Length = tr1.length();
    if (tr1Length < 1e-9)
    {
        // p2 is on the p1 + t12 line. Degenerates to a line.
        m_curveStatus = OK_STRAIGHT_LINE; 
        m_firstArcEndpoint = p2;
        m_endAzi = azi1;
        m_endInc = inc1;

        return;
    }
    
    tr1 /= tr1Length;

    cvf::Vec3d c1 = p1 + r1 * tr1;
    cvf::Vec3d p2c1 = c1 - p2;

    double p2c1Length = p2c1.length();
    if (p2c1Length < r1)
    {
        // Radius is too big. We can not get to point 2 using the requested radius.
        m_curveStatus = FAILED_RADIUS_TOO_LARGE; 

        RiaArcCurveCalculator arc(p1, t1, p2);
        if (   arc.curveStatus() == RiaArcCurveCalculator::OK 
            || arc.curveStatus() == RiaArcCurveCalculator::OK_STRAIGHT_LINE )
        {
            m_c1 = arc.center();
            m_n1 = arc.normal();
            m_firstArcEndpoint = p2;
            m_endAzi = arc.endAzimuth();
            m_endInc = arc.endInclination();
            m_radius = arc.radius();
        }
        else
        {
            m_firstArcEndpoint = p2;
            m_endAzi = azi1;
            m_endInc = inc1;
        }
        return;
    }

    double d = sqrt( p2c1Length * p2c1Length - r1 * r1);
    
    double betha = asin( r1/p2c1Length );
    cvf::Vec3d tp2c1 = p2c1/p2c1Length;
    cvf::Vec3d nc1 = t1 ^ tr1;

    cvf::Vec3d tp11p2 = -tp2c1.getTransformedVector(cvf::Mat3d::fromRotation(nc1, betha));
    
    m_firstArcEndpoint = p2 - d*tp11p2;
    m_c1 = c1;
    m_n1 = nc1;

    RiaOffshoreSphericalCoords endTangent(tp11p2);
    m_endAzi = endTangent.azi();
    m_endInc = endTangent.inc();

}