///////////////////////////////////////////////////////////////////////////////////
//
// Functions mainly used for development and debugging
//
///////////////////////////////////////////////////////////////////////////////////
// In the final version most of this code will be commented out and not compiled

void DebugReport(int zRPnn ) {
  ///////////////////////////////////////////////////////////////////////
  // a choice of print statements to inspect variables dynamically
  ///////////////////////////////////////////////////////////////////////
  // note at 4ms cycle and 115200 baud string length is limited to 46 chars
  // max in a 4ms period, otherwise blocking will occur. If used of WiFi then
  // link corruption may occur; so to prevent rogue values we send data twice and
  // checked on receive as a valid pair of duplicate values, otherwise disgard it.
  String zGM = ""; if (GphMode) {GPmin = 0; GPmax = 7; zGM = "GM7\n";}
  switch (zRPnn) {
    case 0:
      if (RP_Title) {PrintTx += "#AccRawY,AccRawZ,0\n" + zGM; RP_Title = false;}
      PrintTx += String(AccRawY) + ",";
      PrintTx += String(AccRawZ) + ",";
      PrintTx += "0\n";
      break;
      
    case 1:
      if (RP_Title) {PrintTx += "#angle_acc,angle_gyro,angle_diff,0\n" + zGM; RP_Title = false;}
      PrintTx += String(angle_acc) + ",";
      PrintTx += String(angle_gyro) + ",";
      PrintTx += String(angle_acc - angle_gyro) + ",";
      PrintTx += "0\n";
      break;
      
    case 2:
      if (RP_Title) {PrintTx += "#GyrRawX,GyrRawY,GyrRawZ,0\n" + zGM; RP_Title = false;}
      PrintTx += String(gyro_pitch_data_raw) + ",";
      PrintTx += String(gyro_turn_data_raw) + ",";
      PrintTx += String(gyro_yaw_data_raw) + ",";
      PrintTx += "0\n";
      break;
      
    case 3:
      if (RP_Title) {PrintTx += "#PID_p,PID_i,PID_d,PID o/p\n" + zGM; RP_Title = false;}
      PrintTx += String(PID_p) + ",";
      PrintTx += String(pid_i_mem) + ",";
      PrintTx += String(PID_d) + ",";
      PrintTx += String(pid_output) + "\n";
      break;

    case 4:
      if (RP_Title) {PrintTx += "#RangeRaw,Range,0,500\n" + zGM; RP_Title = false;}
      PrintTx += String(RangeRaw) + ",";
      PrintTx += String(Range) + ",";
      PrintTx += "0,500\n";
      break;

    case 5:
      if (RP_Title) {PrintTx += "#RpmLft,RpmRht,0\n" + zGM; RP_Title = false;}
      PrintTx += String(Int34RPM) + ",";
      PrintTx += String(Int35RPM) + ",";
      PrintTx += "0\n";
      break;

    case 6:
      if (RP_Title) {PrintTx += "#DIR_LTnd,DIR_RTnd,0\n" + zGM; RP_Title = false;}
      PrintTx += String(DIR_LTnd) + ",";
      PrintTx += String(DIR_RTnd) + ",";
      PrintTx += "0\n";
      break;

    case 7:
      if (RP_Title) {PrintTx += "#LftPeriod,RhtPeriod,0\n" + zGM; RP_Title = false;}
      PrintTx += String(IntPeriods[0]) + ",";
      PrintTx += String(IntPeriods[1]) + ",";
      PrintTx += "0\n";
      break;


    default:
      // zRPnn was out of range so reset the reporting pointer and send commands
      // to set RP = 0 and RMax to the highest number in your case list. ie. 7
      if (GphMode == 0) {RPnn = 0; PrintTx += "RP0\n";  PrintTx += "RM7\n";}
      break;
  }
}

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

void extendCmdVal(int zVal) {
  // adds a new digit to the right-hand end of cmdVal
  cmdVal = (cmdVal * 10) + zVal;  
}

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

void FpSerialPrint(float zP) {
  // print floating point number limited to 4 significant digits, without CR
  if (abs(zP) < 100.0) {Serial.print(zP); return;}
  if (abs(zP) < 1000.0) {Serial.print(zP,1); return;}
  Serial.print((int)zP);
}

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

void FpSerialPrintln(float zP) {
  // print floating point number limited to 4 significant digits, with CR
  if (abs(zP) < 100.0) {Serial.println(zP); return;}
  if (abs(zP) < 1000.0) {Serial.println(zP,1); return;}
  Serial.println((int)zP);
}

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

void printGains() {
  // print the current gain factors
  // this is used by the Windows PID controller app to load the current gain settings
  PrintTx += "PID=" + String(pid_p_gain) + ",\t";
  if (PID_i_En) {PrintTx += String(pid_i_gain);}
  else {PrintTx += "--.--";}
  PrintTx += ",\t";
  if (PID_d_En) {PrintTx += String(pid_d_gain);}
  else {PrintTx += "--.--";}
  PrintTx += ",\t";
  PrintTx += String(PWM_Start) + "\n";
}

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