File content as of revision 7:25acb844f424:

#include "mbed.h"
#define V_RES 220
#define H_RES 100
#include <math.h>
#define X_MIN 24
#define X_MAX (76+18)
#define Y_MIN 7
#define Y_MAX (V_RES/4 - 15)

#define SX_MIN 30
#define SX_MAX 95
#define SY_MIN 10
#define SY_MAX 48

#define Y_0 ( (Y_MIN + Y_MAX)/2 )
#define X_0 ( (X_MIN + X_MAX)/2 )
#define SIDE 8
#define HSIDE 8
#define SIN_V (0.03)
#define COS_V (0.99955f)
#define BX_0 30
#define BY_0 (V_RES/4 - 15)
#define BX 42
#define BY 15
#define BY2 9
#define DEMO_LENGTH 600


DigitalOut sout(D8); //sync
DigitalOut vout(D7); //video
Ticker t;
uint8_t draw_line_inv = 0;
//9x42
uint8_t miters[] = {
1,1,1,1,1,1,1,1,1,1,0,0,1,1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,0,0,1,1,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
1,1,0,0,1,1,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
1,1,0,0,1,1,0,0,1,1,0,0,0,0,0,0,1,1,0,0,0,1,1,1,1,1,0,0,1,1,0,0,0,0,0,0,1,1,1,1,1,1,
1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,0,1,0,0,1,1,1,1,1,1,0,0,1,1,1,1,1,1,
1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,1,1,1,0,0,0,1,1,1,0,0,0,0,0,0,1,1,0,0,0,
1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,1,1,0,0,
1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,1,1,1,1,0,0,1,1,1,0,0,0,0,0,1,1,1,1,1,1,
1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1,1,1,1,1,0,0,1,1,1,0,0,0,0,0,1,1,1,1,1,1,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,1,1,0,1,0,1,0,1,1,0,0,1,0,0,0,0,0,1,0,0,0,1,0,0,0,1,0,1,1,0,0,0,0,0,0,
1,1,1,1,1,0,1,0,0,1,1,0,1,1,0,0,1,0,1,1,0,0,0,1,1,0,1,1,0,1,1,0,1,1,0,0,0,1,1,1,1,1,
1,0,1,0,1,0,1,0,0,1,1,0,1,0,0,0,1,0,0,1,0,1,0,1,0,0,1,0,0,0,1,0,1,0,0,0,0,1,0,1,0,1,
1,0,1,0,1,0,1,0,0,1,1,0,1,0,0,0,1,0,0,1,0,1,0,1,0,0,1,1,0,1,1,0,1,0,0,0,0,1,0,1,0,1,
1,0,1,0,1,0,0,1,1,0,1,0,1,0,0,0,1,0,0,0,1,1,1,0,0,0,0,0,1,0,0,0,1,0,0,0,0,1,0,1,0,1,

};

uint8_t tv[] = {
0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,0,0,0,1,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,1,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,1,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,0,1,0,0,0,1,0,1,0,1,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,1,0,1,0,1,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,1,0,1,0,1,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,1,0,1,0,0,1,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,0,0,1,1,1,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,0,0,1,1,1,0,0,0,0,0,0,0,0};

uint8_t chr_A[] = {
0,0,1,1,0,0,
0,1,0,0,1,0,
1,0,0,0,0,1,
1,1,1,1,1,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,};

uint8_t chr_D[] = {
1,1,1,1,1,0,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,1,1,1,1,0,};

uint8_t chr_F[] = {
1,1,1,1,1,1,
1,0,0,0,0,0,
1,0,0,0,0,0,
1,1,1,1,1,1,
1,0,0,0,0,0,
1,0,0,0,0,0,
1,0,0,0,0,0,};

uint8_t chr_G[] = {
0,1,1,1,1,0,
1,0,0,0,0,1,
1,0,0,0,0,0,
1,0,0,1,1,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
0,1,1,1,1,0,};

uint8_t chr_H[] = {
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,1,1,1,1,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,};

uint8_t chr_L[] = {
1,0,0,0,0,0,
1,0,0,0,0,0,
1,0,0,0,0,0,
1,0,0,0,0,0,
1,0,0,0,0,0,
1,0,0,0,0,0,
1,1,1,1,1,1,};

uint8_t chr_M[] = {
1,0,0,0,0,1,
1,1,0,0,1,1,
1,0,1,1,0,1,
1,0,1,1,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,};

uint8_t chr_N[] = {
1,0,0,0,0,1,
1,1,0,0,0,1,
1,0,1,0,0,1,
1,0,0,1,0,1,
1,0,0,0,1,1,
1,0,0,0,0,1,
1,0,0,0,0,1,};

uint8_t chr_O[] = {
0,1,1,1,1,0,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
0,1,1,1,1,0,};

uint8_t chr_P[] = {
1,1,1,1,1,0,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,1,1,1,1,0,
1,0,0,0,0,0,
1,0,0,0,0,0,
1,0,0,0,0,0,};

uint8_t chr_R[] = {
1,1,1,1,1,0,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,1,0,
1,0,0,1,0,0,
1,0,0,0,1,0,
1,0,0,0,0,1,};

uint8_t chr_S[] = {
0,1,1,1,1,1,
1,0,0,0,0,0,
1,0,0,0,0,0,
0,1,1,1,1,0,
0,0,0,0,0,1,
0,0,0,0,0,1,
1,1,1,1,1,0,};

uint8_t chr_V[] = {
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
0,1,0,0,1,0,
0,1,0,0,1,0,
0,1,0,0,1,0,
0,0,1,1,0,0,};

uint8_t chr_W[] = {
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,0,0,0,1,
1,0,1,1,0,1,
1,0,1,1,0,1,
1,1,0,0,1,1,
1,0,0,0,0,1,};

uint8_t sprite_tree[] = {
0,0,0,1,0,0,0,
0,0,1,1,1,0,0,
0,1,1,1,1,1,0,
1,1,1,1,1,1,1,
0,1,1,1,1,1,0,
1,1,1,1,1,1,1,
0,0,0,1,0,0,0,
0,0,0,1,0,0,0,};

uint8_t sprite_man[] = {
0,0,0,1,0,0,0,
0,0,1,1,1,0,0,
0,0,0,1,0,0,1,
0,1,1,1,1,1,0,
1,0,1,1,1,0,0,
0,0,1,1,1,0,0,
0,1,0,0,0,1,0,
0,1,0,0,0,1,0};

uint8_t sprite_manl[] = {
0,0,0,1,0,0,0,
0,0,1,1,1,0,0,
1,0,0,1,0,0,0,
0,1,1,1,1,1,0,
0,0,1,1,1,0,1,
0,0,1,1,1,0,0,
0,1,0,0,0,1,0,
0,1,0,0,0,0,0};

uint8_t sprite_manr[] = {
0,0,0,1,0,0,0,
0,0,1,1,1,0,0,
0,0,0,1,0,0,1,
0,1,1,1,1,1,0,
1,0,1,1,1,0,0,
0,0,1,1,1,0,0,
0,1,0,0,0,1,0,
0,0,0,0,0,1,0};

uint8_t *menu[] = {
chr_P,chr_A,chr_N,chr_G,
chr_F,chr_A,chr_S,chr_H,
chr_D,chr_A,chr_M,chr_O,
chr_D,chr_R,chr_A,chr_V,
chr_W,chr_A,chr_L,chr_F,
};

uint16_t l=0; //current line of scan

uint8_t im_line_s[H_RES]; //image sync buffer

uint8_t bl_line_s[H_RES]; //lower 1/4 of screen sync buffer
uint8_t bl_line_v[H_RES]; //lower 1/4 of screen video buffer
uint8_t vb_line_s[H_RES]; //vertical sync, sync buffer
uint8_t vb_line_v[H_RES]; //vertical sync, video buffer

float cube_a[3] = {15.f, 0.f, 0.f};


float cube_pts[8][3] = { {SIDE,SIDE,HSIDE},
                         {-SIDE,SIDE,HSIDE},
                         {-SIDE,-SIDE,HSIDE},
                         {SIDE,-SIDE,HSIDE},
                         {SIDE,-SIDE,-HSIDE},
                         {-SIDE,-SIDE,-HSIDE},
                         {-SIDE,SIDE,-HSIDE},
                         {SIDE,SIDE,-HSIDE} };
                         
float x_rot[3][3] = { {1.f, 0.f, 0.f},
                        {0.f, COS_V, -SIN_V},
                        {0.f, SIN_V, COS_V} };
                        
float y_rot[3][3] = { {COS_V, 0.f, SIN_V},
                        {0.f, 1.f, 0.f},
                        {-SIN_V, 0, COS_V} };
                        
float z_rot[3][3] = { {COS_V, -SIN_V, 0.f},
                        {SIN_V, COS_V, 0.f},
                        {0.f, 0.f, 1.f} };


uint8_t im_line_va[H_RES*V_RES]; //image buffer

//pong variables
#define PADDLE_LEN 10
float p1 = (SY_MIN+SY_MAX)/2-PADDLE_LEN/2, p2 = (SY_MIN+SY_MAX)/2-PADDLE_LEN/2;
float p1v = 0.1, p2v = -0.1;

float bx = (SX_MIN+SX_MAX)/2, by = (SY_MIN+SY_MAX)/2;
float bxv = 0.03, byv = 0.03;

int s1 = 0, s2 = 0;

//fish variables
#define FDEATH_RESET 30000
float fx = (SX_MAX+SX_MIN)/2.f, fy = (SY_MIN+SY_MAX)/2.f, fv = 0.02f, ftheta = 0.0, fa = 0.002;
float uv = 0.005;
float ux[8] = {53,70,78,78,70,53,45,45};
float uy[8] = {10,10,20,35,50,50,35,20};
float uvx[8] = {-1,1,1,1,1,-1,-1,-1};
float uvy[8] = {1,1,1,-1,-1,-1,-1,1};
int ulive[8] = {1,1,1,1,1,1,1,1};

int fdeath_ticker = FDEATH_RESET;
float fs = 0;

//tunnel variables
#define TUN_LEN (SY_MAX - SY_MIN)
uint8_t tunnel[TUN_LEN];
float tunnp = 50;
int tundir = 1;
int leg_ctr = 0;

//raycaster variables
#define mapWidth 24
#define mapHeight 24

uint8_t worldMap[mapWidth][mapHeight]=
{
  {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,2,2,2,2,2,0,0,0,0,3,0,3,0,3,0,0,0,1},
  {1,0,0,0,0,0,2,0,0,0,2,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,2,0,0,0,2,0,0,0,0,3,0,0,0,3,0,0,0,1},
  {1,0,0,0,0,0,2,0,0,0,2,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,2,2,0,2,2,0,0,0,0,3,0,3,0,3,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,4,4,4,4,4,4,4,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,4,0,4,0,0,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,4,0,0,0,0,5,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,4,0,4,0,0,0,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,4,0,4,4,4,4,4,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,4,4,4,4,4,4,4,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
  {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1}
};

float posX = 12, posY = 12;
float dirX = -1, dirY = 0;
float planeX = 0, planeY = 0.66;
float rc_cos = cosf(0.03f);
float rc_sin = sinf(0.03f);

//mode7 variables
float m7_theta = 0.8f;
float m7_x = mapWidth/2, m7_y=mapHeight/2;

DigitalIn p1_in(A1);
DigitalIn p2_in(A0);

void make_checkerboard()
{
    for(int i = 0; i < H_RES; i++)
        for(int j = 0; j < V_RES; j++)
            im_line_va[i+j*H_RES] = 0*((i > 20) && (i < 98)) && ((j%2) ^ (i%2)); //checkerboard
    
}

void clr()
{
    for(int i = 0; i < H_RES; i++)
        for(int j = 0; j < V_RES; j++)
            im_line_va[i+j*H_RES] = 0;
    
}

void init_buffers()
{
    clr();
    for(int i = 0; i < H_RES; i++)
    {
        im_line_s[i] = 1;   
        bl_line_s[i] = 1;
        bl_line_v[i] = 0;
        vb_line_s[i] = 0;
        vb_line_v[i] = 0;
    }    
    im_line_s[0] = 0;
    im_line_s[1] = 0;
    im_line_s[2] = 0;
    bl_line_s[0] = 0;
    vb_line_s[0] = 1;
    bl_line_s[1] = 0;
    vb_line_s[1] = 1;
    
    
    
}
void isr()
{
    uint8_t nop = 0; //use nops or use wait_us
    uint8_t* sptr; //pointer to sync buffer for line
    uint8_t* vptr; //pointer to video buffer for line
    if(l < V_RES){ vptr = im_line_va + ((l/4)*H_RES); sptr = im_line_s; nop = 1; } //pick line buffers
    else if(l < 254){ vptr = bl_line_v; sptr = bl_line_s; nop = 0; }
    else{ vptr = vb_line_v; sptr = vb_line_s; nop = 1;}
    uint8_t lmax = nop?H_RES:12; //number of columns
    for(uint8_t i = 0; i < lmax; i++) //loop over each column
    {
        vout = vptr[i]; //set output pins
        sout = sptr[i];   
        if(nop) //nop delay
        {
            asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");//asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
        }
        else {wait_us(1); if(i > 2) i++;} //wait delay
        
        
    }
    //move to next line
    l++;
    if(l > 255) l = 0;
}

//coordinates for bouncing ball



int k = 0;
uint16_t px = X_0*10;
uint16_t py = Y_0*10;
uint16_t vx = 10;
uint16_t vy = 10;
uint16_t px2 = X_0*10 + 80;
uint16_t py2 = Y_0*10 - 40;
uint16_t vx2 = 3;
uint16_t vy2 = -4;

int16_t sign(int16_t a)
{
    if(a > 0) return 1;
    if(a < 0) return -1;
    return 0;
}

void draw_vert(int16_t y0, int16_t y1, int16_t x0)
{
    for(int16_t i = y0; i < y1; i++)
        im_line_va[H_RES*i + x0] = 1;
}

void draw_horiz(int16_t x0, int16_t x1, int16_t y0)
{
    for(int16_t i = x0; i < x1; i++)
        im_line_va[H_RES*y0 + i] = 1;
}

int16_t imin(int16_t a, int16_t b)
{
    if(a<b) return a;
    return b;
}

int16_t imax(int16_t a, int16_t b)
{
    if(a>b) return a;
    return b;
}

void draw_line(int16_t x0, int16_t y0, int16_t x1, int16_t y1)
{
    if(x0 > x1){ x0 = x0 ^ x1; x1 = x1^x0; x0 = x0^x1;y0 = y0 ^ y1; y1 = y1^y0; y0 = y0^y1; }
    if(x0 == x1){draw_vert(y0,y1,x0);}
    if(y0 == y1){draw_horiz(x0,x1,y0);}
    int16_t dx = x1 - x0;
    int16_t dy = y1 - y0;
    float derr = fabs((float)(dy)/(float)(dx));
    float err = 0.f;
    int16_t y = y0;
    for(int16_t x = x0; x < x1; x++)
    {
        //plotxy
        im_line_va[H_RES*y + x] = !draw_line_inv;
        err += derr;
        while(err >= 0.5f)
        {
            y +=  sign(dy);
            im_line_va[H_RES*y + x] = !draw_line_inv;
            err -= 1.f;
        }   
    }
}

void draw_cube(float cp[][3])
{
    for(uint8_t i = 0; i < 7; i++)
    {
        draw_line((int16_t)cp[i][0]+cube_a[0]+X_0,(int16_t)cp[i][1]+cube_a[1]+Y_0,(int16_t)cp[i+1][0]+cube_a[0]+X_0,(int16_t)cp[i+1][1]+cube_a[1]+Y_0);    
    }
    draw_line((int16_t)cp[0][0]+cube_a[0]+X_0,(int16_t)cp[0][1]+cube_a[1]+Y_0,(int16_t)cp[3][0]+cube_a[0]+X_0,(int16_t)cp[3][1]+cube_a[1]+Y_0);   
    draw_line((int16_t)cp[4][0]+cube_a[0]+X_0,(int16_t)cp[4][1]+cube_a[1]+Y_0,(int16_t)cp[7][0]+cube_a[0]+X_0,(int16_t)cp[7][1]+cube_a[1]+Y_0);   
    draw_line((int16_t)cp[0][0]+cube_a[0]+X_0,(int16_t)cp[0][1]+cube_a[1]+Y_0,(int16_t)cp[7][0]+cube_a[0]+X_0,(int16_t)cp[7][1]+cube_a[1]+Y_0);   
    draw_line((int16_t)cp[1][0]+cube_a[0]+X_0,(int16_t)cp[1][1]+cube_a[1]+Y_0,(int16_t)cp[6][0]+cube_a[0]+X_0,(int16_t)cp[6][1]+cube_a[1]+Y_0);   
    draw_line((int16_t)cp[2][0]+cube_a[0]+X_0,(int16_t)cp[2][1]+cube_a[1]+Y_0,(int16_t)cp[5][0]+cube_a[0]+X_0,(int16_t)cp[5][1]+cube_a[1]+Y_0);   
}

void apply_rot(float cp[][3], float r[][3])
{
    for(uint8_t pti = 0; pti < 8; pti++)
    {
        float* cpt = cp[pti];
        float xn = r[0][0] * cpt[0] + r[0][1] * cpt[1] + r[0][2] * cpt[2]; 
        float yn = r[1][0] * cpt[0] + r[1][1] * cpt[1] + r[1][2] * cpt[2]; 
        float zn = r[2][0] * cpt[0] + r[2][1] * cpt[1] + r[2][2] * cpt[2];  
        cpt[0] = xn;
        cpt[1] = yn;
        cpt[2] = zn;   
    }   
}

void apply_xf_rot(float cpt[], float r[][3])
{
        float xn = r[0][0] * cpt[0] + r[0][1] * cpt[1] + r[0][2] * cpt[2]; 
        float yn = r[1][0] * cpt[0] + r[1][1] * cpt[1] + r[1][2] * cpt[2]; 
        float zn = r[2][0] * cpt[0] + r[2][1] * cpt[1] + r[2][2] * cpt[2];  
        cpt[0] = xn;
        cpt[1] = yn;
        cpt[2] = zn;   
}

void draw_v_check(int8_t r,uint8_t tt)
{
    for(int i = 0; i < H_RES; i++)
        for(int j = 0; j < V_RES; j++)
            im_line_va[i+j*H_RES] = (((i > 20) && (i < 98)) && ( tt ^(((j%(r*2))>=r) ^ ((i%(r*2)))>=r))); //checkerboard
}


int jjj = 0;
int f_count = DEMO_LENGTH + 10;
void draw_cube_spin(int kkkk);
//update bouncing balls

void draw_blank()
{
    for(uint16_t h = X_MIN; h < X_MAX; h++)
    {
        for(uint16_t v = Y_MIN; v < Y_MAX+15; v++)
        {
                im_line_va[v*H_RES + h] = 0;
        }
    }
}

void wipe();

void update_image()
{
    //delay
    k++;
    if(k%4000) return;
    f_count++;
    if(f_count < DEMO_LENGTH/2)
    {
        draw_cube_spin(0);
    }
    else if(f_count < DEMO_LENGTH)
    {
        draw_cube_spin(0);
    }
    else if(f_count < DEMO_LENGTH * 1.22)
    {
        for(int k = 1; k < 10; k++)
            //for(int kk = 1; kk < 3; kk++)
            {
                draw_v_check(k,1);
                draw_v_check(k,1);
                draw_v_check(k,0);
                draw_v_check(k,0); 
            }
            
        draw_v_check(1,0);
        wipe();
        f_count = 0;
    }
}

void wipe()
{
    for(int v = Y_MIN; v < Y_MAX+15; v++)
    {
        for(uint16_t h = X_MIN; h < X_MAX; h++)
        {
            im_line_va[v*H_RES+h] = 1;
        }
        wait(.01);
    }
    
        for(int v = Y_MAX+15; v > Y_MIN; v--)
        {
            for(uint16_t h = X_MIN; h < X_MAX; h++)
            {
                im_line_va[v*H_RES+h] = 0;
            }
            wait(.005);
        }  
    
        for(uint16_t h = X_MIN; h < X_MAX; h++)
        {
            for(uint16_t v = Y_MIN; v < Y_MAX+15; v++)
            {
                im_line_va[v*H_RES + h] = 1;
            }
            wait(.005);
        }
    
        for(uint16_t h = X_MIN; h < X_MAX; h++)
        {
            for(uint16_t v = Y_MIN; v < Y_MAX+15; v++)
            {
                im_line_va[v*H_RES + h] = 0;
            }
            wait(.005);
        }
    
        for(int v = Y_MIN; v < Y_MAX+15; v++)
        {
            for(uint16_t h = X_MIN; h < X_MAX; h++)
            {
                im_line_va[v*H_RES+h] = 0;
            }
        }
}

void draw_cube_spin(int kkkk)
{
    draw_line_inv = kkkk;
    //blank
    for(uint16_t h = X_MIN; h < X_MAX; h++)
    {
        for(uint16_t v = Y_MIN; v < Y_MAX; v++)
        {
            if( (h > 22) && (h < 76+18))
                im_line_va[v*H_RES + h] = kkkk;
        }
    }
    apply_xf_rot(cube_a,y_rot);
    apply_xf_rot(cube_a,y_rot);
    apply_rot(cube_pts,x_rot);
    apply_rot(cube_pts,x_rot);
    apply_rot(cube_pts,y_rot);
    apply_rot(cube_pts,y_rot);
    apply_rot(cube_pts,z_rot);
    //draw ball
    im_line_va[(py/10)*H_RES + (px/10)] = 1;
    draw_line(X_0,Y_0,(px/10),(py/10));
    draw_cube(cube_pts);
    
    //update position/check for bounces
    px += vx;
    py += vy;
    px2 += vx2;
    py2 += vy2;
    if(px/10 > (X_MAX-2) || px/10 < (X_MIN+1)) vx = -vx;
    if(py/10 > (Y_MAX-2) || py/10 < (Y_MIN+1)) vy = -vy;
    if(px2/10 > (X_MAX-2) || px2/10 < (X_MIN+1)) vx2 = -vx2;
    if(py2/10 > (Y_MAX-2) || py2/10 < (Y_MIN+1)) vy2 = -vy2;
    if(k%(8000*50)) return;
    
        jjj = !jjj;
    int bmi = 0;
    for(int y = BY_0; y < BY_0 + BY; y++)
    {
        for(int x = BX_0; x < BX_0 + BX; x++)
        {
            im_line_va[H_RES*y + x] = kkkk^(miters[bmi]*jjj + (!jjj)*tv[bmi]);
            bmi++;
        }
    }
}

int flappy_pong() {
    
    //clear frame
    for(int y = 0; y < PADDLE_LEN; y++) {
        im_line_va[H_RES*(y+(int)p1)+SX_MIN] = 0;
        im_line_va[H_RES*(y+(int)p2)+SX_MAX] = 0;
    }
    im_line_va[H_RES*(int)by+(int)bx] = 0;
    //read inputs
    int cmd1 = p1_in.read();
    int cmd2 = p2_in.read();
    if (cmd1 == 0) p1v = 0.1; else p1v = -0.1f;
    if (cmd2 == 0) p2v = 0.1; else p2v = -0.1f;
    
    p1 += p1v;
    p2 += p2v;
    bx += bxv;
    by += byv;
    
    if (p1 < SY_MIN) p1 = SY_MIN;
    if (p1 > SY_MAX-PADDLE_LEN) p1 = SY_MAX-PADDLE_LEN;
    if (p2 < SY_MIN) p2 = SY_MIN;
    if (p2 > SY_MAX-PADDLE_LEN) p2 = SY_MAX-PADDLE_LEN;
    
    if (bx < SX_MIN) {
        bxv = -bxv; 
        bx = SX_MIN;
        if (by < p1 - 3 || by > p1 + PADDLE_LEN + 3) {
            im_line_va[H_RES*(SY_MIN-2)+2*s1+SX_MIN] = 1;
            bx = (SX_MIN+SX_MAX)/2, by = (SY_MIN+SY_MAX)/2;
            s1++;
            if (s1>14) return 0;
        }
    } 
    if (bx > SX_MAX) {
        bxv = -bxv; 
        bx = SX_MAX;
        if (by < p2 - 3 || by > p2 + PADDLE_LEN + 3) {
            im_line_va[H_RES*(SY_MAX+2)+2*s2+SX_MIN] = 1;
            bx = (SX_MIN+SX_MAX)/2, by = (SY_MIN+SY_MAX)/2;
            s2++;
            if (s2>14) return 0;
        }
    }
    if (by < SY_MIN) {byv = -byv; by = SY_MIN;}
    if (by > SY_MAX) {byv = -byv; by = SY_MAX;}
    
    for(int y = 0; y < PADDLE_LEN; y++) {
        im_line_va[H_RES*(y+(int)p1)+SX_MIN] = 1;
        im_line_va[H_RES*(y+(int)p2)+SX_MAX] = 1;
    }
    im_line_va[H_RES*(int)by+(int)bx] = 1;
    
    wait(1/1200.0f);
    return 1;
}

int fishy() {
    uint16_t xstart, ystart, xend, yend;
    float s = sinf(ftheta);
    float c = cosf(ftheta);
    
    xstart = (uint16_t) fx;
    ystart = (uint16_t) fy;
    

    for(int x = xstart-5; x < xstart + 5; x++) {
        for (int y = ystart - 5; y < ystart + 5; y++) {
            im_line_va[H_RES*y+x] = 0;
        }
    }
    for (int i = 0; i < 8; i++) {
        int index = H_RES*(int)uy[i]+(int)ux[i];
        im_line_va[index] = 0;
    }    

    float fx_old = fx, fy_old = fy;
    fx += fv * s;
    fy += fv * c;
    int cmd = p1_in.read();
    if (cmd) ftheta += fa; else ftheta -= fa;
    if (ftheta >= 2 * 3.1415926) ftheta -= 2 * 3.1415926;
    if (ftheta < 0) ftheta += 2 * 3.1415926;
    
    s = sinf(ftheta);
    c = cosf(ftheta);
    
    xstart = (uint16_t)fx;
    ystart = (uint16_t)fy;
    xend = (uint16_t)(fx+s*4);
    yend = (uint16_t)(fy+c*4);
    
    if (xstart < SX_MIN || xstart > SX_MAX ||
        ystart < SY_MIN || ystart > SY_MAX ||
        xend < SX_MIN || xend > SX_MAX ||
        yend < SY_MIN || yend > SY_MAX) {
        fx = fx_old;
        fy = fy_old;
        xstart = (uint16_t)fx;
        ystart = (uint16_t)fy;
        xend = (uint16_t)(fx+s*4);
        yend = (uint16_t)(fy+c*4);
    }

    draw_line(xstart, ystart, xend, yend);
    for (int i = 0; i < 8; i++) {
        if (!ulive[i]) continue;
        if (xstart == (int)ux[i] && ystart == (int)uy[i]) {
            fdeath_ticker = FDEATH_RESET;
            fs+=1;
            ulive[i] = 0;
        }
        ux[i]+= uv*uvx[i];
        uy[i]+= uv*uvy[i];
        if (ux[i] < SX_MIN) {ux[i] = SX_MIN; uvx[i] = -uvx[i];}
        if (ux[i] > SX_MAX) {ux[i] = SX_MAX; uvx[i] = -uvx[i];}
        if (uy[i] < SY_MIN) {uy[i] = SY_MIN; uvy[i] = -uvy[i];}
        if (uy[i] > SY_MAX) {uy[i] = SY_MAX; uvy[i] = -uvy[i];}  
                  
        int index = H_RES*(int)uy[i]+(int)ux[i];
        im_line_va[index] = 1;
    }
    for (int i = 0; i < 8; i++) {
        im_line_va[H_RES*(SY_MAX+6)+2*(int)i+SX_MIN] = 0;
    }
    for (int i = 0; i < fs; i++) {
        im_line_va[H_RES*(SY_MAX+6)+2*(int)i+SX_MIN] = 1;
    }
    fdeath_ticker--;
    if (fdeath_ticker < 0) fs -= 0.0001f;
    if (fs < 0) fs = 0;

    return 1;
}

void dispchr(int x0, int y0, uint8_t* chr) {
    int bmi = 0;
    for(int y = SY_MIN + y0; y < SY_MIN + y0 + 7; y++)
    {
        for(int x = SX_MIN + x0; x < SX_MIN + x0 + 6; x++)
        {
            im_line_va[H_RES*y + x] = chr[bmi];
            bmi++;
        }
    }
}

void dispsprite(int x0, int y0, uint8_t* chr) {
    int bmi = 0;
    for(int y = SY_MIN + y0; y < SY_MIN + y0 + 8; y++)
    {
        for(int x = SX_MIN + x0; x < SX_MIN + x0 + 7; x++)
        {
            if (chr[bmi]) im_line_va[H_RES*y + x] = chr[bmi];
            bmi++;
        }
    }
}

void disp_tunnel() {
    for (int y = SY_MIN; y < SY_MAX; y++) {
        int tunw = 30 * (y) / TUN_LEN;
        int yy = SY_MAX-(SY_MAX-y)*3/4;
        for (int x = SX_MIN; x < SX_MIN + tunnel[y - SY_MIN]; x++) im_line_va[H_RES*yy+x] = (x&1)^(y&1);
        for (int x = SX_MIN + tunnel[y - SY_MIN]; x < SX_MIN + tunnel[y - SY_MIN] + tunw; x++) im_line_va[H_RES*yy+x] = 0;
        for (int x = SX_MIN + tunnel[y - SY_MIN] + tunw; x < SX_MAX; x++) im_line_va[H_RES*yy+x] = (x&1)^(y&1);
    }
}

void init_tunnel() {
    for (int i = 0; i < TUN_LEN; i++) tunnel[i] = 20;
    disp_tunnel();
    
    int bmi = 0;
    
    for(int y = SY_MIN; y < SY_MIN + BY; y++)
    {
        for(int x = SX_MIN; x < SX_MIN + BX; x++)
        {
            im_line_va[H_RES*y + x + 10] = tv[bmi];
            bmi++;
        }
    }
    dispsprite(1,2,sprite_tree);
    dispsprite(50,2,sprite_tree);
    dispsprite(11,2,sprite_tree);
}

void update_tunnel() {
    for (int j = TUN_LEN-1; j > 0; j--) tunnel[j] = tunnel[j-1];
    
    int x = rand() % 48;
    if (x == 0) tundir = -tundir;
    if (x > 24) tunnel[0] = tunnel[0]+tundir;
    
    if (tunnel[0] < 1) {
        tunnel[0] = 1;
        tundir = 1;
    }
    if (SX_MIN + tunnel[0] + 30 + 1 > SX_MAX) {
        tunnel[0] = SX_MAX-SX_MIN-30-1;
        tundir = -1;
    }
    
    disp_tunnel();
    
    int cmd1 = p1_in.read();
    int cmd2 = p2_in.read();
    if (cmd1) tunnp+=1.0f;
    if (cmd2) tunnp-=1.0f;
    if (tunnp<SX_MIN+8) tunnp = SX_MIN+8;
    if (tunnp>SX_MAX-8) tunnp = SX_MAX-8;
    leg_ctr++;
    if (leg_ctr == 50) leg_ctr = 0;
    if (leg_ctr < 25) dispsprite((int)(tunnp-SX_MIN), 31, sprite_manl);
    else dispsprite((int)(tunnp-SX_MIN), 31, sprite_manr);
    
    wait(0.01);
}

void update_rc() {
    int cmd1 = p1_in.read();
    int cmd2 = p2_in.read();
    if (!cmd1) {
        float oldDirX = dirX;
        dirX = dirX * rc_cos - dirY * rc_sin;
        dirY = oldDirX * rc_sin + dirY * rc_cos;
        float oldPlaneX = planeX;
        planeX = planeX * rc_cos - planeY * rc_sin;
        planeY = oldPlaneX * rc_sin + planeY * rc_cos;
    }
    if (!cmd2) {
        float oldDirX = dirX;
        dirX = dirX * rc_cos + dirY * rc_sin;
        dirY = -oldDirX * rc_sin + dirY * rc_cos;
        float oldPlaneX = planeX;
        planeX = planeX * rc_cos + planeY * rc_sin;
        planeY = -oldPlaneX * rc_sin + planeY * rc_cos;
    }
    if(worldMap[int(posX + dirX * 0.01f)][int(posY)] == false) posX += dirX * 0.01f;
    if(worldMap[int(posX)][int(posY + dirY * 0.01f)] == false) posY += dirY * 0.01f;
    
    int w = SX_MAX-SX_MIN;
    int h = SY_MAX-SY_MIN;
    for (int x = 0; x < SX_MAX - SX_MIN; x++) {
        float cameraX = 2 * x / float(w) - 1; //x-coordinate in camera space
        float rayPosX = posX;
        float rayPosY = posY;
        float rayDirX = dirX + planeX * cameraX;
        float rayDirY = dirY + planeY * cameraX;
        
        int mapX = int(rayPosX);
        int mapY = int(rayPosY);
        
        float sideDistX, sideDistY;
        
        float deltaDistX = sqrtf(1 + (rayDirY * rayDirY) / (rayDirX * rayDirX));
        float deltaDistY = sqrtf(1 + (rayDirX * rayDirX) / (rayDirY * rayDirY));
        float perpWallDist;
        
        int stepX, stepY;
        int hit = 0, side;
        if (rayDirX < 0) {
            stepX = -1;
            sideDistX = (rayPosX - mapX) * deltaDistX;
        } else {
            stepX = 1;
            sideDistX = (mapX + 1.0 - rayPosX) * deltaDistX;
        }
        if (rayDirY < 0) {
            stepY = -1;
            sideDistY = (rayPosY - mapY) * deltaDistY;
        } else {
            stepY = 1;
            sideDistY = (mapY + 1.0 - rayPosY) * deltaDistY;
        }
        
         while (hit == 0)
        {
            //jump to next map square, OR in x-direction, OR in y-direction
            if (sideDistX < sideDistY)
            {
              sideDistX += deltaDistX;
              mapX += stepX;
              side = 0;
            }
            else
            {
              sideDistY += deltaDistY;
              mapY += stepY;
              side = 1;
            }
            //Check if ray has hit a wall
            if (worldMap[mapX][mapY] > 0) hit = 1;
        }
        //Calculate distance projected on camera direction (oblique distance will give fisheye effect!)
        if (side == 0) perpWallDist = (mapX - rayPosX + (1 - stepX) / 2) / rayDirX;
        else           perpWallDist = (mapY - rayPosY + (1 - stepY) / 2) / rayDirY;
        
        //Calculate height of line to draw on screen
        int drawStart,drawEnd;
        int lineHeight = (int)(h / perpWallDist);
        if (lineHeight > h) {
            drawStart = 0;
            drawEnd = h;
        } else {
            drawStart = -lineHeight / 2 + h / 2;
            drawEnd = lineHeight / 2 + h / 2;
        }
        if (side == 0) {
            for(int y = 0; y < drawStart; y++) {
                im_line_va[H_RES*(y+SY_MIN)+x+SX_MIN] = 0;
            }
            for(int y = drawStart;y < drawEnd;y++) {
                im_line_va[H_RES*(y+SY_MIN)+x+SX_MIN] = 1;
            }
            for(int y = drawEnd; y < h; y++) {
                im_line_va[H_RES*(y+SY_MIN)+x+SX_MIN] = 0;
            }
        } else {
            for(int y = 0; y < h; y++) im_line_va[H_RES*(y+SY_MIN)+x+SX_MIN] = 0;
            im_line_va[H_RES*(drawStart+SY_MIN)+x+SX_MIN] = 1;
            im_line_va[H_RES*(drawEnd-1+SY_MIN)+x+SX_MIN] = 1;
        }
    }
}

void update_m7() {
    int w = SX_MAX-SX_MIN;
    int h = SY_MAX-SY_MIN;
    float c = cosf(m7_theta);
    float s = sinf(m7_theta);
    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            float xx, yy;
            yy = y;
            xx = (x-w/2)*(h-yy)/h+w/2;
            xx = xx * (float) mapWidth / w - m7_x;
            yy = yy * (float) mapHeight / h - 2*m7_y;

            float xx_old = xx;
            float yy_old = yy;
            xx = xx_old*c-yy_old*s;
            yy = xx_old*s+yy_old*c;
            xx *= 1.0f;
            yy *= 1.0f;
            
            xx += m7_x;
            yy += m7_y;

            if (xx < 0 || xx >= mapWidth+1 || yy < 0 || yy > mapHeight+1) {
                im_line_va[H_RES*(SY_MIN+y)+x+SX_MIN] = 0;
            } else if (worldMap[(int)yy][(int)xx] == 0) {
                im_line_va[H_RES*(SY_MIN+y)+x+SX_MIN] = 0;
            } else {
                im_line_va[H_RES*(SY_MIN+y)+x+SX_MIN] = 1;
            }
        }
    }
    int cmd1 = p1_in.read();
    int cmd2 = p2_in.read();
    if (!cmd1) m7_theta+=0.03f;
    if (!cmd2) m7_theta-=0.03f;
}

int main() {
    potato:
    init_buffers();
    t.attach_us(&isr,63);
    
    for(;;){update_m7();}
    
    int bmi = 0;
    
    for(int y = SY_MIN; y < SY_MIN + BY2; y++)
    {
        for(int x = SX_MIN; x < SX_MIN + BX; x++)
        {
            im_line_va[H_RES*y + x] = miters[bmi];
            bmi++;
        }
    }
    
    int cursor_pos = 0;
    int menu_offs = 0;
    
    for (;;) {
        int cmd1 = p1_in.read();
        int cmd2 = p2_in.read();
        
        int cursor_x = SX_MIN;
        int cursor_y = SY_MIN + 8*cursor_pos + BY2 + 5;
        
        if (cmd1 || cmd2) im_line_va[H_RES*cursor_y+cursor_x] = 0;
        
        if (!cmd1) cursor_pos++;
        if (!cmd2) cursor_pos--;
        if (!cmd1 && !cmd2) break;
        
        if (cursor_pos < 0) cursor_pos = 0;
        if (cursor_pos > 4) cursor_pos = 4;
        if (cursor_pos > 3) menu_offs = cursor_pos - 3; else menu_offs = 0;
        
        cursor_x = SX_MIN;
        if (cursor_pos < 4) cursor_y = SY_MIN + 8*cursor_pos + BY2 + 5;
        else cursor_y = SY_MIN + 8*3 + BY2 + 5;
        
        for(int y = 0; y < 4; y++) {
            for(int x = 0; x < 4; x++) {
                dispchr(4+7*x,BY2+1+8*y,menu[4*(y+menu_offs)+x]);
            }
        }
        
        im_line_va[H_RES*cursor_y+cursor_x] = 1;
        
        wait(0.1);
    }
    
    init_buffers();
    
    if (cursor_pos == 0) {
        for (;;) {
            if (!flappy_pong()) break;
        }
    } else if (cursor_pos == 1) {
        for (;;) fishy();
    } else if (cursor_pos == 2) {
        for (;;) update_image();
    } else if (cursor_pos == 3) {
        init_tunnel();    
        for (;;) 
        {
            update_tunnel();
        }
    } else if (cursor_pos == 4) {
        for (;;) {
            update_rc();
        }
    }
    goto potato;
}