void setup()
{
  DDRB = (1 << PB3) | (1 << PB4);
  /*
  PORTB = 0;
  //  GTCCR = TSM;
  PLLCSR &= (~(1 << PCKE)) & (~(1 << PLLE));
  TCCR1 = (1 << CTC1) | (1 << CS12) | (1 << CS13);
  OCR1A = 200;
  TIMSK = (1 << OCIE1A);
  GTCCR = (1 << PSR1);
  TCNT1 = 0;

}

volatile int offCounter1 = 100;
volatile int offCounter2 = 100;
volatile int periodCounter = 0;


ISR(TIM1_COMPA_vect)
{
  static int state = 0;
  if (state == 0)
  {
    PORTB |= (1 << PB3) | (1 << PB4);

    periodCounter = 0;

    OCR1A = 94;
    TCNT1 = 0;
        state = 1;
  }
  else if (state == 1)
  {
    if (offCounter1 < offCounter2)
    {
      OCR1A = offCounter1;
    } else
    {
      OCR1A = offCounter2;
    }
    TCNT1 = 0;
    state = 2;
  } else if (state == 2)
  {
    if (offCounter1 < offCounter2)
    {
      PORTB &= ~(1 << PB3);
      state = 3;
      OCR1A = offCounter1 - offCounter2;
    }
    else
    {
      PORTB &= ~(1 << PB4);
            OCR1A = offCounter1 - offCounter2;
      state = 4;
    }
  }
  else if (state == 3)
  {
      PORTB &= ~(1 << PB4);
      OCR1A =
  }
  else if (state == 4)
  {
      PORTB &= ~(1 << PB3);
  }
  else
  {
    if (periodCounter >= offCounter2)
    {
      PORTB &= ~(1 << PB4);
    }

    if (periodCounter >= offCounter1)
    {
      PORTB &= ~(1 << PB3);
    }

  }
}

inline int toCounter(int angle)
{
  return map(angle, 0, 180, 94, 282);
}

enum State
{
  STATE_RIGHT,
  STATE_DOWN,
  STATE_LEFT,
  STATE_UP
};

volatile int horizontalAngle = 0;
volatile int verticalAngle = 180;
State state = STATE_RIGHT;
int delta = 0;
void loop()
{
  offCounter1 = toCounter(horizontalAngle);
  offCounter2 = toCounter(verticalAngle);

  if (state == STATE_RIGHT)
  {
    ++horizontalAngle;

    if (horizontalAngle >= (180 - delta))
    {
      state = STATE_DOWN;
    }
  }
  else if (state == STATE_LEFT)
  {
    --horizontalAngle;

    if (horizontalAngle <= delta)
    {
      state = STATE_UP;
    }

    ++delta;
  }

  else if (state == STATE_UP)
  {
    ++verticalAngle;

    if (verticalAngle >= (180 - delta))
    {
      state = STATE_RIGHT;
    }
  }
  else if (state == STATE_DOWN)
  {
    --verticalAngle;

    if (verticalAngle <= delta)
    {
      state = STATE_LEFT;
    }
  }

  if (delta >= 90)
  {
    delta = 0;
  }
  
}