/*
 * This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).
 * Copyright (c) 2022-2023 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 "conics.h"
#include "upoint.h"
#include "ustraightline.h"

#include <QDebug>

namespace Conic {

using Uncertain::uPoint;
using Uncertain::uStraightLine;

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

ConicBase::ConicBase( const Matrix3d& other) : Matrix3d(other)
{
    // Q_ASSERT( isSymmetric()==true);
    assert( isSymmetric()==true );
}

Matrix3d ConicBase::skew( const Vector3d &x) const
{
    return (Matrix3d() << 0.,-x(2),x(1), x(2),0.,-x(0), -x(1),x(0),0.).finished();
}

bool ConicBase::isSymmetric() const
{
   constexpr double T_sym = 1e-6;

   return (*this -this->adjoint()).norm() < T_sym ;
}


ConicBase & ConicBase::operator=( const Matrix3d & other )
{
    // qDebug() << Q_FUNC_INFO;
    this->Matrix3d::operator= (other);
    Q_ASSERT( isSymmetric()==true );
    assert ( isSymmetric()==true );
    return *this;
}

Matrix3d ConicBase::cof3( const Matrix3d &MM ) const
{
    Matrix3d Cof;

    Cof(0,0) = MM(1,1)*MM(2,2) -MM(2,1)*MM(1,2);
    Cof(0,1) = -( MM(1,0)*MM(2,2) -MM(2,0)*MM(1,2) );
    Cof(0,2) = MM(1,0)*MM(2,1) -MM(2,0)*MM(1,1);

    Cof(1,0) = -( MM(0,1)*MM(2,2) -MM(2,1)*MM(0,2) );
    Cof(1,1) = MM(0,0)*MM(2,2) -MM(2,0)*MM(0,2);
    Cof(1,2) = -(MM(0,0)*MM(2,1) -MM(2,0)*MM(0,1));

    Cof(2,0) = MM(0,1)*MM(1,2) -MM(1,1)*MM(0,2);
    Cof(2,1) = -(MM(0,0)*MM(1,2) -MM(1,0)*MM(0,2));
    Cof(2,2) = MM(0,0)*MM(1,1) -MM(1,0)*MM(0,1);

    return Cof;
}

Ellipse::Ellipse( const uPoint & ux, const double k2 )
{
    uPoint uy = ux.euclidean();

    // cofactor matrix is adjugate due to symmetry
    *this = cof3( k2*uy.Cov() -uy.v()*uy.v().adjoint() );
}



Hyperbola::Hyperbola( const uStraightLine & ul, const double k2 )
{
    uStraightLine um = ul.euclidean();
    *this = k2*um.Cov() -um.v()*um.v().adjoint();
}


Vector3d Hyperbola::centerline() const
{
    const Vector3d x0 = center();
    const double phi_ = angle_rad();
    const double nx = -std::sin(phi_);
    const double ny =  std::cos(phi_);

    return { nx, ny, -nx*x0(0)/x0(2) -ny*x0(1)/x0(2) };
}

double Hyperbola::angle_rad() const
{
    return 0.5*atan2( 2.0*coeff(0,1), coeff(0,0)-coeff(1,1));
}

double Hyperbola::angle_deg() const
{
    constexpr double rho = 180./3.14159;
    return rho * 0.5 * atan2( 2.0*coeff(0,1), coeff(0,0)-coeff(1,1));
}

std::pair<double,double>
Hyperbola::lengthsSemiAxes() const
{
    auto ev = eigenvalues();
    double Delta = determinant();
    double D = topLeftCorner(2,2).determinant();
    assert( -Delta/( ev.first*D ) >= 0. );
    assert( +Delta/( ev.second*D) >= 0. );
    double a = std::sqrt( -Delta/( ev.first*D) );
    double b = std::sqrt( +Delta/( ev.second*D) );

    return {a,b};
}


Vector3d ConicBase::polar( const Vector3d &x ) const
{
    return (*this*x); // i.e., l = C*x
}

bool ConicBase::isCentral() const
{
    return topLeftCorner(2,2).determinant() !=0.;
}

bool ConicBase::isProper() const
{
    return determinant() !=0.;
}

bool ConicBase::isEllipse() const
{
    return topLeftCorner(2,2).determinant()>0.;
}

bool ConicBase::isHyperbola() const
{
    return topLeftCorner(2,2).determinant()<0.;
}

bool ConicBase::isParabola() const
{
    return topLeftCorner(2,2).determinant()==0.;
}



std::pair<double, double> Hyperbola::eigenvalues() const
{
    double p =  -topLeftCorner(2,2).trace();
    double q =   topLeftCorner(2,2).determinant();

    Q_ASSERT_X( q<0.0, Q_FUNC_INFO, "no hyperbola");
    assert( q<0.0 );
    double ev0 = -p/2 -std::sqrt( p*p/4 -q);
    double ev1 = -p/2 +std::sqrt( p*p/4 -q);

    return {ev0,ev1};
}


Vector3d ConicBase::center() const
{
    Vector3d xh;
    if ( isCentral() ) {
        Matrix2d C33 = topLeftCorner(2,2);
        Vector2d ch0 = topRightCorner(2,1);
        Vector2d x0 = -C33.ldlt().solve(ch0);
        xh << x0(0), x0(1), 1.0;
    }
    else {
        qDebug() << "! no central conic";  // TODO(meijoc)
        xh << 0,0,0;
    }
    return xh;
}


std::pair<Vector3d,Vector3d>
ConicBase::intersect( const Vector3d &l) const
{
    Matrix3d MM = skew(l);
    Matrix3d BB = MM.adjoint()*(*this)*MM;

    int idx;       // [den,idx] = max( abs(l) );
    double den = l.array().abs().maxCoeff(&idx);

    // minors ...............................................
    double alpha=0;
    switch (idx) {
    case 0:
        alpha = BB(1,1)*BB(2,2) -BB(2,1)*BB(1,2);
        break;
    case 1:
        alpha = BB(0,0)*BB(2,2) -BB(2,0)*BB(0,2);
        break;
    case 2:
        alpha = BB(0,0)*BB(1,1) -BB(1,0)*BB(0,1);
        break;
    default:
        Q_ASSERT_X( false, "ConicBase::intersect",
                    "intersection of conic and straight line: index out of range");
    }

    // intersection points .......................................
    Q_ASSERT( alpha <= 0 );
    Q_ASSERT(   den >  0 );
    Matrix3d DD = BB +std::sqrt(-alpha)/den*MM;
    int r;
    int c;
    DD.array().abs().maxCoeff( &r, &c);
    Vector3d p = DD.row(r);
    Vector3d q = DD.col(c);

    return {p,q};
}


void ConicBase::transform( const Matrix3d &HH )
{
    Matrix3d TT = cof3(HH);  // cf. PCV (6.57)
    *this = TT*(*this)*TT.transpose();
}



void Ellipse::scale( const double s )
{
    Q_ASSERT( s>=0.0 );
    Vector3d c = center();
    c /= c(2);
    const Matrix3d HH = ( Matrix3d() <<
                          s,   0.0,  c(0)-s*c(0),
                          0.0,   s,  c(1)-s*c(1),
                          0.0, 0.0,          1.0 ).finished();
    this->transform(HH);
}


std::pair<Eigen::VectorXd, Eigen::VectorXd>
Ellipse::poly( const int N) const
{
    Matrix2d Chh = topLeftCorner(2,2);
    Vector2d ch0 = topRightCorner(2,1);
    Vector2d x0 = -Chh.ldlt().solve(ch0);  // centre point
    double c00q =  coeff(2,2) -ch0.dot( Chh.ldlt().solve(ch0));

    assert( std::fabs(c00q)>0. );

    Eigen::EigenSolver<Matrix2d> eig( -Chh/c00q, true);
    Vector2d ev = eig.eigenvalues().real();
    Matrix2d RR = eig.eigenvectors().real();

    if ( ev(0)<0.0 ) {
        ev *= -1;
    }

    const double two_pi = 2*3.14159;
    Eigen::VectorXd t = Eigen::VectorXd::LinSpaced( N, 0, two_pi);

    Eigen::MatrixXd  x(2,N);
    x.row(0) = t.array().sin()/std::sqrt(ev(0));
    x.row(1) = t.array().cos()/std::sqrt(ev(1));

    x = RR*x;                    // rotation
    x.row(0).array() += x0(0);   // translation
    x.row(1).array() += x0(1);

    return { x.row(0), x.row(1)};
}

} // namespace Conic