#include <Arduino.h>
#include "motors.h"
#include "rgb.h"

#include "oled.h"
#include "animations.h"


RGB rgb;
MOTORS motors; 
OLED oled;


TaskHandle_t TaskOled;
TaskHandle_t TaskRgb;
TaskHandle_t TaskMotors;

enum State {
  NEUTRAL,
  HAPPY,
  SAD,
  ANGRY
};

volatile State currentState = NEUTRAL;

void oledTask(void * pvParameters) {
  for(;;) {
    switch(currentState) {
      case HAPPY:
        oled.showAnimation(animacja1[0], animacja1FrameCount, animacja1Height, animacja1Width, 160);
        break;
      case SAD:
        oled.showAnimation(animacja2[0], animacja2FrameCount, animacja2Height, animacja2Width, 160);
        break;
      case ANGRY:
        oled.showMessage("angry");
        break;
      default:
        oled.showMessage("neutral");
        break;
    }
    vTaskDelay(10 / portTICK_PERIOD_MS); 
  }
}

void rgbTask(void * pvParameters) {
  for(;;) {
    switch(currentState) {
      case HAPPY:
        rgb.setColorRGB(255, 250, 0, 100); 
        rgb.blink(3, 300);
        break;
      case SAD:
        rgb.breathe(3, 40); 
        break;
      case ANGRY:
        rgb.setColorRGB(255, 0, 0, 100); 
        rgb.blink(3, 300);
        break;
      default:
        rgb.setColorRGB(0, 0, 0, true); 
        break;
    }
    vTaskDelay(10 / portTICK_PERIOD_MS);
  }
}


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

void setup() {

  oled.init();
  rgb.init();
  motors.init();
  
  xTaskCreate(oledTask, "OLED Task", 4096, NULL, 1, NULL);
  xTaskCreate(rgbTask, "RGB Task", 2048, NULL, 1, NULL);
  xTaskCreate(motorsTask, "RGB Task", 2048, NULL, 1, NULL);
  
  currentState = NEUTRAL; 
}

void loop() {
  currentState = HAPPY;
  delay(5000);
  currentState = NEUTRAL;
  delay(5000);
  currentState = ANGRY;
  delay(5000);
  currentState = SAD;
  delay(5000);
}