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

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


#include <WiFi.h>
#include <WiFiUdp.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, TaskRgb, TaskMotors, TaskWiFi, TaskLogic;

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

volatile RobotEmotion currentState = E_NEUTRAL;


volatile int fatigueLevel = 0;
volatile bool isTired = false;


volatile int sleepTimer = 0;
volatile bool isSleepy = false;

volatile char manualMotors = '0';
volatile char manualRGB = '0';

String ipRobot = "Connection error"; 
WiFiUDP udp;
char incomingPacket[255];


/*================================= WIFI =================================*/

void wifiSetup(const char* password, const char* ssid) {
  const int udpPort = 4210; 

  WiFi.mode(WIFI_STA); 
  WiFi.begin(ssid, password);
  
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }

  WiFi.setSleep(true); 

  udp.begin(udpPort);
  
  ipRobot = WiFi.localIP().toString(); 
}


void wifiTask(void * pvParameters) {
  unsigned long emotionStartTime = 0;
  const unsigned long EMOTION_DURATION_MS = 2500; 

  for(;;) {
    int packetSize = udp.parsePacket();
    
    if (packetSize) {
      int len = udp.read(incomingPacket, 255);
      if (len > 0) {
        incomingPacket[len] = 0; 
        char cmd = incomingPacket[0]; 

        if (currentState == MANUAL) {
            if      (cmd == 'F') manualMotors = 'F';
            else if (cmd == 'B') manualMotors = 'B';
            else if (cmd == 'L') manualMotors = 'L';
            else if (cmd == 'R') manualMotors = 'R';
            else if (cmd == '0') manualMotors = '0';
        }
        
        if      (cmd == 'H') { currentState = E_HAPPY; emotionStartTime = millis(); manualMotors = '0'; }
        else if (cmd == 'S') { currentState = E_SAD; emotionStartTime = millis(); manualMotors = '0'; }
        else if (cmd == 'A') { currentState = E_ANGRY; emotionStartTime = millis(); manualMotors = '0'; }
        else if (cmd == 'N') { currentState = E_NEUTRAL; emotionStartTime = millis(); manualMotors = '0'; }
        else if (cmd == 'M') { currentState = MANUAL; manualMotors = '0'; }
      }
    }

    if (currentState != MANUAL) {
      if (millis() - emotionStartTime >= EMOTION_DURATION_MS) {
        currentState = MANUAL;
        manualMotors = '0'; 
        manualRGB = '0'; 
      }
    }

    vTaskDelay(20 / portTICK_PERIOD_MS); 
  }
}

/*================================= LOGIC =================================*/

void robotLogicTask(void * pvParameters) {
  for(;;) {
    
    if (currentState == MANUAL && manualMotors == '0') {
      sleepTimer++;
    } else {
      sleepTimer = 0;
    }

    if (sleepTimer >= 100) {
      isSleepy = true;
    } else {
      isSleepy = false;
    }

    if (currentState == MANUAL && manualMotors != '0') {
      fatigueLevel++;
    } else {
      if (fatigueLevel > 0) fatigueLevel--;
    }

    if (fatigueLevel >= 100) {
      isTired = true;
    } else if (fatigueLevel == 0) {
      isTired = false;
    }

    vTaskDelay(100 / portTICK_PERIOD_MS); 
  }
}

/*================================= OLED =================================*/
void oledSetup(){
  Wire.begin(SDA_PIN, SCL_PIN);
  delay(250); 
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  roboEyes.begin(OLED_WIDTH, OLED_HEIGHT, 50);

  // display.clearDisplay();
  // display.setTextSize(1);
  // display.setTextColor(SSD1306_WHITE);
  // display.setCursor(0, 10);
  // display.println("Connected:");
  // display.print("IP: ");
  // display.println(ipRobot);
  // display.display();
  // delay(5000); 

  roboEyes.setAutoblinker(ON, 3, 2);
  roboEyes.setIdleMode(ON, 2, 2);
}

void oledTask(void * pvParameters) {

  RobotEmotion lastEmotion = E_NEUTRAL; 

  bool wasSleepy = false;

  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.setSweat(isTired);

    if (isSleepy != wasSleepy) {
      wasSleepy = isSleepy;
      if (isSleepy) {
        roboEyes.setIdleMode(OFF);
        roboEyes.setAutoblinker(OFF);
        roboEyes.close(); 
      } else {
        roboEyes.setIdleMode(ON,2,2);
        roboEyes.setAutoblinker(ON);
        roboEyes.open();   
      }
    }

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


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

  for(;;) {
    if (currentState != lastAction) {
      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;
        default: rgb.fadeColor(0, 0, 0, 500); break;
      }
      lastAction = currentState;
    }

    if (currentState == MANUAL) {
      switch(manualMotors) {
        case 'F': 
          break;
        case 'B': 
          rgb.setBrightness(50);
          rgb.setColorRGB(255, 100, 0);
          rgb.blink(1, 300);
          rgb.setColorRGB(0, 0, 0, 0);
          break;
        case 'L': ; break;
        case 'R': ; break;
        default:; break;
      }
    } else {
      switch(currentState) {
        case E_HAPPY:
          rgb.setBrightness(40);
          rgb.rainbowCycle(1000, 2);
          break;
        case E_SAD:
          rgb.setBrightness(40);
          break;
        case E_ANGRY:
          rgb.setBrightness(100);
          rgb.blink(1, 300);
          break;
        case E_NEUTRAL:
        default:
          break;
      }
    }
    vTaskDelay(20 / portTICK_PERIOD_MS);
  }
}


/*================================= MOTORS =================================*/

void motorsTask(void * pvParameters) {
  for(;;) {
    if (currentState == MANUAL) {
      switch(manualMotors) {
        case 'F': motors.forward(100, 50); break;
        case 'B': motors.backward(100, 50); break;
        case 'L': motors.leftTurn(100, 50); break;
        case 'R': motors.rightTurn(100, 50); break;
        default:  motors.stop(); break;
      }
    } else {
      switch(currentState) {
        case E_HAPPY:
          motors.leftTurn(100, 130);
          motors.rightTurn(100, 130);
          break;
        case E_ANGRY:
          motors.forward(100, 130);
          motors.backward(100, 130);
          break;
        default:
          motors.stop();
          break;
      }
    }
    vTaskDelay(10 / portTICK_PERIOD_MS);
  }
}


/*================================= SETUP =================================*/

void setup() {
  wifiSetup("haslo12345", "siemamaciek");

  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);
  xTaskCreate(wifiTask, "WiFi Task", 4096, NULL, 1, &TaskWiFi);
  xTaskCreate(robotLogicTask, "Logic Task", 2048, NULL, 1, &TaskLogic);
  
  currentState = MANUAL; 
  vTaskDelay(5000 / portTICK_PERIOD_MS);
}

void loop() {
  vTaskDelay(1000 / portTICK_PERIOD_MS);
}