//Importing Libs
#include <LiquidCrystal.h>
#include <Servo.h>
#include <TimedAction.h>

//Defineing Constant Vars
#define Rx 0
#define Tx 1 
#define S1 2
#define S0 3
#define LED 4
#define lefthandpin 5
#define righthandpin 6
#define tailpin 7
#define MotorsEnable 8
#define COLOROUT 54
#define S2 55
#define S3 56

#define D7_1 23
#define D6_1 25
#define D5_1 27
#define D4_1 29
#define E_1 31
#define RS_1 33

#define D7_2 35
#define D6_2 37
#define D5_2 39
#define D4_2 41
#define E_2 43
#define RS_2 45

#define Ultra_TRIG 51
#define Ultra_ECHO 53

#define FR1 52
#define FR2 50

#define FL1 24
#define FL2 22

#define BR1 48
#define BR2 46

#define BL1 28
#define BL2 26

//Values for colors
#define red_max_red 1
#define red_min_red 1
#define red_max_blue 1
#define red_min_blue 1
#define red_max_green 1
#define red_min_green 1

#define blue_max_red 1
#define blue_min_red 1
#define blue_max_blue 1
#define blue_min_blue 1
#define blue_max_green 1
#define blue_min_green 1

#define green_max_red 1
#define green_min_red 1
#define green_max_blue 1
#define green_min_blue 1
#define green_max_green 1
#define green_min_green 1

#define pink_max_red 1
#define pink_min_red 1
#define pink_max_blue 1
#define pink_min_blue 1
#define pink_max_green 1
#define pink_min_green 1

#define yellow_max_red 1
#define yellow_min_red 1
#define yellow_max_blue 1
#define yellow_min_blue 1
#define yellow_max_green 1
#define yellow_min_green 1

#define cyan_max_red 1
#define cyan_min_red 1
#define cyan_max_blue 1
#define cyan_min_blue 1
#define cyan_max_green 1
#define cyan_min_green 1

#define purple_max_red 1
#define purple_min_red 1
#define purple_max_blue 1
#define purple_min_blue 1
#define purple_max_green 1
#define purple_min_green 1

#define white_max_red 1
#define white_min_red 1
#define white_max_blue 1
#define white_min_blue 1
#define white_max_green 1
#define white_min_green 1

#define black_max_red 1
#define black_min_red 1
#define black_max_blue 1
#define black_min_blue 1
#define black_max_green 1
#define black_min_green 1

//Frequency For The Color Sensor
int redFrequency = 0;
int greenFrequency = 0;
int blueFrequency = 0;

// Servo Motors Variables
int lefthandpos = 0;
int righthandpos = 0;
int tailpos = 0;

//UltraSonic Variables
int duration;
int cm;


//Classing
LiquidCrystal lcd_1(RS_1, E_1, D4_1, D5_1, D6_1, D7_1);
LiquidCrystal lcd_2(RS_2, E_2, D4_2, D5_2, D6_2, D7_2);
Servo righthand;
Servo lefthand;
Servo tail; 

//Cat Condistions Moniters
int HP = 100;
int HP_timer = 20;
int heal_delay = 30;
int Food = 100;
int Food_timer = 60;
int feed_delay = 30;
int Pet = 100;
int Pet_timer = 90;
int pet_delay = 60;
int Play = 100;
int Play_timer = 360;
bool playing = false;
int Mood = 5;
int Mood_timer = 450;
int Stats;
int Secret;
int play_score;
int tries;
int t;
int cc;
bool ccheck;
int face = 1;


//Threading 
TimedActio servos = (1111,updateservo);
TimedActio wall = (1111,wall_avoid);
TimedActio cols = (1111,color_commands);
TimedActio lcd2 = (1111,lcd_2_stats);
TimedActio lcd1 = (1111,set_face);

void setup() {
  // Setting Up Pins
  pinMode(9   , OUTPUT);
  pinMode(10  , OUTPUT);
  pinMode(MotorsEnable , OUTPUT);
  pinMode(FR1 , OUTPUT);
  pinMode(FR2 , OUTPUT);
  pinMode(FL1 , OUTPUT);
  pinMode(FL2 , OUTPUT);
  pinMode(BR1 , OUTPUT);
  pinMode(BR2 , OUTPUT);
  pinMode(BL1 , OUTPUT);
  pinMode(BL2,  OUTPUT);
  pinMode(S0,   OUTPUT);
  pinMode(S1,   OUTPUT);
  pinMode(S2,   OUTPUT);
  pinMode(S3,   OUTPUT);
  pinMode(COLOROUT, INPUT);
  pinMode(Ultra_TRIG, OUTPUT);
  pinMode(Ultra_ECHO, INPUT);
  tail.attach(tailpin);
  righthand.attach(righthandpin);
  lefthand.attach(lefthandpin);

  // Setting Contrast
  analogWrite(9,225);
  analogWrite(10,225);
  
  // Setting frequency-scaling to 20%
  digitalWrite(S0,HIGH);
  digitalWrite(S1, LOW);
  digitalWrite(LED,LOW);

  // Starting/Reseting Proccesses & Components
  lcd_1.begin(16,2);
  lcd_2.begin(16,2);
  lcd_1.clear();
  lcd_2.clear();
  Serial.begin(9600);
}

void loop() {
  Serial.println("New Loop");
  updateservo();
  wall_avoid();
  color_commands();
  lcd_2_stats();
  set_face();
  
  while (playing) {
    Secret = random(9);
    Serial.println("Secret: ");
    Serial.println(Secret);
    play_score = 0;
    tries = 0;
    t = 5 - tries;
    while (tries <= 4 and play_score < 5) { 
      t = 5 - tries;
      lcd_2_color(Secret, t);
      cc = color_check();
      if (Secret == cc) {
        Serial.println("Correct");
        tries = 0;
        play_score += 1;
        Secret = random(9);
        Serial.print("Secret: ");
        Serial.println(Secret);
        Serial.print("Score: ");
        Serial.println(play_score);
        lcd_2_color(Secret, t);
      }else {
        Serial.println("Incorrect");
        tries += 1;
        Serial.print("Tries: ");
        Serial.println(t);
        lcd_2_color(Secret, t);
     }
    }
    if (play_score <= 4) {
      playing = false;
      Serial.println("FAILED");
    }else{
      playing = false;
      Serial.println("WON");
      Play += 50;
    }
  }
}

void updateservopos(int R, int L, int T = 200) {
  righthand.write(R);
  lefthand.write(L);
  if (T != 200){
    tail.write(T); 
  }
}
void updateservo() {
  righthand.write(righthandpos);
  lefthand.write(lefthandpos);
  tail.write(tailpos);
}
void wall_avoid() {
  bool wall = wall_check();
  if(wall) {
    Serial.println("Moving Back 1");
    move("back", 1);
    Serial.println("Turning RIGHT 2");
    move("right", 2);
    wall = wall_check();
    if(wall) {
      Serial.println("Turning LEFT 4");
      move("left", 4);
      wall = wall_check();
      if(wall) {
        Serial.println("Turning LEFT 2");
        move("left", 2);
      }
    }
  }
}
int ultra() {
  digitalWrite(Ultra_TRIG, HIGH);
  delayMicroseconds(1);
  digitalWrite(Ultra_TRIG, LOW);
  duration = pulseIn(Ultra_ECHO, HIGH);  
  cm = duration/58;
  Serial.print("Cm: ");
  Serial.println(cm);
  return cm;
}
bool wall_check() {
  if(ultra() <= 5) {
    return true;
  }else{
    return false;
  }
}

// Color List
// 1 RED 
// 2 BLUE
// 3 GREEN
// 4 PINK 
// 5 YELLOW
// 6 CYAN
// 7 PURPLE
// 8 WHITE
// 9 BLACK
// 0 NONE
//int color_check() {
//  ccheck = true;
//  Serial.println("Input a color");
//  while (ccheck) { 
//    if(Serial.available()) {
//      int i = Serial.parseInt();
//      Serial.read();
//      Serial.println(i);
//      return i;
//    }
//  }
//}
int color_check() {
  int r = red();
  int g = green();
  int b = blue();

  if(red_max_red > r and red_min_red < r and red_max_green > g and red_min_green < g and red_max_blue > b and red_min_blue < b) {
//    Serial.println("RED");
    return 1;
  }else if(blue_max_red > r and blue_min_red < r and blue_max_green > g and blue_min_green < g and blue_max_blue > b and blue_min_blue < b) {
//    Serial.println("BLUE");
    return 2;
  }else if(green_max_red > r and green_min_red < r and green_max_green > g and green_min_green < g and green_max_blue > b and green_min_blue < b) {
//    Serial.println("GREEN");
    return 3;
  }else if(pink_max_red > r and pink_min_red < r and pink_max_green > g and pink_min_green < g and pink_max_blue > b and pink_min_blue < b) {
//    Serial.println("PINK");
    return 4;
  }else if(yellow_max_red > r and yellow_min_red < r and yellow_max_green > g and yellow_min_green < g and yellow_max_blue > b and yellow_min_blue < b) {
//    Serial.println("YELLOW");
    return 5;
  }else if(cyan_max_red > r and cyan_min_red < r and cyan_max_green > g and cyan_min_green < g and cyan_max_blue > b and cyan_min_blue < b) {
//    Serial.println("CYAN");
    return 6;
  }else if(purple_max_red > r and purple_min_red < r and purple_max_green > g and purple_min_green < g and purple_max_blue > b and purple_min_blue < b) {
//    Serial.println("PURPLE");
    return 7;
  }else if(white_max_red > r and white_min_red < r and white_max_green > g and white_min_green < g and white_max_blue > b and white_min_blue < b) {
//    Serial.println("WHITE");
    return 8;
  }else if(black_max_red > r and black_min_red < r and black_max_green > g and black_min_green < g and black_max_blue > b and black_min_blue < b) {
//    Serial.println("BLACK"); 
    return 9;
  }else {
//    Serial.println("UNOWN COLOR");
    return 0;
  }
}
int red() {
  // Setting red filtered photodiodes to be read
  digitalWrite(S2,LOW); 
  digitalWrite(S3,LOW);
  // Reading the output frequency
  redFrequency = pulseIn(COLOROUT, LOW);
  // Printing the value on the serial monitor
  Serial.print("R= ");//printing name
  Serial.print(redFrequency);//printing Red color frequency
  Serial.print("  ");
  return redFrequency;
}
int green() {
  // Setting Green filtered photodiodes to be read
  digitalWrite(S2,HIGH);
  digitalWrite(S3,HIGH);
  // Reading the output frequency
  greenFrequency = pulseIn(COLOROUT, LOW);
  // Printing the value on the serial monitor
  Serial.print("G= ");//printing name
  Serial.print(greenFrequency);//printing Green color frequency
  Serial.print("  ");
  return greenFrequency;
}
int blue(){
  // Setting Blue filtered photodiodes to be read
  digitalWrite(S2,LOW);
  digitalWrite(S3,HIGH);
  // Reading the output frequency
  blueFrequency = pulseIn(COLOROUT, LOW);
  // Printing the value on the serial monitor
  Serial.print("B= ");//printing name
  Serial.print(blueFrequency);//printing Blue color frequency
  Serial.println("  ");
  return blueFrequency;
}
void color_commands() {
  int c = color_check();
  if(c == 1) {
    Serial.println("RED");
    heal(20);
  }else if(c == 2) {
    Serial.println("BLUE");
    play();
  }else if(c == 3) {
    Serial.println("GREEN");
    feed(40);
  }else if(c == 4) {
    Serial.println("PINK");
    pet(25);
  }else if(c == 5) {
    Serial.println("YELLOW");
    move("fd", 1);
  }else if(c == 6) {
    Serial.println("CYAN");
    move("bk", 1);
  }else if(c == 7) {
    Serial.println("PURPLE");
    move("rt", 1);
  }else if(c == 8) {
    Serial.println("WHITE");
    move("lt", 1);
  }else if(c == 0) {
    Serial.println("UNOWN");
    idle();
  }
}



void move(char *dir, int time) {
  int d = time*1000;
  //Serial.println(dir);
  if (dir == "fd" or dir == "forward") {
    mvfd();
    delay(d);
    stop();
  }else if(dir == "rt" or dir == "right") {
    mvrt();
    delay(d);
    stop();
  }else if(dir == "lt" or dir == "left") {
    mvlt();
    delay(d);
    stop();
  }else if(dir == "bk" or dir == "back") {
    mvbk();
    delay(d);
    stop();
  }else {
    stop();
  }
}
void mvfd() {
  digitalWrite(MotorsEnable, HIGH);
  digitalWrite(FR1,  HIGH);
  digitalWrite(FR2,  LOW ); 
  digitalWrite(FL1,  HIGH);
  digitalWrite(FL2 , LOW );
  digitalWrite(BR1,  HIGH);
  digitalWrite(BR2,  LOW );
  digitalWrite(BL1,  HIGH);
  digitalWrite(BL2,  LOW );
}
void mvrt() {
  digitalWrite(MotorsEnable, HIGH);
  digitalWrite(FR1,  HIGH);
  digitalWrite(FR2,  LOW);
  digitalWrite(FL1,  LOW);
  digitalWrite(FL2 , HIGH);
  digitalWrite(BR1,  HIGH);
  digitalWrite(BR2,  LOW);
  digitalWrite(BL1,  LOW);
  digitalWrite(BL2,  HIGH);
}
void mvlt() {
  digitalWrite(MotorsEnable, HIGH);
  digitalWrite(FR1,  LOW);
  digitalWrite(FR2,  HIGH);
  digitalWrite(FL1,  HIGH);
  digitalWrite(FL2 , LOW);
  digitalWrite(BR1,  LOW);
  digitalWrite(BR2,  HIGH);
  digitalWrite(BL1,  HIGH);
  digitalWrite(BL2,  LOW);
}
void mvbk() {
  digitalWrite(MotorsEnable, HIGH);
  digitalWrite(FR1,  LOW);
  digitalWrite(FR2,  HIGH);
  digitalWrite(FL1,  LOW);
  digitalWrite(FL2 , HIGH);
  digitalWrite(BR1,  LOW);
  digitalWrite(BR2,  HIGH);
  digitalWrite(BL1,  LOW);
  digitalWrite(BL2,  HIGH);
}
void stop() {
  digitalWrite(MotorsEnable, LOW);
}

// Stats: 
// Full = 5
// Semi Full = 4
// Mid = 3 
// Hungry = 2
// Zero = 1
int Food_stat() {
  int f = Food/20;
  return f;
}
int HP_stat() {
  int h = HP/20;
  return h;
}
int Play_stat() {
  int p = Play/20;
  return p;
}
int Pet_stat() {
  int p = HP/20;
  return p;
}
int Mood_stat() {
  int m = Mood;
  return m;
}
int get_Stats() {
  int f = Food_stat();
  int h = HP_stat();
  int p = Play_stat();
  int e = Pet_stat();
  int m = Mood_stat();
  Stats = f+h+p+e+m;
  return Stats;
}
void lcd_2_stats() {
  int h = HP;
  int f = Food;
  lcd_2.setCursor(0,0);
  lcd_2.print("HP:");
  lcd_2.print(HP);
  lcd_2.print("%");
  lcd_2.setCursor(9,0);
  lcd_2.print("FD:");
  lcd_2.print(Food);
  lcd_2.print("%");
  lcd_2.setCursor(0,1);
  lcd_2.print("Full Stats:");
  lcd_2.print(get_Stats()*4);
  lcd_2.print("%");
}



bool food_time_check() {
  if (Food_timer >= 0) {
    Food_timer += -1;
    return true;
  } else {
    dec_health(-20);
    return false;
  }
}
void dec_health(int i) {
  if(HP_timer <= 0 and Food <= 0) {
    HP += i;
    HP_timer = 30;
  }
}


void feed(int i) {
  if(feed_delay <= 0) {
    Food = i;
    Food_timer = 100;
    feed_delay = 30;
    happy();
  }else {
    Serial.println("No feed");
  }
}
void pet(int i) {
  if(pet_delay <= 0) {
    Pet = i;
    Pet_timer = 90;
    pet_delay = 30;
    happy();
    }else {
    Serial.println("No pet");
  }
}
void heal(int i) {
  if(heal_delay <= 0) {
    HP = i;
    HP_timer = 30; 
    heal_delay = 30;
    happy();
  }else {
    Serial.println("No heal");
  }
}
void play() {
  if(Play_timer <= 0) {
    playing = true;
    Play_timer = 900;
    happy();
  }else{
    Serial.println("No play");
  }
  
}
void mood() {
  Mood = random(6);
  Mood_timer = 450;
}

void lcd_1_face(int i) {
  if(i == 1){
    lcd_1.clear();
    // Draw Sad Face
    lcd_1.setCursor(0,0);
    lcd_1.print("    ________    ");
    lcd_1.setCursor(0,1);
    lcd_1.print("  --        --  ");
  }else if(i == 2) {
    lcd_1.clear();
    // Draw Normal Face
    lcd_1.setCursor(0,0);
    lcd_1.print("  ____________  ");
    lcd_1.setCursor(0,1);
    lcd_1.print("                ");
  }else if(i == 3) {
    lcd_1.clear();
    // Draw Slight Smile
    lcd_1.setCursor(0,0);
    lcd_1.print("  __        __  ");
    lcd_1.setCursor(0,1);
    lcd_1.print("    --------    ");
  }else if(i == 4) {
    lcd_1.clear();
    // Draw Big Slime
    lcd_1.setCursor(0,0);
    lcd_1.print("  -_        _-  ");
    lcd_1.setCursor(0,1);
    lcd_1.print("    --____--    ");
    }
}
void lcd_2_color(int i, int t) {
  if(i == 1){
    lcd_2.setCursor(0,0);
    lcd_2.clear();
    lcd_2.print("Red");
  }else if(i == 2) {
    lcd_2.setCursor(0,0);
    lcd_2.clear();
    lcd_2.print("Blue");          
  }else if(i == 3) {
    lcd_2.setCursor(0,0);
    lcd_2.clear();
    lcd_2.print("Green");     
  }else if (i == 4){
    lcd_2.setCursor(0,0);
    lcd_2.clear();
    lcd_2.print("Pink");
  }else if (i == 5){
    lcd_2.setCursor(0,0);
    lcd_2.clear();
    lcd_2.print("Yellow");
  }else if (i == 6){
    lcd_2.setCursor(0,0);
    lcd_2.clear();
    lcd_2.print("Cyan");
  }else if (i == 7){
    lcd_2.setCursor(0,0);
    lcd_2.clear();
    lcd_2.print("Purple");
  }else if (i == 8){
    lcd_2.setCursor(0,0);
    lcd_2.clear();
    lcd_2.print("White");
  }
  lcd_2.setCursor(0,1);
  lcd_2.print("Tries: ");
  lcd_2.print(t);
}
void set_face() {
  int i = get_Stats();
  i = i/25;
  lcd_1_face(i);
}


void happy() {
  updateservopos(rand() % 30,rand() % 30,180);
  delay(150);
  updateservopos(rand() % 10,rand() % 10);
  delay(150);  
  updateservopos(rand() % 30,rand() % 30);
  delay(150);
  updateservopos(rand() % 10,rand() % 10);
  delay(150);
  updateservopos(rand() % 30,rand() % 30,10);
  delay(150);
  updateservopos(rand() % 10,rand() % 10);
  delay(150);
  updateservopos(rand() % 30,rand() % 30);
  delay(150);
  updateservopos(rand() % 10,rand() % 10);
  delay(150);
  updateservopos(rand() % 30,rand() % 30,180);
  delay(600);
  int T;
  if(rand() % 2 == 1){
    T = 180;
  }else{
    T = 0;
  }
  updateservopos(0,0,T);
}

void idle() {
  int T;
  if(rand() % 2 == 1){
    T = 180;
  }else{
    T = 0;
  }
  updateservopos(0,0,T);
  mvfd();
  delay(100);
  mvrt();
  delay(100);
  mvlt();
  delay(100);
  mvbk();
  delay(100);
  stop();
}

//
// Currently Working on
//
//
//
//
//
//
//
//
//
// Todo: 
// Fix colors 
// Add BlueTooth
// Solve delay problom
// Add Threading
// Fix timers ticks
// 
//
//
//