#include "graphics.h"

/**
 * Given a angle measurement from 0 to 180 degrees, and a corresponding
 * distance, from 0 to MAX_DIST in cm, draw a sonar sweep line on the TFT
 */
void draw_sweep(unsigned int dist, unsigned int angle){
  float side_length, angle_offset, angle_left, angle_right;
  short left_x, left_y, right_x, right_y;
  short max_left_x, max_left_y, max_right_x, max_right_y;
  short middle_x, middle_y;
  float max_side_length = CIRCLE_RADIUS;
  unsigned short color;
  unsigned char red,green,blue;
  
  //Calculate the x1,y1,x2,y2 coordinates to fill in for the triangle
  //First get the length of the sides of the triangle
  side_length = ((float)dist/MAX_DIST) * CIRCLE_RADIUS;
  //Get the offset from the central angle
  angle_offset = INCREMENT_DEG/2.0; 

  angle_left = angle - angle_offset;
  angle_right = angle + angle_offset;

  //Simple trig to figure out correct x and y coords
  left_x = MIDDLE_X - (short)(side_length * cosf(angle_left*(M_PI/180.0)));
  left_y = (short)(side_length * sinf(angle_left*(M_PI/180.0)));
  right_x = MIDDLE_X - (short)(side_length * cosf(angle_right*(M_PI/180.0)));
  right_y = (short)(side_length * sinf(angle_right*(M_PI/180.0)));
  max_left_x = MIDDLE_X - (short)(max_side_length * cosf(angle_left*(M_PI/180.0)));
  max_left_y = (short)(max_side_length * sinf(angle_left*(M_PI/180.0)));
  max_right_x = MIDDLE_X - (short)(max_side_length * cosf(angle_right*(M_PI/180.0)));
  max_right_y = (short)(max_side_length * sinf(angle_right*(M_PI/180.0)));
  middle_x = MIDDLE_X - (short)(max_side_length * cosf(angle * (M_PI/180.0)));
  middle_y = (short)(max_side_length*sinf(angle*(M_PI/180.0)));
  
  //Check to see if we have valid parameters
  if( (dist >= 0 && dist <= MAX_DIST) && (angle >= 0 && angle <= 180)){
    //Now scale the color based on the distance
    if(dist >= MAX_DIST / 2){
      //This is more greenish-yellow colors
      blue = 0;
      green = 255;
      //scale red from 0 to 255 corresponding to max dist to half dist
      red = (unsigned char)(-(MAX_DIST/510.0)*dist + MAX_DIST);
    }
    else{
      //More red-yellow colors
      red = 255;
      blue = 0;
      //scale green from 0 to 255 corresponding to 0 to half dist
      green = (unsigned char)((MAX_DIST/510.0)*dist);
    }
    
    color = get_color(red,green,blue);

    //Clear the previous triangle in this position
    tft_fillTriangle(MIDDLE_X, 0, max_left_x, max_left_y, max_right_x, max_right_y, ILI9340_BLACK);
    //draw the new triangle
    tft_fillTriangle(MIDDLE_X, 0, left_x,left_y, right_x, right_y, color);
  }
  
  else if(dist >= MAX_DIST){
    //Fill this triangle with all green
    tft_fillTriangle(MIDDLE_X, 0, max_left_x, max_left_y, max_right_x, max_right_y, ILI9340_GREEN);
  }
  //Draw a line to mark where the radar is currently at
  //tft_drawLine(MIDDLE_X, 0, middle_x, middle_y, ILI9340_WHITE);

}

/**
 * Given rgb values, convert to appropriate 5-6-5 color encoding. From
 * http://stackoverflow.com/questions/13720937/c-defined-16bit-high-color
 */
unsigned short get_color(unsigned char red, unsigned char green, unsigned char blue)
{
  red   >>= 3;
  green >>= 2;
  blue  >>= 3;
  return (red << 11) | (green << 5) | blue;
}