#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <doslib.h>
#include <iocslib.h>

#ifdef ENGLISH_VERSION
#define PROGRAM_NAME "sprdblen.x"
#else
#define PROGRAM_NAME "sprdbl.x"
#endif

//sin(),cos()を毎回計算せず加法定理を繰り返す
//  誤差が累積するが差分が正確ならば数千回程度で破綻することはないだろう
#define REPEAT_ADDITION_THEOREM

//最大公約数
int gcd (int x, int y) {
  if (x < 0) {
    x = -x;
  }
  if (y < 0) {
    y = -y;
  }
  while (y != 0) {
    int t = x % y;
    x = y;
    y = t;
  }
  return x;
}

//グラフィック画面
#define GRAPHIC_VRAM ((volatile uint16_t *) 0x00c00000)

//CRTC
#define CRTC_RASTER ((volatile uint16_t *) 0x00e80012)

//ビデオコントローラ
#define VICON_TSPALET ((volatile uint16_t *) 0x00e82200)
#define VICON_VISIBLE ((volatile uint16_t *) 0x00e82600)
#define VICON_SPON 0x0040
#define VICON_TXON 0x0020
#define VICON_GXON 0x0010
#define VICON_G4ON 0x0008
#define VICON_G3ON 0x0004
#define VICON_G2ON 0x0002
#define VICON_G1ON 0x0001

//スプライトコントローラ
typedef union {
  uint64_t data;
  struct {
    uint16_t x;
    uint16_t y;
    uint16_t character;
    uint16_t priority;
  };
} sprite_t;
#define SPRC_SPRITE ((volatile sprite_t *) 0x00eb0000)
#define SPRC_CONTROL ((volatile uint16_t *) 0x00eb0808)
#define MPUCS_MASK 0x0400
typedef struct {
  uint32_t data[32];  //1dot=4bit。0～15は左半分、16～31は右半分
} pattern_t;
#define SPRC_PATTERN ((volatile pattern_t *) 0x00eb8000)

//ROM
typedef struct {
  uint16_t data[16];  //1dot=1bit
} font_t;
//  16x16の「亜」のフォントアドレス
#define KANJI_FONT ((const font_t *) 0x00f00000 + 94 * 8)

//画面モード
#define CRTMOD_NUMBER 0
#define SCREEN_WIDTH 512
#define SCREEN_HEIGHT 512
#define SCREEN_ASPECT (4.0 / 3.0)
#define REAL_WIDTH 1024
#define RASTER_TWICE 0
#define RASTER_ZERO_CRT 41
#define RASTER_ZERO_LCD 68
uint16_t top_raster;  //垂直表示期間の先頭のラスター番号
uint16_t bottom_raster;  //垂直帰線期間の先頭のラスター番号

//パターン
//  パターンの数
//  1から512までの整数、省略すると512、257以上は要CYNTHIA64
short patterns;

//実スプライト
//  実スプライトの数
//  2から128までの偶数、省略すると128
short real_sprites;
//  実スプライトの数の半分
short half_sprites;

//スプライトの間隔の分割数
//  1から1000までの整数、省略すると20
short sprite_divisions;

//仮想スプライト
//  仮想スプライトの数
//  実スプライトの数から2048まで、実スプライトの数の半分の倍数、省略すると512
short virtual_sprites;
//  仮想スプライトの配列
//  度数ソートのソート後の配列を表示中の配列とダブルバッファリングする
//  [virtual_sprites]
sprite_t *sprite_array;  //ソート前の配列
sprite_t *array_0;
sprite_t *array_1;
sprite_t *sorted_array;  //ソート後の配列 } array_0とarray_1のいずれか
sprite_t *copied_array;  //表示中の配列   }

//軌道
//  定円の開始角度
//  0から359までの整数、省略すると90
int offset_angle;
//  動円の角度/定円の角度
//  どちらの数も0を除く-100から100までの整数、省略すると17/5
//  既約分数であること
int moving_angle, stable_angle;
//  動円の半径/定円の半径
//  どちらの数も1から100までの整数、省略すると3/2
//  既約分数であること
int moving_radius, stable_radius;
//  軌道の分割数
//  sprite_divisions*virtual_sprites
int orbit_divisions;
//  軌道の配列の長さ
//  2*orbit_divisions
int orbit_length;
//  軌道の配列
//  [orbit_length]
uint16_t *orbit_array;

//レディフラグ
//  メインルーチンで仮想スプライトの配列が完成したらセット
//  ラスター割り込みルーチンが仮想スプライトの配列を受け取ったらクリア
volatile _Bool ready_flag;

//開始位置
//  0番の仮想スプライトを置く軌道の位置に置くか
int start_segment;

//動作モード
#define STOP 0
#define STEP 1
#define RUN 2
short operating_mode;

//番号
//  仮想スプライトの番号
short virtual_number;
//  実スプライトの番号
short real_number;

//バンド
//  バンドの色
//  0から65535までの整数、省略すると0
uint16_t band_color;

//メッセージ
char *message;

//ラスター割り込みルーチン
void __attribute__((interrupt_handler)) raster_interrupt (void) {
  if (virtual_number == 0) {  //フレームの先頭
    VICON_TSPALET[0] = 0x0000;  //バンドなし
    if (!ready_flag) {  //メインルーチンで仮想スプライトの配列が完成していない
      return;
    }
    copied_array = sorted_array;  //ソート後の配列を表示中の配列にコピーする
    ready_flag = 0;  //ラスター割り込みルーチンが仮想スプライトの配列を受け取った
    for (int i = 0; i < real_sprites; i++) {
      SPRC_SPRITE[i].data = copied_array[i].data;
    }
    virtual_number = real_sprites;
    real_number = 0;
  } else {  //フレームの途中
    VICON_TSPALET[0] ^= band_color;  //バンドを反転する
    for (int i = 0; i < half_sprites; i++) {
      SPRC_SPRITE[real_number + i].data = copied_array[virtual_number + i].data;
    }
    virtual_number += half_sprites;
    real_number ^= half_sprites;
  }
  if (virtual_number < virtual_sprites) {  //フレームを継続する
    *CRTC_RASTER = top_raster +
      ((copied_array[virtual_number - half_sprites - 1].y - 2) << RASTER_TWICE);  //次のラスター割り込み位置
  } else {  //フレームを終了する
    virtual_number = 0;  //次のフレームの先頭
    real_number = 0;
    *CRTC_RASTER = bottom_raster;  //次のラスター割り込み位置
  }
}

//スーパーバイザーメインルーチン
void supervisor_main (void) {
  //垂直表示期間の先頭のラスター番号を求める
  //  IPLROM 1.6/crtmod16.xでLCD向けの同期周波数になっている場合に対応する
  top_raster = ((CRTMOD (0x16FF) & 0xff000000) == 0x96000000) ? RASTER_ZERO_LCD : RASTER_ZERO_CRT;
  //垂直帰線期間の先頭のラスター番号を求める
  bottom_raster = top_raster + (SCREEN_HEIGHT << RASTER_TWICE);
  //軌道を作る
  //  グラフィック画面に軌道を表示する
#ifdef ENGLISH_VERSION
  C_PRINT ((UBYTE *) "\x1b*Press BREAK or ESC to cancel");
#else
  C_PRINT ((UBYTE *) "\x1b*BREAK/ESCで中止");
#endif
  orbit_divisions = sprite_divisions * virtual_sprites;
  orbit_length = 2 * orbit_divisions;
  orbit_array = calloc (orbit_length, sizeof (orbit_array[0]));
  double sc = 2.0 * M_PI * (double) stable_angle / (double) orbit_divisions;
  double mc = 2.0 * M_PI * (double) moving_angle / (double) orbit_divisions;
  double sr = (double) stable_radius / (double) (stable_radius + moving_radius);
  double mr = (double) moving_radius / (double) (stable_radius + moving_radius);
  double oa = (M_PI / 180.0) * (double) offset_angle;
  double co = cos (oa);
  double so = sin (oa);
  double ox = (double) (SCREEN_WIDTH >> 1);
  double oy = (double) (SCREEN_HEIGHT >> 1);
  double ry = oy * (15.0 / 16.0);
  double rx = ry / SCREEN_ASPECT;
#ifdef REPEAT_ADDITION_THEOREM
  double cos_sc = cos (sc);
  double sin_sc = sin (sc);
  double cos_mc = cos (mc);
  double sin_mc = sin (mc);
  double cos_st = 1.0;
  double sin_st = 0.0;
  double cos_mt = 1.0;
  double sin_mt = 0.0;
#endif
  for (int i = 0; i < orbit_divisions; i++) {
#ifdef REPEAT_ADDITION_THEOREM
    double x1 = sr * cos_st + mr * cos_mt;
    double y1 = sr * sin_st + mr * sin_mt;
    {
      double t = cos_st * cos_sc - sin_st * sin_sc;
      sin_st = sin_st * cos_sc + cos_st * sin_sc;
      cos_st = t;
    }
    {
      double t = cos_mt * cos_mc - sin_mt * sin_mc;
      sin_mt = sin_mt * cos_mc + cos_mt * sin_mc;
      cos_mt = t;
    }
#else
    double st = sc * (double) i;
    double mt = mc * (double) i;
    double x1 = sr * cos (st) + mr * cos (mt);
    double y1 = sr * sin (st) + mr * sin (mt);
#endif
    double x2 = co * x1 - so * y1;
    double y2 = so * x1 + co * y1;
    int gx = (int) (ox + rx * x2 + 0.5);
    int gy = (int) (oy - ry * y2 + 0.5);
    if (gx < 0 || SCREEN_WIDTH <= gx ||
        gy < 0 || SCREEN_HEIGHT <= gy) {  //ないはずだが念のため
#ifdef ENGLISH_VERSION
      message = "Coordinate error";
#else
      message = "座標エラー";
#endif
      return;
    }
    GRAPHIC_VRAM[gx + REAL_WIDTH * gy] = 14;
    orbit_array[2 * i] = gx + 8;
    orbit_array[2 * i + 1] = gy + 8;
    //BREAK/ESCで中止
    if (B_KEYSNS () != 0) {
      int key = B_KEYINP () & 255;
      if (key == 3 || key == 27) {  //BREAK/ESC
#ifdef ENGLISH_VERSION
        message = "Canceled";
#else
        message = "中止しました";
#endif
        return;
      }
    }
  }
  //パレットを設定する
  //  パレットブロック1～15
  //  パレットコード1～15
  //  225色
  double cv = 2.0 * M_PI * (double) (moving_angle + stable_angle);
  for (int n = 0; n < 225; n++) {
    int b = n / 15 + 1;
    int c = n % 15 + 1;
    double t = (double) n / 225.0;
    int h = (int) (192.0 * t) % 192;
    int s = 31;
    int v = (int) (24.0 + 7.0 * cos (cv * t) + 0.5);
    VICON_TSPALET[b << 4 | c] = HSVTORGB (h, s, v);
  }
  //パターンを定義する
  int actual_patterns = virtual_sprites <= patterns ? virtual_sprites : patterns;
  *SPRC_CONTROL &= ~MPUCS_MASK;
  for (int n = 0; n < actual_patterns; n++) {
    if (n == 256) {
      *SPRC_CONTROL |= MPUCS_MASK;
    }
    int p = n * 225 / actual_patterns;  //パレット通し番号
    //int b = p / 15 + 1;  //パレットブロック
    int c = p % 15 + 1;  //パレットコード
    for (int y = 0; y < 16; y++) {
      uint16_t t = KANJI_FONT[n].data[y];
      uint32_t l = 0;
      uint32_t r = 0;
      for (int i = 0; i < 8; i++) {
        if ((t & (1 << (8 + i))) != 0) {
          l |= c << (4 * i);
        }
        if ((t & (1 << i)) != 0) {
          r |= c << (4 * i);
        }
      }
      SPRC_PATTERN[n & 255].data[y] = l;
      SPRC_PATTERN[n & 255].data[16 + y] = r;
    }
  }
  *SPRC_CONTROL &= ~MPUCS_MASK;
  //グラフィック画面に表示した軌道を隠す
  *VICON_VISIBLE = VICON_SPON | VICON_TXON;
  //仮想スプライト
  half_sprites = real_sprites >> 1;
  sprite_array = calloc (virtual_sprites, sizeof (sprite_array[0]));
  array_0 = calloc (virtual_sprites, sizeof (array_0[0]));
  array_1 = calloc (virtual_sprites, sizeof (array_1[0]));
  sorted_array = array_1;
  copied_array = NULL;
  //レディフラグ
  ready_flag = 0;
  //開始位置
  start_segment = 0;
  //動作モード
  operating_mode = RUN;
  //番号
  virtual_number = 0;
  real_number = 0;
  //仮想スプライトの配列を初期化する
  for (int n = 0; n < virtual_sprites; n++) {
    int k = n * actual_patterns / virtual_sprites;  //パターン番号
    //X座標とY座標
    //sprite_array[n].x = 0;
    //sprite_array[n].y = 0;
    //キャラクタとプライオリティ
    int p = k * 225 / actual_patterns;  //パレット通し番号
    int b = p / 15 + 1;  //パレットブロック
    //int c = p % 15 + 1;  //パレットコード
    sprite_array[n].character = b << 8 | (k & 255);
    sprite_array[n].priority = (k & 256) >> (8 - 2) | 3;
  }
  //ラスター割り込みを開始する
  if (CRTCRAS ((void *) raster_interrupt, bottom_raster) != 0) {
#ifdef ENGLISH_VERSION
    message = "The raster interrupt is in use";
#else
    message = "ラスター割り込みが使用中です";
#endif
    return;
  }
#ifdef ENGLISH_VERSION
  C_PRINT ((UBYTE *) "\x1b*Press BREAK or ESC to exit");
#else
  C_PRINT ((UBYTE *) "\x1b*BREAK/ESCで終了");
#endif
  C_CUROFF ();
  //開始後約5秒でテキスト画面を隠す
  int until_hidden = 60 * 5;
  while (1) {
    //フレーム毎の処理
    if (!ready_flag) {  //ラスター割り込みルーチンが仮想スプライトの配列を受け取った
      //開始後約5秒でテキスト画面を隠す
      //  バンドのパレット0はテキスト画面がなくてもスプライト画面があれば見える
      if (until_hidden != 0 &&
          --until_hidden == 0) {
        *VICON_VISIBLE = VICON_SPON;
      }
      //開始位置を動かす
      if (operating_mode != STOP) {
        start_segment++;
        if (orbit_divisions <= start_segment) {
          start_segment -= orbit_divisions;
        }
        if (operating_mode == STEP) {
          operating_mode = STOP;
        }
      }
      //仮想スプライトの配列を更新する
      int i = 2 * start_segment;
      int step = 2 * sprite_divisions;
      for (int n = 0; n < virtual_sprites; n++) {
        //X座標とY座標
        sprite_array[n].x = orbit_array[i];
        sprite_array[n].y = orbit_array[i + 1];
        i += step;
        if (orbit_length <= i) {
          i -= orbit_length;
        }
      }
      //ソート後の配列を入れ替える
      sorted_array = sorted_array == array_0 ? array_1 : array_0;
      //Y座標の昇順に度数ソートする
      short h[SCREEN_HEIGHT + 16];  //度数
      memset (h, 0, sizeof (h));  //度数をクリアする
      for (int i = 0; i < virtual_sprites; i++) {  //度数を求める
        h[sprite_array[i].y]++;
      }
      for (int y = 1; y < SCREEN_HEIGHT + 16; y++) {  //度数を積み重ねる
        h[y] += h[y - 1];
      }
      for (int i = virtual_sprites - 1; 0 <= i; i--) {  //仮想スプライトを並べる
        sorted_array[--h[sprite_array[i].y]].data = sprite_array[i].data;
      }
      //メインルーチンで仮想スプライトの配列が完成した
      ready_flag = 1;
    }
    //BREAK/ESCで終了
    if (B_KEYSNS () != 0) {
      int key = B_KEYINP () & 255;
      if (key == 3 || key == 27) {  //BREAK/ESC
        break;  //終了
      }
      if (key == 9) {  //TAB
        operating_mode = STEP;
      } else if (operating_mode == RUN) {
        operating_mode = STOP;
      } else {
        operating_mode = RUN;
      }
    }
  }
  C_CURON ();
  C_CLS_AL ();
  C_PRINT ((UBYTE *) "\x1b*");
  //テキスト画面を見せる
  *VICON_VISIBLE = VICON_SPON | VICON_TXON;
  //ラスター割り込みを解除する
  CRTCRAS (NULL, 0);
}

//メインルーチン
int main (int argc, char *argv[]) {
  //オプションを確認する
  int ma = 17, sa = 5;  //動円の角度/定円の角度
  int b = 0;  //バンドの色
  int d = 20;  //スプライトの間隔の分割数
  int o = 90;  //定円の開始角度
  int p = 512;  //パターンの数
  int mr = 3, sr = 2;  //動円の半径/定円の半径
  int s = 128;  //実スプライトの数
  int v = 512;  //仮想スプライトの数
  _Bool h = 0;
  for (int i = 1; !h && i < argc; i++) {
    h = 1;
    if (argv[i][0] == '-') {
      switch (argv[i][1]) {
      case 'a':
        h = !(sscanf (&argv[i][2], "%d/%d", &ma, &sa) == 2 ||
              (++i < argc && sscanf (argv[i], "%d/%d", &ma, &sa) == 2));
        break;
      case 'b':
        h = !(sscanf (&argv[i][2], "%d", &b) == 1 ||
              (++i < argc && sscanf (argv[i], "%d", &b) == 1));
        break;
      case 'd':
        h = !(sscanf (&argv[i][2], "%d", &d) == 1 ||
              (++i < argc && sscanf (argv[i], "%d", &d) == 1));
        break;
      case 'o':
        h = !(sscanf (&argv[i][2], "%d", &o) == 1 ||
              (++i < argc && sscanf (argv[i], "%d", &o) == 1));
        break;
      case 'p':
        h = !(sscanf (&argv[i][2], "%d", &p) == 1 ||
              (++i < argc && sscanf (argv[i], "%d", &p) == 1));
        break;
      case 'r':
        h = !(sscanf (&argv[i][2], "%d/%d", &mr, &sr) == 2 ||
              (++i < argc && sscanf (argv[i], "%d/%d", &mr, &sr) == 2));
        break;
      case 's':
        h = !(sscanf (&argv[i][2], "%d", &s) == 1 ||
              (++i < argc && sscanf (argv[i], "%d", &s) == 1));
        break;
      case 'v':
        h = !(sscanf (&argv[i][2], "%d", &v) == 1 ||
              (++i < argc && sscanf (argv[i], "%d", &v) == 1));
        break;
      }
    }
  }  //for i
  h = (h ||
       !(ma != 0 && -100 <= ma && ma <= 100 &&
         sa != 0 && -100 <= sa && sa <= 100) ||
       !(0 <= b && b <= 65535) ||
       !(1 <= d && d <= 1000) ||
       !(0 <= o && o <= 359) ||
       !(1 <= p && p <= 512) ||
       !(1 <= mr && mr <= 100 &&
         1 <= sr && sr <= 100) ||
       !(2 <= s && s <= 128 && (s & 1) == 0) ||
       !(s <= v && v <= 2048 && v % (s >> 1) == 0));
  if (h) {
#ifdef ENGLISH_VERSION
    printf ("Sample program of a sprite doubler (CYNTHIA64 compatible)\n"
            ">" PROGRAM_NAME " [Options]\n"
            "  -a {Angle of the moving circle}/{Angle of the stable circle}\n"
            "     Integers from -100 to 100 (excluding 0) for both numbers;\n"
            "     defaults to 17/5 if omitted\n"
            "  -b {Band color}\n"
            "     An integer from 0 to 65535; defaults to 0 if omitted\n"
            "  -d {Number of divisions of the sprite spacing}\n"
            "     An integer from 1 to 1000; defaults to 20 if omitted\n"
            "  -o {Starting angle of the stable circle}\n"
            "     An integer from 0 to 359; defaults to 90 if omitted\n"
            "  -p {Number of patterns}\n"
            "     An integer from 1 to 512; defaults to 512;\n"
            "     requires CYNTHIA64 for values 257 or higher\n"
            "  -r {Radius of the moving circle}/{Radius of the stable circle}\n"
            "     Integers from 1 to 100 for both numbers;\n"
            "     defaults to 3/2 if omitted\n"
            "  -s {Number of real sprites}\n"
            "     An even number from 2 to 128; defaults to 128 if omitted\n"
            "  -v {Number of virtual sprites}\n"
            "     A number between the number of real sprites and 2048,\n"
            "     a multiple of half the number of real sprites;\n"
            "     defaults to 512 if omitted\n"
            );
#else
    printf ("スプライトダブラーのサンプルプログラム(CYNTHIA64対応)\n"
            ">" PROGRAM_NAME " [オプション]\n"
            "  -a {動円の角度}/{定円の角度}\n"
            "     どちらの数も-100から100までの整数(0を除く)、省略すると17/5\n"
            "  -b {バンドの色}\n"
            "     0から65535までの整数、省略すると0\n"
            "  -d {スプライトの間隔の分割数}\n"
            "     1から1000までの整数、省略すると20\n"
            "  -o {定円の開始角度}\n"
            "     0から359までの整数、省略すると90\n"
            "  -p {パターンの数}\n"
            "     1から512までの整数、省略すると512、257以上は要CYNTHIA64\n"
            "  -r {動円の半径}/{定円の半径}\n"
            "     どちらの数も1から100までの整数、省略すると3/2\n"
            "  -s {実スプライトの数}\n"
            "     2から128までの偶数、省略すると128\n"
            "  -v {仮想スプライトの数}\n"
            "     実スプライトの数から2048まで、実スプライトの数の半分の倍数、省略すると512\n"
            );
#endif
    return 1;
  }
  int ga = gcd (ma, sa);
  moving_angle = ma / ga;
  stable_angle = sa / ga;
  band_color = b;
  sprite_divisions = d;
  offset_angle = o;
  patterns = p;
  int rg = gcd (mr, sr);
  moving_radius = mr / rg;
  stable_radius = sr / rg;
  real_sprites = s;
  virtual_sprites = v;
  //画面モードを変更する
  message = NULL;
  int saved_fnkmod = C_FNKMOD (-1);
  int saved_width = C_WIDTH (-1);
  CRTMOD (CRTMOD_NUMBER);
  G_CLR_ON ();
  SP_INIT ();
  SP_ON ();
  //スーパーバイザーメインルーチンを呼ぶ
  struct DREGS regs;
  memset (&regs, 0, sizeof (regs));
  SUPER_JSR (supervisor_main, &regs, &regs);
  //画面モードを復元する
  C_WIDTH (saved_width);
  C_FNKMOD (saved_fnkmod);
  if (message != NULL) {
    printf ("%s\n", message);
  }
  //オプションを表示する
  printf ("reminder: " PROGRAM_NAME " -a %d/%d -b %d -d %d -o %d -p %d -r %d/%d -s %d -v %d\n",
          moving_angle, stable_angle,
          band_color,
          sprite_divisions,
          offset_angle,
          patterns,
          moving_radius, stable_radius,
          real_sprites,
          virtual_sprites);
  return 0;
}
