local function mat_mult(A, B)
    local M = {{}, {}, {}}
    for i = 1, 3 do for j = 1, 3 do M[i][j] = A[i][1] * B[1][j] + A[i][2] * B[2][j] + A[i][3] * B[3][j] end end
    return M
end

function get_euler(direction)
    local R
    if direction == "east" then
        R = { {0, 0, 1}, {0, 1, 0}, {-1, 0, 0} }
    elseif direction == "west" then
        R = { {0, -1, 0}, {1, 0, 0}, {0, 0, 1} }
    elseif direction == "south" then
        R = { {1, 0, 0}, {0, 1, 0}, {0, 0, 1} }
    elseif direction == "north" then
        R = { {-1, 0, 0}, {0, -1, 0}, {0, 0, -1} }
    else
        error("No direction selected")
    end

    local M = ship.getTransformationMatrix()
    M = mat_mult(M, R)

    local yaw, pitch, roll = math.atan2(-M[1][3], M[3][3]), -math.asin(M[2][3]), math.atan2(M[2][1], M[2][2])
    return yaw, pitch, roll
end

function DrawBackground(w,h)
    term.clear();
    paintutils.drawFilledBox(1,1,w,h,colors.white);
    paintutils.drawFilledBox(1,1,w,1,colors.red);
    term.setBackgroundColor(colors.red);
    term.setTextColor(colors.orange);
    local info = "CAInnon Frontline"
    term.setCursorPos(w/2-string.len(info)/2,1);
    term.write(info);
    term.setTextColor(colors.white);
    info = "ID: "..tostring(thisID);
    term.setCursorPos(1,1);
    term.write(info);
    term.setBackgroundColor(colors.white);
    term.setTextColor(colors.black);
end

--g = peripheral.wrap("back");
g = peripheral.find("createbigcannons:cannon_mount")

thisID = os.computerID();
commanderID = -1;
centuricBound = -1;
upperBound = 140;
verticalOffset = -0.6;
directions = {"south","north","east","west"}

posx = -1339; --position of yourself
posy = -59;
posz = -602;
xtgt = 0; --position of target
ytgt = 60;
ztgt = 0;
vxtgt = 0;
vztgt = 0;
IYaw = 0;
shipYawPrev = 0;

tgtshipx0 = 0;
tgtshipz0 = 0;

MODE = "flat";
n = 4;
pi = math.pi;
EXEMODE = 0;
switch = "off";
t0 = 0;
t = 0;
dt = 0;
horfix = 0;
verfix = 0;

--configure start
cfg = io.open("config.lua","r");
if cfg == nil then
    cfg = io.open("config.lua", "w");
    cfg:write("-1\n");
    cfg:write("south");
    cfg:close();
end
config = {};
configNum = 0;
markerPos = 0
for line in io.lines("config.lua") do
    configNum = configNum + 1;
    config[configNum] = line;
end
n = 4
commanderID = tonumber(config[1]);
initialDirection= config[2]
verticalOffset = -0.6
--configure end

mod = peripheral.find("modem")
if mod == nil then
    os.shutdown();
end
mod.open(386);
rednet.open(peripheral.getName(mod));

function PanelOld()
    while true do
    local w,h = term.getSize()
    DrawBackground(w,h);
    local info = "Status: "..switch;
    term.setCursorPos(w/2-string.len(info)/2,2);
    term.write(info);
    info = "Player Mode"
    term.setCursorPos(w/2-string.len(info)/2,3);
    term.write(info);
    info = "Ship Mode"
    term.setCursorPos(w/2-string.len(info)/2,4);
    term.write(info);
    info = "Location Mode"
    term.setCursorPos(w/2-string.len(info)/2,5);
    term.write(info);
    info = "Firing Mode: "..MODE;
    term.setCursorPos(w/2-string.len(info)/2,6);
    term.write(info);
    local evn,but,cursx,cursy = os.pullEvent("mouse_click");
    if cursy == 2 then
        if EXEMODE ~= 0 then
            EXEMODE = 0;
            switch ="off";
            DrawBackground(w,h)
            info = "System Paused";
            term.setCursorPos(w/2-string.len(info)/2,3);
            term.write(info);
            sleep(0.4);
        else
            DrawBackground(w,h)
            info = "System Already Paused";
            term.setCursorPos(w/2-string.len(info)/2,3);
            term.write(info);
            sleep(0.4);
        end
    elseif cursy == 3 then
        DrawBackground(w,h);
        info = "Enter the target Player";
        term.setCursorPos(w/2-string.len(info)/2,3);
        term.write(info);
        term.setCursorPos(w/2-3,4);
        targetplayer = read();
        EXEMODE = 1;
        info = "Aiming to that shit";
        term.setCursorPos(w/2-string.len(info)/2,5);
        term.write(info);
        switch = "Player"
        sleep(0.3);
    elseif cursy == 4 then
        DrawBackground(w,h);
        info = "Enter the target Vehicle";
        term.setCursorPos(w/2-string.len(info)/2,3);
        term.write(info);
        term.setCursorPos(w/2-3,4);
        targetship = read();
        EXEMODE = 2;
        switch = "Ship";
        info = "Aiming to that shit";
        term.setCursorPos(w/2-string.len(info)/2,5);
        term.write(info);
        sleep(0.3);
    elseif cursy == 5 then
        DrawBackground(w,h);
        info = "Enter the target x axis";
        term.setCursorPos(w/2-string.len(info)/2,3);
        term.write(info);
        term.setCursorPos(w/2-3,4);
        xtgt0 = tonumber(read());
        info = "Enter the target z axis";
        term.setCursorPos(w/2-string.len(info)/2,5);
        term.write(info);
        term.setCursorPos(w/2-3,6);
        ztgt0 = tonumber(read())
        info = "Enter the target y axis";
        term.setCursorPos(w/2-string.len(info)/2,7);
        term.write(info);
        term.setCursorPos(w/2-3,8);
        ytgt0 = tonumber(read());
        if type(xtgt0)=="number" and type(ztgt0)=="number" and type(ytgt0)=="number" then
            xtgt1 = xtgt0;
            ztgt1 = ztgt0;
            ytgt1 = ytgt0;
            EXEMODE = 3;
            switch = "Location"
            info = "Aiming to that shit";
            term.setCursorPos(w/2-string.len(info)/2,9);
            term.write(info);
        else
            info = "Invalid Values";
            term.setCursorPos(w/2-string.len(info)/2,9);
            term.write(info);
        end
    elseif cursy ==6 then
        if MODE == "flat" then
            MODE = "drop"
        elseif MODE == "drop" then
            MODE = "flat";
        else
            MODE = "flat";
        end
        sleep(0.1);
    end
    sleep(0.1);
end
end

function heightLimit()
vx = spd.x;
vy = spd.y;
vz = spd.z;
v = math.sqrt(spd.x^2+spd.y^2+spd.z^2);
v = v*3.6;
a = (v-v0)/0.35;
v0 = v;
pos = ship.getWorldspacePosition();
posx = pos.x;
posz = pos.z;
h = pos.y;
w = Q.w;
x = Q.z;
y = Q.x;
z = Q.y;
roll = math.atan2(2*(w*x+y*z),(1-2*(x^2+y^2)));
pitch = math.asin(2*(w*y-x*z));
yaw = math.atan2(2*(w*z+x*y),(1-2*(y^2+z^2)));
pitch = pitch*180/pi;
yaw = yaw*180/pi;
roll = roll*180/pi;
omgr = (roll-roll0)/0.12;
omgy = (yaw-yaw0)/0.12;
omgp = (pitch-pitch0)/0.12;
roll0 = roll;
yaw0 = yaw;
pitch0 = pitch;
vtgt = vtgtdat;
end

function Panel()
    while true do
        local cmd = {}
        local w,h = term.getSize()
        DrawBackground(w,h);
        info = "Receiving from: "..config[1]
        term.setCursorPos(w/2-string.len(info)/2,3);
        term.write(info);
        info = "Ship assemble direction: "..config[2]
        term.setCursorPos(w/2-string.len(info)/2,4);
        term.write(info);
        local evn,but,cursx,cursy = os.pullEvent("mouse_click");
        if cursy == 3 then
            DrawBackground(w,h)
            info = "Enter new commander ID"
            term.setCursorPos(w/2-string.len(info)/2,3);
            term.write(info);
            term.setCursorPos(w/2-2,4);
            newID = tonumber(read())
            if type(newID) == "number" then
                config[1] = newID
                commanderID = config[1];
                cfg = io.open("config.lua", "w");
                for i = 1,2 do
                    cfg:write(tostring(config[i]).."\n");
                end
                cfg:close()
            else
                info = "invalid number"
                term.setCursorPos(w/2-string.len(info)/2,4);
                term.write(info);
                sleep(1)
            end
        elseif cursy ==4 then
            DrawBackground(w,h)
            for i = 1,4 do
                info = directions[i]
                term.setCursorPos(w/2-string.len(info)/2,3+i);
                term.write(info);
            end
            local evn,but,cursx,cursy = os.pullEvent("mouse_click");
            if cursy >=4 and cursy<=7 then
                config[2] = directions[cursy-3];
                initialDirection = config[2];
                cfg = io.open("config.lua", "w");
                for i = 1,2 do
                    cfg:write(tostring(config[i]).."\n");
                end
                cfg:close()
            end
        end
        sleep(0.2)
    end
end

function RemoveItem()
    TerminalBackground()
    info = "Type in the number of item to remove"
    term.setCursorPos(W/2-string.len(info)/2,4);
    term.write(info);
    term.setCursorPos(W/2-1,5)
    remov = tonumber(read());
    heightLimit()
    if remov~=nil and remov>0 and remov<=N then
        remov = math.floor(remov)
        for i = remov, N-1 do
            Item[i] = Item[i+1];
            Num[i] = Num[i+1];
            Price[i] = Price[i+1];
        end
        Item[N] = nil;
        Price[N] = nil;
        Num[N] = nil;
        N = N - 1;
        info = "Successfully Removed"
        term.setCursorPos(W/2-string.len(info)/2,6);
        term.write(info);
        sleep(0.5);
    else
        info = "Error Processing"
        term.setCursorPos(W/2-string.len(info)/2,6);
        term.write(info);
        sleep(1);
    end
end

function commandReceive()
    while true do
    id, command = rednet.receive(nil,2);
if command ~= nil then
if id == commanderID then
    if command[1] == 1 then
    elseif command[1] == 3 then
        sleep(0.67)
        xtgt1 = command[2]
        ztgt1 = command[3]
        ytgt1 = command[4]
        EXEMODE = command[1];
    elseif command[1] == 5 then
        horfix = command[2];
    elseif command[1] == 6 then
        verfix = command[2];
    elseif command[1] == 0 then
        EXEMODE = command[1];
    end
end
end
end
end

function TerminalBackground()
    term.setBackgroundColour(colors.white);
    term.clear();
    paintutils.drawLine(1,1,W,1,colors.lightBlue);
    term.setBackgroundColour(colors.lightBlue);
    term.setTextColor(colors.white);
    term.setCursorPos(21,1);
    term.write("CAIsher");
    term.setCursorPos(12,18);
    term.setTextColor(colors.orange);
    term.setBackgroundColour(colors.blue);
    term.write("ADD");
    term.setCursorPos(21,18);
    term.setBackgroundColour(colors.red);
    term.write("REMOVE");
    term.setCursorPos(34,18);
    term.setBackgroundColour(colors.green);
    term.write("CHECK");
    term.setBackgroundColour(colors.white);
    term.setTextColor(colors.black);
    term.setCursorPos(15,2);
    term.write("Item");
    term.setCursorPos(25,17);
    term.write("Total");
    term.setCursorPos(38,2);
    term.write("Price");
end

function MainThread()
while true do
    if EXEMODE ~=0 then
    worldpos = coord.getCoord();
    posx = worldpos.x;
    posz = worldpos.z;
    posy = worldpos.y;
    shipyaw, shippitch, shiproll = get_euler(initialDirection);
    shipyaw = shipyaw*180/pi;
    shippitch = shippitch*180/pi;
    shiproll = shiproll*180/pi;
    shipPitchFix = shippitch;
    omega = ship.getOmega();
    omegay = omega.y*180/pi;
    if ship.getWorldspacePosition().y >144 then
        os.shutdown();
    end
    if EXEMODE ==3 then
        xtgt = xtgt1;
        ztgt = ztgt1;
    end

    yawAngle = math.deg(math.atan2(-(xtgt-posx),(ztgt-posz)))-shipyaw

if yawAngle<0 then --limit for bearing itself
    yawAngle = yawAngle+360;
elseif yawAngle>360 then
    yawAngle = yawAngle-360
end
yawAngle = yawAngle + horfix;
inRange = 1;
  g.setYaw(yawAngle);
  EXEMODE = 0;
else
    sleep(0.5);
end
sleep(0.15)
end
end

parallel.waitForAny(MainThread,commandReceive,Panel)