acc_v2

--Automated Cruise Control v2 by Gazer29
--Uses Automation mod version: Beta 1

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 glassesTerminal = require("component").glasses
local event = require("event")

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

local CONFIG_FILE = "/home/Waypoints.cfg"
local CONFIG = nil
local DEFAULT_CONFIG = {
  array = {
    {
    waypoint = "Station 1",
    speed = 0,
    radius = 10,
    x = 1180,
    z = -333,
    mode = "wait",
    data = 20
    },
    {
    waypoint = "Waypoint 2",
    speed = 10,
    radius = 10,
    x = 1180,
    z = -333,
    mode = nil,
    data = nil
    },
    {
    waypoint = "Waypoint 3",
    speed = 5,
    radius = 10,
    x = 1136,
    z = -333,
    mode = nil,
    data = nil
    }
  }
}

glassesTerminal.removeAll()
atext = glassesTerminal.addText2D()
atext.setText("ATC: OFF") -- no modifier!
atext.setFontSize(42)
atext.addColor(1, 0, 0, 1)
atext.setCondition(atext.addColor(0, 1, 0, 1), "OVERLAY_ACTIVE", true)

btext = glassesTerminal.addText2D()
btext.setText("+10")
btext.setFontSize(60)
btext.addColor(1,0,0,1)
btext.addTranslation(0, 10, 0)
btext.setCondition(btext.addColor(0, 1, 0, 1), "OVERLAY_ACTIVE", true)

ctext = glassesTerminal.addText2D()
ctext.setText("-10")
ctext.setFontSize(60)
ctext.addColor(1,0,0,1)
ctext.addTranslation(0,18,0)
ctext.setCondition(ctext.addColor(0, 1, 0, 1), "OVERLAY_ACTIVE", true)

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
    print("Could not open " .. CONFIG_FILE .. ".")
    print("Loading default config")
    saveConfig(CONFIG_FILE, DEFAULT_CONFIG)
    return DEFAULT_CONFIG
    else
    local config = serial.unserialize(f:read("*a"))
    f:close()
    return config
    end
end

function helpdisplay()
    print("AutomaticTrainControl v1")
    print("Commands:")
    print("'Any number'    - Changes set speed")
    print("'+'      - Displays additional information")
    print("'-'      - Hides additional information")
    print("'ramp'       - Changes to ramp mode, requires further input")
    print("'idle'       - Pauses train control")
    print("'save'       - Saves config file")
    print("'stop'       - Ends program")
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 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 reads()
  while RUNNING do
    user = nil
    user = io.read()
    if user == "stop" then
        print("STOPPING...")
        os.sleep(1)
        stop()
        end
    if user == "." then
        term.clear()
        end
    if user == "idle" then
        MODE = 2
        end
    if user == "ramp" then
        print("Start Speed (type 'x' for current setpoint):")
        c = io.read()
        if c == "x" then
            c = setpoint
            end
        print("End Speed:")
        rampStart = c
        c = io.read()
        print("Ramp Time (seconds):")
        rampEnd = c
        c = io.read()
        rampTime = c
        rampDiff = rampEnd - rampStart
        rampCount = math.floor(rampTime/sleep)
        rampInc = rampDiff / rampCount
        PID:SetPoint(rampStart)
        setpoint = rampStart
        MODE = 1
        end
    if user == "+" then
        display = true
        term.clear()
        end
    if user == "save" then
        saveConfig(CONFIG_FILE, CONFIG)
        end
    if user == "-" then
        display = false
        term.clear()
        os.sleep(1)
        helpdisplay()
        end
    if nil ~= tonumber(user) then
        MODE = 0
        PID:SetPoint(user)
        setpoint = user
        print("Setting Speed: ", user)
        end
    os.sleep(1)
  end
end

-- run through array
function getDistance()
    --active = CONFIG.array[iteration]
    x1 = active.x
    z1 = active.z
    positions = entity.getLocation()
    y1 = positions.getY()
    x = positions.getDistanceToCoordinates(x1,y1,z1)
    return x
    end


local function main()
    while RUNNING do
        array = CONFIG.array
        active = CONFIG.array[iteration]
        distance = getDistance()
        if distance < active.radius or timer ~= nil then
            setpoint = active.speed
            PID:SetPoint(setpoint)
            if active.mode == "wait" then
                if timer == nil then
                    timer = active.data
                    end
                if timer > 0 then
                    timer = timer - 1
                    else
                    timer = nil
                    if iteration >= #array then
                        iteration = 1
                        else
                        iteration = iteration + 1
                        end
                    end
                else
                if iteration >= #array then
                    iteration = 1
                    else
                    iteration = iteration + 1
                    end
                end
            end

        input = GetTrainSpeed()
        if MODE ~= 2 then
            if MODE == 1 then
                if rampCount > 0 then
                  rampCount = rampCount - 1
                  setpoint = setpoint + rampInc
                  PID:SetPoint(setpoint)
                  if rampCount == 0 then
                    setpoint = rampEnd
                    PID:SetPoint(rampEnd)
                    end
                  else
                  Mode = 0
                  end
                end
            PID:SetInput(input)
            PID:Compute()
            output = PID:GetOutput()
            BrakeOut = output * -1
            setThrottle(output)
            setBrake(BrakeOut)
            end
        if display then
            print("CurrentSpeed: ", input)
            if MODE ~= 2 then
                print("SetPoint: ", setpoint)
                end
            print("Waypoint: ",active.waypoint)
            print("Distance: ",distance)
            print("Timer:   ",timer)
            GetTrainPos()
            print("----")
            os.sleep(sleep)
            term.clear()
            else
            os.sleep(sleep)
            end
        checkATC()
        end
  end

function stop()
  RUNNING = false
end

function checkATC()
  if MODE == 0 then
    atext.setText("ATC: "..setpoint.."  Speed: "..(math.floor(input * 1000)/1000))
    ATC = true
    end
  if MODE == 2 then
    atext.setText("ATC: OFF")
    ATC = false
    end
end

function inbounds(x0, y0, x1, y1, x2, y2)
  local output = false
  if x0 > x1 then
    if y0 > y1 then
      if x0 < x2 then
        if y0 < y2 then
          output = true
          end
        end
      end
    end
  return output
end

function pullOverlay()
while RUNNING do
  EVENT, ID, USER, X, Y, BUTTON = event.pull("interact_overlay")
  if inbounds(X,Y,0,0,41,7) then
    if ATC == true then
        MODE = 2
        else
        MODE = 0
        PID:SetPoint(setpoint)
        end
  elseif inbounds(X,Y,0,11,20,16) then
    setpoint = setpoint + 10
    PID:SetPoint(setpoint)
  elseif inbounds(X,Y,0,19,20,24) then
    setpoint = setpoint - 10
    PID:SetPoint(setpoint)
    end
end
end

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

term.clear()
CONFIG = loadConfig()
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)

helpdisplay()

local tb = thread.create(main)
local ta = thread.create(reads)
local tc = thread.create(pullOverlay)
tb:join()
ta:join()
tc:join()
term.clear()