#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