/* Multi-core Task Handler*/
TaskHandle_t primaryTask;
TaskHandle_t animationTask;
SemaphoreHandle_t animationMutex;

/* Semaphore for handling mutex */
void createSemaphore() {
  animationMutex = xSemaphoreCreateMutex();
  xSemaphoreGive(animationMutex);
}

void mutexLock() {
  xSemaphoreTake(animationMutex, portMAX_DELAY);
}

void mutexUnlock() {
  xSemaphoreGive(animationMutex);
}

//Set the animation code, suppose to be used in core 0 only
void setAnimationCode(char filename) {
  Serial.println("Updating animation frame to " + String(filename));
  mutexLock();
  animation = filename;
  mutexUnlock();
}

//Get the animation code, suppose to be used in core 1 only
char getAnimationCode() {
  char anicode = 'a';
  mutexLock();
  anicode = animation;
  mutexUnlock();
  return anicode;
}

//Get the current state of the switch
// true = After human pushed
// false = After robot pushed
bool getSwitchState(){
    int switchState = digitalRead(TOGGLE_SWITCH);
    return (switchState == 1);
}


/* Multi-core process definations */
void startCoreTasks() {
  //core 1
  xTaskCreatePinnedToCore(
    AnimationController,   /* Task function. */
    "animator",     /* name of task. */
    10000,       /* Stack size of task */
    NULL,        /* parameter of the task */
    1,           /* priority of the task */
    &animationTask,      /* Task handle to keep track of created task */
    1
  );
  delay(500);

  //core 0
  xTaskCreatePinnedToCore(
    PrimaryController,   /* Task function. */
    "primary",     /* name of task. */
    10000,       /* Stack size of task */
    NULL,        /* parameter of the task */
    1,           /* priority of the task */
    &primaryTask,      /* Task handle to keep track of created task */
    0
  );          /* pin task to core 0 */
}

//Core 0 code, for movement and primary logics
void PrimaryController( void * pvParameters ) {
  Serial.println("Primary logic process started on core " + String(xPortGetCoreID()));
  clearFrame();
  for (;;) {
    //Loop of primary logics
    //setAnimationCode('a');
    //delay(10000);
    
    //Stepper test
    /*
    forward(50);
    delay(3000);
    backward(50);
    delay(3000);
    rotateAntiClockwise(100);
    delay(1000);
    //rotate backward
    rotateClockwise(100);
    delay(3000);
    
    //Servo test
    //Push button angle: 140
    //Servo arm vertical alignment angle: 125
    
    servoCoverPusher.write(90);
    delay(1000);
    servoSwitchPusher.write(130);
    delay(1000);
    servoCoverPusher.write(0);
    servoSwitchPusher.write(0);
    delay(3000);
    */
    bool switchPushed = getSwitchState();
    if (switchPushed){
      //Human pushed
      setAnimationCode('a');
      delay(4000);
      Serial.println("ON");
      rotateAntiClockwise(50);
      rotateClockwise(100);
      rotateAntiClockwise(100);
      rotateClockwise(50);
      delay(2000);
      pushWithHesitation();
      clearFrame();
      delay(3000);
    }else{
      delay(100);
    }

    //Servo install position = 125 (vertical)
    //servoSwitchPusher.write(125);
    
    //setAnimationCode('j');
    //delay(10000);
  }
}

//Core 1 code, for animation rendering
void AnimationController( void * pvParameters ) {
  Serial.println("Animation render started on core " + String(xPortGetCoreID()));
  for (;;) {
    char anicode = getAnimationCode();
    handleAnimationRendering(anicode);
  }
}