#include <vectrex.h>
#include "controller.h"

/*
   Determine the direction that the vectrex joystick 'bat' is pointing, to use it as
a spinner-like pointing device, returning a sector in the range 0:15.

Note that the segments we want to determine straddle the compass
points, eg segment 0 is from 1/32 left of north to 1/32 right of north.
So we're interested in 16 segments, but they're all on boundaries
that are rotated by 1/32 of a circle (i.e. 11.25 degrees)

(It would have been much easier with unrotated octants! Just test x>y as tan 45 = 1 ...)


                                           349.75d         11.25d, tan=0.2034523
                                              \             /
                                               \   Sector  /      
                                                \    0    /  22.5d tan = ?2 - 1
                                             15      |      1   33.75
                                                     |         /   45d, tan = 1
                                        14           |            2 _56.25
                                                     |             /  67.5d, tan = 1 + ?2
                                     13              |               3
                                                     |                __ 78.75
                                                     |                
                                    12---------------+----------------4 90d tan = infty
                                                     |                __ 101.25
                                                     |                
                                     11              |               5
                                                     |               
                                        10           |            6
                                                     |          
                                             9       |      7
                                                     8



*/

// use signs to map sectors:
static const int map[4][5] = {  /* +n means n >= 0, -n means n < 0 */
  /* 0: +x +y */ {0, 1, 2, 3, 4},
  /* 1: +x -y */ {8, 7, 6, 5, 4},
  /* 2: -x +y */ {0, 15, 14, 13, 12},
  /* 3: -x -y */ {8, 9, 10, 11, 12}
};

int sector(int x, int y) { // x,y signed in range -128:127, result 0:15 from north, clockwise.
  long tangent; // 16 bits
  int quadrant = 0;
  if (x > 0) x = -x; else quadrant |= 2; // make both negative avoids issue with negating -128 
  if (y > 0) y = -y; else quadrant |= 1;
  if (y != 0) {
    // The primary cost of this algorithm is five 16-bit multiplies.
    tangent = (long)x*32L;   // worst case y = 1, tangent = 255*32 so fits in 2 bytes.
    /*
       determine base sector using abs(x)/abs(y).
       in segment:
           0 if         0 <= x/y < tan 11.25   -- centered around 0     N
           1 if tan 11.25 <= x/y < tan 33.75   --                 22.5  NxNE
           2 if tan 33.75 <= x/y < tan 56.25   --                 45    NE
           3 if tan 56.25 <= x/y < tan 78.75   --                 67.5  ExNE
           4 if tan 78.75 <= x/y < tan 90      --                 90    E
    */
    if (tangent > y*6L  ) return map[quadrant][0]; // tan(11.25)*32
    if (tangent > y*21L ) return map[quadrant][1]; // tan(33.75)*32
    if (tangent > y*47L ) return map[quadrant][2]; // tan(56.25)*32
    if (tangent > y*160L) return map[quadrant][3]; // tan(78.75)*32
    // last case is the potentially infinite tan(90) but we never check that.
  }
  return map[quadrant][4];
}

const int circle_x[16] = {
  0,40,80,100,
  120,100,80,40,
  0,-40,-80,-100,
  -120,-100,-80,-40
};

const int circle_y[16] = {
   120,100,80,40,
   0, -40, -80, -100,
   -120, -100, -80, -40,
   0, 40, 80, 100
};

int main(void) {
int x,y,compass;
  enable_controller_1_x();
  enable_controller_1_y();
  for (;;) {
    Joy_Analog();
    Wait_Recal();
    Intensity_a(0x7f);
    Print_Str_d(120, -90, " NEAREST COMPASS \x80");
    Print_Str_d(100, -90, "POINT TO JOYSTICK \x80");
    // Only 8 will be selectable with digital joysticks,
    // All 16 reachable with analog values, which will
    // help tell some emulators from the real machine...
    // (an unintended side-effect of this code)
    x = joystick_1_x(); y = joystick_1_y();
    if (x > 0 ) {
      Print_Str_d(-120, -120, "X > 0, \x80");
    } else if (x < 0) {
      Print_Str_d(-120, -120, "X < 0, \x80");
    } else {
      Print_Str_d(-120, -120, "X = 0, \x80");
    }
    if (y > 0 ) {
      Print_Str_d(-120, -40, "Y > 0 \x80");
    } else if (y < 0) {
      Print_Str_d(-120, -40, "Y < 0 \x80");
    } else {
      Print_Str_d(-120, -40, "Y = 0 \x80");
    }
    compass = sector(x,y);
    switch(compass) {
    case 0: Print_Str_d(y, x, "* 0 N \x80"); break;
    case 1: Print_Str_d(y, x, "* 1 N*NE\x80"); break;
    case 2: Print_Str_d(y, x, "* 2 NE\x80"); break;
    case 3: Print_Str_d(y, x, "* 3 E*NE\x80"); break;
    case 4: Print_Str_d(y, x, "* 4 E \x80"); break;
    case 5: Print_Str_d(y, x, "* 5 E*SE\x80"); break;
    case 6: Print_Str_d(y, x, "* 6 SE\x80"); break;
    case 7: Print_Str_d(y, x, "* 7 S*SE\x80"); break;
    case 8: Print_Str_d(y, x, "* 8 S \x80"); break;
    case 9: Print_Str_d(y, x, "* 9 S*SW\x80"); break;
    case 10: Print_Str_d(y, x, "* 10 SW\x80"); break;
    case 11: Print_Str_d(y, x, "* 11 W*SW\x80"); break;
    case 12: Print_Str_d(y, x, "* 12 W \x80"); break;
    case 13: Print_Str_d(y, x, "* 13 W*NW\x80"); break;
    case 14: Print_Str_d(y, x, "* 14 NW\x80"); break;
    case 15: Print_Str_d(y, x, "* 15 N*NW\x80"); break;
    }
   
    Reset0Ref();
    Intensity_a(0x7f);
    Moveto_d(0, 0);
    Draw_Line_d(y, x);
    Reset0Ref();
    Intensity_a(0x3f);
    Moveto_d(0, 0);
    Draw_Line_d(circle_y[compass], circle_x[compass]);
    // for this demo, no special handling of the dead zone around 0,0
    // while testing robustness of x/y check.
  }

  return 0;
}