#include <vectrex.h>

#include "controller.h"


#define Intensity(x) Intensity_a(x)


static inline void set_scale(unsigned int s) {
  VIA_t1_cnt_lo = s;
}

static void sprintl(char *dest, long num) { // left-aligned

#define show_digit(x) *dest++ = (char)((x)+'0')

  int digit, zeroes;

  // This replaces code that used divide by 10 and modulo 10.  Much faster.


  // handles full 16 bit range of -32768:32767  -  Uses negative numbers to avoid the issue of negating -32768


  if (num >= 0) num = -num; else *dest++ = '-';
  digit = 0;
  zeroes = 1; // CLRing is shorter

  // max 11 add/subtracts...

  if (num <= -20000) { num += 20000; digit += 2; zeroes = 0; }
  if (num <= -10000) { num += 10000; digit += 1; zeroes = 0; }
  if (!zeroes) show_digit(digit);
  digit = 0;
  if (num <= -8000) { num += 8000; digit += 8; zeroes = 0; } else if (num <= -4000) { num += 4000; digit += 4; zeroes = 0; }
  if (num <= -2000) { num += 2000; digit += 2; zeroes = 0; }
  if (num <= -1000) { num += 1000; digit += 1; zeroes = 0; }
  if (!zeroes) show_digit(digit);
  digit = 0;
  if (num <= -800) { num += 800; digit += 8; zeroes = 0; } else if (num <= -400) { num += 400; digit += 4; zeroes = 0; }
  if (num <= -200) { num += 200; digit += 2; zeroes = 0; }
  if (num <= -100) { num += 100; digit += 1; zeroes = 0; }
  if (!zeroes) show_digit(digit);
  digit = 0;
  if (num <= -80) { num += 80; digit += 8; zeroes = 0; } else if (num <= -40) { num += 40; digit += 4; zeroes = 0; }
  if (num <= -20) { num += 20; digit += 2; zeroes = 0; }
  if (num <= -10) { num += 10; digit += 1; zeroes = 0; }
  if (!zeroes) show_digit(digit);
  show_digit((int)-num);
  //*dest = 0x80;

}

static long V1X=0L, V1Y=0L, V2X=0L, V2Y=0L;
static unsigned long timer = 0UL;
#define TIMEOUT 0xAF0UL


int main(void) {
  unsigned int bright, dim;
  static char coords[11];
  // * (volatile char *)0xC81A = 0;// Analog joystick max resolution

  Vec_Joy_Resltn = 0;

  enable_controller_1_x();
  enable_controller_1_y();

  enable_controller_2_x();
  enable_controller_2_y();

  for (;;) {
    // There's an excess of intensities, reset0refs, moveto 0,0's and set_scales

    // in this code, as I was trying to work out if some call was out of order and

    // causing the extreme 'clicking' under Vide.  Works OK on real hardware.


    if (timer == TIMEOUT) {
      // Avoid burn-in if left running for a long time...

      if (bright) bright -= 1U;
      if (dim) dim -= 1U; 
    } else {
      timer += 1UL;
      bright = 0x7FU;
      dim = 0x3FU; 
    }

    Wait_Recal();

    Vec_Joy_Resltn = 0;
    Joy_Analog ();
    Read_Btns ();

    coords[0] = coords[1] = coords[2] = coords[3] = ' ';
    sprintl(&coords[0], (long)Vec_Joy_1_X);
    coords[4] = ',';
    coords[5] = coords[6] = coords[7] = coords[8] = ' ';
    sprintl(&coords[5], (long)Vec_Joy_1_Y);
    coords[9] = 0x80; coords[10] = '\0';

    Reset0Ref (); Intensity(bright); set_scale(127); Moveto_d(0,0);
    Print_Str_d(-10, -52, coords);

    coords[0] = coords[1] = coords[2] = coords[3] = ' ';
    sprintl(&coords[0], (long)Vec_Joy_2_X);
    coords[4] = ',';
    coords[5] = coords[6] = coords[7] = coords[8] = ' ';
    sprintl(&coords[5], (long)Vec_Joy_2_Y);
    coords[9] = 0x80; coords[10] = '\0';
    
    Reset0Ref (); Intensity(bright); set_scale(127); Moveto_d(0,0);
    Print_Str_d(10, -52, coords);

    if ((buttons_held()&255U)
      // allow a smal amount of wobble when controls not being touched

      || Vec_Joy_1_X < V1X-1 
      || Vec_Joy_1_Y < V1Y-1
      || Vec_Joy_2_X < V2X-1
      || Vec_Joy_2_Y < V2Y-1
      || Vec_Joy_1_X > V1X+1
      || Vec_Joy_1_Y > V1Y+1
      || Vec_Joy_2_X > V2X+1
      || Vec_Joy_2_Y > V2Y+1) {
      timer = 0L; V1X = Vec_Joy_1_X; V1Y = Vec_Joy_1_Y; V2X = Vec_Joy_2_X; V2Y = Vec_Joy_2_Y;
    }

    // X

    Reset0Ref(); set_scale(0x7F); Moveto_d(Vec_Joy_1_Y, Vec_Joy_1_X); set_scale(0x8); Intensity(bright); Draw_Line_d(127,127);
    Reset0Ref(); set_scale(0x7F); Moveto_d(Vec_Joy_1_Y, Vec_Joy_1_X); set_scale(0x8); Intensity(bright); Draw_Line_d(-127,127);
    Reset0Ref(); set_scale(0x7F); Moveto_d(Vec_Joy_1_Y, Vec_Joy_1_X); set_scale(0x8); Intensity(bright); Draw_Line_d(-127,-127);
    Reset0Ref(); set_scale(0x7F); Moveto_d(Vec_Joy_1_Y, Vec_Joy_1_X); set_scale(0x8); Intensity(bright); Draw_Line_d(127,-127);
    
    // square with a dot in the middle

    Reset0Ref(); set_scale(0x7F); Moveto_d(Vec_Joy_2_Y, Vec_Joy_2_X);
    set_scale(0x8); Moveto_d(100,100); set_scale(0x10); Intensity(bright); Draw_Line_d(0,-127);
    Reset0Ref(); set_scale(0x7F); Moveto_d(Vec_Joy_2_Y, Vec_Joy_2_X);
    set_scale(0x8); Moveto_d(100,100); set_scale(0x10); Intensity(bright); Draw_Line_d(-127,0);
    Reset0Ref(); set_scale(0x7F); Moveto_d(Vec_Joy_2_Y, Vec_Joy_2_X);
    set_scale(0x8); Moveto_d(-100,-100); set_scale(0x10); Intensity(bright); Draw_Line_d(127,0);
    Reset0Ref(); set_scale(0x7F); Moveto_d(Vec_Joy_2_Y, Vec_Joy_2_X);
    set_scale(0x8); Moveto_d(-100,-100); set_scale(0x10); Intensity(bright); Draw_Line_d(0,127);
    Reset0Ref(); set_scale(0x7F); Moveto_d(Vec_Joy_2_Y, Vec_Joy_2_X);
    Intensity(dim); Draw_Line_d(0,0);

    Intensity(dim);
    Reset0Ref(); Moveto_d(0,0); set_scale(0x7F); Draw_Line_d(127,127);
    Reset0Ref(); Moveto_d(0,0); set_scale(0x7F); Draw_Line_d(-128,127);
    Reset0Ref(); Moveto_d(0,0); set_scale(0x7F); Draw_Line_d(-128,-128);
    Reset0Ref(); Moveto_d(0,0); set_scale(0x7F); Draw_Line_d(127,-128);

    Intensity(bright);

    if (buttons_held() & 1) {
      Reset0Ref(); set_scale(0x7F); Moveto_d(-32,32); set_scale(0x40); Draw_Line_d(127,0);
      Reset0Ref(); set_scale(8); Print_Str_d(6+9, 30, "C1\x80");
      Reset0Ref(); set_scale(8); Print_Str_d(6-9, 30, "B1\x80");
    }

    if (buttons_held() & 2) {
      Reset0Ref(); set_scale(0x7F); Moveto_d(32,32); set_scale(0x40); Draw_Line_d(0,-127);
      Reset0Ref(); set_scale(0x7f); Print_Str_d(48, -32, "C1 B2\x80");
    }

    if (buttons_held() & 4) {
      Reset0Ref(); set_scale(0x7F); Moveto_d(32,-32); set_scale(0x40); Draw_Line_d(-127,0);
      Reset0Ref(); set_scale(0x7f); Print_Str_d(6+9, -94+35, "C1\x80");
      Reset0Ref(); set_scale(0x7f); Print_Str_d(6-9, -94+35, "B3\x80");
    }

    if (buttons_held() & 8) {
      Reset0Ref(); set_scale(0x7F); Moveto_d(-32,-32); set_scale(0x40); Draw_Line_d(0,127);
      Reset0Ref(); set_scale(0x7f); Print_Str_d(-36, -32, "C1 B4\x80");
    }

    if (buttons_held() & (16)) {
      Reset0Ref(); set_scale(0x7F); Moveto_d(-64,64); set_scale(0x7f); Draw_Line_d(127,0);
      Reset0Ref(); set_scale(8); Print_Str_d(6+9, 62, "C2\x80");
      Reset0Ref(); set_scale(8); Print_Str_d(6-9, 62, "B1\x80");
    }

    if (buttons_held() & (32)) {
      Reset0Ref(); set_scale(0x7F); Moveto_d(64,64); set_scale(0x7f); Draw_Line_d(0,-127);
      Reset0Ref(); set_scale(0x7f); Print_Str_d(80, -32, "C2 B2\x80");
    }

    if (buttons_held() & (64)) {
      Reset0Ref(); set_scale(0x7F); Moveto_d(64,-64); set_scale(0x7f); Draw_Line_d(-127,0);
      Reset0Ref(); set_scale(0x7f); Print_Str_d(6+9, -92, "C2\x80");
      Reset0Ref(); set_scale(0x7f); Print_Str_d(6-9, -92, "B3\x80");
    }

    if (buttons_held() & (128)) {
      Reset0Ref(); set_scale(0x7F); Moveto_d(-64,-64); set_scale(0x7f); Draw_Line_d(0,127);
      Reset0Ref(); set_scale(0x7f); Print_Str_d(-68, -32, "C2 B4\x80");
    }

  }
  return 0;
}