//#include <math.h>
#include <avr/io.h>
#include <util/delay.h>
#include "arduinolib.h"
//#include "HardwareSerial.h"

void Arduinolib__dwrite_step(int p, int v, Arduinolib__dwrite_out *out){
    /* set pin 5 of PORTB for output. i.e port 13 */
    if (p == 13){
        DDRB |= _BV(DDB5);
        if (v == 0)
        {
            PORTB &= ~(1<<PORTB5);
            //PORTB &= ~_BV(PORTB5); //put it to low
            return;
        }
        PORTB |= (1<<PORTB5);
        //PORTB |= _BV(PORTB5);
        return;
    }
    DDRB |= _BV(DDB4);
    if (v == 0)
    {
        PORTB &= ~(1<<PORTB4);
        //PORTB &= ~_BV(PORTB4); //put it to low
        return;
    }
    PORTB |= (1<<PORTB4);
    //PORTB |= _BV(PORTB4);
    return;
    
}


/*void Arduinolib__dwrite_step(int p, int v, Arduinolib__dwrite_out *out)
{
 pinMode(p,OUTPUT);
  if(v==0)
	{
	digitalWrite(p,LOW);
	//Serial.write("low");
	return;
	}
	digitalWrite(p,HIGH);
	//Serial.write("high");
}
*/
/*void Mathext__st_cos_reset(Mathext__st_cos_mem *self)
{
  int j;
  self->i = 0;
  for(j = 0; j < 100; ++j)
    self->mem[j] = 0.0;
}

void Mathext__st_cos_step(float a, Mathext__st_cos_out *out, Mathext__st_cos_mem *self)
{
  out->o = self->mem[self->i];
  self->i = (self->i+1) % 100;
  self->mem[self->i] = cosf(a);
}*/