#include <DFRobot_ST7687S_Latch.h>
#include <DFRobot_ST7687S_Widgets.h>

#include "BLEDevice.h"

DFRobot_ST7687S_Widgets::DFRobot_ST7687S_Widgets(uint8_t pin_cs, uint8_t pin_cd, uint8_t pin_wr, uint8_t pin_rck) 
: DFRobot_ST7687S_Latch (pin_cs, pin_cd, pin_wr, pin_rck) {
    uint8_t old_needle_pos[] = {0, 0, 0, 0};
    uint8_t needle_pos[] = {0, 0, 0, 0};
    bool labels_written = false;
}

void DFRobot_ST7687S_Widgets::AmpMeter(int value)
{
  int i = 0;
  int j = 0;
  int i2 = 0;
  int j2 = 0;
  int color = 0;
  int _lineWidth = getLineWidth();
  int _value = map(value, 0, AMPMETER_MAX, AMPMETER_OPEN, AMPMETER_CLOSE);
  setLineWidth(3);

  for (int p = AMPMETER_OPEN; p < AMPMETER_CLOSE; p = p + 50) {
    if (_value <= p) {
      color = rainbow(map(p, AMPMETER_OPEN, AMPMETER_CLOSE, 63, 127));
    }
    else {
      color = DISPLAY_DARKGREEN;
    }

    i = AMPMETER_RADIUS * (cos(PI * p / 2000));
    j = AMPMETER_RADIUS * (sin(PI * p / 2000));
    i2 = (AMPMETER_RADIUS - 5) * (cos(PI * p / 2000));
    j2 = (AMPMETER_RADIUS - 5) * (sin(PI * p / 2000));
    drawLine(i, j, i2, j2, color);
  }
}

void DFRobot_ST7687S_Widgets::PasDisplay(int value) {
  setTextSize(PAS_TEXTSIZE);
  setTextColor(PAS_COLOR);
  setCursor((width / 2) - (PAS_TEXTSIZE * 2), (height / 2) - (PAS_TEXTSIZE * 4));
  print(value);
}

void DFRobot_ST7687S_Widgets::drawNormalTacho() {
  Tacho(TACHO_DEFAULT_COLOR);
}

void DFRobot_ST7687S_Widgets::drawOffRoadTacho() {
  Tacho(TACHO_OFFROAD_COLOR);
}

void DFRobot_ST7687S_Widgets::Tacho( int color) {
  int i = 0;
  int j = 0;
  int i2 = 0;
  int j2 = 0;

  bool thick_line = true;

  fillCircle(0, 0, 3, color);

  for (int p = TACHO_OPEN; p < TACHO_CLOSE; p++) {
    i = TACHO_RADIUS * (cos(PI * p / 2000));
    j = TACHO_RADIUS * (sin(PI * p / 2000));
    drawPixel(i, j, color);
  }

  for (int p = TACHO_OPEN; p < TACHO_CLOSE; p = p + 230) {
    i = TACHO_RADIUS * (cos(PI * p / 2000));
    j = TACHO_RADIUS * (sin(PI * p / 2000));
    i2 = (TACHO_RADIUS - 5) * (cos(PI * p / 2000));
    j2 = (TACHO_RADIUS - 5) * (sin(PI * p / 2000));  
    if (thick_line) {
      setLineWidth(3);
      thick_line = false;    
    }
    else {
      setLineWidth(1);
      thick_line = true;
    }
    drawLine(i, j, i2, j2, color);
  }
}

void DFRobot_ST7687S_Widgets::TachoLabels() {
  int16_t label_x = 0;
  int16_t label_y = 0;

  int label_val = 0;
  bool label_line = true;

  for (int p = TACHO_OPEN; p < TACHO_CLOSE; p = p + 230) {
    if (label_line) {
      label_line = false;
      label_x = ((TACHO_RADIUS - 15) * (cos(PI * p / 2000))) + 64;
      label_y = ((TACHO_RADIUS - 17) * (sin(PI * p / 2000))) + 64;
      setTextColor(DISPLAY_WHITE);
      setTextBackground(CBC_DISPLAY_BACKCOLOR);
      setTextSize(1);
      setCursor(label_x + 60, label_y + 64);

      if (label_val == 0 ) {
        labelText(&label_x, &label_y, " ");
        labelText(&label_x, &label_y, "0");
      }
      else if (label_val == 1 ) {
        labelText(&label_x, &label_y, "1");
        labelText(&label_x, &label_y, "0");
      }
      else if (label_val == 2 ) {
        labelText(&label_x, &label_y, "2");
        labelText(&label_x, &label_y, "0");
      }
      else if (label_val == 3 ) {
        labelText(&label_x, &label_y, "3");
        labelText(&label_x, &label_y, "0");
      }
      else if (label_val == 4 ) {
        labelText(&label_x, &label_y, "4");
        labelText(&label_x, &label_y, "0");
      }
      else if (label_val == 5 ) {
        labelText(&label_x, &label_y, "5");
        labelText(&label_x, &label_y, "0");
      }
      else if (label_val == 6 ) {
        labelText(&label_x, &label_y, "6");
        labelText(&label_x, &label_y, "0");
      }
      label_val += 1;
    }
    else {
      label_line = true;
    }
  }
  labels_written = true;
}

void DFRobot_ST7687S_Widgets::TachoNeedle(uint8_t speed) {
  int _lineWidth = getLineWidth();
  setLineWidth(TACHO_NEEDLE_WIDTH);
  drawLine(needle_pos[0], needle_pos[1], needle_pos[2], needle_pos[3], CBC_DISPLAY_BACKCOLOR);
  if (speed > TACHO_MAX_SPEED)
    speed = TACHO_MAX_SPEED;

  float needle_value = map(speed, 0, TACHO_MAX_SPEED, TACHO_OPEN, TACHO_CLOSE);
  needle_pos[0] = TACHO_NEEDLE_LENGTH * (cos(PI * needle_value / 2000));
  needle_pos[1] = TACHO_NEEDLE_LENGTH * (sin(PI * needle_value / 2000));
  needle_pos[2] = 4 * (cos(PI * needle_value / 2000));
  needle_pos[3] = 4 * (sin(PI * needle_value / 2000));

  old_needle_pos[0] = needle_pos[0];
  old_needle_pos[1] = needle_pos[1];
  old_needle_pos[2] = needle_pos[2];
  old_needle_pos[3] = needle_pos[3];

  drawLine(needle_pos[0], needle_pos[1], needle_pos[2], needle_pos[3], TACHO_NEEDLE_COLOR);
  setLineWidth(_lineWidth);
  TachoLabels();
}

void DFRobot_ST7687S_Widgets::labelText(int16_t* pX, int16_t* pY, const char* ch)
{
  uint8_t       characterBuffer[32] = {0};
  uint8_t       rslt = 0;
  uint8_t       i = 0, j = 0, k = 0;
  uint8_t       var1 = 0;
  uint8_t       textWidth = 0, textHeight = 0;
  while(*ch) {
    rslt = pfCharacterFont((uint8_t*) ch, characterBuffer, &textWidth, &textHeight);
//    fillRect(*pX - cursorX, *pY - cursorY, textWidth * textSize, textHeight * textSize, textBackground);
      ch += rslt;
      Serial.print(ch);
      for(i = 0; i < textWidth; i ++) {
        var1 = characterBuffer[i];
        for(j = 0; j < 8; j ++) {
          if(var1 & (0x01 << j)) {
            for(k = 0; k < textSize; k ++) {
              //if( (!labels_written) || (isAtOldNeedle(*pX + i + k - cursorX, *pY + j - cursorY, old_needle_pos[0], old_needle_pos[1], old_needle_pos[2], old_needle_pos[3]) ) ) {
              //  drawPixel(*pX + i + k - cursorX, *pY + j - cursorY, TACHO_LABEL_COLOR);
              //}
              drawPixel(*pX + i + k - cursorX, *pY + j - cursorY, TACHO_LABEL_COLOR);
            }
          }
        }
      }
    *pX += textWidth * textSize;
  }
}
/*
bool DFRobot_ST7687S_Widgets::isAtOldNeedle(int16_t xpoint, int16_t ypoint, int16_t x0, int16_t y0, int16_t x1, int16_t y1) {
  int16_t       dx = abs(x1 - x0), dy = abs(y1 - y0);
  uint8_t       steep = 0;

  eDirection_t  eDirection = calcLineDirection(x0, y0, x1, y1);
  if(dx < dy) {
    steep = 1;
    swap_int16(x0, y0);
    swap_int16(x1, y1);
    swap_int16(dx, dy);
  }
  int8_t        dirX = (x1 - x0) > 0 ? 1 : -1;
  int8_t        dirY = (y1 - y0) > 0 ? 1 : -1;
  int16_t       endX = x0, endY = y0;
  int32_t       var1 = dy * 2;
  int32_t       var2 = (dy - dx) * 2;
  int32_t       var3 = dy * 2 -dx;

  if(steep) {
    while(endX != x1) {
      if(var3 < 0) {
        var3 += var1;
      } else {
        endY += dirY;
        var3 += var2;
      }
      // drawPixel(endY, endX, color);
      // drawPixelWidth(endY, endX, eDirection, color);
      if(lineWidth == 1) {
//        drawPixel(endY, endX, color);
        if ( (xpoint == endY) && (ypoint == endX) ) {
              Serial.println("redraw");
              return true;
            }
      }
      else if(lineWidth > 1) {
        if(eDirection == eDIRECTION_HORIZONTAL) {
          //drawHLine(endY - (lineWidth / 2), endX, lineWidth, color);
          int8_t        direction = 1;
          int16_t       var1 = endY + lineWidth;
          if(lineWidth < 0) {
            direction = -1;
          }
          for(; endY != var1; endY += direction) {
//            drawPixel(endY, endX, color);
            if( (xpoint == endY) && (xpoint == endX) ) {
              Serial.println("redraw");
              return true;
            }
          }
        }
        else {
//          drawVLine(endY, endX - (lineWidth / 2), lineWidth, color);
          int8_t        direction = 1;
          int16_t       var1 = endX + lineWidth;
          if(lineWidth < 0) {
            direction = -1;
          }
          for(; endX != var1; endX += direction) {
//            drawPixel(endY, endX, color);
            if ( (xpoint == endY) && (ypoint == endX) ) {
              Serial.println("redraw");
              return true;
            }
          }
        }
      }
      endX += dirX;
    }
  } else {
    while(endX != x1) {
      if(var3 < 0) {
        var3 += var1;
      } else {
        endY += dirY;
        var3 += var2;
      }
      // drawPixel(endX, endY, color);
      // drawPixelWidth(endX, endY, eDirection, color);
      if(lineWidth == 1) {
//        drawPixel(endX, endY, color);
        if ( (xpoint == endX) && (ypoint == endY) ) {
              Serial.println("redraw");
              return true;
            }
      }
      else if(lineWidth > 1) {
        if(eDirection == eDIRECTION_HORIZONTAL) {
//          drawHLine(endX - (lineWidth / 2), endY, lineWidth, color);
          int8_t        direction = 1;
          int16_t       var1 = endX + lineWidth;
          if(lineWidth < 0) {
            direction = -1;
          }
          for(; endX != var1; endX += direction) {
//            drawPixel(endX, endY, color);
            if ( (xpoint == endX) && (ypoint == endY) ){
              Serial.println("redraw");
              return true;
            }
          }
        }
        else {
//          drawVLine(endX, endY - (lineWidth / 2), lineWidth, color);
          int8_t        direction = 1;
          int16_t       var1 = endY + lineWidth;
          if(lineWidth < 0) {
            direction = -1;
          }
          for(; endY != var1; endY += direction) {
//            drawPixel(endX, endY, color);
            if ( (xpoint == endX) && (ypoint == endY) ) {
              Serial.println("redraw");
              return true;
            }
          }
        }
      }
      endX += dirX;
    }
  }
  return false;
}
*/
unsigned int DFRobot_ST7687S_Widgets::rainbow(byte value) {
  // Value is expected to be in range 0-127
  // The value is converted to a spectrum colour from 0 = blue through to 127 = red

  byte red = 0;                                                                 // Red is the top 5 bits of a 16 bit colour value
  byte green = 0;                                                               // Green is the middle 6 bits
  byte blue = 0;                                                                // Blue is the bottom 5 bits

  byte quadrant = value / 32;

  if (quadrant == 0) {
    blue = 31;
    green = 2 * (value % 32);
    red = 0;
  }
  if (quadrant == 1) {
    blue = 31 - (value % 32);
    green = 63;
    red = 0;
  }
  if (quadrant == 2) {
    blue = 0;
    green = 63;
    red = value % 32;
  }
  if (quadrant == 3) {
    blue = 0;
    green = 63 - 2 * (value % 32);
    red = 31;
  }
  return (red << 11) + (green << 5) + blue;
}