// This file is part of GOAST, a C++ library for variational methods in Geometry Processing
//
// Copyright (C) 2021 Behrend Heeren & Josua Sassen, University of Bonn <goast@ins.uni-bonn.de>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#include <array>

#include "shell-energy/auxiliary.h"

namespace shell{

  double getDihedralAngle( const Eigen::Vector3d &nk, const Eigen::Vector3d &nl, const Eigen::Vector3d &e ) {
    Eigen::Vector3d crossprod;
    crossprod = nk.cross( nl );
    double aux = std::max( std::min( nk.dot( nl ), 1. ), -1. );
    return crossprod.dot( e ) < 0. ? -std::acos( aux ) : std::acos( aux );
  }

  double getDihedralAngle( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                           const Eigen::Vector3d &Pl ) {
    Eigen::Vector3d nk, nl;
    nk = ( Pk - Pj ).cross( Pi - Pk );
    nk.normalize();
    nl = ( Pl - Pi ).cross( Pj - Pl );
    nl.normalize();
    return getDihedralAngle( nk, nl, Pj - Pi );
  }

  double getAreaSqr( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk ) {
    Eigen::Vector3d temp;
    temp = ( Pk - Pj ).cross( Pi - Pk );
    return 0.25 * temp.squaredNorm();
  }

  double getArea( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk ) {
    return std::sqrt( getAreaSqr( Pi, Pj, Pk ));
  }

  void getNormal( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                  Eigen::Vector3d &normal ) {
    normal = ( Pk - Pj ).cross( Pi - Pk );
    normal.normalize();
  }

  void getCrossOp( const Eigen::Vector3d &a, Eigen::Matrix3d &matrix ) {
    matrix.setZero();
    matrix( 0, 1 ) = -a[2];
    matrix( 0, 2 ) = a[1];
    matrix( 1, 0 ) = a[2];
    matrix( 1, 2 ) = -a[0];
    matrix( 2, 0 ) = -a[1];
    matrix( 2, 1 ) = a[0];
  }

  void getProjection( const Eigen::Vector3d &x, Eigen::Matrix3d &m ) {
    m.setIdentity();
    m -= x * x.transpose();
  }

  void getReflection( const Eigen::Vector3d &x, Eigen::Matrix3d &m ) {
    m.setIdentity();
    m -= 2. * x * x.transpose();
  }

  void getAreaGradient( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                        Eigen::Vector3d &grad ) {
    Eigen::Vector3d normal = ( Pk - Pj ).cross( Pi - Pk );
    normal /= 2. * normal.norm();
    grad = normal.cross( Pj - Pi );
  }

  void getAreaGradK( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                     Eigen::Vector3d &grad ) {
    Eigen::Vector3d a = Pi - Pk, d = Pk - Pj, e = Pj - Pi;
    double area = getArea( Pi, Pj, Pk );
    double temp1 = -0.25 * e.dot( a ) / area, temp2 = 0.25 * e.dot( d ) / area;
    grad = temp1 * d + temp2 * a;
  }

  void getThetaGradK( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                      Eigen::Vector3d &grad ) {
    Eigen::Vector3d e( Pj - Pi );
    getNormal( Pi, Pj, Pk, grad );
    grad *= -0.5 * e.norm() / getArea( Pi, Pj, Pk );
  }

  void getThetaGradILeftPart( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                              Eigen::Vector3d &grad ) {
    Eigen::Vector3d e = Pj - Pi, d = Pk - Pj;
    getThetaGradK( Pi, Pj, Pk, grad );
    grad *= d.dot( e ) / e.squaredNorm();
  }

  void getThetaGradJLeftPart( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                              Eigen::Vector3d &grad ) {
    Eigen::Vector3d e = Pj - Pi, a = Pi - Pk;
    getThetaGradK( Pi, Pj, Pk, grad );
    grad *= a.dot( e ) / e.squaredNorm();
  }

  void getThetaGradI( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                      const Eigen::Vector3d &Pl, Eigen::Vector3d &grad ) {
    Eigen::Vector3d temp;
    getThetaGradILeftPart( Pi, Pj, Pk, grad );
    getThetaGradILeftPart( Pi, Pj, Pl, temp );
    grad -= temp;
  }

  void getThetaGradJ( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                      const Eigen::Vector3d &Pl, Eigen::Vector3d &grad ) {
    Eigen::Vector3d temp;
    getThetaGradJLeftPart( Pi, Pj, Pk, grad );
    getThetaGradJLeftPart( Pi, Pj, Pl, temp );
    grad -= temp;
  }

  void getHessAreaKK( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                      Eigen::Matrix3d &Hess ) {
    Eigen::Vector3d eNormalized( Pj - Pi ), gradAreaK;
    Eigen::Matrix3d proj;
    eNormalized.normalize();

    getAreaGradK( Pi, Pj, Pk, gradAreaK );
    Hess = gradAreaK * gradAreaK.transpose();

    getProjection( eNormalized, proj );
    Hess += -0.25 * ( Pj - Pi ).dot( Pj - Pi ) * proj;
    Hess *= -1. / getArea( Pi, Pj, Pk );
  }

  void getHessAreaIK( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                      Eigen::Matrix3d &Hess ) {
    Eigen::Vector3d e( Pj - Pi ), d( Pk - Pj ), temp1, temp2;
    getAreaGradK( Pj, Pk, Pi, temp1 );
    getAreaGradK( Pi, Pj, Pk, temp2 );
    Hess = temp1 * temp2.transpose();

    Hess += 0.25 * e * d.transpose();
    Hess.diagonal().array() -= 0.25 * d.dot( e );
    Hess *= -1. / getArea( Pi, Pj, Pk );

    Eigen::Matrix3d auxMat;
    getNormal( Pi, Pj, Pk, temp1 );
    getCrossOp( temp1, auxMat );
    Hess += 0.5 * auxMat;
  }


  void getHessThetaKK( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                       Eigen::Matrix3d &Hkk ) {
    double areaSqr = getArea( Pi, Pj, Pk );
    areaSqr *= areaSqr;

    Eigen::Vector3d e( Pj - Pi ), gradArea, normal;
    getAreaGradK( Pi, Pj, Pk, gradArea );
    getNormal( Pi, Pj, Pk, normal );

    Eigen::Matrix3d mat1, mat2;
    getCrossOp( e, mat1 );
    mat2 = gradArea * normal.transpose();

    Hkk = e.norm() / ( 4. * areaSqr ) * mat1 + e.norm() / areaSqr * mat2;
  }

  void getHessThetaIK( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                       Eigen::Matrix3d &Hik ) {

    double area = getArea( Pi, Pj, Pk );
    double areaSqr = area * area;

    Eigen::Vector3d e( Pj - Pi ), d( Pk - Pj ), gradArea, normal;
    getAreaGradK( Pj, Pk, Pi, gradArea );
    getNormal( Pi, Pj, Pk, normal );

    Eigen::Matrix3d mat1, mat2, mat3;
    mat1 = e * normal.transpose();
    getCrossOp( d, mat2 );
    mat3 = 1. / ( 2. * area * e.norm()) * mat1 + e.norm() / ( 4. * areaSqr ) * mat2;

    mat1 = gradArea * normal.transpose();
    Hik = mat3 + e.norm() / areaSqr * mat1;
  }

  void getHessThetaJK( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                       Eigen::Matrix3d &Hjk ) {

    double area = getArea( Pi, Pj, Pk );
    double areaSqr = area * area;

    Eigen::Vector3d e( Pi - Pj ), a( Pi - Pk ), gradArea, normal;
    getAreaGradK( Pk, Pi, Pj, gradArea );
    getNormal( Pi, Pj, Pk, normal );

    Eigen::Matrix3d mat1, mat2, mat3;
    mat1 = e * normal.transpose();
    getCrossOp( a, mat2 );
    mat3 = 1. / ( 2. * area * e.norm()) * mat1 + e.norm() / ( 4. * areaSqr ) * mat2;

    mat1 = gradArea * normal.transpose();
    Hjk = mat3 + e.norm() / areaSqr * mat1;
  }

  void getHessThetaILeftPartI( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                               Eigen::Matrix3d &HiLeft ) {
    Eigen::Vector3d e( Pj - Pi ), d( Pk - Pj ), eNormalized( Pj - Pi ), gradThetaK, temp;
    eNormalized.normalize();
    Eigen::Matrix3d mat1, mat2, Refl;
    getThetaGradK( Pi, Pj, Pk, gradThetaK );
    getReflection( eNormalized, Refl );
    temp = Refl * d;
    mat1 = temp * gradThetaK.transpose();
    getHessThetaIK( Pi, Pj, Pk, mat2 );
    HiLeft = -1. / e.squaredNorm() * mat1 + d.dot( e ) / e.squaredNorm() * mat2;
  }

  void getHessThetaJLeftPartI( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                               Eigen::Matrix3d &HjLeft ) {
    Eigen::Vector3d e( Pj - Pi ), d( Pk - Pj ), eNormalized( Pj - Pi ), gradThetaK, temp, thetak;
    eNormalized.normalize();
    Eigen::Matrix3d mat1, mat2, Refl;
    getThetaGradK( Pi, Pj, Pk, gradThetaK );
    getReflection( eNormalized, Refl );
    temp = Refl * ( d - e );
    mat1 = temp * gradThetaK.transpose();
    getHessThetaJK( Pi, Pj, Pk, mat2 );

    HjLeft = 1. / e.squaredNorm() * mat1 + d.dot( e ) / e.squaredNorm() * mat2;
  }

  void getHessThetaII( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                       const Eigen::Vector3d &Pl, Eigen::Matrix3d &Hii ) {
    Eigen::Matrix3d temp;
    getHessThetaILeftPartI( Pi, Pj, Pk, Hii );
    getHessThetaILeftPartI( Pi, Pj, Pl, temp );
    Hii -= temp;
  }

  void getHessThetaJI( const Eigen::Vector3d &Pi, const Eigen::Vector3d &Pj, const Eigen::Vector3d &Pk,
                       const Eigen::Vector3d &Pl, Eigen::Matrix3d &Hji ) {
    Eigen::Vector3d edge( Pj - Pi ), d( Pk - Pj ), c( Pj - Pl );
    Eigen::Vector3d diff( d - edge ), sum( c + edge );
    double eLengthSqr = edge.squaredNorm();

    //
    Eigen::Vector3d thetak, thetal, grad;
    getThetaGradK( Pi, Pj, Pk, thetak );
    getThetaGradK( Pj, Pi, Pl, thetal );
    grad = edge.dot( d ) * thetak - edge.dot( c ) * thetal;

    Eigen::Matrix3d Hjk, Hjl, tensorProduct;
    getHessThetaJK( Pi, Pj, Pk, Hjk );
    getHessThetaIK( Pj, Pi, Pl, Hjl );

    // Hess part
    Hji = edge.dot( d ) / eLengthSqr * Hjk.transpose() - edge.dot( c ) / eLengthSqr * Hjl.transpose();

    Hji += -2. / ( eLengthSqr * eLengthSqr ) * grad * edge.transpose();
    Hji += 1. / eLengthSqr * thetak * diff.transpose();
    Hji -= 1. / eLengthSqr * thetal * sum.transpose();
  }
}