-- Crazy Ivan II
cs1=1735
cs2=1916
cs3=1942
cs4=2060
cs5=1852
ch1=2350
ch2=2565
ch3=2500
ch4=2615
ch5=2225
gs1=0
gs2=0
function StartWait()
  local x = {}
  local i,wcount
  set7seg(0)
  repeat
    x[1],x[2],x[3],x[4],x[5] = snabel()
    x[1] = ((x[1] - cs1) * 100) / (ch1-cs1)
    x[2] = ((x[2] - cs2) * 100) / (ch2-cs2)
    x[3] = ((x[3] - cs3) * 100) / (ch3-cs3)
    x[4] = ((x[4] - cs4) * 100) / (ch4-cs4)
    x[5] = ((x[5] - cs5) * 100) / (ch5-cs5)
    i = 1
    wcount = 0
    repeat
      if x[i] > 90 then
        wcount = wcount + 1
      end
      i = i + 1
    until i == 6
  until wcount >= 5 or ccheck()
  set7seg(10)
  print("Starting")
  delay(50000)
end
function Rotate(D)
  local x = {}
  local dmin,dmax
  local First = 0
  local i,wcount
  x[1],x[2],x[3],x[4],x[5] = snabel()
  Remember = ((x[3] - cs3) * 100) / (ch3-cs3)
  if D == 1 then
    motor(1,6,0,6)
  else
    motor(0,6,1,6)
  end
  print("Soeger Sort")
  repeat
    x[1],x[2],x[3],x[4],x[5] = snabel()
    x[1] = ((x[1] - cs1) * 100) / (ch1-cs1)
    x[2] = ((x[2] - cs2) * 100) / (ch2-cs2)
    x[3] = ((x[3] - cs3) * 100) / (ch3-cs3)
    x[4] = ((x[4] - cs4) * 100) / (ch4-cs4)
    x[5] = ((x[5] - cs5) * 100) / (ch5-cs5)
    dmax = max(x[1],x[2],x[3],x[4],x[5])
  until dmax < 60 or ccheck()
  print("Soeger Hvid")
  repeat
    x[1],x[2],x[3],x[4],x[5] = snabel()
    x[1] = ((x[1] - cs1) * 100) / (ch1-cs1)
    x[2] = ((x[2] - cs2) * 100) / (ch2-cs2)
    x[3] = ((x[3] - cs3) * 100) / (ch3-cs3)
    x[4] = ((x[4] - cs4) * 100) / (ch4-cs4)
    x[5] = ((x[5] - cs5) * 100) / (ch5-cs5)
    i = 2
    wcount=0
    repeat
      if x[i] > 60 then
        wcount=wcount+1
      end
      i = i + 1
    until i == 5
  until ((wcount >=2) and (x[3] > 70)) or ccheck()
  motor(0,0,0,0)
end
function AlignToWall()
  local Done = 0
  local sh1,sh2,sh3,sh4
  print("Align to Wall")
  motor(0,6,0,0)
  repeat
    sh1,sh2,sh3,sh4 = s()
    if sh1 < 100 then
      if sh2 > (sh4 - 10) then
        Done = 1
      end
      if sh3 > (sh4 - 5) then
        Done = 1
      end
    end
  until Done == 1 or ccheck()
  motor(1,6,1,6)
  print("Align completed")
end
function FollowWall(WallDistanceMin,WallDistanceMax,D)
  local sh1,sh2,sh3,sh4
  local ssh1 = 0
  local ssh2 = 0
  local ssh3 = 0
  local ssh4 = 0
  local s1,s2,gs1,gs2
  local r1,r2
  local wall = 0
  local Angle = 0
  local x = {}
  local y = {}
  local i,wcount
  local Align = 0
  repeat
    sh1,sh2,sh3,sh4 = s()
    if sh4 > WallDistanceMax or sh1 > WallDistanceMax then
      if Align == 0 then
        AlignToWall()
        Align = 1
      end
    else
      Align = 0
    end
    if D == 1 then
      if sh4 < 50 and sh1 < 100 then
        motor(0,0,0,0)
        print("Corner value ="..sh3)
        return 1
      end
    end
    if D == 2 then
      y[1],y[2],y[3],y[4],y[5] = snabel()
      x[1] = ((y[1] - cs1) * 100) / (ch1-cs1)
      x[2] = ((y[2] - cs2) * 100) / (ch2-cs2)
      x[3] = ((y[3] - cs3) * 100) / (ch3-cs3)
      x[4] = ((y[4] - cs4) * 100) / (ch4-cs4)
      x[5] = ((y[5] - cs5) * 100) / (ch5-cs5)
      i = 1
      wcount = 0
      repeat
        if x[i] > 80 then
          wcount = wcount + 1
        end
        i = i + 1
      until i == 6
      if wcount >=1 then
        motor(0,0,0,0)
        return
      end
    end
    if sh4 >= WallDistanceMin and sh4 <= WallDistanceMax then
      Angle = 0
    elseif sh4 < WallDistanceMin then
      Angle = 10
    else
      Angle = -15
    end
    sh2 = sh2 + Angle
    if abs(sh4-sh2) < 10 then
      s1=6
      s2=6
    else
      if sh4 > sh2 then
        s1=-2
        s2=4
      else
        s1=4
        s2=-2
      end
    end
    if s1 ~= gs1 or s2 ~= gs2 then
      if s1 < 0 then
        r1 = 0
      else
        r1 = 1
      end
      if s2 < 0 then
        r2 = 0
      else
        r2 = 1
      end
      motor(r1,abs(floor(s1)),r2,abs(floor(s2)))
      gs1 = s1
      gs2 = s2
    end
    ssh1=sh1
    ssh2=sh2
  until ccheck()
  motor(0,0,0,0)
end
function Corner(Start)
  local sh1,sh2,sh3,sh4
  local h2 = 0
  local h3 = 0
  local o = 0
  local h = 0
  repeat
    sh1,sh2,sh3,sh4 = s()
    if sh4 < 70 then
      motor(1,4,0,1)
    else
      motor(1,4,1,4)
    end
    if sh2 < 70 then
      if h2 == 0 then
        h = h + 1
        if h > 20 then
          h2 = 1
        end
      end
    else
      h = h - 1
      if h < 0 then
        h = 0
      end
    end
    if (h2 == 1) then
      if (sh2 > 90) and (h2 == 1) then
        o = o + 1
      else
        o = o - 1
        if o < 0 then
          o = 0
        end
      end
    end
  until (o > 20) or ccheck()
  motor(0,0,0,0)
end
function FindEdge(D) -- 1=Left,2=right
  local Diode={1,5}
  local x={}
  local dmin,dmax
  --print("Search Edge")
  if D == 1 then
    motor(1,4,0,0)
  else
    motor(0,0,1,4)
  end
  repeat
    x[1],x[2],x[3],x[4],x[5] = snabel()
    x[1] = ((x[1] - cs1) * 100) / (ch1-cs1)
    x[2] = ((x[2] - cs2) * 100) / (ch2-cs2)
    x[3] = ((x[3] - cs3) * 100) / (ch3-cs3)
    x[4] = ((x[4] - cs4) * 100) / (ch4-cs4)
    x[5] = ((x[5] - cs5) * 100) / (ch5-cs5)
    dmax = max(x[1],x[2],x[3],x[4],x[5])
    dmin = min(x[1],x[2],x[3],x[4],x[5])
    x[1] = ((x[1] - dmin) * 100) / (dmax-dmin)
    x[2] = ((x[2] - dmin) * 100) / (dmax-dmin)
    x[3] = ((x[3] - dmin) * 100) / (dmax-dmin)
    x[4] = ((x[4] - dmin) * 100) / (dmax-dmin)
    x[5] = ((x[5] - dmin) * 100) / (dmax-dmin)
  until x[Diode[D]] < 40 or ccheck()
  motor(0,0,0,0)
  --print("Found!")
end
function Junction(D) -- 1=Left,2=right
  local x={}
  local Diode={2,4}
  local Korr1={6,0}
  local Korr2={0,6}
  local Korr3={0,6}
  local Korr4={6,0}
  local wcount = 0
  local dmax,dmin
  FindEdge(D)
  repeat
    x[1],x[2],x[3],x[4],x[5] = snabel()
    x[1] = ((x[1] - cs1) * 100) / (ch1-cs1)
    x[2] = ((x[2] - cs2) * 100) / (ch2-cs2)
    x[3] = ((x[3] - cs3) * 100) / (ch3-cs3)
    x[4] = ((x[4] - cs4) * 100) / (ch4-cs4)
    x[5] = ((x[5] - cs5) * 100) / (ch5-cs5)
    dmax = max(x[1],x[2],x[3],x[4],x[5])
    dmin = min(x[1],x[2],x[3],x[4],x[5])
    x[1] = ((x[1] - dmin) * 100) / (dmax-dmin)
    x[2] = ((x[2] - dmin) * 100) / (dmax-dmin)
    x[3] = ((x[3] - dmin) * 100) / (dmax-dmin)
    x[4] = ((x[4] - dmin) * 100) / (dmax-dmin)
    x[5] = ((x[5] - dmin) * 100) / (dmax-dmin)
    if x[Diode[D]] >= 25 and x[Diode[D]] <= 45 then
      motor(1,6,1,6)
    elseif x[Diode[D]] < 25 then
      motor(1,6 - Korr1[D] ,1,6 - Korr2[D])
    else
      motor(1,6 - Korr3[D] ,1,6 - Korr4[D])
    end
    wcount = wcount + 1
  until wcount > 80 or ccheck()
  motor(0,0,0,0)
end
function RecoverLine(D)
  local x = {}
  local i,wcount
  local Stat = 0
  motor(1,D,1,D)
  repeat
    x[1],x[2],x[3],x[4],x[5] = snabel()
    x[1] = ((x[1] - cs1) * 100) / (ch1-cs1)
    x[2] = ((x[2] - cs2) * 100) / (ch2-cs2)
    x[3] = ((x[3] - cs3) * 100) / (ch3-cs3)
    x[4] = ((x[4] - cs4) * 100) / (ch4-cs4)
    x[5] = ((x[5] - cs5) * 100) / (ch5-cs5)
    i = 1
    wcount = 0
    repeat
      if x[i] > 70 then
        wcount = wcount + 1\
      end\
      i = i + 1\
    until i == 6\
    if wcount > 1 then\
      Stat = Stat + 1\
    else\
      if Stat > 0 then\
        Stat = Stat - 1\
      end\
    end\
  until Stat >= 2 or ccheck()\
  motor(0,0,0,0)\
end\
function junktest()\
  while scheck() do\
    dummy = sget(1)\
    -- print("Junkdata "..strbyte(dummy,1))\
  end\
end\
function delay(x)\
  if x > 0 then\
    repeat\
      x = x - 1\
    until x == 0\
  end\
end\
function activate(c)\
  junktest()\
  chipselect(c)\
  repeat until ack() == 1\
  chipselect(0)\
  sput(strchar(10),1)\
end\
function deactivate(c)\
  junktest()\
  chipselect(c)\
  repeat until ack() == 1\
  chipselect(0)\
  sput(strchar(11),1)\
end\
function h(x) return tonumber(x,16) end\
function b(x) return tonumber(x,2) end\
function s()\
  local x\
  local i\
  local CheckSum\
  local Try = 0\
  repeat\
    junktest()\
    chipselect(2)\
    repeat until ack() == 1\
    chipselect(0)\
    sput(strchar(100),1)\
    repeat until scheck()\
    x = sget(6)\
    i = 1\
    CheckSum = 0\
    repeat\
      CheckSum = CheckSum + strbyte(x,i)\
      if CheckSum > 255 then\
        CheckSum = CheckSum - 256\
      end\
      i = i + 1\
    until i == 6\
    if CheckSum ~= strbyte(x,6) then\
      print("Checksum Fejl Sharp")\
    end\
    Try = Try + 1\
  until CheckSum == strbyte(x,6) or Try > 10\
  return strbyte(x,1),strbyte(x,2),strbyte(x,3),strbyte(x,4)\
end\
function motor(a1,a2,b1,b2)\
  local mx1 = a2\
  local my1 = 0\
  local my2 = 0\
  local mx2 = b2\
  junktest()\
  if a2 < 0 then\
    a1 = 0\
    a2 = abs(a2)\
  end\
  if b2 < 0 then\
    b1 = 0\
    b2 = abs(b2)\
  end\
  if a2 == 1 then\
    mx1 = 1\
    my1 = 0\
  elseif a2 == 2 then\
    mx1 = 1\
    my1 = 1\
  elseif a2 == 3 then\
    mx1 = 32\
    my1 = 33\
  elseif a2 == 4 then\
    mx1 = 136\
    my1 = 136\
  elseif a2 == 5 then\
    mx1 = 73\
    my1 = 33\
  elseif a2 == 6 then\
    mx1 = 73\
    my1 = 41\
  elseif a2 == 7 then\
    mx1 = 170\
    my1 = 168\
  elseif a2 == 8 then\
    mx1 = 170\
    my1 = 170\
  elseif a2 == 9 then\
    mx1 = 170\
    my1 = 171\
  elseif a2 == 10 then\
    mx1 = 182\
    my1 = 214\
  elseif a2 == 11 then\
    mx1 = 221\
    my1 = 182\
  elseif a2 == 12 then\
    mx1 = 238\
    my1 = 238\
  elseif a2 == 13 then\
    mx1 = 222\
    my1 = 254\
  elseif a2 == 14 then\
    mx1 = 254\
    my1 = 254\
  elseif a2 == 15 then\
    mx1 = 255\
    my1 = 254\
  elseif a2 == 16 then\
    mx1 = 255\
    my1 = 255\
  end\
  if b2 == 1 then\
    mx2 = 1\
    my2 = 0\
  elseif b2 == 2 then\
    mx2 = 1\
    my2 = 1\
  elseif b2 == 3 then\
    mx2 = 32\
    my2 = 33\
  elseif b2 == 4 then\
    mx2 = 136\
    my2 = 136\
  elseif b2 == 5 then\
    mx2 = 73\
    my2 = 33\
  elseif b2 == 6 then\
    mx2 = 73\
    my2 = 41\
  elseif b2 == 7 then\
    mx2 = 170\
    my2 = 168\
  elseif b2 == 8 then\
    mx2 = 170\
    my2 = 170\
  elseif b2 == 9 then\
    mx2 = 170\
    my2 = 171\
  elseif b2 == 10 then\
    mx2 = 182\
    my2 = 214\
  elseif b2 == 11 then\
    mx2 = 221\
    my2 = 182\
  elseif b2 == 12 then\
    mx2 = 238\
    my2 = 238\
  elseif b2 == 13 then\
    mx2 = 222\
    my2 = 254\
  elseif b2 == 14 then\
    mx2 = 254\
    my2 = 254\
  elseif b2 == 15 then\
    mx2 = 255\
    my2 = 254\
  elseif b2 == 16 then\
    mx2 = 255\
    my2 = 255\
  end\
  chipselect(1)\
  repeat until ack() == 1\
  chipselect(0)\
  sput(strchar(200,a1,mx1,my1,b1,mx2,my2),7)\
end\
function snabel()\
  local SD\
  local s1\
  local s2\
  local s3\
  local s4\
  local s5\
  local t1\
  local t2\
  local dummy\
  local i\
  local CheckSum\
  local Try = 0\
  repeat\
    junktest()\
    chipselect(3)\
    repeat until ack() == 1\
    chipselect(0)\
    sput(strchar(100),1)\
    repeat until scheck()\
    SD = sget(11)\
    i = 1\
    CheckSum = 0\
    repeat\
      CheckSum = CheckSum + strbyte(SD,i)\
      if CheckSum > 255 then\
        CheckSum = CheckSum - 256\
      end\
      i = i + 1\
    until i == 11\
    if CheckSum ~= strbyte(SD,11) then\
      print("Checksum Fejl Snabel")\
    end\
    Try = Try + 1\
  until CheckSum == strbyte(SD,11) or Try > 10\
  t1 = strbyte(SD,1)\
  t2 = strbyte(SD,2)\
  s1 = (t1 * 256) + t2\
  t1 = strbyte(SD,3)\
  t2 = strbyte(SD,4)\
  s2 = (t1 * 256) + t2\
  t1 = strbyte(SD,5)\
  t2 = strbyte(SD,6)\
  s3 = (t1 * 256) + t2\
  t1 = strbyte(SD,7)\
  t2 = strbyte(SD,8)\
  s4 = (t1 * 256) + t2\
  t1 = strbyte(SD,9)\
  t2 = strbyte(SD,10)\
  s5 = (t1 * 256) + t2\
  return s1,s2,s3,s4,s5\
end\
function test()\
  local c\
  local speed1 = 0\
  local speed2 = 0\
  local ret1,ret2\
  local s1,s2\
  local q1,q2,q3,q4,q5\
  local x={}\
  local dmin,dmax\
  activate(2)\
  activate(3)\
  print("Robot Manual Control")\
  repeat\
    repeat until ccheck()\
    c = cget(1)\
    if c == " " then\
      speed1 = 0\
      speed2 = 0\
    elseif c == "a" and speed1 < 15 then\
      speed1 = speed1 + 1\
    elseif c == "z" and speed1 > -15 then\
      speed1 = speed1 - 1\
    elseif c == "k" and speed2 < 15 then\
      speed2 = speed2 + 1\
    elseif c == "m" and speed2 > -15 then\
      speed2 = speed2 - 1\
    elseif c == "s" then\
      s1,s2,s3,s4 = s()\
      print("Sharp = "..s1.." "..s2.." "..s3.." "..s4)\
    elseif c == "t" then\
      x[1],x[2],x[3],x[4],x[5] = snabel()\
      print("Snabel A "..x[1].." "..x[2].." "..x[3].." "..x[4].." "..x[5])\
      x[1] = ((x[1] - cs1) * 100) / (ch1-cs1)\
      x[2] = ((x[2] - cs2) * 100) / (ch2-cs2)\
      x[3] = ((x[3] - cs3) * 100) / (ch3-cs3)\
      x[4] = ((x[4] - cs4) * 100) / (ch4-cs4)\
      x[5] = ((x[5] - cs5) * 100) / (ch5-cs5)\
      print("Snabel B "..x[1].." "..x[2].." "..x[3].." "..x[4].." "..x[5])\
      dmax = max(x[1],x[2],x[3],x[4],x[5])\
      dmin = min(x[1],x[2],x[3],x[4],x[5])\
      x[1] = ((x[1] - dmin) * 100) / (dmax-dmin)\
      x[2] = ((x[2] - dmin) * 100) / (dmax-dmin)\
      x[3] = ((x[3] - dmin) * 100) / (dmax-dmin)\
      x[4] = ((x[4] - dmin) * 100) / (dmax-dmin)\
      x[5] = ((x[5] - dmin) * 100) / (dmax-dmin)\
      print("Snabel C "..x[1].." "..x[2].." "..x[3].." "..x[4].." "..x[5])\
    else\
      if c >= "0" and c <= "8" then\
        speed1 = tonumber(c)\
        speed2 = tonumber(c)\
      end\
    end\
    if speed1 < 0 then\
      ret1 = 0\
    else\
      ret1 = 1\
    end\
    if speed2 < 0 then\
      ret2 = 0\
    else\
      ret2 = 1\
    end\
    motor(ret2,abs(speed2),ret1,abs(speed1))\
    set7seg(abs(speed1))\
    print("Motor1="..speed1.."  - Motor2="..speed2)\
  until c == "q"\
  motor(0,0,0,0)\
  --deactivate(3)\
  --deactivate(2)\
end\
sreset(74000)\
motor(1,1,1,1)\
delay(5)\
motor(0,0,0,0)