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

//#define TESTMODE 1

typedef int int8_t;
typedef unsigned int uint8_t;
typedef long int int16_t;

#define TRUE 1
#define FALSE 0

const int8_t racetrack[11] = { 4, 127,0, 0,127, -127,0, 0,-127 };
const int8_t parked_car[13] = { 5,  8,4, 0,32, -16,0, 0,-32, 8,-4, 0,0 };
int8_t moving_car[13];
#define MAXV 15  // Originally 7. Now 15.

static inline void rotate_vl(uint8_t angle, const int8_t *original, int8_t *rotated) {
  *(volatile unsigned int *)0xC836 = angle;
  *(volatile int *)0xC823 = original[0];
  rotated[0] = original[0];
  Rot_VL((int8_t *)original+1, rotated+1);
}

static inline void set_scale(uint8_t scale) {/*VIA_t1_cnt_lo*/ *(volatile unsigned char *)0xD004 = scale;}

static void position_and_scale(int8_t y, int8_t x, uint8_t scale) {
  Reset0Ref();
  set_scale(0x7f);  // scale for moving
  Moveto_d(y, x);
  set_scale(scale); // scale for drawing
}

void cDraw_VLc(int8_t *vList) {
  register int count = *(vList++);// count in list
  do {
	VIA_port_a = *(vList++);	// first y coordinate to dac
	VIA_port_b = 0;				// mux enable, dac to -> integrator y (and x)
	VIA_port_b++;				// mux disable, dac only to x
	VIA_port_a = *(vList++);	// dac -> x
	VIA_shift_reg =  (uint8_t)0xff; // full "unblank" output
	VIA_t1_cnt_hi = 0;			// start timer
	while ((VIA_int_flags & 0x40) == 0); // wait till timer finishes
	VIA_shift_reg = 0;			// output full blank
  } while (--count >=0);			// loop thru all vectors
}

typedef enum gamestate {
  PRE_RACE,
  WAITING_FOR_CHEQUERED_FLAG,
  RACING,
  FINISHED,
  TIMED_OUT,
  DISQUALIFIED,
  CRASHED
} gamestate;

static gamestate state = PRE_RACE;

uint8_t laps_run, laps_left;

static inline void car(int8_t y, int8_t x, uint8_t angle) {
  rotate_vl(angle>>2, parked_car, moving_car);
  position_and_scale(y, x, 127);
  cDraw_VLc((int8_t *)moving_car);
}

static inline void track(void) {
  position_and_scale(-128, -128, 255);
  cDraw_VLc((int8_t *)racetrack);
  if (state == WAITING_FOR_CHEQUERED_FLAG || ((state == RACING) && (laps_left == 1)) || (state == FINISHED)) {
    Reset0Ref();
    Moveto_d(-24,0);
    Draw_Line_d(-40,0); // This would be better as a 50% dashed line
  }
  position_and_scale(-48, -48, 96);
  cDraw_VLc((int8_t *)racetrack);
}

static inline int8_t inside(int16_t y, int16_t x) {
  #define yedge 48L
  #define xedge 48L
  if (y < 0L) y = -y; if (x < 0L) x = -x;
  return (y <= yedge && x <= xedge);
}

static inline int8_t outside(int16_t y, int16_t x) {
  return ((y <= -126L) || (y >= 126L) || (x <= -126L) || (x >= 126L));
}

static int8_t BadY, BadX; // coordinates of where the crash happened.
                          // can be used to draw the car bursting into flames :-)

int crashed(int16_t Y, int16_t X) {
  int8_t x,y;
  int8_t each, *ptr = moving_car+1;
  position_and_scale((int8_t)(Y>>8L),(int8_t)(X>>8L), 127);
  BadY=(int8_t)(Y>>8L); BadX=(int8_t)(X>>8L);
  position_and_scale(BadY,BadX,127);
  if (outside(Y>>8L,X>>8L)) return 2;   // front point
  if (inside(Y>>8L,X>>8L)) return 1;
  x = y = 0L;
  for (each = 0; each < 4; each++) {
    y += *ptr++; x += *ptr++;
    Reset0Ref(); Moveto_d((int8_t)((Y>>8L)+((int16_t)y)), (int8_t)((X>>8L)+((int16_t)x)));
    if (outside((Y>>8L)+((int16_t)y),(X>>8L)+((int16_t)x))) {
      BadY += y; BadX += x; return 2;   // test x and y ranges
    }
    if (inside((Y>>8L)+((int16_t)y),(X>>8L)+((int16_t)x))) {
      BadY += y; BadX += x; return 1;
    }
  }
  return 0;
}

// don't need 16 bits for this game. This is overkill.
// Actually could use BIOS code...  The Vectrex rotation
// handles 64 angles per circle - my own code has assumed
// 256 angles per circle.
const int16_t qsine[65] = {
     0,     402,    803,   1205,   1605,   2005,   2404,   2801,
  3196,    3589,   3980,   4369,   4756,   5139,   5519,   5896,
  6269,    6639,   7005,   7366,   7723,   8075,   8423,   8765,
  9102,    9434,   9759,  10079,  10393,  10701,  11002,  11297,
  11585,  11866,  12139,  12406,  12665,  12916,  13159,  13395,
  13622,  13842,  14053,  14255,  14449,  14634,  14810,  14978,
  15136,  15286,  15426,  15557,  15678,  15790,  15892,  15985,
  16069,  16142,  16206,  16260,  16305,  16339,  16364,  16379,
  16384,
};

static inline int16_t hsine(uint8_t x) {
  return x>=64 ? qsine[128-x] : qsine[x];
}

static inline int16_t sine(uint8_t x) {
  return x&0x80 ? -hsine(x&0x7f) : hsine(x);
}

static inline int16_t SIN(uint8_t x) {
  return (sine(x&255U));
}

static inline int16_t COS(uint8_t x) {
  return (sine((x+64U)&255U));
}

void N1(int tick) {
  position_and_scale(0,0,(uint8_t)tick<<1);
  Moveto_d(-12,-8); // to 8,12
  Moveto_d(0,8); // to 8,0
  Draw_Line_d(24,0); // to 8,24
  Moveto_d(-24,16); // to 24,0
}

void N2(int tick) {
  position_and_scale(0,0,(uint8_t)tick<<1);
  Moveto_d(-12,-8); // to 8,12
  Moveto_d(24,0); // to 0,24
  Draw_Line_d(0,16); // to 16,24
  Draw_Line_d(-12,0); // to 16,12
  Draw_Line_d(0,-16); // to 0,12
  Draw_Line_d(-12,0); // to 0,0
  Draw_Line_d(0,16); // to 16,0
  Moveto_d(0,8); // to 24,0
}

void N3(int tick) {
  position_and_scale(0,0,(uint8_t)tick<<1);
  Moveto_d(-12,-8); // to 8,12
  Draw_Line_d(0,16); // to 16,0
  Moveto_d(12,-8); // to 8,12
  Draw_Line_d(0,8); // to 16,12
  Moveto_d(-12,0); // to 16,0
  Draw_Line_d(24,0); // to 16,24
  Draw_Line_d(0,-16); // to 0,24
  Moveto_d(-24,24); // to 24,0
}

void speedometer(uint8_t v) {
  position_and_scale(-48+4,0, 255);
  switch (v) {
  // Eyeballed! I really should use sin and cos to draw the speedo more accurately!
  // at some point I decided to have 15 speeds instead of 7. 
  case 0:   Draw_Line_d(32,-24+4); break;
  case 1:   Draw_Line_d(33,-18); break;
  case 2:   Draw_Line_d(35,-16);   break;
  case 3:   Draw_Line_d(36,-13);   break;
  case 4:   Draw_Line_d(37,-10);   break;
  case 5:   Draw_Line_d(38,-7);   break;
  case 6:   Draw_Line_d(39,-4);    break;
  case 7:   Draw_Line_d(39,-2);    break;
  case 8:   Draw_Line_d(39, 2);    break;
  case 9:   Draw_Line_d(39, 4);    break;
  case 10:   Draw_Line_d(38, 7);   break;
  case 11:   Draw_Line_d(37, 10);   break;
  case 12:   Draw_Line_d(36, 13);   break;
  case 13:   Draw_Line_d(35, 16);   break;
  case 14:   Draw_Line_d(33,-18);  break;
  case 15:   Draw_Line_d(32,24-4);  break;
  }
}

char *pnum(char *p, int16_t num, int8_t decimal) { // does NOT add terminating \x80 or \x00
#define add_digit(x) *p++ = (x)
  char digit, zeroes;

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

  if (num >= 0) num = -num; else add_digit('-');

  digit = '0';
  zeroes = 1; // Lets us use CLR which is faster
  // max 11 add/subtracts...
  if (num <= -20000) { num += 20000; digit += 2; zeroes = 0; }
  if (num <= -10000) { num += 10000; digit += 1; zeroes = 0; }
  if (!zeroes) add_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) add_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) add_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 (decimal) {
    add_digit('.');
    zeroes = 0;
  }
  if (!zeroes) add_digit(digit);
  add_digit((char)(-num)+'0');
  return p;
}

void heat_counter(uint8_t lap) {
  char HEAT[13] = {'H', 'E', 'A', 'T', ' ' };
  char *endp = HEAT+5;
  endp = pnum(endp, lap, FALSE); *endp++ = 0x80; *endp = '\0';
  Reset0Ref(); Print_Str_d(110, -110, HEAT);
}

void timeleft(int16_t centiseconds) {
  char TIME[14] = {'T', 'I', 'M', 'E', ' ' };
  char *endp = TIME+5;
  endp = pnum(endp, centiseconds, TRUE); *endp++ = 0x80; *endp = '\0';
  Reset0Ref(); Print_Str_d(110, -8, TIME);
}

void lapsleft(int16_t remaining) {
  char LAPS[14] = {'L', 'A', 'P', 'S', ' ' };
  char *endp = LAPS+5;
  endp = pnum(endp, remaining, FALSE); *endp++ = 0x80; *endp = '\0';
  Reset0Ref(); Print_Str_d(110, -124, LAPS);
}

void instructions(uint8_t laps, uint8_t secs) {
  char instructions[] = {
    (char)-8,(char)80, // WxH
    (char)-110,(char)-126, // y,x
    'F','I','N','I','S','H',' ','?','?',' ',
    'L','A','P','S',' ','I','N',' ',' ',' ',' ','S',(char)0x80,0
  };
  char *endp;
  Reset0Ref();
  // secs MUST be between 10 and 99!
  endp = pnum((char *)instructions+11, (int16_t)secs, FALSE);
  endp = pnum((char *)instructions+22, (int16_t)laps, FALSE);
  *endp++ = 'S'; *endp++ = 0x80; *endp = '\0';
  Print_Str_hwyx((char *)instructions);
}

typedef enum progression {
  STARTINGLINE,
  FIRST,
  SECOND,
  THIRD,
  FOURTH
} PROGRESSION;

PROGRESSION movement = STARTINGLINE;

void lap_counter(int8_t y, int8_t x) {
  static int8_t q;

  if (x<=0 && y < 0) {
    q = FOURTH;
  } else if (x > 0 && y < 0) {
    q = FIRST;
  } else if (x > 0 && y >= 0) {
    q = SECOND;
  } else if (x <= 0 && y >= 0) {
    q = THIRD;
  }
  switch (movement) {
  case STARTINGLINE:
    // q's on FOURTH!(?)
    if (q == THIRD) {state = DISQUALIFIED;};
    if (q == FOURTH)      movement = STARTINGLINE;
    else if (q == FIRST)  movement = FIRST;
    //else                  movement = q;
    break;
  case FIRST:
    if (q == FOURTH) {state = DISQUALIFIED;};
    if (q == SECOND)      movement = SECOND;
    //else                  movement = q;
    break;
  case SECOND:
    if (q == FIRST) {state = DISQUALIFIED;};
    if (q == THIRD)       movement = THIRD;
    //else                  movement = q;
    break;
  case THIRD:
    if (q == SECOND) {state = DISQUALIFIED;};
    if (q == FOURTH)      movement = FOURTH;
    //else                  movement = q;
    break;
  case FOURTH:
    if (q == THIRD) {state = DISQUALIFIED;};
    if (q == FIRST) {
       laps_left -= 1;
       laps_run += 1;
       if (laps_left == 0) {
         state = FINISHED;
       }
                           movement = FIRST;
    } //else                 movement = q;
    break;
  }
  //static char debug[4] = {'?', ' ', 0x80, 0};
  //static char debug2[4] = {'?', ' ', 0x80, 0};
  //(void)pnum(debug, q, FALSE);         Reset0Ref(); Print_Str_d(-32, -48, debug);
  //(void)pnum(debug2, movement, FALSE); Reset0Ref(); Print_Str_d(-32, 40, debug2);
}

int main(void) {
  // This is *almost a 1-button game (turn left).  But not quite.
  // There is a boatload of CPU power still free for all sorts of additions!
  static int countdown = 3;
  // We'll tune these once the features are added.
  uint8_t heat = 1;
  uint8_t subsec_timer = 0;
  int16_t secs_taken = 0L;
  int16_t ticks_left = 127;
  int16_t timer = 30000; // hundredths of a second left. 30 second max per heat.
  state = WAITING_FOR_CHEQUERED_FLAG;
  int16_t Y = (int16_t)-88<<(int16_t)8, X = 0;
  int8_t y, x;
  // Angle and Velocity
  uint8_t A = 128, V = 0; // A = 128 is horizontal, pointing right

  // A race is a fixed number of heats.  In each heat you must run X laps within a certain time limit.
  // Lap and heat counting not yet fully implemented but getting there!

  // There *is* a speed at around V=8 or 9 where you can *just about* circle on automatic, but it
  // will eventually hit a wall.  The number of laps needed must be in excess of the number that
  // you can get away with on automatic!

  for (;;) {
    Wait_Recal();
    check_buttons();
    Intensity_7F();
    track();
    y = (int8_t)(Y>>(int16_t)8); x = (int8_t)(X>>(int16_t)8);
    car (y, x, A);
    switch (state) {
    case PRE_RACE:
      // output message about #laps and #seconds here *only*.
    case WAITING_FOR_CHEQUERED_FLAG:
      // if the driver presses the accelerator while waiting for the flag to start,
      // it is a false start and he is penalised by losing this race.
#ifdef TESTMODE
      laps_left = 2;
      instructions(laps_left*30 /* seconds*/, laps_left); // 5 second laps are probably optimal!
      timer = (laps_left-1)*3000;
#else
      laps_left = 10;
      instructions(laps_left*6 /* seconds*/, laps_left); // 5 second laps are probably optimal!
      timer = (laps_left-1)*600;
#endif
      heat_counter(heat);
      Reset0Ref(); set_scale(32);
      ticks_left -= 4;
      if (ticks_left <= 0) {
        countdown -= 1;
        ticks_left = 127;
      }
      Y = (int16_t)-88<<(int16_t)8; X = 0;
      y = (int8_t)(Y>>(int16_t)8);  x = (int8_t)(X>>(int16_t)8);
      A = 128;
      switch (countdown) {
      case 3: N3((int8_t)ticks_left);
        continue;
      case 2: N2((int8_t)ticks_left);
        continue;
      case 1: N1((int8_t)ticks_left);
        continue;
      case 0:
        state = RACING;
        countdown = 3;
        heat += 1;
        movement = STARTINGLINE;
        laps_run = 0;
        secs_taken = 0L;
        continue;
      }
    case RACING:
      lap_counter(y,x);
      speedometer(V);
      timeleft(timer);
      lapsleft(laps_left);
      timer -= 2L; // 50 FPS is 100 centiseconds.
      subsec_timer += 1;
      if (subsec_timer == 50) {
        subsec_timer = 0;
        secs_taken += 1;
      }
      ticks_left -= 4;
      if (ticks_left > 0) {
        Reset0Ref();
        Print_Str_d(0, -48, " START!\x80");
      } else ticks_left = 0;
      if (timer <= 0L) {
        state = TIMED_OUT;
        break;
      }
      switch (crashed(Y, X)) {
        case 2:
          y = (int8_t)(Y>>(int16_t)8); x = (int8_t)(X>>(int16_t)8);
        case 1:
          V=0;
          state = CRASHED;
        case 0:;
      }
      if (state == CRASHED) break;
      if (button_1_1_held()) A+=1; // turn left
      if (button_1_2_held()) A-=1; // turn right
      if (button_1_3_pressed() && (--V > 127)) V = 0; // brake (reverse is not allowed)
      if (button_1_4_pressed() && (++V > MAXV)) V = MAXV;   // accelerate
      // perhaps to make it a harder game, automatically accelerate as play progresses?
      // Speeds restricted to 0 through 7 (or 15) for now
      X -= (int16_t)V*(int16_t)(int8_t)(COS(A)>>(int16_t)8); Y -= (int16_t)V*(int16_t)(int8_t)(SIN(A)>>(int16_t)8);
      // test position on next frame once updated position is drawn.
      continue;
    case FINISHED:
      Reset0Ref();
      Print_Str_d(33, -48, "  YOU\x80");
      Reset0Ref();
      Print_Str_d(16, -48, "  WIN!\x80");
      {
      static char tmp[16], *ptr;
      Reset0Ref();
      ptr = pnum(tmp, laps_run, FALSE);
      *ptr++ = ' ';  *ptr++ = 'L';  *ptr++ = 'A';  *ptr++ = 'P';  *ptr++ = 'S';  *ptr++ = 0x80; *ptr = 0;
      Print_Str_d(-4, -48,  tmp);
      Reset0Ref();
      ptr = pnum(tmp, secs_taken, FALSE);
      *ptr++ = ' ';  *ptr++ = 'S';  *ptr++ = 'E';  *ptr++ = 'C';  *ptr++ = 'S';  *ptr++ = 0x80; *ptr = 0;
      Print_Str_d(-20, -48,  tmp);
      }
      ticks_left = 127;
      if (button_1_3_pressed()) state = WAITING_FOR_CHEQUERED_FLAG;
      continue;
    case DISQUALIFIED:
      Reset0Ref();
      Print_Str_d(16, -42, "DONT GO\x80");
      Reset0Ref();
      Print_Str_d(-4, -49, "BACKWARD\x80");
      ticks_left = 127;
      if (button_1_3_pressed()) state = WAITING_FOR_CHEQUERED_FLAG;
      continue;
    case TIMED_OUT:
      Reset0Ref();
      Print_Str_d(12, -50, "   TOO\x80");
      Reset0Ref();
      Print_Str_d(-4, -48, "  SLOW!\x80");
      ticks_left = 127;
      if (button_1_3_pressed()) state = WAITING_FOR_CHEQUERED_FLAG;
      continue;
    case CRASHED:
      Reset0Ref();
      Print_Str_d(0, -48, "CRASHED!\x80");
      Reset0Ref();
      // I had a small correction in here for wraparound on the crash location, which
      // I removed thinking it was overkill (especially when I narrowed the outer wall a little),
      // but I still see crash marks on the wrong wall, so I need to remember what that correction
      // was and put it back in. Or remove the crash mark (which was only originally a debugging
      // aid, but then I thought it would be nice to draw an engine fire at that location! - which
      // I haven't done yet... )
#ifdef DRAW_ENGINE_FIRE
      position_and_scale(BadY,BadX, 127);
      // draw a fire here! maybe sparks like the battlezone volcano
      Moveto_d(-4,-4);Draw_Line_d(8,8); Moveto_d(-4,-4); // (this is just an X)
      Moveto_d( 4,-4);Draw_Line_d(-8,8); Moveto_d(4,-4);
#endif
      ticks_left = 127;
      if (button_1_3_pressed()) state = WAITING_FOR_CHEQUERED_FLAG;
    }
  }
  return 0;
}