@name Lightning Positioning System
@inputs 
@outputs Location:vector [Dist Angles Lengths Sum]:array Polygon:table 
@outputs G Gcos X Y Z X1 Y1 D I J R1 R2 R3 [Ey Ex P1 P2 P3 P4 P5 P6 P7 P8 Vector]:vector  
@persist [Beacon Dist]:array Polygon:table [Ey Ex P1 P2 P3 Vector]:vector
@trigger 
@model models/props_junk/PopCan01a.mdl
#[
function number trilaterateZ(P1:vector,P2:vector,P3:vector,R1,R2,R3){
    Ex=(P2-P1)/(P2:distance(P1)) #(vector2) Is the unit vector in the direction from P1 to P2 (vector2) equal to vec(cos(A)),sin(A),0)
    I=Ex:dot(P3-P1) #(number)Is the signed magnitude of the X component 

    Ey=((P3-P1)-I*Ex)/(P3-P1-I*Ex):length() #(vector2)is the unit vector in the Y direction note that points P1 and P2 and P3 are all in the Z=0 plane in the coordinate system
    D=P2:distance(P1)   #(number) round(P2-P1) #thex distance between P1 and P2
    J=Ey:dot(P3-P1) #(number) signed magnitude of the Y component in the coordinate system of the vector from P1 to P3
    X=(R1^2-R2^2+D^2)/(2*D)
    Y=(R1^2-R3^2-X^2+(X-I)^2+J^2)/(2*J)
    Z=sqrt(R1^2-X^2-Y^2)
    Ez=Ex:cross(Ey)
    
return Z
}]#

#this trilaterates with an unsigned Z axis
function vector trilaterate(P1:vector,P2:vector,P3:vector,R1,R2,R3){
    Ex=(P2-P1)/(P2:distance(P1)) #(vector2) Is the unit vector in the direction from P1 to P2 (vector2) equal to vec(cos(A)),sin(A),0)
    I=Ex:dot(P3-P1) #(number)Is the signed magnitude of the X component 
    #I=Ex:dot(P3-P1) #original
#(300,0,0)-(0,0,0) = (300,0,0) / 300 = (1,0,0)
#(1,0,0) ((0,100,0)-(0,0,0)) = 1*0 + 0*100 + 0*0 = 0
    #P3-P1 means the vector distance of 3 from 1
    Ey=((P3-P1)-I*Ex)/(P3-P1-I*Ex):length() #(vector2)is the unit vector in the Y direction note that points P1 and P2 and P3 are all in the Z=0 plane in the coordinate system
#((0,100,0)-(0*(1,0,0)) = (0,100,0) / ((0,100,0)-(0,0,0) - (0*(1,0,0))) = (0,100,0)/100=(0,1,0)
    D=P2:distance(P1)   #(number) round(P2-P1) #the distance between P1 and P2
#300
    J=Ey:dot(P3-P1) #(number) signed magnitude of the Y component in the coordinate system of the vector from P1 to P3
#(0,1,0) (0,100,0)-(0,0,0) = (0,1,0) (0,100,0) = 0*0 + 1*100 + 0*0 = 100
    X=(R1^2-R2^2+D^2)/(2*D)
#
    Y=(R1^2-R3^2-X^2+(X-I)^2+J^2)/(2*J)
    Z=sqrt(R1^2-X^2-Y^2)
    Ez=Ex:cross(Ey)
    
    
return P1+X*Ex+Y*Ey+Z*Ez 
}

#This function assumes the 4th node is not in the plane of the other 3. The 4th becomes the positive Z axis
function vector trilaterate(P1:vector,P2:vector,P3:vector,P4:vector,R1,R2,R3,R4){
    XYZ=trilaterate(P1,P2,P3,R1,R2,R3)
    
    R1a=P4:distance(P1)
    R2a=P4:distance(P2)
    R3a=P4:distance(P3)
    
    Z1=trilaterate(P1,P2,P3,R1a,R2a,R3a)
    if(!Z1:z()){
        print(round(P1)+" "+round(P2)+" "+" "+round(P3))
        print("Z axis sign cannot be determined. The 4th vector must be off the plane of the 1st 3.")
        return vec()
    }else{    
        #Side1=R4
        #Side2=R1
        #Side3=P4:distance(P1)
        XYZ1=vec(XYZ:x(),XYZ:y(),-XYZ:z())
        R41=P4:distance(XYZ)
        R42=P4:distance(XYZ1)
        
        Sign=1
    #[    if(R4==R41){
            Sign=1
        }]#
    
        if(R4==R42){
            Sign=-1
        }
    
        return vec(XYZ:x(),XYZ:y(),XYZ:z()*Sign) #Z needs to be fixed... 
    }
}





###########This function is incomplete, it's supposed to 
###########decide upon what 4 points, most esspecially if point 4 is off the plane of the first 3. 
#[function array pointSelect(Point:vector){ #Since that point should be unknown I'm not sure what I'm going to do.
    Distance=array()
    for(C=1,Beacon:count()){
         Distance[C,number]=Beacon[C,vector]:distance(Point)
    }
    Points=array()
    for(C=1,Beacon:count()){
        Points:pushVector(Beacon[Distance:minIndex(),vector])
        Distance:remove(Distance:minIndex())
    }
    
}]#

if(first()){
    
    Beacon=array()
    Beacon:pushVector(vec(0,0,0))         #P1
    Beacon:pushVector(vec(300,0,0))       #P2
    Beacon:pushVector(vec(0,200,0))       #P3
    Beacon:pushVector(vec(150,-444,10))  #P4
    Beacon:pushVector(vec(-100,-100,150)) #P5
    Beacon:pushVector(vec(304,-344,320))  #P6
    Beacon:pushVector(vec(342,440,232))   #P7
    Beacon:pushVector(vec(-32,44,22))     #P8
    Polygon=table()
    for(C=1,Beacon:count()){
        holoCreate(C,Beacon[C,vector])
        
        for(D=1,Beacon:count()){
            if((C!=D)&!Polygon[D+"-"+C,number]){
                Polygon[C+"-"+D,number]=Beacon[C,vector]:distance(Beacon[D,vector])
            }
        }
    }   
    holoColor(1,vec(255,0,0)) #red
    holoColor(2,vec(0,255,0)) #green
    holoColor(3,vec(0,0,255)) #blue
    holoColor(4,vec(255,255,0)) #yellow
    holoColor(5,vec(0,255,255)) #cyan
    holoColor(6,vec(255,0,255))  #magenta
    holoColor(7,vec(128,0,255))  #purple
    holoColor(8,vec(255,0,128))  #hot pink
    
    function number lawcos(G,A,B){ #the angle output is the angle opposite G
       return acos((G^2-A^2-B^2)/(-2*A*B))
    }
}

interval(100)
E=entity()
O=owner()
Location=O:pos()

#process the distance from beacon to target
function array vector:dist(){
    Dist=array()
    for(C=1,Beacon:count()){
        Dist[C,number]=This:distance(Beacon[C,vector]) 
    }    
    
    return Dist
}

Dist=O:pos():dist()

A=Polygon["1-2",number]
B=Polygon["1-3",number]
G=Polygon["2-3",number]

#Get angles via Law of Cosines

#AngG=acos((G^2-A^2-B^2)/(-2*A*B))
#AngB=acos((B^2-A^2-G^2)/(-2*A*G))
AngG=lawcos(G,A,B) #BGA angle
AngB=lawcos(B,A,G) #ABG
AngA=180-AngG-AngB #BAG
Angles=array(AngG,AngA,AngB)
Lengths=array(G,A,B)

Y1=(G*sin(AngG)*sin(AngB))/sin(AngA+AngB) #for triangulating the X axis of Beacon 3
X1=Y1/tan(AngG) #triangulates the Y axis of beacon 3
#Y1=A*sin(AngA)
#Gcos=cos(AngG)
#Xa=asin((sin(90)*Y1)/B)
#Xb=180-Xa-90
#X1=0
#print("X1 "+X1+"  Y1"+Y1)
#Y1=200
#X1=200
function vector4 r123(N1,N2,N3,N4){
    R1=Dist[N1,number]
    R2=Dist[N2,number]
    R3=Dist[N3,number]
    R4=Dist[N4,number]
    return vec4(R1,R2,R3,R4)
}


R1=Dist[1,number]
R2=Dist[2,number]
R3=Dist[3,number]
R4=Dist[4,number]
P1=vec(0,0,0)
P2=vec(Beacon[1,vector]:distance(Beacon[2,vector]),0,0) 
P3=vec(X1,Y1,0)


#get 4th beacon
Vector=trilaterate(P1,P2,P3,P4,R1,R2,R3,R4) #trilaterate player from points 1,2,3

#Get Point 4 from Points 1,2 and 3
Dist=Beacon[4,vector]:dist()
R1=Dist[1,number]
R2=Dist[2,number]
R3=Dist[3,number]
P4=trilaterate(P1,P2,P3,R1,R2,R3)
Dist=array()
#Dist=Beacon[5,vector]:dist()
#r123(4,3,2)
B=5
R1=Beacon[B,vector]:distance(Beacon[1,vector])
R2=Beacon[B,vector]:distance(Beacon[2,vector])
R3=Beacon[B,vector]:distance(Beacon[3,vector])
R4=Beacon[B,vector]:distance(Beacon[4,vector])
P5=trilaterate(P1,P2,P3,P4,R1,R2,R3,R4)

holoCreate(50,P5)
holoScale(50,vec(2,0.5,0.5))
holoColor(50,vec(128,128,0))
#Dist=array()
#Dist=Beacon[6,vector]:dist()
#[V4=vec4()
V4=r123(1,2,3,4)]#
B=6
R1=Beacon[B,vector]:distance(Beacon[1,vector])
R2=Beacon[B,vector]:distance(Beacon[2,vector])
R3=Beacon[B,vector]:distance(Beacon[3,vector])
R4=Beacon[B,vector]:distance(Beacon[4,vector])
#print(V4:x()+" "+V4:y()+" "+V4:z()+" "+V4:w())
P6=trilaterate(P1,P2,P3,P4,R1,R2,R3,R4) 
holoCreate(60,P6)
holoScale(60,vec(2,0.5,0.5))
holoColor(60,vec(200,150,233))
B=7
R1=Beacon[B,vector]:distance(Beacon[5,vector])
R2=Beacon[B,vector]:distance(Beacon[3,vector])
R3=Beacon[B,vector]:distance(Beacon[6,vector])
R4=Beacon[B,vector]:distance(Beacon[2,vector])

P7=trilaterate(P5,P3,P6,P2,R1,R2,R3,R4) 
holoCreate(61,P7)
holoScale(61,vec(2,0.5,0.5))
holoColor(61,vec(120,150,200))
B=8
R1=Beacon[B,vector]:distance(Beacon[5,vector])
R2=Beacon[B,vector]:distance(Beacon[7,vector])
R3=Beacon[B,vector]:distance(Beacon[6,vector])
R4=Beacon[B,vector]:distance(Beacon[2,vector])

P8=trilaterate(P5,P7,P6,P2,R1,R2,R3,R4) 
holoCreate(61,P8)
holoScale(61,vec(2,0.5,0.5))
holoColor(61,vec(120,15,233))

#[
Dist:removeNumber(Dist:minIndex())
R1=Dist:min()
Dist:removeNumber(Dist:minIndex())
R2=Dist:min()
Dist:removeNumber(Dist:minIndex())
R3=Dist:min()
]#



#PDa=sqrt((P2:x()-P1:x())^2+(P2:y()-P1:y())^2+(P2:z()-P1:z())^2) #Is equal to verctors P2-P1  #Make this a funciton
#PDb=P2:distance(P1)

#Exa=acos((G^2-A^2-B^2)/(-2*A*B))
#Ex=cos(Exa)


#point 4 will serve as the origin of Z, with the equation Z-Z1=+/-Z2 Z is the Z offset of P4, Z1 is the trilaterated object and Z2 is the adjusted Z coordinate

#[
Angles=array()
Lengths=array()
Sum=array()
for(C=1,Beacon:count()){

    if(C==1){C1=Beacon:count()}else{C1=C-1}
    
    #assign lengths
    A=Dist[C1,number]
    B=Dist[C,number]
    D=Polygon[C+"-"+C1,number]+Polygon[C1+"-"+C,number]
    
    #Get angles via Law of Cosines
    AngD=acos((D^2-A^2-B^2)/(-2*A*B))
    AngB=acos((B^2-A^2-D^2)/(-2*A*D))
    AngA=180-AngD-AngB
    
    #store triangle data
    Sum:pushNumber(AngA+AngB+AngD)
    Angles:pushVector(vec(AngD,AngB,AngA))
    Lengths:pushVector(vec(A,B,D))
    
}
#]

#[

#include <stdio.h>
#include <math.h>
 
/* No rights reserved (CC0, see http://wiki.creativecommons.org/CC0_FAQ).
 * The author has waived all copyright and related or neighboring rights
 * to this program, to the fullest extent possible under law.
*/
 
/* Largest nonnegative number still considered zero */
#define   MAXZERO  0.0
 
typedef struct vec3d	vec3d;
struct vec3d {
	double	x;
	double	y;
	double	z;
};
 
/* Return the difference of two vectors, (vector1 - vector2). */
vec3d vdiff(const vec3d vector1, const vec3d vector2)
{
	vec3d v;
	v.x = vector1.x - vector2.x;
	v.y = vector1.y - vector2.y;
	v.z = vector1.z - vector2.z;
	return v;
}
 
/* Return the sum of two vectors. */
vec3d vsum(const vec3d vector1, const vec3d vector2)
{
	vec3d v;
	v.x = vector1.x + vector2.x;
	v.y = vector1.y + vector2.y;
	v.z = vector1.z + vector2.z;
	return v;
}
 
/* Multiply vector by a number. */
vec3d vmul(const vec3d vector, const double n)
{
	vec3d v;
	v.x = vector.x * n;
	v.y = vector.y * n;
	v.z = vector.z * n;
	return v;
}
 
/* Divide vector by a number. */
vec3d vdiv(const vec3d vector, const double n)
{
	vec3d v;
	v.x = vector.x / n;
	v.y = vector.y / n;
	v.z = vector.z / n;
	return v;
}
 
/* Return the Euclidean norm. */
double vnorm(const vec3d vector)
{
	return sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
}
 
/* Return the dot product of two vectors. */
double dot(const vec3d vector1, const vec3d vector2)
{
	return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z;
}
 
/* Replace vector with its cross product with another vector. */
vec3d cross(const vec3d vector1, const vec3d vector2)
{
	vec3d v;
	v.x = vector1.y * vector2.z - vector1.z * vector2.y;
	v.y = vector1.z * vector2.x - vector1.x * vector2.z;
	v.z = vector1.x * vector2.y - vector1.y * vector2.x;
	return v;
}
 
/* Return zero if successful, negative error otherwise.
 * The last parameter is the largest nonnegative number considered zero;
 * it is somewhat analoguous to machine epsilon (but inclusive).
*/
int trilateration(vec3d *const result1, vec3d *const result2,
                  const vec3d p1, const double r1,
                  const vec3d p2, const double r2,
                  const vec3d p3, const double r3,
                  const double maxzero)
{
	vec3d	ex, ey, ez, t1, t2;
	double	h, i, j, x, y, z, t;
 
	/* h = |p2 - p1|, ex = (p2 - p1) / |p2 - p1| */
	ex = vdiff(p2, p1);
	h = vnorm(ex);
	if (h <= maxzero) {
		/* p1 and p2 are concentric. */
		return -1;
	}
	ex = vdiv(ex, h);
 
	/* t1 = p3 - p1, t2 = ex (ex . (p3 - p1)) */
	t1 = vdiff(p3, p1);
	i = dot(ex, t1);
	t2 = vmul(ex, i);
 
	/* ey = (t1 - t2), t = |t1 - t2| */
	ey = vdiff(t1, t2);
	t = vnorm(ey);
	if (t > maxzero) {
		/* ey = (t1 - t2) / |t1 - t2| */
		ey = vdiv(ey, t);
 
		/* j = ey . (p3 - p1) */
		j = dot(ey, t1);
	} else
		j = 0.0;
 
	/* Note: t <= maxzero implies j = 0.0. */
	if (fabs(j) <= maxzero) {
		/* p1, p2 and p3 are colinear. */
 
		/* Is point p1 + (r1 along the axis) the intersection? */
		t2 = vsum(p1, vmul(ex, r1));
		if (fabs(vnorm(vdiff(p2, t2)) - r2) <= maxzero &&
		    fabs(vnorm(vdiff(p3, t2)) - r3) <= maxzero) {
			/* Yes, t2 is the only intersection point. */
			if (result1)
				*result1 = t2;
			if (result2)
				*result2 = t2;
			return 0;
		}
 
		/* Is point p1 - (r1 along the axis) the intersection? */
		t2 = vsum(p1, vmul(ex, -r1));
		if (fabs(vnorm(vdiff(p2, t2)) - r2) <= maxzero &&
		    fabs(vnorm(vdiff(p3, t2)) - r3) <= maxzero) {
			/* Yes, t2 is the only intersection point. */
			if (result1)
				*result1 = t2;
			if (result2)
				*result2 = t2;
			return 0;
		}
 
		return -2;
	}
 
	/* ez = ex x ey */
	ez = cross(ex, ey);
 
	x = (r1*r1 - r2*r2) / (2*h) + h / 2;
	y = (r1*r1 - r3*r3 + i*i) / (2*j) + j / 2 - x * i / j;
	z = r1*r1 - x*x - y*y;
	if (z < -maxzero) {
		/* The solution is invalid. */
		return -3;
	} else
	if (z > 0.0)
		z = sqrt(z);
	else
		z = 0.0;
 
	/* t2 = p1 + x ex + y ey */
	t2 = vsum(p1, vmul(ex, x));
	t2 = vsum(t2, vmul(ey, y));
 
	/* result1 = p1 + x ex + y ey + z ez */
	if (result1)
		*result1 = vsum(t2, vmul(ez, z));
 
	/* result1 = p1 + x ex + y ey - z ez */
	if (result2)
		*result2 = vsum(t2, vmul(ez, -z));
 
	return 0;
}
 
int main(void)
{
	vec3d	p1, p2, p3, o1, o2;
	double	r1, r2, r3;
	int	result;
 
	while (fscanf(stdin, "%lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg",
	                     &p1.x, &p1.y, &p1.z, &r1,
	                     &p2.x, &p2.y, &p2.z, &r2,
	                     &p3.x, &p3.y, &p3.z, &r3) == 12) {
		printf("Sphere 1: %g %g %g, radius %g\n", p1.x, p1.y, p1.z, r1);
		printf("Sphere 2: %g %g %g, radius %g\n", p2.x, p2.y, p2.z, r2);
		printf("Sphere 3: %g %g %g, radius %g\n", p3.x, p3.y, p3.z, r3);
		result = trilateration(&o1, &o2, p1, r1, p2, r2, p3, r3, MAXZERO);
		if (result)
			printf("No solution (%d).\n", result);
		else {
			printf("Solution 1: %g %g %g\n", o1.x, o1.y, o1.z);
			printf("  Distance to sphere 1 is %g (radius %g)\n", vnorm(vdiff(o1, p1)), r1);
			printf("  Distance to sphere 2 is %g (radius %g)\n", vnorm(vdiff(o1, p2)), r2);
			printf("  Distance to sphere 3 is %g (radius %g)\n", vnorm(vdiff(o1, p3)), r3);
			printf("Solution 2: %g %g %g\n", o2.x, o2.y, o2.z);
			printf("  Distance to sphere 1 is %g (radius %g)\n", vnorm(vdiff(o2, p1)), r1);
			printf("  Distance to sphere 2 is %g (radius %g)\n", vnorm(vdiff(o2, p2)), r2);
			printf("  Distance to sphere 3 is %g (radius %g)\n", vnorm(vdiff(o2, p3)), r3);
		}
	}
 
	return 0;
}

#]






#########################Secondary function for trilateration#################
function array trilaterate2(P1:vector,P2:vector,P3:vector,R1,R2,R3,Maxzero){
    #/* h = |p2 - p1|, ex = (p2 - p1) / |p2 - p1| */
    Ex=P2-P1
    H=Ex:length()
    
    if(H<=Maxzero){
        #p1 and p2 are concentric
        return array(vec(),vec(),-1)
    }
    Ex=Ex/H
    
    #/* t1 = p3 - p1, t2 = ex (ex . (p3 - p1)) */
    T1=P3-P1
    I=Ex:dot(T1)
    T2=Ex*I
    
    #/* ey = (t1 - t2), t = |t1 - t2| */
    Ey=T1-T2
    T=Ey:length()
    if(T>Maxzero){
        Ey=Ey-T
        
        #/*   J=Ey dot (P3-P1) */
        J=Ey:dot(T1)
    }else{J=0}
        
    #/*Note: t<=maxzero implies j=0.0 */
    if(abs(J) <=Maxzero){
        #/*p1,p2,and p3 are colinear. */
        #/* Is point p1 + (r1 along the axis) the intersection? */
        T2=P1+(Ex*R1)
        if(((abs((P2-T2):length())-R2)<=Maxzero) & ((abs((P3-T2):length())-R3)<=Maxzero)){
            #/* Yes, t2 is the only intersection point. */
            return array(vec(),vec(),0)
        }
    }
}
