EngineControlv2

--Engine Control v2 by Gazer29
--Uses Automation mod

local component = require("component")
local term = require ("term")
local entity = component.entity_link.getAPI("immersiverailroading:locomotivediesel")
local thread = require("thread")
require("PIDController")
local serial = require("serialization")
local fs = require("filesystem")
local event = require("event")
local modem = component.modem

local ATC = false
local RUNNING = true
local MODE = 3 -- 0 SET,
local sleep = 1 -- seconds
local display = false
local iteration = 1
local timer = nil

local CONFIG_FILE = "/home/CMD.dat"
local CONFIG = nil
local DEFAULT_CONFIG = {}
local data = nil

local function saveConfig(file, cfg) -- Adapted from HandieAndy's code
  local f = io.open(file, "w")
  if f == nil then error("Couldn't open " .. file .. " to write config.") end
  f:write(serial.serialize(cfg, 100000))
  f:close()
  print("Saved to file: ", CONFIG_FILE)
end

local function loadConfig() -- Adapted from HandieAndy's code
  local f = io.open(CONFIG_FILE, "r")
  if f == nil then
    return DEFAULT_CONFIG
    else
    local config = serial.unserialize(f:read("*a"))
    f:close()
    return config
    end
end

function GetTrainPos()
    positions = entity.getLocation()
    xpos = positions.getX()
    ypos = positions.getY()
    zpos = positions.getZ()
    print("X:   ", xpos)
    print("Z:   ", zpos)
end

function GetTrainSpeed()
    speed = entity.getCurrentSpeed()
    return speed
end

function GetUUID()
    uuid = entity.getUUID()
    return uuid
end

function setThrottle(x)
    if 0 >= x then x = 0 end
    if 1 <= x then x = 1 end
    entity.setThrottleLevel(x)
    if display then
        print("Throttle: ", x)
        end
end

function setBrake(x)
    if 0 >= x then x = 0 end
    if 1 <= x then x = 1 end
    entity.setAirBrakeLevel(x)
    if display then
        print("Brake:   ", x)
        end
end

local function main()
    while RUNNING do
        os.sleep(sleep)
        input = GetTrainSpeed()
        if data ~= nil then
            if data[uuid] ~= nil then
                setpoint = data[uuid].setspeed
                print(setpoint)
                PID:SetPoint(setpoint)
                end
            end
        PID:SetInput(input)
        PID:Compute()
        output = PID:GetOutput()
        BrakeOut = output * -1
        setThrottle(output)
        setBrake(BrakeOut)
        end
  end

function stop()
  RUNNING = false
end

local function handleMessage(host, msg)
  local packet = serial.unserialize(msg)
  if packet == nil then
    print("nil packet")
    return
  end
  if packet.t == "STATUS" then
        --print(packet.v)
        data = packet.v
  else
    print("unknown command")
  end
end

function start()
    modem.open(54421)
    while RUNNING do
      local eventName, p1, p2, p3, p4, p5 = event.pull()
      if eventName == "modem_message" then
        --print("recv")
        handleMessage(p2, p5)
      end
    end
    modem.close(54421)
end

----------- Main loop ------------

setpoint = 0
PID:new(0.01, 0.5, 0.0001, 0)
PID:SetInput((GetTrainSpeed()))
PID:SetOutput(0)
PID:SetPoint(setpoint)
PID:SetOutputLimits(-1, 1)
PID:SetMode(1)
uuid = GetUUID()

--main()
local td = thread.create(start)
local ta = thread.create(main)
ta:join()
td:join()