#include "svt.h"
    #include "draw.h"
    //Draw.c by RorschachUK

    //Almost everything in here has been found in public domain on the internet
    //(There's nothing new under the sun).  I tried a few different ways to
    //get points plotted but this was the only one I found to be relatively
    //stable - it's pretty naive though (1x1 rectangles).  The Trakr's
    //architecture sends graphics as a series of tokenised commands and
    //parameters from the vehicle to the remote control, where they are
    //decoded and drawn - there are no commands covering points, lines or
    //circles so I have to fall back on this method of creating a succession
    //of points (or rectangles, really) which end up being squirted through
    //the radio link and drawn back on the remote.  In other words, it is
    //never going to be quick and efficient until there are more drawing
    //primitives supported by the tokenising scheme and the remote's
    //firmware - and so I didn't bother adding fill routines because they
    //would be painfully slow.

    //I've included fast approximation functions for some maths calculations
    //which should be good enough for graphics purposes but are not accurate

    //Convert RGB values (and transparency flag) into a Color struct
    Color RGBColor(unsigned char R, unsigned char G, unsigned char B, unsigned char T) {
       Color ret;
       ret.R=R;
       ret.G=G;
       ret.B=B;
       ret.Transparent=T;
       return ret;
    }

    //Plot a point on the screen in a given colour by drawing a rectangle from x,y to x,y
    void Plot(int x, int y, Color c) {
       if(x>=0 && x <=159 && y >= 0 && y <= 119)
          DrawRectangle(x,y,x,y,c);
    }

    //Draw a line connecting two points, in a given colour.  This routine works
    //out an optimised series of points to plot.
    void DrawLine(int x0, int y0, int x1, int y1, Color col) {
       int dy = y1 - y0;
       int dx = x1 - x0;
       int stepx, stepy;

       if (dy < 0) { dy = -dy;  stepy = -1; } else { stepy = 1; }
       if (dx < 0) { dx = -dx;  stepx = -1; } else { stepx = 1; }
       dy <<= 1;
       dx <<= 1;

       Plot(x0, y0, col);
       if (dx > dy) {
          int fraction = dy - (dx >> 1);
          while (x0 != x1) {
             if (fraction >= 0) {
                y0 += stepy;
                fraction -= dx;
             }
             x0 += stepx;
             fraction += dy;
             Plot(x0, y0, col);;
          }
       } else {
          int fraction = dx - (dy >> 1);
          while (y0 != y1) {
             if (fraction >= 0) {
                x0 += stepx;
                fraction -= dy;
             }
             y0 += stepy;
             fraction += dx;
             Plot(x0, y0, col);;
          }
       }
    }

    //A square root approximation, I think it's derived from John Carmack's
    //fast inverse square from the Quake source code.  Trakr seemed confused
    //about whether it supported sqrt - there's a <math.h>, but if I use it
    //the function's not linked.  OTOH if I call this routine sqrt instead
    //of qsqrt, the compiler complains I'm redefining an existing function!
    float qsqrt(float number) {
        long i;
        float x, y;
        const float f = 1.5F;

        x = number * 0.5F;
        y  = number;
        i  = * ( long * ) &y;
        i  = 0x5f3759df - ( i >> 1 );
        y  = * ( float * ) &i;
        y  = y * ( f - ( x * y * y ) );
        y  = y * ( f - ( x * y * y ) );
        return number * y;
    }

    //Draw a circle with a given centre, radius and colour.  Uses
    //symmetry so it only needs to figure out one eighth of it and
    //mirrors those points to the other seven eighths.
    void DrawCircle(int xCenter, int yCenter, int radius, Color c) {
       int x, y, r2;

       r2 = radius * radius;
       Plot(xCenter, yCenter + radius, c);
       Plot(xCenter, yCenter - radius, c);
       Plot(xCenter + radius, yCenter, c);
       Plot(xCenter - radius, yCenter, c);

       y = radius;
       x = 1;
       y = (int) (qsqrt(r2 - 1) + 0.5);
       while (x < y) {
          Plot(xCenter + x, yCenter + y, c);
          Plot(xCenter + x, yCenter - y, c);
          Plot(xCenter - x, yCenter + y, c);
          Plot(xCenter - x, yCenter - y, c);
          Plot(xCenter + y, yCenter + x, c);
          Plot(xCenter + y, yCenter - x, c);
          Plot(xCenter - y, yCenter + x, c);
          Plot(xCenter - y, yCenter - x, c);
          x += 1;
          y = (int) (qsqrt(r2 - x*x) + 0.5);
       }
       if (x == y) {
          Plot(xCenter + x, yCenter + y, c);
          Plot(xCenter + x, yCenter - y, c);
          Plot(xCenter - x, yCenter + y, c);
          Plot(xCenter - x, yCenter - y, c);
       }
    }

    //Draw a set of points as a polygon.  Join up the first
    //and last points to close the poly.
    void DrawPoly(int numPoints, iPoint points[], Color c) {
       //quit if not at least two points
       if (numPoints<2)
          return;
       //loop through each line to draw
       int i, x0, y0, x1, y1;
       for (i=1;i<numPoints;i++) {
          x0=points[i-1].x;
          y0=points[i-1].y;
          x1=points[i].x;
          y1=points[i].y;
          DrawLine(x0, y0, x1, y1, c);
       }
       //final line to join it back up
       if (numPoints>2) {
          x1=points[0].x;
          y1=points[0].y;
          x0=points[numPoints-1].x;
          y0=points[numPoints-1].y;
          DrawLine(x0, y0, x1, y1, c);
       }
    }

    //Fast sin approximation from a parabola segment
    float qsin(float number) {
       //always wrap input angle to -PI..PI
       while (number < -3.14159265)
          number += 6.28318531;
       while (number >  3.14159265)
          number -= 6.28318531;

       //compute sine
       if (number < 0)
          return 1.27323954 * number + .405284735 * number * number;
       else
          return 1.27323954 * number - 0.405284735 * number * number;
    }

    //fast cos approximation, just shifts the input by pi/2 to make it sin
    float qcos(float number) {
       number += 1.57079632;
       return qsin(number);
    }

    //Rotate a point a given angle around an origin
    void RotatePoint(iPoint *point, int origX, int origY, float radAngle) {
       int x =  origX + (int) ((point->x - origX) * qcos(radAngle) - (point->y - origY) * qsin(radAngle));
       int y = origY + (int) ((point->x - origX) * qsin(radAngle) + (point->y - origY) * qcos(radAngle));
       point->x=x;
       point->y=y;
    }

    //Rotate an entire polygon about an origin point
    void RotatePoly(int numPoints, iPoint polygon[], int origX, int origY, float radAngle) {
       int i;
       for(i=0; i<numPoints;i++)
          RotatePoint(&polygon[i], origX, origY, radAngle);
    }

    //Shift a polygon in 2D space
    void MovePoly(int numPoints, iPoint polygon[], int shiftX, int shiftY) {
       int i;
       for(i=0; i<numPoints; i++) {
          polygon[i].x += shiftX;
          polygon[i].y += shiftY;
       }
    }

    //Rescale a point with respect to an origin and scale factor
    void ScalePoint(iPoint *point, int origX, int origY, float scaleFactor) {
       int x = origX + (int) ((point->x - origX) * scaleFactor);
       int y = origY + (int) ((point->y - origY) * scaleFactor);
       point->x=x;
       point->y=y;
    }

    //Rescale the dimensions of a polygon
    void ScalePoly(int numPoints, iPoint polygon[], int origX, int origY, float scaleFactor) {
       int i;
       for(i=0; i<numPoints;i++)
          ScalePoint(&polygon[i], origX, origY, scaleFactor);
    }