/*This code (excluding external libraries) is the intellectual property of Team 1127 Lotus Robotics*/
#include <Keypad.h>
#include <Arduino.h>
#include <avr/pgmspace.h>
#include <TVout.h>
#include <TVoutfonts.h>
#include <fontALL.h>
#include "lyle.h"
#include "testimage.h"

#define led1 2
#define led2 11
#define led3 9
#define segbl 44
#define segtl 42
#define segm 40
#define segt 38
#define segtr 36
#define segbr 34
#define segb 32
#define MAX_LOOP 76800
#define DELAY_TIME 6
#define RED_MIN 36
#define RED_MAX 197
#define GREEN_MIN 51
#define GREEN_MAX 175
#define BLUE_MIN 120
#define BLUE_MAX 220
#define PIXELS_IN_IMAGE 2700
#define IMAGE_ROWS 45
#define IMAGE_COLS 60
/*#define RED_MIN 36
#define RED_MAX 116
#define GREEN_MIN 51
#define GREEN_MAX 211
#define BLUE_MIN 120
#define BLUE_MAX 243*/
#define VERTICAL_LINE_START 22
#define VSCALE 2
#define KEYPAD_ROWS 6
#define KEYPAD_COLS 4
#define CONVEX_HULL_ANGLE_CHANGE .1

#define DISABLE_CAMERA
#define SPLASH_SCREEN

#ifndef DISABLE_CAMERA
  byte colorImage [2700][3];//Forum users: change this line to "int x;" or whatever you see fit to remove ram usage from the equation
#endif
byte binaryImage[45][60];
byte currentPixel[3];
byte segs[7] = {segbl, segtl, segm, segt, segtr, segbr, segb};
byte segPatterns[10][7] = {{LOW, LOW, HIGH, LOW, LOW, LOW, LOW},     //0
                          {HIGH, HIGH, HIGH, HIGH, LOW, LOW, HIGH}, //1
                          {LOW, HIGH, LOW, LOW, LOW, HIGH, LOW},    //2
                          {HIGH, HIGH, LOW, LOW, LOW, LOW, LOW},    //3
                          {HIGH, LOW, LOW, HIGH, LOW, LOW, HIGH},   //4
                          {HIGH, LOW, LOW, LOW, HIGH, LOW, LOW},    //5
                          {LOW, LOW, LOW, LOW, HIGH, LOW, LOW},     //6
                          {HIGH, HIGH, HIGH, LOW, LOW, LOW, HIGH},  //7
                          {LOW, LOW, LOW, LOW, LOW, LOW, LOW},      //8
                          {HIGH, LOW, LOW, LOW, LOW, LOW, LOW}};    //9
char keys[KEYPAD_ROWS][KEYPAD_COLS] = {
  {'1', '2', '3', '4'},
  {'5', '6', '7', '8'},
  {'9', 'a', 'b', 'c'},
  {'d', 'e', 'f', 'g'},
  {'h', 'i', 'j', 'k'},
  {'+', '-', 'p', 'n'}
};
byte rowPins[KEYPAD_ROWS] = {41, 39, 37, 35, 33, 31};
byte colPins[KEYPAD_COLS] = {49, 47, 45, 43};
Keypad remote = Keypad( makeKeymap(keys), rowPins, colPins, KEYPAD_ROWS, KEYPAD_COLS);
byte numberOfBlobs;

TVout TV;
#ifdef SPLASH_SCREEN
  TVout warmUp;
#endif

void printNumberTo7Seg(byte num);


void setup()
{
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);
  Serial.begin(115200);
  remote.setDebounceTime(50);
  #ifdef SPLASH_SCREEN
    warmUp.begin(NTSC);
    warmUp.select_font(font8x8ext);
    warmUp.println("Team 1127 Lotus\nRobotics Vision\nTracking\nDiagnostics\nUtility");
    warmUp.draw_rect(0, 50, warmUp.hres()-1, 10, WHITE, BLACK);
    for (byte i = 0; i < warmUp.hres(); i++)
    {
      warmUp.draw_column(i, 50, 60, WHITE);
      warmUp.delay(1);
    }
    warmUp.end();
  #endif
  TV.begin(NTSC);//, 64, 45);
  TV.select_font(font4x6);
  pinMode(segbl, OUTPUT);
  pinMode(segtl, OUTPUT);
  pinMode(segm, OUTPUT);
  pinMode(segt, OUTPUT);
  pinMode(segtr, OUTPUT);
  pinMode(segbr, OUTPUT);
  pinMode(segb, OUTPUT);
  digitalWrite(segbl, HIGH);
  digitalWrite(segtl, HIGH);
  digitalWrite(segt, HIGH);
  digitalWrite(segtr, HIGH);
  digitalWrite(segbr, HIGH);
  digitalWrite(segb, HIGH);
  digitalWrite(segm, HIGH);
  printNumberTo7Seg(0);
  Serial.println(micros());
  for (int i = 0; i < 2700; i++)
  {
    #ifdef DISABLE_CAMERA
      currentPixel[0] = pgm_read_byte_near(&(testimage[i][0]));
      currentPixel[1] = pgm_read_byte_near(&(testimage[i][1]));
      currentPixel[2] = pgm_read_byte_near(&(testimage[i][2]));
    #else
      currentPixel[0] = random(256);
      currentPixel[1] = random(256);
      currentPixel[2] = random(256);
    #endif
    if (currentPixel[0] >= RED_MIN && currentPixel[0] <= RED_MAX && currentPixel[1] >= GREEN_MIN && currentPixel[1] <= GREEN_MAX && currentPixel[2] >= BLUE_MIN && currentPixel[0] <= BLUE_MAX)
      binaryImage[i/60][i%60] = true;
    else
      binaryImage[i/60][i%60] = false;
  }
  printNumberTo7Seg(1);
  removeSmallObjects(false);
  printNumberTo7Seg(2);
  dumpFullStatusToTV();
  printNumberTo7Seg(3);
  TV.bitmap(72, 0, lyle);
  randomSeed(analogRead(0));
  blobifyBinaryImage2();
  removeBlobsSmallerThan(10);
  reorderBlobs();
  dumpFullStatusToTV();
  dumpBinaryImageToSerial();

}
void loop()
{
  #ifndef DISABLE_LYLE_BLINKS
    if (random(1000000) == 0 || analogRead(4) > 10  )
    {
      TV.draw_rect(90, 8, 3, 5, WHITE, WHITE);
      TV.draw_rect(101, 5, 3, 3, WHITE, WHITE);
      TV.draw_rect(102, 4, 1, 5, WHITE, WHITE);
      delay(100);
      TV.draw_rect(90, 8, 3, 5, BLACK, BLACK);
      TV.draw_rect(101, 5, 3, 3, BLACK, BLACK);
      TV.draw_rect(102, 4, 1, 5, BLACK, BLACK);
      randomSeed(analogRead(0));
    }
  #endif
  //TV.print(remote.getKey());
  for (byte i = 2; i < numberOfBlobs+2; i++)
  {
    highlightSpecifiedBlobOnTV(i);
    delay(500);
  }
}

void dumpBinaryImageToSerial()
{
  for (byte i = 0; i < 45; i++)
  {
    for (byte k = 0; k < 60; k++)
      if (binaryImage[i][k])
        Serial.print(binaryImage[i][k]);
      else
        Serial.print(' ');
    Serial.println();
  }
}

void dumpBinaryImageToTV()
{
  for (byte i = 0; i < 45; i++)
  {
    for (byte k = 0; k < 60; k++)
      if (binaryImage[i][k])
        TV.set_pixel(k+1, i+1, WHITE);
      else
        TV.set_pixel(k+1, i+1, BLACK);
  }
  /*TV.set_pixel(0, 0,WHITE);
  TV.set_pixel(63, 0,WHITE);
  TV.set_pixel(0, 44,WHITE);
  TV.set_pixel(63, 44,WHITE);
  */
}

void dumpFullStatusToTV()
{
  dumpBinaryImageToTV();
  TV.draw_rect(0,0,61,46,1);
  TV.set_cursor(0,48);
  TV.print("Team 1127\n");
  #ifdef DISABLE_CAMERA
    TV.print("Not using Axis camera. Using\ninternal test images instead\n");
  #endif

}

void highlightSpecifiedBlobOnTV(byte specifiedBlob)
{
  for (byte i = 0; i < IMAGE_ROWS; i++)
    for (byte k = 0; k < IMAGE_COLS; k++)
      if (binaryImage[i][k] == specifiedBlob)
        TV.set_pixel(k+1, i+1, WHITE); //+1s account for 1 pixel border around camera feed on the screen
      else
        TV.set_pixel(k+1, i+1, BLACK);
}


byte retriveColorValueFromFlash(int pixelNo, byte color)
{
  return pgm_read_byte_near(&(testimage[pixelNo][color]));
}

void removeSmallObjects(boolean useAll8Corners)
{
  byte row, col;
  for (int i = 0; i < PIXELS_IN_IMAGE; i++)
  {
    row = i/60;
    col = i%60;
    if (binaryImage[row][col])
    {
      if (useAll8Corners)
      {
      }
      else
      {
        if ((row == 0 || !binaryImage[row-1][col]) && (row == 45 || !binaryImage[row+1][col]) && (col == 0 || !binaryImage[row][col-1]) && (col == 60 || !binaryImage[row][col+1]))
          binaryImage[row][col] = false;
      }
    }
    else
      continue;
  }
}

/** do not use */
void removeNotSoSmallObjects()
{
  Serial.println("WARNING: Using removeNotSoSmallObjects.\nResults may be invalid.");
  byte row, col;
  for (int i = 0; i < PIXELS_IN_IMAGE; i++)
  {
    row = i/60;
    col = i%60;
    if (binaryImage[row][col])
    {
      binaryImage[row][col] = false;
      removeSmallObjects(false);
      binaryImage[row][col] = true;
    }
  }
  removeSmallObjects(false);
}

void blobifyBinaryImage()
{
  byte blobIndex = 2; // 0 means "black" pixel, 1 means "white" pixel, >2 means pixel belongs the the same group as others with its id
  for (byte row = 0; row < 45; row++)
  {
    for (byte col = 0; col < 60; col++)
    {
      if (binaryImage[row][col])
      {
        //Find out if this pixel belongs to the same blob by checking the 8 adjacesnt squares for a pixel with the same blobIndex
        for ( char offsetRow = -1; offsetRow <= 1; offsetRow++)
        {
          if (row + offsetRow < 0 || row + offsetRow > 45)
            continue;
          for (char offsetCol = -1; offsetCol <= 1; offsetCol++)
          {
            if (col + offsetCol < 0 || col + offsetCol > 60)
              continue;
            byte tempPixel = binaryImage[row+offsetRow][col+offsetCol];
            if (tempPixel == 0 || tempPixel == 1 || tempPixel == blobIndex)
            {
              //Not a different blob
            }
            else
            {
            }}}}}}}
void blobifyBinaryImage2()
{
  byte blobId = 2;
  byte currentPixel;
  for (byte row = 0; row < IMAGE_ROWS; row++)
  {
    for (byte col = 0; col < IMAGE_COLS; col++)
    {
      currentPixel = binaryImage[row][col];
      if (currentPixel == 1)
      {
        recursivelyFloodPixelIdToAdjacentPixels(row, col, blobId);
        blobId++;
      }
    }
  }
  numberOfBlobs = blobId - 2;
}
void recursivelyFloodPixelIdToAdjacentPixels(byte row, byte col, byte blobId)
{
  binaryImage[row][col] = blobId;
  for (char offsetRow = -1; offsetRow <= 1; offsetRow++)
  {
    if (row + offsetRow < 0 || row + offsetRow > IMAGE_ROWS)
      continue; //Prevents it from going outside the picture
    for (char offsetCol = -1; offsetCol <= 1; offsetCol++)
    {
      if (offsetRow == 0 && offsetCol == 0)
        continue;//It only checks adjacent pixels, not the one it is working with
      if (col + offsetCol < 0 || col + offsetCol > IMAGE_COLS)
        continue;//Prevents it from going outside the picture
      byte newPixel = binaryImage[row + offsetRow][col + offsetCol];
      if (newPixel == 1)
        recursivelyFloodPixelIdToAdjacentPixels(row+offsetRow, col+offsetCol, blobId);
    }
  }
}

void removeBlobsSmallerThan(byte minBlobSize)
{
  byte blobSizes[numberOfBlobs + 2];// The first two will not be true blobs, so they are safe to ignore
  for (byte i = 0; i < numberOfBlobs+2; i++)
    blobSizes[i] = 0;
  for (byte row = 0; row < IMAGE_ROWS; row++)
    for (byte col = 0; col < IMAGE_COLS; col++)
      blobSizes[ binaryImage[row][col] ] ++;// Counts the population of each blob, and puts it in blobSizes
  /*for (byte i = 0; i < numberOfBlobs+2; i++)
  {
    Serial.print(i);
    Serial.print(": ");
    Serial.println(blobSizes[i]);
  }*/
  if (blobSizes[1] != 0)
  {
    TV.print("Assertion failed! blobSizes[1] = ");
    TV.println((int)blobSizes[1]);
  }
  for (byte blobId = 2; blobId < numberOfBlobs + 2; blobId++)
    if (blobSizes[blobId] < minBlobSize)
      for (byte row = 0; row < IMAGE_ROWS; row++)
        for (byte col = 0; col < IMAGE_COLS; col++)
          if (binaryImage[row][col] == blobId)
            binaryImage[row][col] = 0;
  // Blobs are now inconsistently indexed
}

void reorderBlobs()
{
  for (byte row = 0; row < IMAGE_ROWS; row++)
    for (byte col = 0; col < IMAGE_COLS; col++)
      if (binaryImage[row][col])
        binaryImage[row][col] = 1;
  blobifyBinaryImage2();
}

void convexHull(byte blobId)
{
  char currentPixel[] = {-1, -1};
}

void printNumberTo7Seg(byte num)
{
  for (byte i = 0; i < 7; i++)
    digitalWrite(segs[i], segPatterns[num % 10][i]);
}