void motor_secure_pwm_write(uint8 pPin, uint16 pCommand) {
  if (pCommand > MAX_COMMAND) {
    pwmWrite(pPin, MAX_COMMAND);
  } else {
    pwmWrite(pPin, pCommand);
  }
}

int main (void)
{
	int bright ;

	printf ("Raspberry Pi wiringPi PWM test program\n") ;

	if (wiringPiSetup () == -1)
	exit (1) ;

	pinMode (1, PWM_OUTPUT) ;

	for (;;)
	{
	for (bright = 0 ; bright < 1024 ; ++bright)
	{
		pwmWrite (1, bright) ;
		delay (1) ;
	}

	for (bright = 1023 ; bright >= 0 ; --bright)
	{
		pwmWrite (1, bright) ;
		delay (1) ;
	}
	}

	return 0 ;
}

/**
   Will make the engine brake. Note : it brakes hard.
 */
void motor_brake() {
  mot.state = BRAKE;
  mot.previousCommand = mot.command;
  mot.command = 0;
  pwmWrite(PWM_2_PIN, 0);
  pwmWrite(PWM_1_PIN, 0);
}

int main(void)
{
   boardConfig();

   tickConfig( 1, 0 );

   digitalConfig( 0, ENABLE_DIGITAL_IO );


   uint8_t dutyCycle = 0; /* 0 a 255 */

   analogConfig( ENABLE_ANALOG_INPUTS );

   pwmConfig( 0,     PWM_TIMERS_ENABLE );

   pwmConfig( PWM0,  PWM_OUTPUT_ENABLE );
   pwmConfig( PWM7,  PWM_OUTPUT_ENABLE );

   pwmWrite( PWM7, 0 );
   pwmWrite( PWM0, 0 );

   while(1) {
      dutyCycle = analogRead(AI1) / 4 ;
      pwmWrite( PWM7, dutyCycle );
      pwmWrite( PWM0, dutyCycle );
   }
   return 0 ;
}

int main(void)
{
	int i;

	if(wiringPiSetup() < 0){ //when initialize wiringPi failed, print message to screen
		printf("setup wiringPi failed !\n");
		return -1; 
	}
	
	pinMode(LedPin, PWM_OUTPUT);//pwm output mode

	while(1){
		for(i=0;i<1024;i++){
			pwmWrite(LedPin, i);
			delay(2);
		}
		delay(1000);
		for(i=1023;i>=0;i--){
			pwmWrite(LedPin, i);
			delay(2);
		}
	}

	return 0;
}

void drive_straight(int left45_sensor, int left90_sensor, int right45_sensor, int right90_sensor) // 2 or 4 sensors?
{
                                           // analog read values of 2 or 4 sensors passed to function
  static int previous_error = 0;
  static int Kp = 16, Ki = 1, Kd = 4;      // constants for scaling P I D effects (will need adjusting)
  static int error, P, I = 0,  D;      // error variables
  int total; 
  
  error = (right45_sensor + right90_sensor)/2 - (left_45sensor + left90_sensor)/2;
  
  P = error * Kp;
  
  I = (I + error)*Ki;    
  
  D = (error - previous_error) * Kd;       // may take out
  previous_error = error;
  
  total = (P+I+D);

  {
    L_enable_val -= (total);
      L_enable_val = constrain(L_enable_val, 30000, 65535);      // may need to adjust
    
    R_enable_val += (total); 
      R_enable_val = constrain(R_enable_val, 30000, 65535);
    
    pwmWrite(left_enable, L_enable_val);            // enable pins and values 
                                                     // must be global
    pwmWrite(right_enable, R_enable_val);          // arduino uses analogWrite
  }
}

/**
 * Plays the specified MIDI note number where 60 = Middle C, 69 = A (440 Hz).
 * Note 0 is a rest (silence) and note 1 is a buzz (low freq).
 */
void Gpio::startBuzzer(int note)
{
	if (!mHwPwmInit)
		usePibrellaPwm();

	if (note == 0){
		pwmWrite(HW_PWM_PIN, 0);
		mBuzzerClock = 0;
		return;
	}

	// Convert MIDI note number to frequency
	double freq;
	if (note == 1)
		freq = 20;
	else
		freq = 440.0 * pow(2.0, ((note - 69) / 12.0));

	// Fiddle the clock value so that it sounds good from 1 octave below
	// middle C to one octave above, i.e. 3 full octaves.
	int clock = (int)(60000.0 / freq);
	if (mBuzzerClock != clock){
		pwmSetClock(clock);
		pwmWrite(HW_PWM_PIN, 50);
		mBuzzerClock = clock;
	}
}

int main(void) {
		
		int distancia0=100, distancia1=100, distancia2=100;
		int m1=975, m0=975;
		
		char str[20];
		printf("Nome do arquivo: ");
		fgets(str,20,stdin);
		if(str[strlen(str)-1]=='\n'){str[strlen(str)-1]='\0';}
		strcat(str,".txt");
		puts(str);
		
		FILE *fp;
		fp = fopen(str,"w");
		
		setup();
		while(distancia1 > 10){
			
			distancia0=getCM0();
			delay(50);
			distancia1=getCM1();
			delay(50);
			distancia2=getCM2();
			delay(50);
			
			if (distancia1 < 40){
				m0=975;
				m1=975;
			}else if (distancia0 < 100 && distancia2 > 100){
				m0=970;
				m1=964;
			}else if (distancia2 < 100 && distancia0 > 100){
				m0=965;
				m1=970;
			}else if(distancia0 < 100 && distancia2 < 100){
				m0=970;
				m1=970;
			}else{
				m0=965;
				m1=964;
			}
			
			pwmWrite(MOTOR0,m0);
			pwmWrite(MOTOR1,m1);
			
			printf("Distance0: %dcm\n", distancia0);
			fprintf(fp,"%d\t",distancia0);
			printf("Distance1: %dcm\n", distancia1);
			fprintf(fp,"%d\t",distancia1);
			printf("Distance2: %dcm\n", distancia2);
			fprintf(fp,"%d\t",distancia2);
			printf("\n");
			fprintf(fp,"%d\t",m0);
			fprintf(fp,"%d\n",m1);
			
		}
		fclose(fp);
        return 0;
}

int main(int argc, const char *argv[]) {
    unsigned int timeBase = 3000000;
    char buffer[10];
    char prevLine[10];

    if (geteuid() != 0) {
        // chown root <file>
        // sudo chmod u+s <file>
        char *exec = rindex(argv[0], '/');
        exec++;
        fprintf(stderr, "You must be root to run \"%s\". Program should be suid root. This is an error.\n", exec) ;
        return 1;
    }

    atexit(destroy);
    signal(SIGHUP, terminated);
    signal(SIGINT, terminated);
    signal(SIGKILL, terminated);
    signal(SIGPIPE, terminated);
    signal(SIGALRM, terminated);
    signal(SIGTERM, terminated);

    wiringPiSetupGpio();
    pinMode(18, PWM_OUTPUT);
    pwmSetMode(PWM_MODE_MS);
    pwmWrite(18, 0);

    char tone[3] = "--";
    int octave;
    unsigned int duration;

    char *line = fgets(buffer, 10, stdin);
    assert(line != NULL);

    int parsed = sscanf(line, "%c%c%i %i", &tone[0], &tone[1], &octave, &duration);

    while (parsed == 4) {
        printf("%s%i, 1/%i\n", tone, octave, duration);
        fflush(stdout);
        int divisor = 1 << (8 - octave - 2);
        int toneIndex = 0;

        for (toneIndex = 0; toneIndex < 13; toneIndex++) {
            if (0 == strncmp(tone, TONE_NAME[toneIndex], 2)) {
                break;
            }
        }

        assert(toneIndex < 13);

        if (toneIndex < 12) {
            float frequency = TONE_HZ[toneIndex] / divisor;
            unsigned int range = (unsigned int)(600000.0f / frequency);
            pwmSetRange(range);
            pwmWrite(18, range >> 1);
            usleep(timeBase / duration);
            pwmWrite(18, 0);
            usleep(20000);
        } else {

/**
   Will release the motor. Call motor_restart() to get out of this mode
 */
void motor_compliant() {
  mot.state = COMPLIANT;
  mot.previousCommand = mot.command;
  mot.command = 0;
  digitalWrite(SHUT_DOWN_PIN, LOW);
  pwmWrite(PWM_2_PIN, 0);
  pwmWrite(PWM_1_PIN, 0);
}

void writeAngle(int angle){
	if(angle > 135){
		pwmWrite(WHEEL_PIN, 135);
	} else if (angle < 45){
		pwmWrite(WHEEL_PIN, 45);
	} else {
		pwmWrite(WHEEL_PIN, angle);
	}
}

/// Play a chromatic scale.
void playScale(MorseToken) {
  analogWrite(piezoTxN, 0);
  for (size_t i(24); i < sizeof(noteHz)/sizeof(noteHz[0]); i += 1) {
    pwmFrequency(noteHz[i]+0.5);
    pwmWrite(getDuty());
    delayMicroseconds(getDitMicros());
    pwmWrite(0);
  }
}

void release_door(void)
{
	pwmWrite(1, 75); //servo goto open position
	usleep(600000); //wait until servo reaches open position
	pwmWrite(1, 0); //stop servo motor
	sleep(2); //wait with tray opened for 1 seconds
	pwmWrite(1, 30); //servo goto closed position
	usleep(600000); //wait until servo reaches closed position
	pwmWrite(1, 0); //stop servo motor
}

void initMotor(){
	pinMode(ESC_PIN, PWM_OUTPUT);
	pinMode(WHEEL_PIN, PWM_OUTPUT);
	
	pwmWrite(ESC_PIN, 180);
	sleep(1);
	pwmWrite(ESC_PIN, 0);
	sleep(1);
	pwmWrite(ESC_PIN, NEUTRAL_SPEED);
	sleep(1);
	pwmWrite(ESC_PIN, NEUTRAL_SPEED);
	pwmWrite(WHEEL_PIN, NEUTRAL_ANGLE);
}

void Control::demo(){
    float millis = 1.0;
    int tick = calcTicks(millis, HERTZ);
    while(1){
        pwmWrite(PIN_BASE + 16, tick);
        delay(2000);
        millis=1.6;
        tick = calcTicks(millis, HERTZ);
        pwmWrite(PIN_BASE + 16, tick);         delay(2000);
        millis=1.0;
        tick = calcTicks(millis, HERTZ);
        pwmWrite(PIN_BASE + 16, tick);
        delay(2000);
    }
}

static void pwmAudioOutput()
{
#if (USE_AUDIO_INPUT==true)
	adc_count = 0;
	startSecondAudioADC();
#endif

#if (AUDIO_MODE == HIFI)
	int out = output_buffer.read();
	pwmWrite(AUDIO_CHANNEL_1_PIN, out & ((1 << AUDIO_BITS_PER_CHANNEL) - 1));
	pwmWrite(AUDIO_CHANNEL_1_PIN_HIGH, out >> AUDIO_BITS_PER_CHANNEL);
#else
	pwmWrite(AUDIO_CHANNEL_1_PIN, (int)output_buffer.read());
#endif
}

void Motors_comands_void(void)
 {

	 static int debinainte_in  = 0;
	 static int debinainte_out = 0;
	 static int debinapoi_in   = 0;
	 static int debinapoi_out  = 0;
	 static int pwm_out1       = 0;
	 static int pwm_out2       = 0;
	 int i = 0;

	 if (buf_control_comands.DirectionOfMotor > 50)
	 {
		 pwm_out1 = (100 *(buf_control_comands.DirectionOfMotor - 50));
		 pwm_out2 = 0;
		 softPwmWrite (PIFACE,pwm_out1);
		 softPwmWrite (PIFACE+1,pwm_out2) ;
	 }else if (buf_control_comands.DirectionOfMotor < 50)
	 {
		 pwm_out1 = 0 ;
		 pwm_out2 = 100 *(50 - buf_control_comands.DirectionOfMotor);
		 softPwmWrite (PIFACE,pwm_out1);
		 softPwmWrite (PIFACE+1,pwm_out2) ;
	 }
	 else
	 {
		 pwm_out1=0;
		 pwm_out2=0;
		 softPwmWrite (PIFACE,pwm_out1);
		 softPwmWrite (PIFACE+1,pwm_out2) ;
	 }

	 if (buf_control_comands.rbi_dreapta  == 1 )
	 {
		 pwmWrite(1,45);
	 }
	 else if (buf_control_comands.rbi_stanga  == 1)
	 {
		 pwmWrite(1,105);

	 }
	 else
	 {
		 pwmWrite(1,75);
	 }


 }

void Motor::control(unsigned short level)
{
#ifndef NDEBUG
	assert(0 <= level && level <= 1000);
#endif
	pwmWrite(pin,levelToCtrl(level));
}

void output_init()
{
    pinMode(PWM_PIN,PWM_OUTPUT);
    /* 500/1000 = 0.5 duty cycle */

#if 1
    pwmSetRange(1000) ;
    pwmWrite(PWM_PIN, 500);
#else
    pwmSetRange(2600) ;
    pwmWrite(PWM_PIN, 800);
#endif

    pwmSetClock(252);

}

void Control::adjustMotorSpeed(int motor, double speed){
    double milliseconds = speed*(MaxTime-MinTime)+MinTime;
    milliseconds = inputNormalizer(milliseconds, MinTime, MaxTime);
    //printf("Milliseconds: %f", milliseconds);
    double tick = calcTicks(milliseconds, HERTZ);
    pwmWrite(PIN_BASE + motor, tick);
}