

// --------------------------------------------------------------------------------

int16_t Calibrate_Accelerometers(int16_t zCalAcc) {
  // called when TEST == true to determine offset values if MPU6050 is detected
  // with mounting of MPU6050 the Y and Z axice are transposed
  // note Y-axis does not apply when vertical, this would be average gravity reading
  // these measurements are run as a state machine
  switch(zCalAcc) {
    case 1: // initialise calibration process
      // we re-initialise the MPU6050 in case it has had a collision and crashed the MPU
  //      Serial.println(F("TEST Acc offsets..."));
      Set_LEDTask(1); CalCnt = 0;
      MPU_6050_Initialise();
      AccSumX = 0; AccSumY = 0; AccSumZ = 0; zCalAcc++; break;
    case 2: // take measurements
      Wire.beginTransmission(MPU_address);
      Wire.write(0x3B);                         // starting with register 0x3B (ACCEL_XOUT_H)
      I2C_Err = Wire.endTransmission();         // End the transmission
      delayMicroseconds(I2Cdel);                // Allow MPU time to respond

      Wire.requestFrom(MPU_address,6);  // request 6 register read
      if (CalCnt >= 100) {
        AccRawX = Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
        AccRawY = Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
        AccRawZ = Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_YOUT_H) & 0x40 (ACCEL_YOUT_L)
        AccSumX = AccSumX + AccRawX; AccSumY = AccSumY + AccRawY; AccSumZ = AccSumZ + AccRawZ;
      } else {
        // discard first 100 readings
        Wire.read(); Wire.read(); Wire.read(); Wire.read();; Wire.read(); Wire.read();
      } CalCnt++; if (CalCnt >= 2100) {zCalAcc++;}
      break;
    case 3: // report the results
      // Use the negative of these values as offsets to correct any drift
      AccRawX = AccSumX/2000; AccRawY = AccSumY/2000;  AccRawZ = AccSumZ/2000;
  //      Serial.print(F("Acc X Cal.= ")); Serial.println(AccRawX);
  //      Serial.print(F("Acc Y Cal.= ")); Serial.println(AccRawY);
  //      Serial.print(F("Acc Z Cal.= ")); Serial.println(AccRawZ);
  //      Serial.println(F("Accelerometers offsets measured\n"));
       Set_LEDTask(0); zCalAcc = 0; break;
  }
  return zCalAcc;
}

// --------------------------------------------------------------------------------

int16_t Calibrate_Gyros(int16_t zCalGyr) {
  // called when CalGyr > 0 to determine gyro offset values if MPU6050 is detected
  // these measurements are run as a state machine
  // we use AccSums to save on variable space
  switch(zCalGyr) {
    case 1: // initialise
  //      Serial.println(F("TEST Gyro offsets..."));
      Set_LEDTask(1); CalCnt = 0;  setMainMode(0);
      DispDel = 0; StatMode = 1; StatDel = 0; // display mode is forced when in calibration mode
      AccSumX = 0; AccSumY = 0; AccSumZ = 0; zCalGyr++; break;
    case 2: // wait for button release if pressed
      if (sw0State && sw1State) {zCalGyr++;}  // wait for both going HIGH
      break;
    case 3: // take measurements
      Wire.beginTransmission(MPU_address);
      Wire.write(0x43);                         // starting with register 0x3B (GYRO_XOUT_H)
      I2C_Err = Wire.endTransmission();         //End the transmission

      Wire.requestFrom(MPU_address,6);  // request 6 register read
      if (CalCnt >= 100) {
        GyrRawX = Wire.read()<<8|Wire.read();  // 0x43 (ACCEL_XOUT_H) & 0x44 (ACCEL_XOUT_L)
        GyrRawY = Wire.read()<<8|Wire.read();  // 0x45 (ACCEL_YOUT_H) & 0x46 (ACCEL_YOUT_L)
        GyrRawZ = Wire.read()<<8|Wire.read();  // 0x47 (ACCEL_YOUT_H) & 0x48 (ACCEL_YOUT_L)
        AccSumX = AccSumX + GyrRawX; AccSumY = AccSumY + GyrRawY; AccSumZ = AccSumZ + GyrRawZ;
      } else {
        // discard first 100 readings
        Wire.read(); Wire.read(); Wire.read(); Wire.read();; Wire.read(); Wire.read();
      } CalCnt++; if (CalCnt >= 1100) {zCalGyr++;}
      break;
    case 4: // report the results
      // Use the negative of these values as offsets to correct any drift
      GyrRawX = AccSumX/1000; GyrRawY = AccSumY/1000;  GyrRawZ = AccSumZ/1000;
      gyro_pitch_calibration_value = GyrRawX;
       Set_LEDTask(0); zCalGyr = 0;
      if (TEST) {return zCalGyr;}   // exit here in TEST mode
      
      DispMode = 2; DispDel = 0;  // Gyro offsets
      DispNext = 5;               // switch back to angle display
  //      Serial.print(F("Gyr X Cal.= ")); Serial.println(GyrRawX);
  //      Serial.print(F("Gyr Y Cal.= ")); Serial.println(GyrRawY);
  //      Serial.print(F("Gyr Z Cal.= ")); Serial.println(GyrRawZ);
  //      Serial.println(F("Gyro offsets measured\n"));
      break;
  }
  return zCalGyr;
}

// ---------------------------------------------------------------------------------

void CalOffsets() {
  // called when in TEST mode to perform continuous offset measurement cycles
  // this also includes turning the laser range finder ON and OFF for measurements
  if (CalDel > 0) {CalDel--; return;}
  
  switch (CalMPU) {
    case 1: // initialise test
      CalPnt = Calibrate_Accelerometers(1);
      StatMode = 1; StatDel = 0;    // display battery status data
      DispMode = 7; DispDel = 0;    // display CALIBRATING
      CalMPU++; Exp = ExpCal; break;
    case 2: // perform accelerometer offset calibration
      CalPnt = Calibrate_Accelerometers(CalPnt);
      if (CalPnt == 0) {
        StatMode = 0;               // don't display status data
        DispMode = 1; DispDel = 0;  // display accelerometer offsets
        CalMPU++; CalDel = 1500;     // delay before next
        SLEEP = true;               // sleep whilst displaying results
      } break;
    case 3: // initialise test
      CalPnt = Calibrate_Gyros(1);
      StatMode = 1; StatDel = 0;    // display battery status data
      DispMode = 7; DispDel = 0;    // display CALIBRATING
      CalMPU++; SLEEP = false; Exp = ExpCal; break;
    case 4: // perform gyro offset calibration
      CalPnt = Calibrate_Gyros(CalPnt);
      if (CalPnt == 0) {
        StatMode = 0;               // don't display status data
        DispMode = 2; DispDel = 0;  // display accelerometer offsets
        CalMPU++; CalDel = 1500;     // delay before next
        SLEEP = true;               // sleep whilst displaying results
      } break;
    case 5: // perform laser ranging tests
      VL53L1X_ON();                   // turn laser ON and stare expression
      Exp = ExpStare; SLEEP = false;
      StatMode = 1; StatDel = 0;      // display battery status data
      DispMode = 16; DispDel = 0;     // display lasaer ranging
      CalMPU++; CalDel = 1500; break; // delay before next
    case 6: // turn OFF laser range finder
      VL53L1X_OFF();
      CalMPU = 1; break;            // restart
      
    default: CalMPU = 1; break;
  }
}

// ---------------------------------------------------------------------------------

void ExpEngine() {
  // called every 40ms to perform a range of FACE tasks base on the value of Exp
  // Exp states are as follows
  //  0 ExpStart          // Start expression state
  //  1 ExpCal            // calibrating expression state
  //  2 ExpHappy          // Happy expression state
  //  3 ExpHapAng         // Happy angled expression state
  //  4 ExpLying          // Lying down expression state
  //  5 ExpReady          // Ready expression state
  //  6 ExpReadyAnx       // Ready anxious tilting expression state
  //  7 ExpActive         // Active balancing expression state
  //  8 ExpSafe           // Safe expression state
  //  9 ExpSleep          // Sleeping expression stat
  // 10 ExpStare          // staring straight ahead expression

  if (FaceDel > 0) {FaceDel--; return;}

  if (SLEEP) {Exp = ExpSleep;}
  switch(Exp) {
    case ExpStart:    FaceStart(); break;   // Start expression state
    case ExpCal:      FaceCal(); break;     // calibrating expression state
    case ExpHappy:    FaceHappy(); break;   // Happy expression state
    case ExpHapAng:   FaceHapAng(); break;  // Happy angled expression state
    case ExpLying:    FaceRnd(); break;     // Lying down expression state
    case ExpReady:    FaceRnd(); break;     // Ready expression state
    case ExpReadyAnx: FaceRdyAnx(); break;  // Ready anxious tilting expression state
    case ExpActive:   FaceAct(); break;     // Active balancing expression state
    case ExpSafe:     FaceHappy(); break;   // Safe expression state
    case ExpSleep:    FaceSleep(); break;   // Sleeping expression state
    case ExpStare:    FaceStare(); break;   // Staring expression state
  }
  FaceShow = 1;     // trigger Core 0 to draw face
  // Serial.println(FaceDel);
}

// ---------------------------------------------------------------------------------

void FaceAct() {
  // generate active expressions
  // if no demand then display happy anxious faces
  if (received_byte == 0) {FaceHapAng();}  // no joystick demand
  else {FaceUnhappy();}                     // joystick movement
}

// ---------------------------------------------------------------------------------

void FaceCal() {
  // generate calibration expressions
  setBrowsRnd();    // set random eyebrows
  eye_Ox = random(-16,16); eye_Oy = random( 2,16);
  if (random(2) > 0) {setMouth(1);} else {setMouth(5);}    // choose a random mouth expression
  FaceDel = 12;     // delay of 480ms
}

// ---------------------------------------------------------------------------------

void FaceHapAng() {
  // generate happy angle slightly concerned expressions
  switch(random(4)) {
    case 0: setBrows(0); break;
    case 1: setBrows(1); break;
    case 2: setBrows(2); break;
    case 3: setBrows(3); break;
  }
  eye_Ox = random(-16,16); eye_Oy = random( 2,16);
  switch(random(4)) {
    case 0: setMouth(1); break;
    case 1: setMouth(3); break;
    case 2: setMouth(7); break;
    case 3: setMouth(9); break;
  }
  FaceDel = map(abs((int)angle_gyro),90,0,3,12);
  FaceDel = constrain(FaceDel,3,12);
}

// ---------------------------------------------------------------------------------

void FaceHappy() {
  // generate happy expressions
  switch(random(4)) {
    case 0: setBrows(3); break;
    case 1: setBrows(4); break;
    case 2: setBrows(5); break;
    case 3: setBrows(6); break;
  }
  eye_Ox = random(-16,16); eye_Oy = random(-16,16);
  switch(random(3)) {
    case 0: setMouth(1); break;
    case 1: setMouth(4); break;
    case 2: setMouth(5); break;
  }
  FaceDel = 12;     // delay of 480ms
}

// ---------------------------------------------------------------------------------

void FaceRdyAnx() {
  // robot being lifted up tilting towards start angle
  setBrows(random(4));  // random centre down
  switch(ExpPnt) {
    case 0: eye_Ox = 16; break;
    case 1: eye_Ox = 0; break;
    case 2: eye_Ox = -16; break;
    case 3: eye_Ox = 0; break;
  }  eye_Oy = random(-16,16);
  ExpPnt++; if (ExpPnt > 3) {ExpPnt = 0;}
  switch(random(6)) {
    case 0: setMouth(2); break;
    case 1: setMouth(3); break;
    case 2: setMouth(6); break;
    case 3: setMouth(7); break;
    case 4: setMouth(8); break;
    case 5: setMouth(9); break;
  }
  
  FaceDel = map(abs((int)angle_gyro),90,0,12,3);
  FaceDel = constrain(FaceDel,3,12);
}

// ---------------------------------------------------------------------------------

void FaceRnd() {
  // generate random expressions
  setBrowsRnd();    // set random eyebrows
  eye_Ox = random(-16,16); eye_Oy = random(-16,16);
  setMouthRnd();    // choose a random mouth expression
  FaceDel = 12;     // delay of 480ms
}

// ---------------------------------------------------------------------------------

void FaceSleep() {
  // generate a sleeping expression
  setBrows(3);      // flat centre
  eye_Ox = 0; eye_Oy = 0;
  setMouth(1);      // flat mouth
  FaceDel = 12;     // delay of 480ms
}

// ---------------------------------------------------------------------------------

void FaceStare() {
  // look straight ahead
  setBrows(3);      // flat centre
  eye_Ox = 0; eye_Oy = 0;
  setMouth(10);     // flat mouth, tapered down
  FaceDel = 12;     // delay of 480ms
}

// ---------------------------------------------------------------------------------

void FaceStart() {
  // generate a straight expression
  setBrows(3);      // flat centre
  eye_Ox = random(-16,16); eye_Oy = random(-8,8);
  setMouth(10);     // flat mouth, tapered down
  FaceDel = 12;     // delay of 480ms
}

// ---------------------------------------------------------------------------------

void FaceUnhappy() {
  // generate unhappy expressions
  int16_t zB = random(7); setBrows(zB);
  eye_Ox = random(-16,16);
  eye_Oy = 0;
  if (zB < 3) {eye_Oy = random(-16,0);}
  else if (zB > 3) {eye_Oy = random(0,16);}
  switch(random(7)) {
    case 0: setMouth(10); break;
    case 1: setMouth(2); break;
    case 2: setMouth(3); break;
    case 3: setMouth(6); break;
    case 4: setMouth(7); break;
    case 5: setMouth(8); break;
    case 6: setMouth(9); break;
  }
  FaceDel = 12;     // delay of 480ms
}

// ---------------------------------------------------------------------------------

void doHeadTasks() {
  // called every 40ms perform a range of Head tasks
  if (HeadDel > 0) {HeadDel--; return;}

  int16_t zInc = 6;         // head movement increment
  if (TEST) {}              // task performed in TEST mode is defined by SW0
  else if (SLEEP) {HeadTask = 4;}             // move head to centre
  else if (Exp == ExpHappy) {HeadTask = 2;}   // move whilst happy
  else if (Exp == ExpActive) {
    if ((received_byte == 0) && !WiiOveride) {HeadTask = 2;}  // move whilst active but no demand
    else {HeadTask = 4;}
  }
  else {HeadTask = 4;}      // default centre the head
  
  switch(HeadTask) {
    case 0: // default task does nothing
      break;
    case 1: // centre
      HeadCtrNow(); break;
    case 2: // follow eyes with pauses inbetween
      if (HeadVal == HeadTgt) {
        if (eye_Ox < 0) {HeadTgt = map(eye_Ox,-16,0,Head_150,Head_90);}
        else if (eye_Ox > 0) {HeadTgt = map(eye_Ox,0,16,Head_90,Head_30);}
        HeadDel = 10 + random(25); return; // set a delay before moving to next target
      } break;
    case 3: // simple short movements
      HeadTestTask(); break;
    case 4: // move to centre
      HeadTgt = Head_90; zInc = 20;
      if (HeadVal == HeadTgt) {HeadTask = 0;}
      break;
    case 99: // disconnects servo and returns to default task
      detachHeadServo(); HeadTask = 0; break;
  }
  bool zHeadMove = false;
       if (HeadVal < HeadTgt) {zHeadMove = true; HeadVal += zInc; if (HeadVal > HeadTgt) {HeadVal = HeadTgt;}}
  else if (HeadVal > HeadTgt) {zHeadMove = true; HeadVal -= zInc; if (HeadVal < HeadTgt) {HeadVal = HeadTgt;}}
  if (zHeadMove) {servoHead.writeMicroseconds(HeadVal);}
}

// ---------------------------------------------------------------------------------

void HeadCtrNow() {
  // move the head immediately to the centre position and set task to 0
  if (!HeadAtt) {attachHeadServo();}  // ensure servo is attached
  HeadVal = Head_90; HeadTgt = HeadVal;
  servoHead.writeMicroseconds(HeadVal);
  HeadDel = 25; HeadTask = 99;  // detatch head after a delay
}

// ---------------------------------------------------------------------------------

void HeadTestTask() {
  // simple short sweep side to side in TEST mode
  if (!HeadAtt) {attachHeadServo();}  // ensure servo is attached

  switch (HeadSubTsk) {
    case 0: HeadVal = 1400; break;
    case 1: HeadVal = 1500; break;
    case 2: HeadVal = 1600; break;
    case 3: HeadVal = 1500; break;
  }
  HeadSubTsk++; if (HeadSubTsk > 3) {HeadSubTsk = 0;}
  servoHead.writeMicroseconds(HeadVal); HeadTgt = HeadVal;
  //  Serial.println("HeadVal=" + String(HeadVal));
  HeadDel = 50; // apply a 2 sec delay
}

// ---------------------------------------------------------------------------------

void doLEDTasks() {
  // called every 40ms perform a range of LED tasks
  // pressing SW0 or SW1 will force LED illumination
  if (!CalGyr && ((digitalRead(sw0Pin) == LOW) || (digitalRead(sw1Pin) == LOW))) {
    // either or both buttons are pressed, but not in calibration mode
    FastLED.clear();
    if (digitalRead(sw0Pin) == LOW) {LED[0].setRGB(0,0,32);}
    if (digitalRead(sw1Pin) == LOW) {LED[3].setRGB(0,0,32);}
    FastLED.show(); return;
  }
  // if a delay has been set then abort the tasks until count == 0
  if (LEDDel > 0) {LEDDel--; return;}

  int16_t zLEDTask = LEDTask; if (SLEEP) {zLEDTask = 0;}
  switch(zLEDTask) {
    case 0: LEDTask0(); break;  // default LED sequence, dot walking left,right, left
    case 1: LEDTask1(); break;  // colourful calibration sequence
    case 2: LEDTask2(); break;  // puilsing heart sequence
  }
}

// ---------------------------------------------------------------------------------

void LEDTask0() {
  // generate default LED sequence, dot walking left,right, left
  FastLED.clear();
  LEDX[0].setRGB(0,0,2); if (WiFiConnected) {LEDX[0].setRGB(0,2,0);}
  switch (LED_Cnt) {
    case 0: LED[0] = LEDX[0]; break;
    case 1: LED[1] = LEDX[0]; break;
    case 2: LED[2] = LEDX[0]; break;
    case 3: LED[3] = LEDX[0]; break;
    case 4: LED[2] = LEDX[0]; break;
    case 5: LED[1] = LEDX[0]; break;
  }
  LED_Cnt++; if (LED_Cnt > 5) {LED_Cnt = 0;}
  LEDshow = true;   // trigger a strip show event
  LEDDel = 10;      // delay of 400ms
  if (SLEEP) {LEDDel = 30;}
}

// ---------------------------------------------------------------------------------

void LEDTask1() {
  // generate fast calibrating LED sequence
  FastLED.clear();
  switch (LED_Cnt) {
    case 0: LED[0].setRGB(0,0,2); LED[1].setRGB(0,2,0); LED[2].setRGB(2,0,0); LED[3].setRGB(0,0,0); break;
    case 1: LED[0].setRGB(0,0,0); LED[1].setRGB(0,0,2); LED[2].setRGB(0,2,0); LED[3].setRGB(2,0,0); break;
    case 2: LED[0].setRGB(2,0,0); LED[1].setRGB(0,0,0); LED[2].setRGB(0,0,2); LED[3].setRGB(0,2,0); break;
    case 3: LED[0].setRGB(0,2,0); LED[1].setRGB(2,0,0); LED[2].setRGB(0,0,0); LED[3].setRGB(0,0,2); break;
  }
  LED_Cnt++; if (LED_Cnt > 3) {LED_Cnt = 0;}
  LEDshow = true;   // trigger a strip show event
  LEDDel = 2;
}

// ---------------------------------------------------------------------------------

void LEDTask2() {
  // pulsing heart LED sequence
  FastLED.clear(); LEDDel = 1;
  switch (LED_Cnt) {
    case 0: LED[2].setRGB(HeartBright,0,0); break;
    case 1: LEDDel = 2; break;
    case 2: LED[2].setRGB(HeartBright/2,0,0); break;
    case 3:
      if (Exp == ExpActive) {
        if ((received_byte == 0x00) && (HeartDel < 50)) {
          HeartDel++;                             // slow pulse rate down
          HeartBright = 29 - (HeartDel/2);        // reduce brightness
        } else {HeartDel = 6; HeartBright = 24;}  // fast pace and brightness because moving
        LEDDel = HeartDel;                        // set delay timer
      } else {
        LEDDel = map((int)angle_gyro,-90,0,50,3); HeartDel = LEDDel;
        HeartBright = map((int)angle_gyro,-90,0,4,64);
      }
      break;
  }
  LED_Cnt++; if (LED_Cnt > 3) {LED_Cnt = 0;}
  LEDshow = true;   // trigger a strip show event
}

// --------------------------------------------------------------------------------

void MotorDriveTask() {
  // called at 125Hz from the main task loop to control motor PWMs and dither
  // control variables are set in other functions
  // Generate dither signal automatically, note h/w PWM is 30kHz
  //  Dither = -Dither; // alternating 30Hz signal
  Dither = -Dither;
  DithON = false;   // clear the applied flag
  
  // now create PWM counter signals
  // left wheel motor
  int16_t zPWM = PWM_Lft; int16_t zStart = PWM_Start + 25;
  if (zPWM >  0) {
    if (zPWM < zStart) {zPWM += Dither; zPWM = constrain(zPWM,0,255); DithON = true;}
    analogWrite(PinLftA, 255 - zPWM); analogWrite(PinLftB, 255);
    DIR_Lft = 1; DIR_LTnd += PWM_Lft;          // set direction and increment trend
  } else if (PWM_Lft == 0) {
    analogWrite(PinLftA, 0); analogWrite(PinLftB, 0); DIR_Lft = 0; DIR_LTnd = 0;
  } else {
    if (zPWM > -zStart) {zPWM += Dither; zPWM = constrain(zPWM,-255,0); DithON = true;}
    analogWrite(PinLftA, 255); analogWrite(PinLftB, 255 + zPWM);
    DIR_Lft = -1; DIR_LTnd += PWM_Lft;         // set direction and decrement trend
  }
  
  // right wheel motor
  zPWM = PWM_Rht; 
  if (zPWM >  0) {
    if (zPWM < zStart) {zPWM += Dither; zPWM = constrain(zPWM,0,255); DithON = true;}
    analogWrite(PinRhtA, 255); analogWrite(PinRhtB, 255 - zPWM);
    DIR_Rht = 1; DIR_RTnd += PWM_Rht;          // set direction and increment trend
  } else if (PWM_Rht == 0) {
    analogWrite(PinRhtA, 0); analogWrite(PinRhtB, 0); DIR_Rht = 0; DIR_RTnd = 0;
  } else {
    if (zPWM > -zStart) {zPWM += Dither; zPWM = constrain(zPWM,-255,0); DithON = true;}
    analogWrite(PinRhtA, 255 + zPWM); analogWrite(PinRhtB, 255);
    DIR_Rht = -1; DIR_RTnd += PWM_Rht;         // set direction and decrement trend
  }
}

// --------------------------------------------------------------------------------

void MotorSweepTask() {
  // performs a motor sweep for both wheels when in TEST mode
  // confirms that your motor wiring and H-bridge drivers are working correctly
  switch (MotorSwp) {
    case 1:
      PWM_Cnt+= PWM_Inc;
      if (PWM_Cnt > 120) {PWM_Cnt = 120; PWM_Inc = -1;}
      else if (PWM_Cnt < -120) {PWM_Cnt = -120; PWM_Inc = 1;}
      PWM_Lft = PWM_Cnt; PWM_Rht = PWM_Cnt;
      break;
    case 2:
      PWM_Cnt = 0; PWM_Inc = 1; MotorSwp = 0;
      PWM_Lft = PWM_Cnt; PWM_Rht = PWM_Cnt;
       break;
  }
  MotorDriveTask();
}

// ---------------------------------------------------------------------------------

void Set_LEDTask(int16_t zLT) {
  // called to change LED task, and reset values in the process
  if (LEDTask == zLT) {return;} // no need to change anything
  
  LEDTask = zLT; LED_Cnt = 0; LEDDel = 0; // set task and reset variables, immediately
}

// ---------------------------------------------------------------------------------
