#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>

uint8_t mem[64*1024];

typedef struct flagbits {
  unsigned int c : 1;
  unsigned int u1 : 1;
  unsigned int p : 1;
  unsigned int u3 : 1;

  unsigned int a : 1;
  unsigned int u5 : 1;
  unsigned int z : 1;
  unsigned int s : 1;
} flagbits;

typedef struct reg {
  uint8_t flags;
  uint8_t a;
  uint8_t c;
  uint8_t b;
  uint8_t e;
  uint8_t d;
  uint8_t l;
  uint8_t h;
} reg;

typedef struct regp {
  uint16_t psw;
  uint16_t bc;
  uint16_t de;
  uint16_t hl;
} regp;

union regset {
  struct reg reg;
  struct regp regp;
} regset;

#define A regset.reg.a
#define B regset.reg.b
#define C regset.reg.c
#define D regset.reg.d
#define E regset.reg.e
#define H regset.reg.h
#define L regset.reg.l
#define M mem[HL]

#define BC regset.regp.bc
#define DE regset.regp.de
#define HL regset.regp.hl

#define flags (*(struct flagbits*)&regset.reg.flags)

uint16_t IP = 0x100;
uint16_t SP = 0;
unsigned int tmp;

// Dump registers
void dump() {
  printf("\n");
  printf("A=%02X BC=%04X DE=%04X HL=%04X M=%02X SP=%04X ", A, BC, DE, HL, M, SP);
  printf("%c", flags.s ? 'S' : '-');
  printf("%c", flags.z ? 'Z' : '-');
  printf("%c", flags.a ? 'A' : '-');
  printf("%c", flags.p ? 'P' : '-');
  printf("%c", flags.c ? 'C' : '-');

  printf("\n");
  printf("IP=%04X : %02X %02X\n", IP, mem[IP], mem[IP+1]);
}

uint8_t imm8() {
  uint8_t r = *(uint8_t*)&mem[IP];
  IP++;
  return r;
}

uint16_t imm16() {
  uint16_t r = *(uint16_t*)&mem[IP];
  IP+=2;
  return r;
}

void push(uint16_t v) {
  SP -= 2;
  *(uint16_t*)&mem[SP] = v;
}

uint16_t pop() {
  uint16_t v = *(uint16_t*)&mem[SP];
  SP += 2;
  return v;
}

void in(uint8_t port) {

}

void out(uint8_t port) {

}

int has_even_parity(uint8_t x){
  unsigned int count = 0, i, b = 1;

  for(i = 0; i < 8; i++){
      if( x & (b << i) ){count++;}
  }

  if( (count % 2) ){return 0;}
  return 1;
}

void cpm_syscall(int number) {
  switch(number) {
  case 0:
    exit(0);
    break;
  case 2:
    printf("%c", E);
    break;
  case 0x0C:
    H=0x02;
    L=0x00;
    break;;
  default:
    fprintf(stderr, "Fatal: Unhandled CP/M syscall C=%02Xh\n", number);
    exit(1);
  }
}

void call(uint16_t v) {
  switch(v) {
  case 0:
    exit(0);
    break;;
  case 5:
    cpm_syscall(C);
    break;;
  default:
    push(IP);
    IP=v;
    break;
  }
}

void interrupt(int number) {
  call(number << 3);
}

#define add16c(a, b) \
  tmp = a + b; \
  flags.c = (tmp >> 16) & 1

#define unarync(a, op) \
  tmp=a; \
  tmp op; \
  flags.p = has_even_parity(tmp & 8); \
  flags.z = !(tmp); \
  flags.s = (tmp >> 7) & 1; \
  a=tmp

#define aritht(a, op, b) \
  tmp=a; \
  tmp=a op b; \
  flags.c = (tmp >> 8) & 1; \
  flags.p = has_even_parity(tmp & 8); \
  flags.z = !(tmp); \
  flags.s = (tmp >> 7) & 1;

#define arith(a, op, b) \
  aritht(a, op, b); \
  a=tmp

// Execute a single instruction
void step() {
  uint8_t opcode = mem[IP];
  IP++;
  switch(opcode) {
  case 0x00: break;;
  case 0x01: BC=imm16(); break;;
  case 0x02: mem[BC]=A; break;;
  case 0x03: BC++;
  case 0x04: unarync(B, ++); break;;
  case 0x05: unarync(B, --); break;;
  case 0x06: B=imm8(); break;;
  case 0x07: tmp=A; A = A << 1 | flags.c; flags.c = tmp >> 7; break;;

  case 0x09: HL=add16c(HL,BC); break;;
  case 0x0a: A=mem[BC]; break;;
  case 0x0b: BC--;
  case 0x0c: unarync(B, ++); break;;
  case 0x0d: unarync(B, --); break;;
  case 0x0e: C=imm8(); break;;

  case 0x11: DE=imm16(); break;;
  case 0x12: mem[DE]=A; break;;
  case 0x13: DE++;
  case 0x14: unarync(D, ++); break;;
  case 0x15: unarync(D, --); break;;
  case 0x16: D=imm8(); break;;

  case 0x19: HL=add16c(HL,DE); break;;
  case 0x1a: A=mem[DE]; break;;
  case 0x1b: DE--;
  case 0x1c: unarync(E, ++); break;;
  case 0x1d: unarync(E, --); break;;
  case 0x1e: E=imm8(); break;;

  case 0x21: HL=imm16(); break;;
  case 0x22: *(uint16_t*)&mem[imm16()]=HL; break;;
  case 0x23: HL++;
  case 0x24: unarync(H, ++); break;;
  case 0x25: unarync(H, --); break;;
  case 0x26: H=imm8(); break;;

  case 0x29: HL=add16c(HL,HL); break;;
  case 0x2a: HL=*(uint16_t*)&mem[imm16()]; break;;
  case 0x2b: HL--;
  case 0x2c: unarync(L, ++); break;;
  case 0x2d: unarync(L, --); break;;
  case 0x2e: L=imm8(); break;;
  case 0x2f: A=~A; break;;

  case 0x31: SP=imm16(); break;;
  case 0x32: mem[imm16()]=A; break;;
  case 0x33: SP++;
  case 0x34: unarync(M, ++); break;;
  case 0x35: unarync(M, --); break;;
  case 0x36: M=imm8(); break;;
  case 0x37: flags.c = 1; break;;

  case 0x39: HL=add16c(HL,SP); break;;
  case 0x3a: A=mem[imm16()]; break;;
  case 0x3b: SP--;
  case 0x3c: unarync(A, ++); break;;
  case 0x3d: unarync(A, --); break;;
  case 0x3e: A=imm8(); break;;
  case 0x3f: flags.c = !flags.c; break;;

  case 0x40: B=B; break;;
  case 0x41: B=C; break;;
  case 0x42: B=D; break;;
  case 0x43: B=E; break;;
  case 0x44: B=H; break;;
  case 0x45: B=L; break;;
  case 0x46: B=M; break;;
  case 0x47: B=A; break;;

  case 0x48: C=B; break;;
  case 0x49: C=C; break;;
  case 0x4a: C=D; break;;
  case 0x4b: C=E; break;;
  case 0x4c: C=H; break;;
  case 0x4d: C=L; break;;
  case 0x4e: C=M; break;;
  case 0x4f: C=A; break;;

  case 0x50: D=B; break;;
  case 0x51: D=C; break;;
  case 0x52: D=D; break;;
  case 0x53: D=E; break;;
  case 0x54: D=H; break;;
  case 0x55: D=L; break;;
  case 0x56: D=M; break;;
  case 0x57: D=A; break;;

  case 0x58: E=B; break;;
  case 0x59: E=C; break;;
  case 0x5a: E=D; break;;
  case 0x5b: E=E; break;;
  case 0x5c: E=H; break;;
  case 0x5d: E=L; break;;
  case 0x5e: E=M; break;;
  case 0x5f: E=A; break;;

  case 0x60: H=B; break;;
  case 0x61: H=C; break;;
  case 0x62: H=D; break;;
  case 0x63: H=E; break;;
  case 0x64: H=H; break;;
  case 0x65: H=L; break;;
  case 0x66: H=M; break;;
  case 0x67: H=A; break;;

  case 0x68: L=B; break;;
  case 0x69: L=C; break;;
  case 0x6a: L=D; break;;
  case 0x6b: L=E; break;;
  case 0x6c: L=H; break;;
  case 0x6d: L=L; break;;
  case 0x6e: L=M; break;;
  case 0x6f: L=A; break;;

  case 0x70: M=B; break;;
  case 0x71: M=C; break;;
  case 0x72: M=D; break;;
  case 0x73: M=E; break;;
  case 0x74: M=H; break;;
  case 0x75: M=L; break;;
  case 0x76: exit(0); break;;
  case 0x77: M=A; break;;

  case 0x78: A=B; break;;
  case 0x79: A=C; break;;
  case 0x7a: A=D; break;;
  case 0x7b: A=E; break;;
  case 0x7c: A=H; break;;
  case 0x7d: A=L; break;;
  case 0x7e: A=M; break;;
  case 0x7f: A=A; break;;

  case 0x80: arith(A, +, B); break;;
  case 0x81: arith(A, +, C); break;;
  case 0x82: arith(A, +, D); break;;
  case 0x83: arith(A, +, E); break;;
  case 0x84: arith(A, +, H); break;;
  case 0x85: arith(A, +, L); break;;
  case 0x86: arith(A, +, M); break;;
  case 0x87: arith(A, +, A); break;;

  case 0x88: arith(A, +flags.c+, B); break;;
  case 0x89: arith(A, +flags.c+, C); break;;
  case 0x8A: arith(A, +flags.c+, D); break;;
  case 0x8B: arith(A, +flags.c+, E); break;;
  case 0x8C: arith(A, +flags.c+, H); break;;
  case 0x8D: arith(A, +flags.c+, L); break;;
  case 0x8E: arith(A, +flags.c+, M); break;;
  case 0x8F: arith(A, +flags.c+, A); break;;

  case 0x90: arith(A, -, B); break;;
  case 0x91: arith(A, -, C); break;;
  case 0x92: arith(A, -, D); break;;
  case 0x93: arith(A, -, E); break;;
  case 0x94: arith(A, -, H); break;;
  case 0x95: arith(A, -, L); break;;
  case 0x96: arith(A, -, M); break;;
  case 0x97: arith(A, -, A); break;;

  case 0x98: arith(A, -flags.c-, B); break;;
  case 0x99: arith(A, -flags.c-, C); break;;
  case 0x9A: arith(A, -flags.c-, D); break;;
  case 0x9B: arith(A, -flags.c-, E); break;;
  case 0x9C: arith(A, -flags.c-, H); break;;
  case 0x9D: arith(A, -flags.c-, L); break;;
  case 0x9E: arith(A, -flags.c-, M); break;;
  case 0x9F: arith(A, -flags.c-, A); break;;

  case 0xA0: arith(A, &, B); break;;
  case 0xA1: arith(A, &, C); break;;
  case 0xA2: arith(A, &, D); break;;
  case 0xA3: arith(A, &, E); break;;
  case 0xA4: arith(A, &, H); break;;
  case 0xA5: arith(A, &, L); break;;
  case 0xA6: arith(A, &, M); break;;
  case 0xA7: arith(A, &, A); break;;

  case 0xA8: arith(A, ^, B); break;;
  case 0xA9: arith(A, ^, C); break;;
  case 0xAA: arith(A, ^, D); break;;
  case 0xAB: arith(A, ^, E); break;;
  case 0xAC: arith(A, ^, H); break;;
  case 0xAD: arith(A, ^, L); break;;
  case 0xAE: arith(A, ^, M); break;;
  case 0xAF: arith(A, ^, A); break;;

  case 0xB0: arith(A, |, B); break;;
  case 0xB1: arith(A, |, C); break;;
  case 0xB2: arith(A, |, D); break;;
  case 0xB3: arith(A, |, E); break;;
  case 0xB4: arith(A, |, H); break;;
  case 0xB5: arith(A, |, L); break;;
  case 0xB6: arith(A, |, M); break;;
  case 0xB7: arith(A, |, A); break;;

  case 0xB8: aritht(A, -, B); break;;
  case 0xB9: aritht(A, -, C); break;;
  case 0xBA: aritht(A, -, D); break;;
  case 0xBB: aritht(A, -, E); break;;
  case 0xBC: aritht(A, -, H); break;;
  case 0xBD: aritht(A, -, L); break;;
  case 0xBE: aritht(A, -, M); break;;
  case 0xBF: aritht(A, -, A); break;;

  case 0xC0: if (!flags.z) IP=pop(); break;;
  case 0xC1: BC=pop(); break;;
  case 0xC2: tmp=imm16(); if (!flags.z) IP=tmp; break;;
  case 0xC3: IP=imm16(); break;;
  case 0xC4: tmp=imm16(); if (!flags.z) call(tmp); break;;
  case 0xC5: push(BC); break;;
  case 0xC6: arith(A, +, imm8()); break;;
  case 0xC7: interrupt(0); break;;

  case 0xC8: if (flags.z) IP=pop(); break;;
  case 0xC9: IP=pop(); break;;
  case 0xCA: tmp=imm16(); if (flags.z) IP=tmp; break;;
  case 0xCC: tmp=imm16(); if (flags.z) call(tmp); break;;
  case 0xCD: call(imm16()); break;;
  case 0xCE: arith(A, +flags.c+, imm8()); break;;
  case 0xCF: interrupt(1); break;;

  case 0xD0: if (!flags.c) IP=pop(); break;;
  case 0xD1: DE=pop(); break;;
  case 0xD2: tmp=imm16(); if (!flags.c) IP=tmp; break;;
  case 0xD3: out(imm8()); break;;
  case 0xD4: tmp=imm16(); if (!flags.c) call(tmp); break;;
  case 0xD5: push(DE); break;;
  case 0xD6: arith(A, -, imm8()); break;;
  case 0xD7: interrupt(2); break;;

  case 0xD8: if (flags.c) IP=pop(); break;;
  case 0xDA: tmp=imm16(); if (flags.c) IP=tmp; break;;
  case 0xDB: in(imm8()); break;;
  case 0xDC: tmp=imm16(); if (flags.c) call(tmp); break;;
  case 0xDE: arith(A, -flags.c-, imm8()); break;;
  case 0xDF: interrupt(3); break;;

  case 0xE0: if (!flags.p) IP=pop(); break;;
  case 0xE1: HL=pop(); break;;
  case 0xE2: tmp=imm16(); if (!flags.p) IP=tmp; break;;
  case 0xE3: tmp=HL; HL=pop(); push(tmp); break;;
  case 0xE4: tmp=imm16(); if (!flags.p) call(tmp); break;;
  case 0xE5: push(HL); break;;
  case 0xE6: arith(A, &, imm8()); break;;
  case 0xE7: interrupt(4); break;;

  case 0xE8: if (flags.p) IP=pop(); break;;
  case 0xE9: IP=HL; break;;
  case 0xEA: tmp=imm16(); if (flags.p) IP=tmp; break;;
  case 0xEB: tmp=HL; HL=DE; DE=tmp;; break;;
  case 0xEC: tmp=imm16(); if (flags.p) call(tmp); break;;
  case 0xEE: arith(A, ^, imm8()); break;;
  case 0xEF: interrupt(5); break;;

  case 0xF0: if (!flags.s) IP=pop(); break;;
  case 0xF1: regset.regp.psw=pop(); break;;
  case 0xF2: tmp=imm16(); if (!flags.s) IP=tmp; break;;
  case 0xF4: tmp=imm16(); if (!flags.s) call(tmp); break;;
  case 0xF5: push(regset.regp.psw); break;;
  case 0xF6: arith(A, |, imm8()); break;;
  case 0xF7: interrupt(6); break;;

  case 0xF8: if (flags.s) IP=pop(); break;;
  case 0xF9: SP=HL; break;;
  case 0xFA: tmp=imm16(); if (flags.s) IP=tmp; break;;
  case 0xFC: tmp=imm16(); if (flags.s) call(tmp); break;;
  case 0xFE: aritht(A, -, imm8()); break;;
  case 0xFF: interrupt(7); break;;

  default:
    IP--;
    dump();
    fprintf(stderr, "Invalid opcode at IP=%04X\n", IP);
    exit(1);
    break;
  }
}

void copy_cmdline(char* str) {
  int i, c;
  uint8_t *len = &mem[0x80];
  char* ptr = (char*)&mem[0x81];
  c = strlen(str);
  // Clip at max length
  if (c>0x7E) {
    fprintf(stderr, "Command line too long, max is 126 bytes\n");
    exit(1);
  }
  memcpy(ptr, str, c);
  ptr[c]=0x0D;
  *len=c;
}

int main(int argc, char** argv) {
  memset(&mem, sizeof(mem), 0);
  // Prepare default exit into int 20h
  mem[0]=0x76;
  push(0);

  argc--;
  argv++;

  while(argc && argv[0][0]=='-') {
    switch(argv[0][1]) {
    default:
      fprintf(stderr, "Unknown option %s\n", argv[0]);
      exit(1);
      break;
    }
    argc--;
    argv++;
  }

  if (argc) {
    FILE* fd = fopen(argv[0], "r");
    fread(mem + IP, 1, sizeof(mem) - IP, fd);
    argc--;
    argv++;
  } else {
    fprintf(stderr, "No COM file specified\n");
    exit(1);
  }

  if (argc) {
    copy_cmdline(argv[0]);
  } else {
    copy_cmdline("");
  }

  while(1) {
    dump();
    step();
  }
}