@name Entity Aimer
@inputs Enemy:entity N

@inputs X Y Z On XYZ:vector 

@outputs TarPitchVel TarYawVel Fire ID NME


@outputs N F:vector
@persist K Xd Yd Zd P1 P2 P3 N1

#Initilization Variables
@persist ChipStartup Self:entity Mass
@persist MassPitchConstant MassYawConstant MassRollConstant 

#General Variables
@persist Enemy:entity
#this turret does not depend no any 
#specific interval to aim, but a faster
#interval will allow it to aim faster
if (On) {
interval(1)
#######################################
#Startup Calculations
#######################################start

#if (ChipStartup==0) {
if (N!=0) {N1 = N}

if (entity(N1):getColor()==vec(255,0,0)) {ID = 1 entity():setColor(255,0,0)}
if (entity(N1):getColor()==vec(0,255,0)) {ID = 2 entity():setColor(0,255,0)}
if (entity(N1):getColor()==vec(0,0,255)) {ID = 3 entity():setColor(0,0,255)}

    ChipStartup = 1
    Self = entity(N1)
    Mass = Self:mass()
    #Mass = 1

    #RS is the rotation speed constant
    RS = 15
    #origonally 30
#}
#######################################end

Offset = Enemy:height() / 2
#Temporay for testing
EnemyPosition = Enemy:pos() + vec(0,0,Offset)
#if (!Tog) {XYZ1 = XYZ}
#EnemyPosition = XYZ1
EnemyVelocity = Enemy:vel()

#######################################
# Rotation Part 1
# Inputs - EnemyPosition:vector EnemyVelocity:vector
#  
#The following converts a global 
#position and velocity to a position 
#and velocity local to the oriantation 
#of the chip
#######################################start
RelativePosition = EnemyPosition - Self:pos()
#RelativePosition = aimVector
RelativeVelocity = EnemyVelocity - Self:vel()
#RelativeVelocity = 

#needs to lead target by a small amount 0.015 work very well on props
LeadTime=0.018
RelativePosition += EnemyVelocity*LeadTime

#rotates the vectors so they are local to the turret
RelativePosition = RelativePosition:rotate( ang(0,-(Self:angles():yaw()),0) )
RelativePosition = RelativePosition:rotate(ang(-Self:angles():pitch(),0,0) )
RelativePosition = RelativePosition:rotate(ang(0,0,-(Self:angles():roll()) ) )

#rotates Velocity
RelativeVelocity = RelativeVelocity:rotate( ang(0,-(Self:angles():yaw()),0) )
RelativeVelocity = RelativeVelocity:rotate(ang(-Self:angles():pitch(),0,0) )
RelativeVelocity = RelativeVelocity:rotate(ang(0,0,-(Self:angles():roll()) ) )
#rotate Yaw then Pitch
RelativeVelocity = RelativeVelocity:rotate(ang(0,-RelativePosition:toAngle():yaw(),0))
RelativeVelocity = RelativeVelocity:rotate(ang(RelativePosition:toAngle():pitch(),0,0))
#Apply the new values
TarPitchVel = toDeg(-RelativeVelocity:z()/RelativePosition:length() )
TarYawVel = toDeg( RelativeVelocity:y()/RelativePosition:length() )
TarRollVel = 0
TarAngVel = ang(TarPitchVel,TarYawVel,TarRollVel)


AimDistanceAngle = RelativePosition:toAngle()
AimDistanceAngle:setRoll(-Self:angles():roll())
AimDistanceAngle = angnorm(AimDistanceAngle)
#######################################end

Object = entity(N1):pos() + entity(N1):forward()
Aim = entity(N1):forward()
Ranger = rangerOffset(50000,Object,Aim)
Target = Ranger:entity():id()

MinAng = 1
NME = Enemy:id()
if (Target==NME & NME!=0 & AimDistanceAngle:pitch()<MinAng & AimDistanceAngle:yaw()<MinAng) {Fire = 1} else {Fire = 0}

AngVelToApply = AimDistanceAngle*RS + TarAngVel - Self:angVel()
entity(N1):applyAngVel(AngVelToApply)

#######################################end



#a speed indicator- remove later
AngVel = Self:angVel()
RollVel = Self:angVel():roll()




K=10

M=entity(N1):mass()
X = XYZ:x()
Y = XYZ:y()
Z = XYZ:z()

Xd = X-entity(N1):pos():x()
Yd = Y-entity(N1):pos():y()
Zd = Z-entity(N1):pos():z()

P1 = (K*Xd-entity(N1):vel():x())*M
P2 = (K*Yd-entity(N1):vel():y())*M
P3 = (K*Zd-entity(N1):vel():z())*M
F = vec(P1,P2,P3)
entity(N1):applyForce(vec(P1,P2,P3))
}
