// 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/bending_energy.h"
#include "shell-energy/auxiliary.h"

void shell::bending_deformed_gradient( const Eigen::MatrixXd &V_undef, const Eigen::MatrixXd &V_def,
                                       const Eigen::MatrixXi &F, const Eigen::MatrixXi &uE,
                                       const Eigen::VectorXi &EMAP, const Eigen::MatrixXi &EF,
                                       const Eigen::MatrixXi &EI, Eigen::MatrixXd &grad ) {
  grad.resize( V_def.rows(), V_def.cols());
  grad.setZero();

  for ( int edgeIdx = 0; edgeIdx < uE.rows(); ++edgeIdx ) {
    // Check for boundary edge
    if ( EF( edgeIdx, 0 ) == -1 || EF( edgeIdx, 1 ) == -1 )
      continue;

    int pi = uE( edgeIdx, 0 );
    int pj = uE( edgeIdx, 1 );
    int pk = F( EF( edgeIdx, 0 ), EI( edgeIdx, 0 ));
    int pl = F( EF( edgeIdx, 1 ), EI( edgeIdx, 1 ));

    // set up vertices and edges
    Eigen::Vector3d Pi, Pj, Pk, Pl, temp;

    // first get undefomed quantities
    Pi = V_undef.row( pi );
    Pj = V_undef.row( pj );
    Pk = V_undef.row( pk );
    Pl = V_undef.row( pl );
    double delTheta = getDihedralAngle( Pi, Pj, Pk, Pl );


    // compute Ak + Al
    temp = ( Pk - Pj ).cross( Pi - Pk );
    double vol = temp.norm() / 2.;
    temp = ( Pl - Pi ).cross( Pj - Pl );
    vol += temp.norm() / 2.;
    double elengthSqr = ( Pj - Pi ).dot( Pj - Pi );

    // now get the deformed values
    Pi = V_def.row( pi );
    Pj = V_def.row( pj );
    Pk = V_def.row( pk );
    Pl = V_def.row( pl );

    // compute weighted differnce of dihedral angles
    delTheta -= getDihedralAngle( Pi, Pj, Pk, Pl );
    delTheta *= -6. * elengthSqr / vol;

    // compute first derivatives of dihedral angle
    Eigen::Vector3d thetak, thetal, thetai, thetaj;
    getThetaGradK( Pi, Pj, Pk, thetak );
    getThetaGradK( Pj, Pi, Pl, thetal );
    getThetaGradI( Pi, Pj, Pk, Pl, thetai );
    getThetaGradJ( Pi, Pj, Pk, Pl, thetaj );

    // assemble in global vector
    for ( int i : { 0, 1, 2 } ) {
      grad( pi, i ) += delTheta * thetai[i];
      grad( pj, i ) += delTheta * thetaj[i];
      grad( pk, i ) += delTheta * thetak[i];
      grad( pl, i ) += delTheta * thetal[i];
    }
  }
}

void shell::bending_undeformed_gradient( const Eigen::MatrixXd &V_undef, const Eigen::MatrixXd &V_def,
                                         const Eigen::MatrixXi &F, const Eigen::MatrixXi &uE,
                                         const Eigen::VectorXi &EMAP, const Eigen::MatrixXi &EF,
                                         const Eigen::MatrixXi &EI, Eigen::MatrixXd &grad ) {
  grad.resize( V_def.rows(), V_def.cols());
  grad.setZero();

  for ( int edgeIdx = 0; edgeIdx < uE.rows(); ++edgeIdx ) {
    // Check for boundary edge
    if ( EF( edgeIdx, 0 ) == -1 || EF( edgeIdx, 1 ) == -1 )
      continue;

    int pi = uE( edgeIdx, 0 );
    int pj = uE( edgeIdx, 1 );
    int pk = F( EF( edgeIdx, 0 ), EI( edgeIdx, 0 ));
    int pl = F( EF( edgeIdx, 1 ), EI( edgeIdx, 1 ));

    // set up vertices and edges
    Eigen::Vector3d Pi, Pj, Pk, Pl, temp;

    // first get defomed quantities
    Pi = V_def.row( pi );
    Pj = V_def.row( pj );
    Pk = V_def.row( pk );
    Pl = V_def.row( pl );
    double delTheta = getDihedralAngle( Pi, Pj, Pk, Pl );

    // now get the undeformed values
    Pi = V_undef.row( pi );
    Pj = V_undef.row( pj );
    Pk = V_undef.row( pk );
    Pl = V_undef.row( pl );

    // compute Ak + Al, |e|^2 and diff. o dihedral angles
    temp = ( Pk - Pj ).cross( Pi - Pk );
    double vol = temp.norm() / 2.;
    temp = ( Pl - Pi ).cross( Pj - Pl );
    vol += temp.norm() / 2.;
    double elengthSqr = ( Pj - Pi ).dot( Pj - Pi );
    // note sign here!
    delTheta -= getDihedralAngle( Pi, Pj, Pk, Pl );

    // derivatives    
    Eigen::Vector3d gradk, gradl, gradi, gradj, gradTheta, gradArea;
    double factorGradTheta = -2. * delTheta * elengthSqr / vol;
    double factorGradArea = -1. * delTheta * delTheta * elengthSqr / ( vol * vol );
    double factorGradEdgeLengthSqr = 2. * delTheta * delTheta / vol;

    // d_k
    getThetaGradK( Pi, Pj, Pk, gradTheta );
    getAreaGradK( Pi, Pj, Pk, gradArea );
    gradk = factorGradTheta * gradTheta + factorGradArea * gradArea;

    // d_l
    getThetaGradK( Pj, Pi, Pl, gradTheta );
    getAreaGradK( Pj, Pi, Pl, gradArea );
    gradl = factorGradTheta * gradTheta + factorGradArea * gradArea;

    // d_i
    getThetaGradI( Pi, Pj, Pk, Pl, gradTheta );
    getAreaGradK( Pj, Pk, Pi, gradArea );
    gradi = factorGradTheta * gradTheta + factorGradArea * gradArea;
    getAreaGradK( Pl, Pj, Pi, gradArea );
    gradi += factorGradArea * gradArea;
    gradi += factorGradEdgeLengthSqr * ( Pi - Pj );

    // d_j
    getThetaGradJ( Pi, Pj, Pk, Pl, gradTheta );
    getAreaGradK( Pk, Pi, Pj, gradArea );
    gradj = factorGradTheta * gradTheta + factorGradArea * gradArea;
    getAreaGradK( Pi, Pl, Pj, gradArea );
    gradj += factorGradArea * gradArea;
    gradj += factorGradEdgeLengthSqr * ( Pj - Pi );

    // assemble in global vector
    for ( int i = 0; i < 3; i++ ) {
      grad( pi, i ) += 3 * gradi[i];
      grad( pj, i ) += 3 * gradj[i];
      grad( pk, i ) += 3 * gradk[i];
      grad( pl, i ) += 3 * gradl[i];
    }

  }
}