#include <display_syscalls.h>
#include <display.h>
#include <keyboard_syscalls.h>
#include <keyboard.hpp>
#include <math.h>
#include <color.h>

short unsigned int heightcolor(float z, float z_min, float z_max);
void fillArea(int x, int y, int width, int height, int color);
int PRGM_GetKey(void);
void plot(int x0, int y0, int color);

int main(void) {
	//program variables
	int key;
	int done = 0;
	
	//graphing variables
	float start = 0;
	float t;
	float x;
	float y = 100;
	int h = 2;
	float g = -9.8;
	float vy = 0;
	float vx = 100;
	char buffer2[10];
	

	
	for(t=start; t < 2; t += 0.01) {
		//y = h+0.5*g* t*t + vy*t;
		//x = vx*t;
		
		x = vx*t;
		y += vy;
		vy -= 9.8;
		
		//start debug
		strcpy(buffer2,"  ");
		itoa(x, buffer2+2);
		PrintXY(3, 1, buffer2, TEXT_MODE_NORMAL, TEXT_COLOR_BLACK);
		
		strcpy(buffer2,"  ");
		itoa(x, buffer2+2);
		PrintXY(3, 2, buffer2, TEXT_MODE_NORMAL, TEXT_COLOR_BLACK);
		
		PrintXY(1, 1, "  X", TEXT_MODE_NORMAL, TEXT_COLOR_BLACK);
		PrintXY(1, 2, "  Y", TEXT_MODE_NORMAL, TEXT_COLOR_BLACK);
		//end debug
		
		plot(x, LCD_HEIGHT_PX - y, COLOR_BLACK);
		Bdisp_PutDisp_DD();
	}
	
	
	
	while(!done) {
		key = PRGM_GetKey();
	
		switch(key) {
			case KEY_PRGM_MENU:
			done = 1;
			break;
		}
	}
 
	return 1;
}


//routines
short unsigned int heightcolor(float z, float z_min, float z_max) {
         float frac = ((z-z_min)/(z_max-z_min));
         
         //color!
         float r = (0.25f)-frac;
         float g = (0.5f)-frac;
         float b = (0.75f)-frac;

         //calculate the R/G/B values
         r = (r>0.f)?r:-r; g = (g>0.f)?g:-g; b = (b>0.f)?b:-b;   //absolute value
         r = (0.25f)-r; g = (1.f/3.f)-g; b = (0.25f)-b;   //invert
         r = (r>0.f)?(6.f*r):0.f; g = (g>0.f)?(6.f*g):0.f; b = (b>0.f)?(6.f*b):0.f;   //scale the chromatic triangles
         r = (r>1.f)?1.f:r; g = (g>1.f)?1.f:g; b = (b>1.f)?1.f:b;   //clip the top of the chromatic triangles
         if (frac < 0.25f) r = (r+1.f)/2.f;   //adjust the bottom end of the scale so that z_min is red, not black
         if (frac > 0.75f) b = (b+1.f)/2.f;   //adjust the top end of the scale so that z_max is blue, not black
         return (short unsigned int)(0x0000ffff & (((int)(31.f*r) << 11) | ((int)(63.f*g) << 5) | ((int)(31.f*b))));   //put the bits together
}

void fillArea(int x, int y, int width, int height, int color) { 
    //only use lower two bytes of color 
    char* VRAM = (char*)0xA8000000; 
    VRAM += 2*(LCD_WIDTH_PX*y + x); 
    for(int j=y; j<y+height; j++) { 
       for(int i=x; i<x+width;  i++) { 
          *(VRAM++) = (color&0x0000FF00)>>8; 
          *(VRAM++) = (color&0x000000FF); 
       } 
       VRAM += 2*(LCD_WIDTH_PX-width); 
    } 
 }
 
int PRGM_GetKey(void) {
  unsigned char buffer[12];
  PRGM_GetKey_OS( buffer );
  return ( buffer[1] & 0x0F ) * 10 + ( ( buffer[2] & 0xF0 ) >> 4 );
}

void plot(int x0, int y0, int color) { 
    char* VRAM = (char*)0xA8000000; 
    VRAM += 2*(y0*LCD_WIDTH_PX + x0); 
    *(VRAM++) = (color&0x0000FF00)>>8; 
    *(VRAM++) = (color&0x000000FF); 
    return; 
 }

//my routines