#pragma once
#ifndef ELLIPSOID_JACOBIAN_AD_H__
#define ELLIPSOID_JACOBIAN_AD_H__

#include "ellipsoid.h"

// Modified from ellipsoid.h
// Computing the Jacobian with AD

using FloatAD2 = SimpleAD::Scalar<Float, 2>;
using Vector3AD2 = SimpleAD::Matrix<Float, 3, 1, 2>;
using Array3AD2 = SimpleAD::Array1D<Float, 3, 2>;

struct EllipseJacobianAD {
    EllipseJacobianAD() {}

    EllipseJacobianAD(const Vector3 &C1, const Vector3 &C2, const Vector3 &C3,
        const Vector3AD2 &_O, const Vector3AD2 &abc, 
        const FloatAD2 &tau, const FloatAD2 &phi)
        : O(_O) { init(C1, C2, C3, abc, tau, phi); }

    void init(const Vector3 &C1, const Vector3 &C2, const Vector3 &C3,
        const Vector3AD2 &abc, const FloatAD2 &tau, const FloatAD2 &phi);

    Float computeJacobian();

    // utils
    FloatAD2 quadDotAD(const Vector3AD2 &u, const Vector3AD2 &v, const Vector3AD2 &diag) const;

    Vector3AD2 O, TN, UN;
    FloatAD2 theta, m1, m2;

    FloatAD2 u, v;
};

struct EllipsoidJacobianAD {
    EllipsoidJacobianAD(const Vector3 &_f1, const Vector3 &_f2, 
            const FloatAD2 &_tau, const FloatAD2 &_phi)
        : f1(_f1), f2(_f2), tau(_tau), phi(_phi) { init(); }

    void init();

    EllipseJacobianAD intersectTri(const Vector3 &C1, const Vector3 &C2, const Vector3 &C3);

    // utils
    Vector3AD2 worldToEllipsoidAD(const Vector3AD2 &p_world) const;
    Vector3AD2 ellipsoidToWorldAD(const Vector3AD2 &p_ellipsoid) const;
    Vector3AD2 worldToSphereAD(const Vector3AD2 &p_world) const;
    Vector3AD2 sphereToWorldAD(const Vector3AD2 &p_sphere) const;

    Vector3 f1, f2;
    FloatAD2 tau, phi;

    Vector3 center;
    FloatAD2 a, b, c;

    Frame ellipsoid_frame;
};

#endif