/*

  first request:
  Is there an algorithm for rendering cubic bezier curves as straight line segments,
  where the length of each segment is not determined by fixed t intervals, but rather
  by the angle from one segment to the next being less than or equal to some given small
  angle off from straight ahead (i.e. A-B-C being 180 degrees plus or minus some small
  given angle.)  If such an algorithm exists, write a C procedure to generate the line
  segments given the list of bezier control points that form the piecewise bezier path.

  Followup to create Vectrex code:
  I use an old system where efficient code uses 8 bit integers (uint8_t or int8_t)
  and 16 bit uint16_t and int16_t when necessary, but most arithmetic is best done
  in 8 bit for speed reasons. The vector display uses a coordinate range of -128:127
  in both axes. Create a program similar to the above that will work within these
  constraints. Have it call MoveAbs(int8_t x, int8_t y) and LineAbs(int8_t x, int8_t y)
  to output the results. Trig functions such as acos are not available. sqrt is not
  supplied. Approximations are acceptable.
 */

// TO DO: Similar code, but using classic t-step bezier generation without the fancy stuff...

// cc -Wall -o smooth-curve-vectrex smooth-curve-vectrex.c

#ifdef __linux__

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
static void Moveto_d(int8_t y, int8_t x) {
  printf("  Moveto_d(%d,%d);\n", y, x);
}
static void Draw_Line_d(int8_t y, int8_t x) {
  printf("  Draw_line_d(%d,%d);\n", y, x);
}
static void Reset0Ref(void) {
  printf("  Reset0Ref();\n");
}
static void Wait_Recal(void) {
  printf("  Wait_Recal();\n");
}
static void Intensity_7F(void) {
  printf("  Intensity_7F();\n");
}
static void Intensity_5F(void) {
  printf("  Intensity_5F();\n");
}
void set_scale(const unsigned int scale) {
}
#else

#include <vectrex.h>

#define int8_t int
#define uint8_t unsigned int
#define int16_t long
#define uint16_t unsigned long
int8_t abs(int8_t x) {
  if (x >= 0) return x;
  if (x == -128) return 127;
  return -x;
}

static inline __attribute__((always_inline)) 
void set_scale(const unsigned int scale)
{
	dp_VIA_t1_cnt_lo = scale;
};

static void exit(int n) {
  for (;;) {
    n++;
  }
}

#endif
static int8_t lastx = 0, lasty = 0;
#define MAX_RAM 128
static uint8_t next_free_ram = 255U;
int8_t savedx[MAX_RAM];
int8_t savedy[MAX_RAM];

void MoveAbs(int8_t x, int8_t y) {
  #ifdef __linux__
  printf("MoveAbs(%d,%d);\n", x, y);
  #endif
  next_free_ram += 1U; if (next_free_ram == MAX_RAM) exit(1);
  lastx = savedx[next_free_ram] = x;
  lasty = savedy[next_free_ram] = y;
}

void LineAbs(int8_t x, int8_t y) {
  #define bad_subtract(a, b, sum)                                  \
  (                                                                \
    (                                                              \
      (a ^ b)                                                      \
      &                                                            \
      (                                                            \
         (int8_t) (                                                \
                           (sum=(int8_t)((uint8_t)a - (uint8_t)b)) \
                           ^                                       \
                           a                                       \
                  )                                                \
      )                                                            \
    ) < 0                                                          \
  )
  #ifdef __linux__
  printf("LineAbs(%d,%d);\n", x, y);
  #endif
  int8_t dx; // = x-lastx;
  int8_t dy; // = y-lasty;
  // Rather excessive paranoia for drawing lines longer than 127 units.
  if (bad_subtract(x, lastx, dx) || bad_subtract(y, lasty, dy)) {
    // split in two using 16 bit arithmetic!
    int16_t dx = x-lastx;
    int16_t dy = y-lasty;
    if ((int8_t)(dy-(dy>>1)) == -128 || (int8_t)(dx-(dx>>1)) == -128) {
      int8_t dx2 = (int8_t)(dx>>2), dy2 = (int8_t)(dy>>2);
      //Draw_Line_d(dy2, dx2);
      next_free_ram += 1U; if (next_free_ram == MAX_RAM) exit(1);
      savedx[next_free_ram] = dx2;
      savedy[next_free_ram] = dy2;
      //Draw_Line_d(dy2, dx2);
      next_free_ram += 1U; if (next_free_ram == MAX_RAM) exit(1);
      savedx[next_free_ram] = dx2;
      savedy[next_free_ram] = dy2;
      //Draw_Line_d(dy2, dx2);
      next_free_ram += 1U; if (next_free_ram == MAX_RAM) exit(1);
      savedx[next_free_ram] = dx2;
      savedy[next_free_ram] = dy2;
      //Draw_Line_d((int8_t)(dy-(dy2)-(dy2)-(dy2)), (int8_t)(dx-(dx2)-(dx2)-(dx2)));
      next_free_ram += 1U; if (next_free_ram == MAX_RAM) exit(1);
      savedx[next_free_ram] = (int8_t)(dx-(dx2)-(dx2)-(dx2));
      savedy[next_free_ram] = (int8_t)(dy-(dy2)-(dy2)-(dy2));
    } else {
      //Draw_Line_d((int8_t)(dy>>1), (int8_t)(dx>>1));
      next_free_ram += 1U; if (next_free_ram == MAX_RAM) exit(1);
      savedx[next_free_ram] = (int8_t)(dx>>1);
      savedy[next_free_ram] = (int8_t)(dy>>1);
      //Draw_Line_d((int8_t)(dy-(dy>>1)), (int8_t)(dx-(dx>>1)));
      next_free_ram += 1U; if (next_free_ram == MAX_RAM) exit(1);
      savedx[next_free_ram] = (int8_t)(dx-(dx>>1));
      savedy[next_free_ram] = (int8_t)(dy-(dy>>1));
    }
  } else {
    //Draw_Line_d(dy, dx);
    next_free_ram += 1U; if (next_free_ram == MAX_RAM) exit(1);
    savedx[next_free_ram] = dx;
    savedy[next_free_ram] = dy;
  }
  lastx = x; lasty = y;
}


/* -----------------------------------------------------------------------
   Basic fixed-point / small-int vector helpers
   All coordinates are in -128..127 (int8_t). Intermediates use int16_t.
   ----------------------------------------------------------------------- */

typedef struct {
    int8_t x, y;
} V2s8;

/* Add with clamping to int8_t range. */
static int8_t clamp_s8(int16_t v) {
    if (v > 127)  return 127;
    if (v < -128) return -128;
    return (int8_t)v;
}

static V2s8 v2_add(V2s8 a, V2s8 b) {
    V2s8 r;
    r.x = clamp_s8((int16_t)a.x + (int16_t)b.x);
    r.y = clamp_s8((int16_t)a.y + (int16_t)b.y);
    return r;
}

static V2s8 v2_sub(V2s8 a, V2s8 b) {
    V2s8 r;
    r.x = clamp_s8((int16_t)a.x - (int16_t)b.x);
    r.y = clamp_s8((int16_t)a.y - (int16_t)b.y);
    return r;
}

/* Scale by 1/2 (for De Casteljau at t = 0.5). */
static V2s8 v2_half(V2s8 a) {
    V2s8 r;
    r.x = (int8_t)(a.x / 2);  /* arithmetic shift is fine here */
    r.y = (int8_t)(a.y / 2);
    return r;
}

/* Squared length (16-bit). */
static uint16_t v2_len2(V2s8 a) {
    int16_t dx = a.x;
    int16_t dy = a.y;
    return (uint16_t)(dx * dx + dy * dy);
}

/* -----------------------------------------------------------------------
   Cubic Bézier representation and subdivision
   ----------------------------------------------------------------------- */

typedef struct {
    V2s8 p0, p1, p2, p3;
} CubicS8;

/* De Casteljau split at t = 0.5, everything in 8-bit with clamped sums. */
static void cubic_split_half(const CubicS8 *c,
                             CubicS8 *left,
                             CubicS8 *right) {
    V2s8 p01 = v2_half(v2_add(c->p0, c->p1));
    V2s8 p12 = v2_half(v2_add(c->p1, c->p2));
    V2s8 p23 = v2_half(v2_add(c->p2, c->p3));

    V2s8 p012  = v2_half(v2_add(p01, p12));
    V2s8 p123  = v2_half(v2_add(p12, p23));
    V2s8 p0123 = v2_half(v2_add(p012, p123));

    left->p0 = c->p0;
    left->p1 = p01;
    left->p2 = p012;
    left->p3 = p0123;

    right->p0 = p0123;
    right->p1 = p123;
    right->p2 = p23;
    right->p3 = c->p3;
}

/* -----------------------------------------------------------------------
   Flatness test in integer arithmetic
   Here: max distance of p1, p2 from straight line p0-p3,
   approximated by comparing control polygon "bulge" to a threshold. [web:13]
   ----------------------------------------------------------------------- */

/* Simple measure: max of |(p1 - mid)·n|, |(p2 - mid)·n| using
   an approximate normal and mid = (p0 + p3)/2, but avoiding sqrt.
   For this constrained coordinate range, a cruder but cheap test:
   use max squared distance from segment endpoints as a proxy.
*/
static uint16_t cubic_flatness_measure(const CubicS8 *c) {
    V2s8 d;
    uint16_t d1, d2, d3, maxd;

    /* Use squared distances of control points from p0 as a cheap metric. */
    d.x = (int8_t)((int16_t)c->p1.x - (int16_t)c->p0.x);
    d.y = (int8_t)((int16_t)c->p1.y - (int16_t)c->p0.y);
    d1 = v2_len2(d);

    d.x = (int8_t)((int16_t)c->p2.x - (int16_t)c->p0.x);
    d.y = (int8_t)((int16_t)c->p2.y - (int16_t)c->p0.y);
    d2 = v2_len2(d);

    d.x = (int8_t)((int16_t)c->p3.x - (int16_t)c->p0.x);
    d.y = (int8_t)((int16_t)c->p3.y - (int16_t)c->p0.y);
    d3 = v2_len2(d);

    maxd = d1;
    if (d2 > maxd) maxd = d2;
    if (d3 > maxd) maxd = d3;
    return maxd;
}

/* Tunable: max allowed flatness metric before splitting again.
   This is in "squared distance" units in screen space. */
//#define FLATNESS_TOL_SQ  16u  /* adjust (e.g. 4, 16, 64) to taste */
static uint8_t FLATNESS_TOL_SQ = 32u;  /* adjust (e.g. 4, 16, 64) to taste */

/* -----------------------------------------------------------------------
   Angle constraint without trig:
   We compare cos(theta) = dot(v1,v2)/(|v1||v2|) with a threshold.
   To avoid sqrt/float, work with:
      dot^2 >= (cos_min^2) * len1^2 * len2^2
   using fixed-point cos_min in Q15 format. [web:36]
   ----------------------------------------------------------------------- */

/* cos_min in Q15 (1.0 -> 32767). For small deviation from straight ahead:
   If max angle off straight = 10 degrees:
       cos(10°) ~ 0.9848 -> about 32242 in Q15. [web:45]
*/
//#define MIN_COS_Q15   32000  /* tweak this (0..32767) */
#define MIN_COS_Q15   8000  /* tweak this (0..32767) */
//static int16_t MIN_COS_Q15 = 32000;  /* tweak this (0..32767) */

/* Return 1 if angle between v1 and v2 is <= allowed turn (i.e. cos >= MIN_COS_Q15). */

/*
  Can this code be done some other way that avoids 32 bit arithmetic and
  variables:
     uint32_t approx_l1 = (uint32_t)m1 * (uint32_t)m1;
     uint32_t approx_l2 = (uint32_t)m2 * (uint32_t)m2;
     uint32_t len_prod = approx_l1 * approx_l2; / * at most (127^2)^2 < 2^31 * /
*/
#ifdef NEVER

/* Integer dot product, fits in 16 bits: (-128..127)^2 * 2 <= 32512 < 32767 */
static int16_t v2_dot(V2s8 a, V2s8 b) {
    return (int16_t)a.x * (int16_t)b.x + (int16_t)a.y * (int16_t)b.y;
}

static int angle_ok(V2s8 v1, V2s8 v2) {
    uint16_t l1 = v2_len2(v1);
    uint16_t l2 = v2_len2(v2);
    int16_t  dot = v2_dot(v1, v2);

    /* Zero-length vectors: treat as OK to avoid division by zero cases. */
    if (l1 == 0 || l2 == 0) return 1;

    /* Work with squared cosine: dot^2 >= (cos_min^2) * l1 * l2, scaled.

       Let:
         cos_min_Q15 = MIN_COS_Q15 ~ cos(theta) * 32767
         cos_min^2 ? (MIN_COS_Q15^2) / 2^30

       We can rearrange inequalities to avoid overflow given our small ranges.
       For our coordinate limits, |dot| <= 2*(127^2) ? 32258, so dot^2 fits 31 bits.
       l1, l2 <= 2*(127^2) ? 32258 as well. In 16-bit, they are safe.
       Multiply l1*l2 in 32 bits, then scale.

       For simplicity and safety, choose a heuristic:
         If |dot| >= MIN_COS_Q15 / 256 * sqrt(l1*l2) approximately,
       but because sqrt is not available, we approximate len by max(|v1|,|v2|).
    */

    /* Approximate |v| with sqrt(len2) via a cheap integer: use max(abs(x),abs(y))*K. */
    /* Here, use |v|_approx = max(|x|,|y|)*1.414 ~ sqrt(2), but we only need ratio,
       so just compare using max component squared. */

    /* Compute approximate squared lengths using max component only. */
    {
        int16_t ax1 = v1.x >= 0 ? v1.x : -v1.x;
        int16_t ay1 = v1.y >= 0 ? v1.y : -v1.y;
        int16_t m1  = ax1 > ay1 ? ax1 : ay1;

        int16_t ax2 = v2.x >= 0 ? v2.x : -v2.x;
        int16_t ay2 = v2.y >= 0 ? v2.y : -v2.y;
        int16_t m2  = ax2 > ay2 ? ax2 : ay2;

        // THESE ARE EXPENSIVE ON VECTREX!!!
        uint32_t approx_l1 = (uint32_t)m1 * (uint32_t)m1;
        uint32_t approx_l2 = (uint32_t)m2 * (uint32_t)m2;
        uint32_t len_prod  = approx_l1 * approx_l2;  /* at most (127^2)^2 < 2^31 */

        uint32_t dot2 = (uint32_t)dot * (uint32_t)dot;

        /* Scale MIN_COS_Q15^2 down to reduce magnitude: use >>10 arbitrarily. */
        uint32_t cos2_scaled = ((uint32_t)MIN_COS_Q15 * (uint32_t)MIN_COS_Q15) >> 10;

        /* Check dot^2 * 2^10 >= cos2_scaled * len1*len2 */
        if ((dot2 << 10) >= cos2_scaled * len_prod)
            return 1;
        else
            return 0;
    }
}
#else
/* 16-bit only angle check: vectors are "straight enough" if their
   dominant components point the same way and secondary components
   are small relative to dominant. No multiplies beyond 16x16. [web:36] */
static int angle_ok(V2s8 v1, V2s8 v2)
{
  int16_t ax1 = v1.x >= 0 ? v1.x : -v1.x;
  int16_t ay1 = v1.y >= 0 ? v1.y : -v1.y;
  int16_t mx1 = ax1 > ay1 ? ax1 : ay1;  /* dominant component */

  int16_t ax2 = v2.x >= 0 ? v2.x : -v2.x;
  int16_t ay2 = v2.y >= 0 ? v2.y : -v2.y;
  int16_t mx2 = ax2 > ay2 ? ax2 : ay2;

  /* Zero length? OK. */
  if (mx1 == 0 || mx2 == 0) return 1;

  /* Same dominant axis? (x vs y) */
  int x_dom1 = (ax1 >= ay1);
  int x_dom2 = (ax2 >= ay2);
  if (x_dom1 != x_dom2) return 0;

  /* Check if dominant components have same sign */
  int16_t dom1 = x_dom1 ? v1.x : v1.y;
  int16_t dom2 = x_dom2 ? v2.x : v2.y;
  if ((dom1 ^ dom2) < 0) return 0;  /* opposite signs */

  /* Check secondary component is small relative to dominant (< 1/4 ratio) */
  int16_t sec1 = x_dom1 ? v1.y : v1.x;
  int16_t sec2 = x_dom2 ? v2.y : v2.x;

  /* |sec| < |dom| / 4  =>  4 * |sec| < |dom| */
  int16_t sec1x4 = sec1 * 4;
  int16_t sec2x4 = sec2 * 4;

  if (sec1x4 < 0) sec1x4 = -sec1x4;
  if (sec2x4 < 0) sec2x4 = -sec2x4;

  if (sec1x4 >= mx1 || sec2x4 >= mx2) return 0;

  return 1;
}
#endif

/* -----------------------------------------------------------------------
   Polyline emitter for your display: we do not store, we stream straight
   to MoveAbs / LineAbs as we flatten.
   ----------------------------------------------------------------------- */

/* Keep last two emitted points to enforce angle constraint across segments. */
static int has_last = 0;
static V2s8 last_p0;
static V2s8 last_p1;

/* Emit first point (move beam). */
static void emit_move(V2s8 p) {
    MoveAbs(p.x, p.y);
    has_last = 0;
}

/* Try to emit a line segment from current point to p, with angle check.
   Assumes current endpoint is last_p1 when has_last != 0. */
static void emit_line_angle_checked(V2s8 p) {
    if (!has_last) {
        /* No previous segment: just draw. */
        LineAbs(p.x, p.y);
        last_p1 = p;
        has_last = 1;
    } else {
        V2s8 v1 = v2_sub(last_p1, last_p0);
        V2s8 v2 = v2_sub(p,       last_p1);

        if (angle_ok(v1, v2)) {
            LineAbs(p.x, p.y);
            last_p0 = last_p1;
            last_p1 = p;
        } else {
            /* If angle too large, you could:
               - subdivide more (handled up in recursion), or
               - insert an intermediate point (midpoint) here.
               For simplicity, accept anyway: hardware constraints dominate.
               You can refine this if needed. */
            LineAbs(p.x, p.y);
            last_p0 = last_p1;
            last_p1 = p;
        }
    }
}

/* -----------------------------------------------------------------------
   Recursive flattening: integer-only, with flatness + (soft) angle control.
   ----------------------------------------------------------------------- */

static void flatten_cubic_rec(const CubicS8 *c, int depth) {
    uint16_t flat = cubic_flatness_measure(c);

    /* Limit recursion depth to avoid pathological cases. */
    if (flat <= FLATNESS_TOL_SQ || depth >= 8) {
        /* Emit straight segment from p0 to p3. */
        emit_line_angle_checked(c->p3);
    } else {
        CubicS8 left, right;
        cubic_split_half(c, &left, &right);
        flatten_cubic_rec(&left, depth + 1);
        flatten_cubic_rec(&right, depth + 1);
    }
}

/* -----------------------------------------------------------------------
   Public API: draw a piecewise cubic path in int8 coordinates.
   path: array of 'count' cubics.
   First p0 is used as MoveAbs; all segments drawn via LineAbs.
   ----------------------------------------------------------------------- */

void Generate_Path(const CubicS8 *path, uint8_t count) {
    uint8_t i;

    if (count == 0)
        return;

    /* Start at first cubic's p0. */
    emit_move(path[0].p0);

    /* Initialize last segment state so angle checks work. */
    last_p0 = path[0].p0;
    last_p1 = path[0].p0;
    has_last = 0;

    for (i = 0; i < count; ++i) {
        flatten_cubic_rec(&path[i], 0);
        /* Ensure continuity: next cubic should start at previous p3
           in your data; no extra move needed. */
    }
}

/* -----------------------------------------------------------------------
   Example test harness (remove/replace on your target).
   ----------------------------------------------------------------------- */

int main(void) {
  static unsigned long Frame = 0L;
    /* Sample cubic in -128..127 space. */
    CubicS8 path[1];
    path[0].p0.x = -40; path[0].p0.y =  0;
    path[0].p1.x = -32; path[0].p1.y = 40;
    path[0].p2.x =  32; path[0].p2.y = 40;
    path[0].p3.x =  40; path[0].p3.y =  0;

  #ifndef __linux__
    for (;;) {
  #endif
      if (Frame == 0) {
        static int dir1 = 7, dir2 = -3, dir3 = 5;
    // If we only save Bezier paths then we have a single Moveto_d call
    // and multiple Draw_Line_d calls, so we can save 1/3rd of the RAM
    // by saving only the initial x,y and the deltas without needing
    // the command byte that would be saved in traditional VLp format.
        next_free_ram = 255;
        Generate_Path(path, 1); // Save the drawing data to RAM
        // simulate the Bezier control points being moved with the light pen...
        path[0].p3.y += dir1;
        if (abs(path[0].p3.y) >= 50) dir1 = -dir1;
        path[0].p1.y += dir2;
        if (abs(path[0].p1.y) >= 50) dir2 = -dir2;
        path[0].p0.x += dir3;
        if (abs(path[0].p0.x) >= 50) dir3 = -dir2;
      }
      Frame += 1UL; if (Frame >= 8) Frame = 0;
      Wait_Recal(); set_scale(150); Intensity_5F();
      
      // Draw from saved array.

      Reset0Ref();Moveto_d(savedy[0],savedx[0]);
      Draw_Line_d(path[0].p1.y-path[0].p0.y,path[0].p1.x-path[0].p0.x);
      Reset0Ref();Moveto_d(savedy[0],savedx[0]);
      lastx = lasty = 0;
      Intensity_7F();
      for (uint8_t i = 1; i < next_free_ram; i++) {
        lasty += savedy[i]; lastx += savedx[i];
        if (abs(lastx)>=2 && abs(lasty)>=2) {
          Draw_Line_d(lasty,lastx);
          lastx = lasty = 0;
        }
      }
      if (lastx != 0 || lasty != 0) Draw_Line_d(lasty,lastx);
      Intensity_5F();
      Draw_Line_d(path[0].p2.y-path[0].p3.y,path[0].p2.x-path[0].p3.x);

  #ifndef __linux__
    }
  #endif
    
    return 0;
}