#include <avr/io.h>
#include "math.h"
#include "sqrt32.h"

#define LED_PIN PB4

#define PI 3.1415926535897932384626433832795
#define rLDC 250.0
#define ARMLENGTH 500.0
#define ARMLENGTH2 250000.0


int main(void) {

  // Configure LED_PIN (PB4) as output
  DDRB = (1 << LED_PIN);
  
  double x=0,y=0,z=0;

  double x1=-rLDC;
  double y1=0;
  double z1=0;

  double x2=150.0; //ToDo
  double y2=sin(60.0 / 180.0 * PI) * rLDC;
  double z2=0;

  double x3=150.0; //TODO
  double y3=-sin(60.0 / 180.0 * PI) * rLDC;
  double z3=0;

  PORTB |= (1 << LED_PIN);

  do
  {
	x+=0.001;
	y=sin(x);
	z1=sqrt(ARMLENGTH2 - pow(x-x1, 2) - pow(y-y1, 2)) + z;
	z2=sqrt(ARMLENGTH2 - pow(x-x2, 2) - pow(y-y2, 2)) + z;
	z3=sqrt(ARMLENGTH2 - pow(x-x3, 2) - pow(y-y3, 2)) + z;
	if((z1>0.0)||(z2>0.0)||(z3>0.0))
	{
		PORTB ^= 1 << LED_PIN;
	}
	if(x>PI)
	{
		x=0;
	}
  }
  while (1);

  return 0;
}