/*
 * This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).
 * Copyright (c) 2022-2026 Jochen Meidow, Fraunhofer IOSB
 *
 * This program 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.
 *
 * This program 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 for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <https://www.gnu.org/licenses/>.
 */


#include "matfun.h"
#include "statistics/iscov.h"
#include "udistance.h"
#include "uncertain/uelement.h"
#include "upoint.h"
#include "ustraightline.h"

#include <Eigen/Core>

#include <QDebug>

#include "qassert.h"
#include "qtdeprecationdefinitions.h"

#include <cassert>
#include <cmath>
#include <cstdlib>


using Eigen::Matrix;
using Eigen::Matrix2d;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

using Matfun::sign;
using Matfun::cof3;

using Stats::isCovMat;


namespace Uncertain {


//! Construction of uncertain point via 3-vector x and its covariance matrix
uPoint::uPoint( const Vector3d & x, const Matrix3d & Sigma_xx)
    : uElement (x, Sigma_xx)
{
    // qDebug() << Q_FUNC_INFO;
}


//! confidence ellipse
Matrix3d uPoint::conicMatrix(const double k2) const
{
    const uPoint ux = this->euclidean();
    return cof3( k2*ux.Cov() -ux.v()*ux.v().transpose() );
}


//! Get axis-aligned bounding box
Aabb<double,2> uPoint::bbox() const
{
    const double uu = v(0);
    const double vv = v(1);
    const double ww = v(2);

    const Matrix<double,2,3> JJ {
        { 1/ww,      0,   -uu/(ww*ww)},
        {    0,   1/ww,   -vv/(ww*ww)}  };
    const Matrix2d Cov_xx = JJ * Cov() * JJ.transpose();

    const double x = v(0) / v(2);
    const double y = v(1) / v(2);

    const double x_min = x - sqrt(Cov_xx(0, 0));
    const double x_max = x + sqrt(Cov_xx(0, 0));
    const double y_min = y - sqrt(Cov_xx(1, 1));
    const double y_max = y + sqrt(Cov_xx(1, 1));

    return Aabb<double,2>{
        Vector2d(x_min, y_min),
        Vector2d(x_max, y_max)
    };
}


//! Get Euclidean distance to uncertain straight line 'ul'
uDistance uPoint::distanceEuclideanTo( const uStraightLine & ul) const
{
    const Vector3d l = ul.v();
    const Vector3d x = v();
    const double n = l.head(2).norm(); // ||l_h||
    const double d_val = v().dot(l) / (abs( v(2))*n);
    const double nax2 = n*fabs(x(2));

    /* JJx(0) = l(0)/(abs(x(2))*n);
    JJx(1) = l(1)/(abs(x(2))*n);
    JJx(2) = l(2)/(abs(x(2))*n) -x.dot(l)*sign(x(2))/( abs(x(2))*abs(x(2)) *n);

    JJl(0) = x(0)/(abs(x(2))*n) -x.dot(l)*l(0) / (abs(x(2)) *n*n*n);
    JJl(1) = x(1)/(abs(x(2))*n) -x.dot(l)*l(1) / (abs(x(2)) *n*n*n);
    JJl(2) = x(2)/(abs(x(2))*n);*/

    Vector3d JJx;
    JJx(0) = l(0)/nax2;
    JJx(1) = l(1)/nax2;
    JJx(2) = (-l(0)*x(0) -l(1)*x(1))/( x(2)*nax2 );

    Vector3d JJl;
    JJl(0) = ( (-l(0)*l(1)*x(1) -l(0)*l(2)*x(2) +l(1)*l(1)*x(0) ) *fabs(x(2))) / ( x(2)*x(2)*n*n*n);
    JJl(1) = ( ( l(0)*l(0)*x(1) -l(0)*l(1)*x(0) -l(1)*l(2)*x(2) ) *fabs(x(2))) / ( x(2)*x(2)*n*n*n);
    JJl(2) = x(2)/nax2;

    const double d_var = JJx.dot( Cov() * JJx) + JJl.dot(ul.Cov() * JJl);

    return {d_val,d_var};
}


//! Get uncertain point in homogeneous coordinates, Euclidean normalized
uPoint uPoint::euclidean() const
{
    const double uu = v(0); // w'=u/w
    const double vv = v(1); // v'=v/w
    const double ww = v(2); // w'=w/w=1
    const Matrix3d JJ {
        { 1/ww,   0, -uu/(ww*ww) },
        { 0,   1/ww, -vv/(ww*ww) },
        { 0,      0,        0    } };

    return { v()/v(2), JJ*Cov()*JJ.adjoint() };
}


//! Get uncertain point, transformed via 3x3 transformation matrix
uPoint uPoint::transformed( const Matrix3d & TT) const
{
    return {TT*v(), TT*Cov()*TT.adjoint() };
}


//! Check if uncertain straight line 'ul' is incident.
bool uPoint::isIncidentWith( const uStraightLine & ul,
                             const double T) const
{
    const double d = v().dot(ul.v());                                       //  d = x'*l
    const double var_d = ul.v().dot(Cov() * ul.v()) + v().dot( ul.Cov() * v()); // uncorrelated
    Q_ASSERT( var_d>0. );

    return (d*d) / var_d < T;
}


//! Get algebraic distance of 'this' and straight line of 'ul'
uDistance uPoint::distanceAlgebraicTo( const uStraightLine & ul ) const
{
    const double d = sign( v(2) ) * v().dot(ul.v());
    const Vector3d JJx = ul.v().adjoint();
    const Vector3d JJl = sign( v(2) ) * v().adjoint();
    // JJ = [l', sign(x(3))*x'];

    const double var_d = JJx.dot( Cov() * JJx) + JJl.dot(ul.Cov() * JJl); // uncorrelated

    return {d, var_d};
}


//! Get uncertain point in homogeneous coordinates, spherically normalized
uPoint uPoint::sphericalNormalized() const
{
    uPoint ux(*this);
    ux.normalizeSpherical();
    return ux;  // Compiler invokes the copy constructor.
}


//! cross product of two vectors representing points, l=cross(x,y)
uStraightLine uPoint::cross( const uPoint & other) const
{
    // qDebug() << Q_FUNC_INFO;

    const Matrix3d Sx = skew( this->v() );
    const Matrix3d Sy = skew( other.v());

    const Vector3d m_val = Sx*other.v();         // cross(x,y)
    const Matrix3d m_cov = -Sy*this->Cov()*Sy -Sx*other.Cov()*Sx;  // S' = -S

    Q_ASSERT_X( m_val.norm()>0, Q_FUNC_INFO, "identical points");
    assert( isCovMat(m_cov) );

    return {m_val, m_cov };
}

} // namespace Uncertain