/*
 * C function find_intersectionsso.c -
 *
 * Calculates intersections between the spanwise segments of surface
 * Xp1,Yp1,Zp1 and the panels of surface Xp2,Yp2,Zp2.
 *
 * Inputs:
 * Xp1,Yp1,Zp1 and the panels of surface Xp2,Yp2,Zp2.
 * Xp1,Yp1,Zp1: m1*n1 array of wing or wake vertices
 * Xp2,Yp2,Zp2: m2*n2 array of fuselage vertices
 * xyzint: x*3 array of intersections. If surface Xp1,Yp1,Zp1 is a wing
 *     x=m+1. If it is a wake x can take any value, depending on the
 *     geometries.
 *
 * Outputs: This code does not return any outputs. The results of its 
 *          calculations are stored in input array Aphi, xyzint
 *
 * Compile this by typing at the terminal 
 * gcc -fPIC -shared -o find_intersectionsso.so find_intersectionsso.c
 *
 * This is a C language file to be used with the SDPMflut 
 * distribution.
 * Copyright (C) 2024-2026 Grigorios Dimitriadis 
 *
 * 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 Affero 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 "math.h"

void cross(double xcrossy[], double x[], double y[])
    /* Calculates the cross product between vectors x and y */
{
    xcrossy[0]=x[1]*y[2]-x[2]*y[1];  /* x-component of cross product */
    xcrossy[1]=x[2]*y[0]-x[0]*y[2];  /* y-component of cross product */
    xcrossy[2]=x[0]*y[1]-x[1]*y[0];  /* z-component of cross product */
    return;
}

int in_triangle(double xl[], double yl[], double xat0, double yat0, double tol)
    /* Determines if point xat0,yat0 lies inside triangle defined by vertices in
    * xl,yl, allowing a tolerance tol. */
{
    double r1[2],r2[2],r3[2],r0[2],r12[2],r23[2],r31[2],cond[3];
    int iv,inside;
    /* Assemble the three vertices of the triangle in vectors r1, r2, r3 */
    r1[0]=xl[0];
    r1[1]=yl[0];
    r2[0]=xl[1];
    r2[1]=yl[1];
    r3[0]=xl[2];
    r3[1]=yl[2];
    /* Assemble point xat0, yat0 in vector r0 */
    r0[0]=xat0;
    r0[1]=yat0;
    /* Calculate distances between vertices */
    for (iv=0; iv < 2; iv++) {
        r12[iv]=r1[iv]-r2[iv];
        r23[iv]=r2[iv]-r3[iv];
        r31[iv]=r3[iv]-r1[iv];
    }
    /* Condition for lying on or inside segment 23 */
    cond[0]=(r31[0]*r23[1]-r31[1]*r23[0])*((r3[0]-r0[0])*r23[1]-(r3[1]-r0[1])*r23[0]);
    /* Condition for lying on or inside segment 31 */
    cond[1]=(r12[0]*r31[1]-r12[1]*r31[0])*((r1[0]-r0[0])*r31[1]-(r1[1]-r0[1])*r31[0]);
    /* Condition for lying on or inside segment 12 */
    cond[2]=(r23[0]*r12[1]-r23[1]*r12[0])*((r2[0]-r0[0])*r12[1]-(r2[1]-r0[1])*r12[0]);

    /* Determine if all conditions are met, allowing for a small negative
    * tolerance */
    if(cond[0] > -tol && cond[1] > -tol && cond[2] > -tol){
        inside=1;
    }else{
        inside=0;
    }
    return inside;
}

void find_intersections(double *xyzint, double *Xp1, double *Yp1,
                        double *Zp1, double *Xp2, double *Yp2, double *Zp2,
                        int m1p1, int n1p1, int m2p1, int n2p1, int m3, int n3)
{
    double x0[2],y0[2],z0[2],x0l[2],y0l[2],z0l[2],x[3],y[3],z[3];
    double r1[3],r2[3],normal[3],tau1[3],tau2[3],xl[3],yl[3],zl[3];
    double xc,yc,zc,area,sat0,xat0,yat0,prodz0l,tauw[3];
    double xat0g,yat0g,zat0g,mag;
    int    m1,m2,n1,n2,i1,j1,i2,j2,iv,inside,nint;

    /* Calculate sizes of Xp1, Xp2 matrices minus 1 */
    m1=m1p1-1;
    m2=m2p1-1;
    n1=n1p1-1;
    n2=n2p1-1;

    /* Initialize number of intersections */
    nint=0;
    /* Cycle through the spanwise edges of body 1 */
    for (i1=0; i1 < m1p1; i1++) {
        for (j1=0; j1 < n1; j1++) {
            /* Line segment of body 1 */
            x0[0]= *(Xp1+i1*n1p1+j1);
            y0[0]= *(Yp1+i1*n1p1+j1);
            z0[0]= *(Zp1+i1*n1p1+j1);
            x0[1]= *(Xp1+i1*n1p1+(j1+1));
            y0[1]= *(Yp1+i1*n1p1+(j1+1));
            z0[1]= *(Zp1+i1*n1p1+(j1+1));
            /* Cycle through all panels of body 2 */
            for (i2=0; i2 < m2; i2++) {
                for (j2=0; j2 < n2; j2++) {
                    inside=0;   /* Set inside to 0, even if area=0 */
                    /* Split each panel into two planar triangles
                    * Start with first triangle */
                    x[0]= *(Xp2+i2*n2p1+j2);
                    x[1]= *(Xp2+i2*n2p1+(j2+1));
                    x[2]= *(Xp2+(i2+1)*n2p1+(j2+1));
                    y[0]= *(Yp2+i2*n2p1+j2);
                    y[1]= *(Yp2+i2*n2p1+(j2+1));
                    y[2]= *(Yp2+(i2+1)*n2p1+(j2+1));
                    z[0]= *(Zp2+i2*n2p1+j2);
                    z[1]= *(Zp2+i2*n2p1+(j2+1));
                    z[2]= *(Zp2+(i2+1)*n2p1+(j2+1));
                    /* Calculate area of triangle */
                    area=fabs(1/2.0*(x[0]*(y[1]-y[2])+x[1]*(y[2]-y[0])+x[2]*(y[0]-y[1])));
                    /* The nose and tail of the fuselage only have one triangle
                    * but four vertices. This means that one of the two
                    * triangles has zero area. Exclude this from the
                    * calculation. */
                    if(area > 1e-16) {
                        /* Calculate control point of current triangle */
                        xc=(x[0]+x[1]+x[2])/3.0;
                        yc=(y[0]+y[1]+y[2])/3.0;
                        zc=(z[0]+z[1]+z[2])/3.0;
                        /* Calculate unit vector normal to the triangle's plane */
                        r1[0]=x[2]-x[0];
                        r1[1]=y[2]-y[0];
                        r1[2]=z[2]-z[0];
                        r2[0]=x[1]-x[0];
                        r2[1]=y[1]-y[0];
                        r2[2]=z[1]-z[0];
                        cross(normal,r1,r2);
                        mag=sqrt(normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2]);
                        normal[0]=normal[0]/mag;
                        normal[1]=normal[1]/mag;
                        normal[2]=normal[2]/mag;
                        /* Calculate first tangent unit vector parallel to side 1 */
                        mag=sqrt(r2[0]*r2[0]+r2[1]*r2[1]+r2[2]*r2[2]);
                        tau1[0]=r2[0]/mag;
                        tau1[1]=r2[1]/mag;
                        tau1[2]=r2[2]/mag;
                        /* Calculate second tangent unit vector orthogonal to normal and tau1 */
                        cross(tau2,normal,tau1);
                        /* Calculate coordinates of panel vertices in local panel coordinates */
                        for (iv=0; iv < 3; iv++){
                            xl[iv]=(x[iv]-xc)*tau1[0]+(y[iv]-yc)*tau1[1]+(z[iv]-zc)*tau1[2];
                            yl[iv]=(x[iv]-xc)*tau2[0]+(y[iv]-yc)*tau2[1]+(z[iv]-zc)*tau2[2];
                            zl[iv]=(x[iv]-xc)*normal[0]+(y[iv]-yc)*normal[1]+(z[iv]-zc)*normal[2];
                        }
                        /* Calculate coordinates of line segment in panel's local frame of
                        * reference */
                        for (iv=0; iv < 2; iv++){
                            x0l[iv]=(x0[iv]-xc)*tau1[0]+(y0[iv]-yc)*tau1[1]+(z0[iv]-zc)*tau1[2];
                            y0l[iv]=(x0[iv]-xc)*tau2[0]+(y0[iv]-yc)*tau2[1]+(z0[iv]-zc)*tau2[2];
                            z0l[iv]=(x0[iv]-xc)*normal[0]+(y0[iv]-yc)*normal[1]+(z0[iv]-zc)*normal[2];
                        }
                        /* Calculate product of elements of z0l */
                        prodz0l=z0l[0]*z0l[1];
                        /* Calculate unit vector tangent to line passing through wing points */
                        tauw[0]=x0l[1]-x0l[0];
                        tauw[1]=y0l[1]-y0l[0];
                        tauw[2]=z0l[1]-z0l[0];
                        mag=sqrt(tauw[0]*tauw[0]+tauw[1]*tauw[1]+tauw[2]*tauw[2]);
                        tauw[0]=tauw[0]/mag;
                        tauw[1]=tauw[1]/mag;
                        tauw[2]=tauw[2]/mag;
                        /* Calculate intersection between extension of line
                        * x0l,y0l,z0l and plane at zl=0.
                        * Equation of line is [x0l(1) y0l(1) z0l(1)]+sat0*tauw=[xat0 yat0 0]
                        * Third equation: z0l(1)+sat0*tauw(3)=0 */
                        sat0= -z0l[0]/tauw[2];
                        /* First equation */
                        xat0=x0l[0]+sat0*tauw[0];
                        /* Second equation */
                        yat0=y0l[0]+sat0*tauw[1];
                        /* mexPrintf("%f, %f, %f,  %f \n",area,sat0,xat0,yat0); */
                        /* Determine if point (xat0,yat0) lies inside current panel */
                        inside=in_triangle(xl,yl,xat0,yat0,1e-12);
                        if(inside==1 && prodz0l < 0.0) { /* Only counts as an intersection if the signs of z0l are different */
                            /* mexPrintf("%i  %i  %i  %i \n",i1,j1,i2,j2); */
                            nint++;
                            /* Calculate coordinates of intersection point in global
                            * coordinates */
                            xat0g=tau1[0]*xat0+tau2[0]*yat0+xc;
                            yat0g=tau1[1]*xat0+tau2[1]*yat0+yc;
                            zat0g=tau1[2]*xat0+tau2[2]*yat0+zc;
                            /* Store coordinates of intersection point */
                            *(xyzint+i1*n3)=xat0g;
                            *(xyzint+i1*n3+1)=yat0g;
                            *(xyzint+i1*n3+2)=zat0g;
                        }
                    }

                    if (inside == 0){
                        /* If we have not found an intersection yet, check the second triangle */
                        x[0]= *(Xp2+(i2+1)*n2p1+(j2+1));
                        x[1]= *(Xp2+(i2+1)*n2p1+j2);
                        x[2]= *(Xp2+i2*n2p1+j2);
                        y[0]= *(Yp2+(i2+1)*n2p1+(j2+1));
                        y[1]= *(Yp2+(i2+1)*n2p1+j2);
                        y[2]= *(Yp2+i2*n2p1+j2);
                        z[0]= *(Zp2+(i2+1)*n2p1+(j2+1));
                        z[1]= *(Zp2+(i2+1)*n2p1+j2);
                        z[2]= *(Zp2+i2*n2p1+j2);
                        /* Calculate area of triangle */
                        area=fabs(1/2.0*(x[0]*(y[1]-y[2])+x[1]*(y[2]-y[0])+x[2]*(y[0]-y[1])));
                        /* The nose and tail of the fuselage only have one triangle
                        * but four vertices. This means that one of the two
                        * triangles has zero area. Exclude this from the
                        * calculation. */
                        if(area > 1e-16) {
                            /* Calculate control point of current triangle */
                            xc=(x[0]+x[1]+x[2])/3.0;
                            yc=(y[0]+y[1]+y[2])/3.0;
                            zc=(z[0]+z[1]+z[2])/3.0;
                            /* Calculate unit vector normal to the triangle's plane */
                            r1[0]=x[2]-x[0];
                            r1[1]=y[2]-y[0];
                            r1[2]=z[2]-z[0];
                            r2[0]=x[1]-x[0];
                            r2[1]=y[1]-y[0];
                            r2[2]=z[1]-z[0];
                            cross(normal,r1,r2);
                            mag=sqrt(normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2]);
                            normal[0]=normal[0]/mag;
                            normal[1]=normal[1]/mag;
                            normal[2]=normal[2]/mag;
                            /* Calculate first tangent unit vector parallel to side 1 */
                            mag=sqrt(r2[0]*r2[0]+r2[1]*r2[1]+r2[2]*r2[2]);
                            tau1[0]=r2[0]/mag;
                            tau1[1]=r2[1]/mag;
                            tau1[2]=r2[2]/mag;
                            /* Calculate second tangent unit vector orthogonal to normal and tau1 */
                            cross(tau2,normal,tau1);
                            /* Calculate coordinates of panel vertices in local panel coordinates */
                            for (iv=0; iv < 3; iv++){
                                xl[iv]=(x[iv]-xc)*tau1[0]+(y[iv]-yc)*tau1[1]+(z[iv]-zc)*tau1[2];
                                yl[iv]=(x[iv]-xc)*tau2[0]+(y[iv]-yc)*tau2[1]+(z[iv]-zc)*tau2[2];
                                zl[iv]=(x[iv]-xc)*normal[0]+(y[iv]-yc)*normal[1]+(z[iv]-zc)*normal[2];
                            }
                            /* Calculate coordinates of line segment in panel's local frame of
                            * reference */
                            for (iv=0; iv < 2; iv++){
                                x0l[iv]=(x0[iv]-xc)*tau1[0]+(y0[iv]-yc)*tau1[1]+(z0[iv]-zc)*tau1[2];
                                y0l[iv]=(x0[iv]-xc)*tau2[0]+(y0[iv]-yc)*tau2[1]+(z0[iv]-zc)*tau2[2];
                                z0l[iv]=(x0[iv]-xc)*normal[0]+(y0[iv]-yc)*normal[1]+(z0[iv]-zc)*normal[2];
                            }
                            /* Calculate product of elements of z0l */
                            prodz0l=z0l[0]*z0l[1];
                            /* Calculate unit vector tangent to line passing through wing points */
                            tauw[0]=x0l[1]-x0l[0];
                            tauw[1]=y0l[1]-y0l[0];
                            tauw[2]=z0l[1]-z0l[0];
                            mag=sqrt(tauw[0]*tauw[0]+tauw[1]*tauw[1]+tauw[2]*tauw[2]);
                            tauw[0]=tauw[0]/mag;
                            tauw[1]=tauw[1]/mag;
                            tauw[2]=tauw[2]/mag;
                            /* Calculate intersection between extension of line
                            * x0l,y0l,z0l and plane at zl=0.
                            * Equation of line is [x0l(1) y0l(1) z0l(1)]+sat0*tauw=[xat0 yat0 0]
                            * Third equation: z0l(1)+sat0*tauw(3)=0 */
                            sat0= -z0l[0]/tauw[2];
                            /* First equation */
                            xat0=x0l[0]+sat0*tauw[0];
                            /* Second equation */
                            yat0=y0l[0]+sat0*tauw[1];
                            /* mexPrintf("%f, %f, %f,  %f \n",area,sat0,xat0,yat0); */
                            /* Determine if point (xat0,yat0) lies inside current panel */
                            inside=in_triangle(xl,yl,xat0,yat0,1e-12);
                            if(inside==1 && prodz0l < 0.0) { /* Only counts as an intersection if the signs of z0l are different */
                                /* mexPrintf("%i  %i  %i  %i \n",i1,j1,i2,j2); */
                                nint++;
                                /* Calculate coordinates of intersection point in global
                                * coordinates */
                                xat0g=tau1[0]*xat0+tau2[0]*yat0+xc;
                                yat0g=tau1[1]*xat0+tau2[1]*yat0+yc;
                                zat0g=tau1[2]*xat0+tau2[2]*yat0+zc;
                                /* Store coordinates of intersection point */
                                *(xyzint+i1*n3)=xat0g;
                                *(xyzint+i1*n3+1)=yat0g;
                                *(xyzint+i1*n3+2)=zat0g;
                            }
                        }
                    }
                }
            }
        }
    }
}