/**Code credits to TARO YOSHINO from Youtube, sonydevworld, and Elegoo.  I also wrote some I guess.**/

#include <Camera.h>
#include <File.h>
#include <Flash.h>
#include <DNNRT.h>
#include <SDHCI.h>
#include <Servo.h>

// include the library code:
#include <LiquidCrystal.h>

// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(7, 8, 9, 10, 11, 12);

#define BAUDRATE 115200

#define OFFSET_X 48
#define OFFSET_Y 8
#define BOX_WIDTH 224
#define BOX_HEIGHT 224
#define DNN_IMG_WIDTH 28
#define DNN_IMG_HEIGHT 28
#define TOTAL_PICTURE_COUNT     (9)

SDClass theSD;
DNNRT dnnrt;
DNNVariable input(DNN_IMG_WIDTH*DNN_IMG_HEIGHT);
int take_picture_count = 0;
int led_color=0;

int greenLedPin = 6;
int yellowLedPin = 5;
int redLedPin = 4;
int servoPin = 2;

String gStringResult = "";

Servo myservo;  // create servo object to control a servo

int pos = 0;    // variable to store the servo position

void printError(enum CamErr err)
{
  Serial.print("Error: ");
  switch (err)
    {
      case CAM_ERR_NO_DEVICE:
        Serial.println("No Device");
        break;
      case CAM_ERR_ILLEGAL_DEVERR:
        Serial.println("Illegal device error");
        break;
      case CAM_ERR_ALREADY_INITIALIZED:
        Serial.println("Already initialized");
        break;
      case CAM_ERR_NOT_INITIALIZED:
        Serial.println("Not initialized");
        break;
      case CAM_ERR_NOT_STILL_INITIALIZED:
        Serial.println("Still picture not initialized");
        break;
      case CAM_ERR_CANT_CREATE_THREAD:
        Serial.println("Failed to create thread");
        break;
      case CAM_ERR_INVALID_PARAM:
        Serial.println("Invalid parameter");
        break;
      case CAM_ERR_NO_MEMORY:
        Serial.println("No memory");
        break;
      case CAM_ERR_USR_INUSED:
        Serial.println("Buffer already in use");
        break;
      case CAM_ERR_NOT_PERMITTED:
        Serial.println("Operation not permitted");
        break;
      default:
        break;
    }
}

void CamCB(CamImage img){
  if(!img.isAvailable()) {
    Serial.println("No image available");  
    return;
  }
  img.convertPixFormat(CAM_IMAGE_PIX_FMT_RGB565);
  gStringResult="";
  int x;
  int index=0;
  static uint8_t label[9]={0,1,2,3,4,5,6,7,8};

  uint16_t*buf=(uint16_t*)img.getImgBuff();
  float* input_buffer = input.data();
  for (int i =0; i <DNN_IMG_WIDTH * DNN_IMG_HEIGHT; ++i, ++buf){
      input_buffer[i] = (float)(((*buf & 0x07E0) >> 5) << 2) ;  // extract green
  
  }

  Serial.println("DNN forward");
  dnnrt.inputVariable(input, 0);
  dnnrt.forward();
  DNNVariable output = dnnrt.outputVariable(0);
  float max_value =0.0;
  for(int i=0; output.size() > i; ++i){
    if(output[i] > max_value) {
      max_value = output[i];
      index = i;
    }
  }

  Serial.print("Result : ");
  Serial.println(label[index] + " (" + String(output[index]) + ")");
  if(label[index]!=10){
    gStringResult+=String(label[index]);
    }
    else{
      gStringResult+=String(" ");
  }

  digitalWrite(greenLedPin, LOW);  //This turns off the LED light
  digitalWrite(yellowLedPin, LOW);  //This turns off the LED light
  digitalWrite(redLedPin, LOW);  //This turns off the LED light
  //Displaying like this because my labels in my NN are bad and I should feel bad
  switch(index){
    
    case 0: poisonIvy(); break;
    case 1: displayResult("Turkey Tail", 2); break; //No dangerous look-alikes
    case 2: displayResult("Grass",0); break;        //Maybe fine but let's just not eat grass, ok?
    case 3: displayResult("Oyster",1); break;       //There are dangerous look alikes
    case 4: displayResult("Dog", -1); break;        //Don't eat dogs.  Not cool.
    case 5: displayResult("House", -1); break;      //This isn't Hansel and Gretel
    case 6: displayResult("Airplane", -1); break;   //Didn't some guy eat an airplane sometime?  Maybe it was a bike. That'd make more sense.
    case 7: displayResult("Fig Tree", 2); break;
    case 8: displayResult("Morel",1); break; 
  }

  //Wait a bit so we're not just spammed with readings
  delay(5000); 
}

void displayResult(String plant, int ledColor){
  //0 - Red (unsafe to eat)
  //1 - Yellow (maybe safe to eat - need to look into it more)
  //2 - Green (safe to eat)

  //display.println(plant);
  lcd.clear();
  lcd.print(plant);
  if(ledColor==2)
    digitalWrite(greenLedPin, HIGH);
  else if(ledColor==1)
    digitalWrite(yellowLedPin, HIGH);
  else if(ledColor==0)
    digitalWrite(redLedPin, HIGH);
  
}

void poisonIvy(){
  displayResult("POISON IVY!",0);
  //Spray the poison ivy!!

  for (pos = 0; pos <= 90; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
    Serial.print("POS1: ");
    Serial.println(pos);
  }
  delay(4000);  //Spraying poison ivy for this time
  for (pos = 90; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
    Serial.print("POS2: ");
    Serial.println(pos);
  }  
}


void setup() {
  Serial.begin(BAUDRATE);

  pinMode(greenLedPin, OUTPUT);
  digitalWrite(greenLedPin, LOW);  //This turns off the LED light
  pinMode(yellowLedPin, OUTPUT);
  digitalWrite(yellowLedPin, LOW);  //This turns off the LED light
  pinMode(redLedPin, OUTPUT);
  digitalWrite(redLedPin, LOW);  //This turns off the LED light
  myservo.attach(servoPin);  // attaches the servo 

  
  Serial.begin(BAUDRATE);
  while (!Serial)
    {
      ; /* wait for serial port to connect. Needed for native USB port only */
    }
  // set up the LCD's number of columns and rows: 
  lcd.begin(16, 2);
  lcd.setCursor(0,0);
  lcd.print("Identify Plants!");


  CamErr err;

  Serial.println("Prepare camera");
  err = theCamera.begin();
  if (err != CAM_ERR_SUCCESS)
    {
      printError(err);
    }


  /* Initialize SD */
  while (!theSD.begin()) 
    {
      /* wait until SD card is mounted. */
      Serial.println("Insert SD card.");
    }
    
  Serial.println("Loading network model");
  File nnbfile = theSD.open("model.nnb",FILE_READ);
  if(!nnbfile){
    Serial.println("nnb not found");
    while(1);
  }
  Serial.println("Initialize DNNRT");
  int ret = dnnrt.begin(nnbfile);
  if(ret<0){
    Serial.println("DNNRT initialize error");
    while(1);
    
  }
  
  /* Start video stream.
   * If received video stream data from camera device,
   *  camera library call CamCB.
   */

  Serial.println("Start streaming");
  err = theCamera.startStreaming(true, CamCB);
  if (err != CAM_ERR_SUCCESS)
    {
      printError(err);
    } else{
      Serial.println("Camera should be doing something now");
    }
  Serial.println("Set still picture format");
  err = theCamera.setStillPictureImageFormat(
     DNN_IMG_WIDTH,
     DNN_IMG_HEIGHT,
     CAM_IMAGE_PIX_FMT_JPG);
  if (err != CAM_ERR_SUCCESS)
    {
      printError(err);
    }
}

//Logic is in the camera stream instead
void loop(){}