include "inc/cmdsys.plh"
include "inc/args.plh"
include "inc/fileio.plh"
include "inc/int32.plh"
include "inc/sincos.plh"
include "dhgr.tk/inc/dhgrlib.plh"
include "dhgr.tk/inc/dhgrutils.plh"

sysflags resxtxt1|reshgr1|resxhgr1

// Bit patterns and associated default NTSC colors
// NTSC_BLACK     = 0000
// NTSC_DRKBLUE   = 0001  73,  14, 116
// NTSC_DRKGREEN  = 0010   0, 104, 100
// NTSC_BROWN     = 0100  55, 113,  11
// NTSC_MAGENTA   = 1000 127,  23,  27
// NTSC_PURPLE    = 1001
// NTSC_MEDBLUE   = 0011
// NTSC_LTBLUE    = 1011
// NTSC_AQUA      = 0111
// NTSC_GREEN     = 0110
// NTSC_ORANGE    = 1100
// NTSC_PINK      = 1101
// NTSC_YELLOW    = 1110
// NTSC_WHITE     = 1111

const RED = 0
const GRN = 1
const BLU = 2
// According to what I could find out about the NTSC color wheel:
//   Red maxes at 103.5 degrees
//   Green maxes at 240.7 degrees
//   Blue maxes at 347.1 degrees
const RED_PHASE_NTSC     = 104
const GREEN_PHASE_NTSC   = 241
const BLUE_PHASE_NTSC    = 347
// Ideal phase angles for 4 phase
const RED_PHASE_IDEAL    = 90
const GREEN_PHASE_IDEAL  = 270
const BLUE_PHASE_IDEAL   = 360
// Equal 120 deg phase angles
const RED_PHASE_EQUAL    = 120
const GREEN_PHASE_EQUAL  = 240
const BLUE_PHASE_EQUAL   = 360
// Simplified phase angles: 90 deg between R and B, 135 between RG and BG
const RED_PHASE_SIMPLE   = 90
const GREEN_PHASE_SIMPLE = 225
const BLUE_PHASE_SIMPLE  = 360
const GREY_CHROMA = 32 * 4 / 3
// Flags
const GREY_HACK   = $01 // Remap GREY1->GREY2 hack
const MEM_MODE    = $02 // Render to memory surface
const DUMP_STATE  = $04 // Dump internal state
const RAW_INFILE  = $08 // Raw 560x192 24BPP RGB values
const MATCH_ONE   = 8
const MATCH_ZERO  = 0
byte  clrRot = $00,$02,$04,$06,$08,$0A,$0C,$0E
byte         = $01,$03,$05,$07,$09,$0B,$0D,$0F
word hgrScan[] // HGR scanline order for saving memory buffer as DHGR
word = $0000,$0400,$0800,$0C00,$1000,$1400,$1800,$1C00
word = $0080,$0480,$0880,$0C80,$1080,$1480,$1880,$1C80
word = $0100,$0500,$0900,$0D00,$1100,$1500,$1900,$1D00
word = $0180,$0580,$0980,$0D80,$1180,$1580,$1980,$1D80
word = $0200,$0600,$0A00,$0E00,$1200,$1600,$1A00,$1E00
word = $0280,$0680,$0A80,$0E80,$1280,$1680,$1A80,$1E80
word = $0300,$0700,$0B00,$0F00,$1300,$1700,$1B00,$1F00
word = $0380,$0780,$0B80,$0F80,$1380,$1780,$1B80,$1F80
word = $0028,$0428,$0828,$0C28,$1028,$1428,$1828,$1C28
word = $00A8,$04A8,$08A8,$0CA8,$10A8,$14A8,$18A8,$1CA8
word = $0128,$0528,$0928,$0D28,$1128,$1528,$1928,$1D28
word = $01A8,$05A8,$09A8,$0DA8,$11A8,$15A8,$19A8,$1DA8
word = $0228,$0628,$0A28,$0E28,$1228,$1628,$1A28,$1E28
word = $02A8,$06A8,$0AA8,$0EA8,$12A8,$16A8,$1AA8,$1EA8
word = $0328,$0728,$0B28,$0F28,$1328,$1728,$1B28,$1F28
word = $03A8,$07A8,$0BA8,$0FA8,$13A8,$17A8,$1BA8,$1FA8
word = $0050,$0450,$0850,$0C50,$1050,$1450,$1850,$1C50
word = $00D0,$04D0,$08D0,$0CD0,$10D0,$14D0,$18D0,$1CD0
word = $0150,$0550,$0950,$0D50,$1150,$1550,$1950,$1D50
word = $01D0,$05D0,$09D0,$0DD0,$11D0,$15D0,$19D0,$1DD0
word = $0250,$0650,$0A50,$0E50,$1250,$1650,$1A50,$1E50
word = $02D0,$06D0,$0AD0,$0ED0,$12D0,$16D0,$1AD0,$1ED0
word = $0350,$0750,$0B50,$0F50,$1350,$1750,$1B50,$1F50
word = $03D0,$07D0,$0BD0,$0FD0,$13D0,$17D0,$1BD0,$1FD0
var surfMem, surfSpan
var[12] ntscChroma
var prevRed, prevBlu, prevGrn
byte[256] gammaRGB // RGB gamma correction
var[3] phase   = RED_PHASE_NTSC, GREEN_PHASE_NTSC, BLUE_PHASE_NTSC
byte gamma     = 0 // Gamma correction
var brightness = 0
var saturation = 255 // 1.0
var tint       = 0
byte errDiv    = 4
byte flags     = GREY_HACK
var rgbErr // Running color error array
var arg

// Handy numeric functions

def min(a, b)#1
  return a < b ?? a :: b
end

def max(a, b)#1
  return a > b ?? a :: b
end

def dist(dr, dg, db)#2 // Linear distance
  res[t_i32] rr, gg

  loadi16(dr)
  muli16(dr)
  store32(@rr)
  loadi16(dg)
  muli16(dg)
  store32(@gg)
  loadi16(db)
  muli16(db)
  add32(@gg)
  add32(@rr)
  store32(@rr)
  return rr:[0], rr:[1]
end

def atoi(strptr)#1
  var num, len, sign

  sign = 1
  num = 0
  len = ^strptr
  strptr++
  if ^strptr == '-'
    sign = -1
    strptr++
    len--
  elsif ^strptr == '+'
    strptr++
    len--
  fin
  while len and ^strptr >= '0' and ^strptr <= '9'
    num = num * 10 + ^strptr - '0'
    strptr++
    len--
  loop
  return num * sign
end

def calcChroma(angle)#0
  var r, g, b
  byte i

  for i = 0 to 3
    // Calculate RGB for this DHGR pixel
    r = saturation + mulcos(255, angle - phase[RED])
    g = saturation + mulcos(255, angle - phase[GRN])
    b = saturation + mulcos(255, angle - phase[BLU])
    ntscChroma[i*3 + RED] = min(255, max(0, (r + 2) / 4))
    ntscChroma[i*3 + GRN] = min(255, max(0, (g + 2) / 4))
    ntscChroma[i*3 + BLU] = min(255, max(0, (b + 2) / 4))
    // Next NTSC chroma pixel
    angle = angle - 90
  next
end

def rgbMatch(r, g, b, errptr, cx)#1
  var currRed, currGrn, currBlu
  var er, eg, eb
  byte pix,
  res[t_i32] pd

  // Previous RGB minus current chroma cycle
  prevRed = (prevRed * 3) / 4
  prevGrn = (prevGrn * 3) / 4
  prevBlu = (prevBlu * 3) / 4
  // Current potential RGB
  cx = cx * 3
  currRed = prevRed + ntscChroma[cx+RED]
  currGrn = prevGrn + ntscChroma[cx+GRN]
  currBlu = prevBlu + ntscChroma[cx+BLU]
  // Match next chroma subcycle
  pd:[0], pd:[1] = dist(r - prevRed, g - prevGrn, b - prevBlu)
  dist(r - currRed, g - currGrn, b - currBlu)
  if islt32(@pd)
    // RGB better matched with next chroma color
    prevRed = currRed
    prevGrn = currGrn
    prevBlu = currBlu
    pix = MATCH_ONE
  else
    pix = MATCH_ZERO
  fin
  // Propogate error down and forward
  er            = r - prevRed
  errptr=>[RED] = er
  eg            = g - prevGrn
  errptr=>[GRN] = eg
  eb            = b - prevBlu
  errptr=>[BLU] = eb
  errptr=>[RED+3] = er + errptr=>[RED+3]
  errptr=>[GRN+3] = eg + errptr=>[GRN+3]
  errptr=>[BLU+3] = eb + errptr=>[BLU+3]
  return pix
end

def rgbInit#0
  var i
  res[t_i32] g32

  if flags & DUMP_STATE
    puts("Gamma = "); puti(sext(gamma)); putln
  fin
  when gamma
    is 255 // (i + 1 / i^2) / 2
      for i = 0 to 255
        loadi16(i)
        muli16(i)
        addi16(127)
        divi16(255)
        neg32
        addi16(255)
        addi16(255 - i)
        divi16(2)
        store32(@g32)
        gammaRGB[255 - i] = g32
      next
      break
    is 254 // 1 - i^2
      for i = 0 to 255
        loadi16(i)
        muli16(i)
        addi16(127)
        divi16(255)
        neg32
        addi16(255)
        store32(@g32)
        gammaRGB[255 - i] = g32
      next
      break
    is 2 // 1/(i^2)
      for i = 0 to 255
        loadi16(i)
        muli16(i)
        addi16(127)
        divi16(255)
        store32(@g32)
        gammaRGB[i] = g32
      next
      break
    is 1 // (i + i^2) / 2
      for i = 0 to 255
        loadi16(i)
        muli16(i)
        addi16(127)
        divi16(255)
        addi16(i)
        divi16(2)
        store32(@g32)
        gammaRGB[i] = g32
      next
      break
    otherwise // i
      for i = 0 to 255
        gammaRGB[i] = i
      next
  wend
  if brightness
    for i = 0 to 255
      gammaRGB[i] = max(0, min(255, gammaRGB[i] + brightness))
    next
  fin
  calcChroma(tint)
  if flags & DUMP_STATE
    puts("Err Div = "); puti(errDiv); putln
    puts("Brightness = "); puti(brightness); putln
    puts("Tint = "); puti(tint); putln
    puts("Saturation = "); puti(saturation); putln
    puts("Chroma cycle RGB =\n")
    for i = 0 to 3
      putc('[')
      puti(ntscChroma[i*3 + RED]); putc(',')
      puti(ntscChroma[i*3 + GRN]); putc(',')
      puti(ntscChroma[i*3 + BLU]); puts("]\n")
    next
    if not (flags & MEM_MODE); getc; fin
  fin
  if flags & MEM_MODE
    surfMem, surfSpan = dhgrAllocBl7Mem(SCR_WIDTH, SCR_HEIGHT)
    dhgrSurfMem(OP_XOR, SCR_HEIGHT, surfMem, surfSpan)
    dhgrOp(OP_SRC) // Force op recalc
    dcgrColor(CLR_BLACK)
    dhgrClearBl7(0, 0, surfSpan >> 2, SCR_HEIGHT)
  else
    dhgrMode(DHGR_COLOR_MODE)
  fin
end

def rgbExit#0
  heaprelease(rgbErr)
  if not (flags & MEM_MODE)
    dhgrMode(DHGR_TEXT_MODE)
  fin
end

def pnmReadElement(refnum, bufptr)#1
  var lenptr

  lenptr = bufptr
  repeat
    ^lenptr = 0
    bufptr  = lenptr + 1
    if fileio:read(refnum, bufptr, 1) == 1 and ^bufptr == '#' // Comment
      ^lenptr++
      bufptr++
      while fileio:read(refnum, bufptr, 1) == 1 and ^bufptr >= ' '
      loop
    else
      repeat // Read white space seperated element
        ^lenptr++
        bufptr++
      until fileio:read(refnum, bufptr, 1) <> 1 \
         or ^bufptr <= ' ' \
         or ^lenptr > 32
    fin
  until ^lenptr and ^(lenptr + 1) <> '#' // Repeat until not comment
  if flags & DUMP_STATE; puts(lenptr); putc(' '); fin
  return lenptr
end

def pnmVerifyHeader(refnum)#1
  byte[128] buf
  if flags & DUMP_STATE; puts("PNM = "); fin
  pnmReadElement(refnum, @buf)
  if buf[0] <> 2 and buf[1] <> 'P' and buf[2] <> '6'
    puts("Invalid PNM magic #: "); putc(buf[1]); putc(buf[2]); putln
    return FALSE
  fin
  if atoi(pnmReadElement(refnum, @buf)) <> 560
    puts("Width not 560: "); puts(@buf); putln
    return FALSE
  fin
  if atoi(pnmReadElement(refnum, @buf)) <> 192
    puts("Height not 192: "); puts(@buf); putln
    return FALSE
  fin
  if atoi(pnmReadElement(refnum, @buf)) <> 255
    puts("Depth not 255: "); puts(@buf); putln
    return FALSE
  fin
  if flags & DUMP_STATE; putln; fin
  return TRUE
end

def rgbImportExport(rgbfile, dhgrfile)#0
  byte refnum, chromaBits
  var i, j, c, r, g, b
  var rgbScanline, rgbptr, errptr

  refnum = fileio:open(rgbfile)
  if refnum
    if not (flags & RAW_INFILE)
      if not pnmVerifyHeader(refnum)
        fileio:close(refnum)
        return
      fin
    fin
    rgbInit
    rgbScanline = heapalloc(560 * 3)
    rgbErr      = heapalloc(561 * 3 * 2)
    if rgbErr and rgbScanline
      // Init error propogation array
      memset(rgbErr, 0, 560 * 3 * 2)
      memset(rgbScanline, 0, 560 * 3)
      for j = 0 to 191
        fileio:read(refnum, rgbScanline,  560 * 3)
        prevRed, prevGrn, prevBLu = 96, 96, 96 // Reset prev RGB
        rgbptr = rgbScanline
        errptr = rgbErr
        for i = 0 to 139
          chromaBits = 0
          for c = 0 to 3
            // Calc best match
            r = gammaRGB[rgbptr->[RED]]
            g = gammaRGB[rgbptr->[GRN]]
            b = gammaRGB[rgbptr->[BLU]]
            if errDiv
              r = r + errptr=>[RED] / errDiv
              g = g + errptr=>[GRN] / errDiv
              b = b + errptr=>[BLU] / errDiv
            fin
            chromaBits = (chromaBits >> 1) | rgbMatch(r, g, b, errptr, c)
            rgbptr = rgbptr + 3
            errptr = errptr + 3 * 2
          next
          if flags & GREY_HACK and chromaBits == $0A // Bits are reversed
            // Map GREY1 -> GREY2
            chromaBits = $05
          fin
          dcgrColor(clrRot[chromaBits])
          dcgrPixel(i, j)
        next
        if flags & MEM_MODE; putc('.'); fin
        if ^$C000 == $83
          break
        fin
      next
      fileio:close(refnum)
      if ^dhgrfile
        if flags & MEM_MODE
          heaprelease(rgbScanline) // Free up some memory
          if MACHID & $F0 <> $B0
            // Use allocated buffer on non 128K //e
            rgbScanline = heapalloc($2000)
          else
            // Use HGR memory (already reserved) on 128K //e
            rgbScanline = $2000
          fin
          if rgbScanline
            memset(rgbScanline, 0, $2000)
            fileio:destroy(dhgrfile)
            fileio:create(dhgrfile, $06, $2000)
            refnum = fileio:open(dhgrfile)
            if refnum
              // Reorder scanlines into HGR format
              rgbptr = surfMem
              for i = 0 to SCR_HEIGHT-1
                memcpy(rgbScanline+hgrScan[i], rgbptr, 40)
                rgbptr = rgbptr + surfSpan
              next
              // Write AUX bytes
              fileio:write(refnum, rgbScanline, $2000)
              rgbptr = surfMem + surfSpan / 2
              for i = 0 to SCR_HEIGHT-1
                memcpy(rgbScanline+hgrScan[i], rgbptr, 40)
                rgbptr = rgbptr + surfSpan
              next
              // Write MAIN bytes
              fileio:write(refnum, rgbScanline, $2000)
              fileio:close(refnum)
            fin
            putln
          fin
        else
          screenWrite(dhgrfile)
        fin
      fin
      if not (flags & MEM_MODE); getc; fin
      rgbExit
    fin
  else
    puts("Unable to open "); puts(rgbfile); putln
  fin
end

puts("DHGR RGB converter 1.2\n")
arg = argNext(argFirst)
if ^arg
  while ^(arg + 1) == '-'
    when toupper(^(arg + 2))
      is 'B' // Set brightness
        if ^arg > 2
          ^(arg + 2) = ^arg - 2
          brightness = atoi(arg + 2)
        fin
        break
      is 'C' // Disable hack for GREY remapping
        flags = flags & ~GREY_HACK
        break
      is 'D' // Dump internal staet
        flags = flags | DUMP_STATE
        break
      is 'E' // Set error strength
        if ^arg > 2
          errDiv = ^(arg + 3) - '0'
          if ^arg > 3
            errDiv = errDiv * 10 + ^(arg + 4) - '0'
          fin
        fin
        break
      is 'G' // Set gamma amount
        if ^arg > 2
          ^(arg + 2) = ^arg - 2
          gamma = atoi(arg + 2)
        fin
        break
      is 'P' // RGB phase angle
        when toupper(^(arg + 3))
          is 'I' // Use ideal 4 sub-phase angles
            phase[RED] = RED_PHASE_IDEAL
            phase[GRN] = GREEN_PHASE_IDEAL
            phase[BLU] = BLUE_PHASE_IDEAL
            break
          is 'E' // Use equal 120 deg phase angles
            phase[RED] = RED_PHASE_EQUAL
            phase[GRN] = GREEN_PHASE_EQUAL
            phase[BLU] = BLUE_PHASE_EQUAL
            break
          is 'S' // Use simplified 90 degree and opposite phase angles
            phase[RED] = RED_PHASE_SIMPLE
            phase[GRN] = GREEN_PHASE_SIMPLE
            phase[BLU] = BLUE_PHASE_SIMPLE
            break
          //is 'N' // Use theoretical NTSC phase angles
          otherwise
            phase[RED] = RED_PHASE_NTSC
            phase[GRN] = GREEN_PHASE_NTSC
            phase[BLU] = BLUE_PHASE_NTSC
            break
        wend
        break
      is 'R' // Raw input mode - no PNM header
        flags = flags | RAW_INFILE
        break
      is 'S' // Adjust saturation
        if ^arg > 2
          ^(arg + 2) = ^arg - 2
          saturation = saturation - atoi(arg + 2)
        fin
        break
      is 'T' // Adjust tint
        if ^arg > 2
          ^(arg + 2) = ^arg - 2
          tint = tint + atoi(arg + 2)
        fin
        break
      is 'V' // No video output, memory mode only (for portable VM)
        flags = flags | MEM_MODE
        break
      otherwise
        puts("? option:"); putc(^(arg + 2)); putln
    wend
    arg = argNext(arg)
  loop
  if ^arg
    rgbImportExport(arg, argNext(arg))
  fin
  return 0
fin
puts("Usage:\n")
puts(" DHGRRGB\n")
puts("  [-B#]  = Brightness: -255..255\n")
puts("  [-C]   = Composite output\n")
puts("  [-D]   = Dump state\n")
puts("  [-E#]  = Error strength: 1..255\n")
puts("                           0 = no err\n")
puts("  [-G#]  = Gamma: 2, 1, 0, -1, -2\n")
puts("  [-P<I,E,S,N>] = Phase: Ideal, Equal\n")
puts("                         Simple, NTSC\n")
puts("  [-R]   = Raw image file, no header\n")
puts("  [-S#]  = Saturation: -255..255\n")
puts("  [-T#]  = Tint: -360..360 (in degrees)\n")
puts("  [-V]   = no Video output, mem only\n")
puts(" IMAGEFILE [DHGRFILE]\n")
done
