//------------
//=================================================
// Task to handle the gyro and the other sensors hooked up to the sensor mux
//=================================================
task gyro()
{
	float currDir = 0.0; float prevDir = 0.0;
	long currtime,prevtime;
	ir_mux_status=HTSMUXreadPowerStatus(IR_MUX);				// read the sensor multiplexor status
	gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX);		// read the sensor multiplexor status
	while (ir_mux_status || gyro_mux_status)  					// check good battery power on both muxes
	{
		PlayTone(750,25);
		wait1Msec(500);
	}

	wait1Msec(300);

	for (int i=0;i<5;i++)            // check if there is too much spread in the data
	{
		if (gyro_noise>10)
		{
			PlayTone (250,25);
			wait1Msec(500);
		}
	}
	calibrate = 2;
	prevtime = nPgmTime;
	while(true)
	{
		currtime=nPgmTime;
		rawgyro = (float)HTGYROreadRot(HTGYRO);
		constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
		relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
		prevtime = currtime;
		wait1Msec(1);
		prevDir = currDir;
	}
}

task main()
{
	nMotorEncoder[motorRight] = 0;
	nMotorEncoder[motorLeft] = 0;
	motor[motorLeft] = 20;
	motor[motorRight] = 20;
	while ( SensorValue[IRsensor] != 6)
	{
		wait1Msec(10);
		nxtDisplayCenteredTextLine(3, "Sensor Value: %d", SensorValue[IRsensor]);

	}

		motor[motorLeft] = 0;
		motor[motorRight] = 0;
		PlayTone(736, 50);
		wait1Msec(1000);

		//Now go for x amount of encoder counts//

		motor[motorLeft] = 20;
		motor[motorRight] = 20;
		while ( nMotorEncoder[motorLeft] < (360 * 4 * 9.5))
		{
			wait10Msec(1);
		}

		motor[motorLeft] = 0;
		motor[motorRight] = 0;
		PlayTone(736, 50);
		wait1Msec(1000);

}

//--------------------------------------------------------------------------------------------------
static void* PlayDtmfThread
(
    void* contextPtr
)
{
    DtmfThreadCtx_t *threadCtxPtr = (DtmfThreadCtx_t*) contextPtr;
    size_t           dtmfLen = strlen(threadCtxPtr->dtmfPtr);
    uint32_t         i;

    for (i=0 ; i< dtmfLen ; i++)
    {
        LE_INFO("Play digit.%c", threadCtxPtr->dtmfPtr[i]);

        PlayTone(threadCtxPtr->pipefd[1], threadCtxPtr->sampleRate,
                 Digit2LowFreq(threadCtxPtr->dtmfPtr[i]), DTMF_AMPLITUDE,
                 Digit2HighFreq(threadCtxPtr->dtmfPtr[i]), DTMF_AMPLITUDE,
                 threadCtxPtr->duration);

        if (threadCtxPtr->pause)
        {
            // Play silence
            PlayTone(threadCtxPtr->pipefd[1], threadCtxPtr->sampleRate,
                     0, 0,
                     0, 0,
                     threadCtxPtr->pause);
        }
    }

    return NULL;
}

// Allow the user to calibrate the scales
void calibrateScales()
{
  int calibrateWeight = 0;

  eraseDisplay();
  nxtDisplayCenteredTextLine(0, "GlideWheel-AS");
  nxtDisplayCenteredTextLine(2, "Place the object");
  nxtDisplayCenteredTextLine(3, "on the scales");
  nxtDisplayCenteredTextLine(4, "and press");
  nxtDisplayCenteredTextLine(5, "[enter]");
  nxtDisplayCenteredTextLine(6, "to calibrate");
  while (nNxtButtonPressed != kEnterButton) EndTimeSlice();
  debounce();
  eraseDisplay();
  calibrateWeight = weighObject();
  nxtDisplayCenteredTextLine(0, "GlideWheel-AS");
  nxtDisplayCenteredTextLine(2, "Enter the weight");
  nxtDisplayCenteredTextLine(3, "in grams");
  nxtDisplayCenteredTextLine(7, "-     OK     +");
  while (true)
  {
    nxtDisplayCenteredBigTextLine(5, "%d", calibrateWeight);
    switch(nNxtButtonPressed)
    {
      case kLeftButton: PlayTone(500,10); calibrateWeight--; calibrateWeight = max2(0, calibrateWeight); break;
      case kRightButton: PlayTone(1000,10); calibrateWeight++; break;
      case kEnterButton: PlayTone(1500,10);gramsPerUnit = (float)calibrateWeight / (float)MSANGreadRaw(MSANG); eraseDisplay(); return;
    }
    wait1Msec(20);
    debounce();
  }
}

int StallCode(tMotor motorSentTo, int wantedPower)
{
	int motorIndex;  //index value for the arrays we are storing values in.
	int direction = 0;
	switch(motorSentTo) //which motor power is being sent to
	{
		case LeftMotor: // This is the name of one of the motors as referenced in the configuraiton.
			motorIndex = 0;
			break;
		case RightMotor:
			motorIndex = 1;
			break;
		/*case ForkLift:
			motorIndex = 2;
			break;*/
		default:
			break;
	}

	if (abs(wantedPower) < deadZone)  // Power below threshold, mark as stopped.
		direction = 0;
  else
  	direction = (wantedPower < 0) ? -1 : 1;

	if (direction == 0 || lastDirection[motorIndex] != direction)  // Stopped or changed direction.	Allow whatever power desired this time.
		{
    	lastDirection[motorIndex] = direction;
			timeLastSigMove[motorIndex]	 = time1[T1];
			encLastSigMove[motorIndex] = nMotorEncoder[motorSentTo];

			return wantedPower;
		}

 	lastDirection[motorIndex] = direction;

	if ( abs(encLastSigMove[motorIndex] - nMotorEncoder[motorSentTo]) > sigMove)  // Moved far enough to be considered significant, mark
		{
			timeLastSigMove[motorIndex]	= time1[T1];
			encLastSigMove[motorIndex] = nMotorEncoder[motorSentTo];

			return wantedPower;
		}

	if ( (time1[T1] - timeLastSigMove[motorIndex]) > wayTooLong )  // Time since last significant move too long, stalled
		{
			PlayTone(650,4);
			return 0;
		}

	if ( (time1[T1] - timeLastSigMove[motorIndex]) > tooLong )  // Time since last significant move too long, stalled
		{
			PlayTone(365,4);
			return wantedPower / 2;
		}

	return wantedPower;	// Haven’t moved far enough yet to be significant but haven’t timed out yet
}

/* Shooter speed decrease feedback */
task soundSpeedDown(){
  //        140 = Tempo
  //          5 = Default octave
  //    Quarter = Default note length
  //        10% = Break between notes
  //
  PlayTone(  932,   19); wait1Msec( 214);  // Note(A#, Duration(Eighth))
  PlayTone(  698,   19); wait1Msec( 214);  // Note(F, Duration(Eighth))
}

task main()
{
	nxtDisplayCenteredTextLine(3, "Servos");
	wait1Msec(750);
	servo [scoopCover] = 230;
	servo [leftLatch] = 252;
	servo [rightLatch] = 10;
	eraseDisplay();
	wait1Msec(750);
	nxtDisplayCenteredTextLine(3, "Lift 1");
	nxtDisplayCenteredTextLine(5, "Orange stops");
	while (nNxtButtonPressed != 3)
	{
		motor [leftLift] = -50;
	}
	while (nNxtButtonPressed != (-1))
	{
	}
	motor [leftLift] = 0;
	eraseDisplay();
	wait1Msec(750);
	nxtDisplayCenteredTextLine(3, "Lift 2");
	nxtDisplayCenteredTextLine(5, "Orange stops");
	while (nNxtButtonPressed != 3)
	{
		motor [rightLift] = -50;
	}
	while (nNxtButtonPressed != (-1))
	{
	}
	motor [rightLift] = 0;
	eraseDisplay();
	wait1Msec(750);
	nxtDisplayCenteredTextLine(3, "Arm");
	nxtDisplayCenteredTextLine(5, "Orange stops");
	while (nNxtButtonPressed != 3)
	{
		motor [arm] = -15;
	}
	if (nNxtButtonPressed == 3) {
		while (nNxtButtonPressed != (-1))
		{
		}
		motor [arm] = 0;
		eraseDisplay();
	}
	nxtDisplayCenteredTextLine(3, "Stored.");
	PlayTone (1000, 70);
	wait1Msec (1000);
	PlayTone (1000, 70);
	wait1Msec (1000);
}

// All done tones.
void Cocacola()
{
  //        125 = Tempo
  //          5 = Default octave
  //    Quarter = Default note length
  //        10% = Break between notes
  //

  PlayTone(  988,   22); wait1Msec( 240);  // Note(E6, Duration(Eighth))
  PlayTone( 1320,   22); wait1Msec( 240);  // Note(A6, Duration(Eighth))
  PlayTone( 1109,   43); wait1Msec( 480);  // Note(F#6)
  PlayTone(  880,   43); wait1Msec( 480);  // Note(D6)
  PlayTone(    0,   86); wait1Msec( 960);  // Note(Rest, Duration(Half))
  return;
}

void swapJoys()
{
  motor[leftMotor] = 0;
  motor[rightMotor] = 0;
  joySwap = !joySwap;
  PlayTone(784, 50);
}

task NavigateByTachometer()
{
     tnFlags flags = tnFlagsReadOnly;
     ResetAllTachoCounts(flags.leftMotor);
     ResetAllTachoCounts(flags.rightMotor);
     unsigned int time;
     unsigned int delta;
     while(true)
     {
          TextOut(0, flags.posLine,
              "P: " + v2AsString(flags.position) +
              "          " , DRAW_OPT_NORMAL);
          TextOut(0, flags.headLine, "H: " +
              NumToStr(flags.heading) +
              "          " , DRAW_OPT_NORMAL);
          time = CurrentTick();
          tnFlags temp = __updateHeadingPosition(flags, time + 50);
          if(temp.convergenceFailure)
              PlayTone(2000, 100);
          else
              flags = temp;
          delta = CurrentTick() - time;
          tnFlagsReadOnly = flags;
          if(delta < 25)
              Wait(25 - delta);

     }
}

/*turn on the spot if the timer expires, return false if it could not find another branch*/
bool turnOnSpot(short target, bool greyPatch){

  float cur_compass = compass();

  //start rotating
  stop();
  control(-rotateSpeed, rotateSpeed);

  while(compass() - cur_compass < 390){
    float diff = compass() - cur_compass;
    if(SensorValue[Light] < target && (diff < 180 || diff > 250)){ // ignore the original branch
      time1[T1] = 0;
      PlayTone(1175,20);
      stop();
      adjustCount++;
      return true; // return true if it finds another branch
    }
  }

  // on the endpoint
  stop();
  
  if(!greyPatch){ // if not yet detect a grey patch, turn back
      control(-rotateSpeed, rotateSpeed);
      while(SensorValue[Light] > target){}
      stop();
  }

  return false;
}

void navigateToWaypoint(float xtarget, float ytarget)
{
    while(!atTarget(xtarget, x) || (!atTarget(ytarget, y)))
    {
        float distance = calcDistance(xtarget, ytarget);
        float angle = calcAngle(xtarget, ytarget);

        float driveDistance = 0; //initialise
        if (distance < stepDistance)
        {
            driveDistance = distance;
        }
        else
        {
            driveDistance = stepDistance;
        }

        // co-ordinate system is different so cos and sin are swapped - take from current location not origin
        float tempWaypointX = x + (driveDistance * cos(angle));
        float tempWaypointY = y + (driveDistance * sin(angle));


        driveToWaypoint(tempWaypointX, tempWaypointY);
        monteCarlo();

        eraseDisplay();
        drawMap();
        drawParticles();
        wait1Msec(100);

    }

    PlayTone(784, 15);
}

//************************
// Segue Linha
//************************
void F_STATE_LINHA()
{
  MTASK_SET_RUN(MT_STOP_BUTTON);

  int Error = 0;

  while(ESTADO_IS_CURRENT())
  {
    //=====================================
    int Error = LLreadAverage(S1)-45;

    if(Error ==-45){
      Error = 0;
      PlayTone(200,1);
    }
  	nxtDisplayStringAt(20, 20, "%i",Error);

    motor[motorA]=-20 - Error;
    motor[motorC]=-20 + Error;

    //Se detectou rampa com acceleracao, sobe a rampa
    if(ACCEL_Rampa)ESTADO_SET_TARGET(ST_SOBE);
    //=====================================
  }
}

void initialize()
{
  motor[frontLeftWheel] = 0;
  motor[frontRightWheel] = 0;
  motor[backLeftWheel] = 0;
  motor[backRightWheel] = 0;
  motor[leftArm] = 0;
  motor[rightArm] = 0;
  nMotorEncoder[rightArm] = 0;
  //nMotorEncoderTarget[rightArm] = 10;
  servo[leftClaw] = LCLAWOPEN;
  servo[rightClaw] = RCLAWOPEN;

  string BatteryLevel = externalBatteryAvg;

  nxtDisplayCenteredBigTextLine (3, BatteryLevel);

  if(externalBatteryAvg<13000){
    PlayTone(500,10*(13000-externalBatteryAvg));
  }

  wait1Msec(5000);

  nxtDisplayCenteredBigTextLine (5, "Teleop Running.");

}

void SG(int samples){

	PlayTone(300,10);
	//De quantos em quantos valores ele ira ler
	if(samples<=100){
		SG_jump=1;
	}else{
		SG_jump=2;
	}
	//Configs: Jump, yScale, EXIT
	eraseDisplay();
	SG_ySpace=0;
	SG_yScale=2;
	SG_update=0;
	SG_exit=0;
	SG_pos=1;
	SG_xSpace=1;
	while(SG_exit==0){
		SG_Button();
		if(SG_update==0){
			eraseDisplay();
			for(int i=1;i<100 && i<samples/SG_jump;i++){
			  nxtDrawRect(i+SG_xSpace, SG_ySpace+AUlt[(i-1)*SG_jump]/SG_yScale,i+1+SG_xSpace,SG_ySpace+AUlt[i*SG_jump]/SG_yScale);
		  }
			SG_Config(0);
		  SG_update=1;
		}
	}
}

void init()
{
    bSmartDiagnostics = true; //true to enable smart diagnostic screen
    bCompetitionMode = true; //true to enable competition mode

    displaySplash("Mecanum Bot", "", true);

    bool ok = false;
    while(!ok)
    {
        const int testcount = 2;
	    bool test[testcount] = {
	        errorcheck(1,0,1,MOTORCON),
	        errorcheck(1,0,2,MOTORCON)};
	    string desc[testcount] = {"MC1-1","MC1-2"};
	    ok = error_display(test,desc,testcount);
	    if (!ok) {
	        PlayTone(440, 50);
	        if (test[0] == false && test[1] == false){
	            nxtDisplayCenteredTextLine(7, "Reboot MC!");
	        }
	    }
	    else { ClearSounds(); }
    }

    eraseDisplay();
    gyro_init(HTGYRO);
    wait1Msec(50);
    nxtbarOn();
    return;
}

void ToneClass::RandomTone(DigitalClass::DigitalPortEnum digitalPort, unsigned char lengthMs, int numCount){

	srand(time(NULL));
	for (int i = 0; i <= numCount; i++){
		int num3 = rand() % NoteCount;
		PlayTone(digitalPort, (NoteEnum)num3, lengthMs);
	}
}

task main()
{
  eraseDisplay();
  bNxtLCDStatusDisplay = true; // Enable top status line display
  wifi_startup();             // This is where the work is done.
  PlaySound(soundBeepBeep);   // Make some noise when we're connected.
  PlayTone(500, 100);         // Make some noise when we're connected.
}

void beep() {
  //        100 = Tempo
  //          5 = Default octave
  //    Quarter = Default note length
  //        10% = Break between notes
  PlayTone( 932,    27); wait1Msec(  75);  // Note(A#, Duration(32th))
  return;
}

void boop() {
  //        100 = Tempo
  //          6 = Default octave
  //    Quarter = Default note length
  //        10% = Break between notes
  PlayTone( 1047,  27); wait1Msec( 75);  // Note(F, Duration(32th))
  return;
}