#include <Arduino.h>
#include <Wire.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <FluxGarage_RoboEyes.h>

#include "settings.h"
#include "motors.h"
#include "rgb.h"


Adafruit_SSD1306 display(OLED_WIDTH, OLED_HEIGHT, &Wire, -1);
RoboEyes<Adafruit_SSD1306> roboEyes(display);

RGB rgb;
MOTORS motors; 

TaskHandle_t TaskOled;
TaskHandle_t TaskRgb;
TaskHandle_t TaskMotors;

enum RobotEmotion {
  E_NEUTRAL,
  E_HAPPY,
  E_SAD,
  E_ANGRY
};

volatile RobotEmotion currentState = E_NEUTRAL;


void oledSetup(){
  Wire.begin(SDA_PIN, SCL_PIN);
  delay(250); 
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  roboEyes.begin(OLED_WIDTH, OLED_HEIGHT, 50);
  roboEyes.setAutoblinker(ON, 3, 2);
  roboEyes.setIdleMode(ON, 2, 2);
}

void oledTask(void * pvParameters) {
  RobotEmotion lastEmotion = E_NEUTRAL; 

  for(;;) {
    if (currentState != lastEmotion) {
      lastEmotion = currentState;
      
      switch(currentState) {
        case E_HAPPY: 
          roboEyes.setMood(HAPPY);
          break;
        case E_SAD:   
          roboEyes.setMood(TIRED); 
          break;
        case E_ANGRY: 
          roboEyes.setMood(ANGRY);
          break;
        default:    
          roboEyes.setMood(DEFAULT);
          break;
      }
    }

    roboEyes.update(); 
    
    vTaskDelay(20 / portTICK_PERIOD_MS); 
  }
}

void rgbTask(void * pvParameters) {
  RobotEmotion lastEmotion = E_NEUTRAL; 
  
  rgb.setColorRGB(0, 0, 0, true);

  for(;;) {
    if (currentState != lastEmotion) {
      
      switch(currentState) {
        case E_HAPPY:
          rgb.fadeColor(255, 250, 0, 500); 
          break;
        case E_SAD:
          rgb.fadeColor(118, 116, 237, 500);
          break;
        case E_ANGRY:
          rgb.fadeColor(255, 0, 0, 500);
          break;
        case E_NEUTRAL:
        default:
          rgb.fadeColor(0, 0, 0, 500);
          break;
      }
      lastEmotion = currentState;
    }

    switch(currentState) {
      case E_HAPPY:
        rgb.blink(1, 300); 
        break;
      case E_SAD:
        rgb.breathe(1, 40); 
        break;
      case E_ANGRY:
        rgb.blink(1, 300);
        break;
      case E_NEUTRAL:
      default:
        break;
    }

    vTaskDelay(20 / portTICK_PERIOD_MS);
  }
}

void motorsTask(void * pvParameters) {
  for(;;) {
    switch(currentState) {
      case E_HAPPY:
        motors.leftTurn(100, 1000);
        motors.rightTurn(100, 1000);
        break;
      case E_SAD:
        motors.stop();
        break;
      case E_ANGRY:
        motors.forward(100, 1000);
        motors.backward(100, 1000);
        break;
      default:
        motors.stop();
        break;
    }
    vTaskDelay(10 / portTICK_PERIOD_MS);
  }
}

void setup() {

  rgb.init();
  motors.init();
  oledSetup();
  
  xTaskCreate(oledTask, "OLED Task", 4096, NULL, 1, &TaskOled);
  xTaskCreate(rgbTask, "RGB Task", 2048, NULL, 1, &TaskRgb);
  xTaskCreate(motorsTask, "Motors Task", 2048, NULL, 1, &TaskMotors);
  
  currentState = E_NEUTRAL; 
  vTaskDelay(5000 / portTICK_PERIOD_MS);
}

void loop() {
  currentState = E_HAPPY;
  vTaskDelay(5000 / portTICK_PERIOD_MS);
  
  currentState = E_NEUTRAL;
  vTaskDelay(5000 / portTICK_PERIOD_MS);
  
  currentState = E_ANGRY;
  vTaskDelay(5000 / portTICK_PERIOD_MS);
  
  currentState = E_SAD;
  vTaskDelay(5000 / portTICK_PERIOD_MS);
}