#include<Servo.h>
#define PWM_ON_CLK 3
#define PWM_OFF    2
#define BTN1 4
#define BTN2 5
#define BTN3 6
#define LED1 7
#define LED2 12
#define LED3 13
#define J1_PIN 8
#define J2_PIN 9
#define J3_PIN 10

#define J1zeropos 60.0
#define J2zeropos 60.0
#define J3zeropos 60.0

#define l1  150.0
#define l2  342.0
#define x0   50.0
#define y0   70.0
#define xe  -25.0
#define ze  -10.0


Servo J1;
Servo J2;
Servo J3;

int a1;
int a2;
int a3;

float J1pos = J1zeropos;
float J2pos = J2zeropos;
float J3pos = J3zeropos;

bool analogFlag;

class Position3D
{
  private:

  float x;
  float y;
  float z;

  public:

  Position3D()
  {
    x = 0;
    y = 0;
    z = 0;
  }

 Position3D(float x, float y, float z)
  {
    this->x = x;
    this->y = y;
    this->z = z;
  }

  Position3D set( float x, float y, float z)
  {
    this->x = x;
    this->y = y;
    this->z = z;
    return *this;
  }

  Position3D rotateX(float angle)
  {}

  Position3D rotateY(float angle)
  {}

  Position3D rotateZ(float angle)
  {
    float angleRad = angle*M_PI/180;
    float s = cos(angle*M_PI/180);
    float c = cos(angle*M_PI/180);
    float tempx = c*x - s*y;
    float tempy = s*x + c*y;
    x = tempx;
    y = tempy;
    return *this;
  }

  Position3D shiftX(float dx)
  {
    x = x + dx;
    return *this;
  }

  Position3D shiftY(float dy)
  {
    y = y + dy;
    return *this;
  }

  Position3D shiftZ(float dz)
  {
    z = z + dz;
    return *this;
  }

};

Position3D Pe(1,2,3);

void setOff()
{
  analogWrite(PWM_ON_CLK,0);
  digitalWrite(PWM_OFF,HIGH);
  digitalWrite(LED1,HIGH);
  digitalWrite(LED2,LOW);
  digitalWrite(LED3,LOW);
  analogFlag = false;
}

void setOp()
{
  analogWrite(PWM_ON_CLK,127);
  digitalWrite(PWM_OFF,LOW);
  digitalWrite(LED1,LOW);
  digitalWrite(LED2,LOW);
  digitalWrite(LED3,HIGH);
  analogFlag = true;
}

void setNop()
{
  analogWrite(PWM_ON_CLK,127);
  digitalWrite(PWM_OFF,LOW);
  digitalWrite(LED1,LOW);
  digitalWrite(LED2,HIGH);
  digitalWrite(LED3,LOW);
  analogFlag = false;
}

boolean getOp()
{
  return analogFlag;
}

void setup() {

  pinMode(BTN1,INPUT_PULLUP);
  pinMode(BTN2,INPUT_PULLUP);
  pinMode(BTN3,INPUT_PULLUP);

  pinMode(LED1,OUTPUT);
  pinMode(LED2,OUTPUT);
  pinMode(LED3,OUTPUT);
  
  digitalWrite(LED1,LOW);
  digitalWrite(LED2,LOW);
  digitalWrite(LED3,LOW);
  
  pinMode(PWM_OFF,OUTPUT);
  digitalWrite(PWM_OFF,LOW);

  setOff();

  Serial.begin(9600);
  J1.attach(J1_PIN);
  J2.attach(J2_PIN);
  J3.attach(J3_PIN);
  J1.write(J1pos);
  J2.write(J2pos);
  J3.write(J3pos);

}

void loop() {

  if( digitalRead(BTN1) == LOW)
  {
    setOff();
  }

  if( digitalRead(BTN2) == LOW)
  {
    setNop();
  }

  if( digitalRead(BTN3) == LOW)
  {
    setOp();
  }

/*
  a1 = analogRead(A0);
  a2 = analogRead(A1);
  a3 = analogRead(A2);
  
  if(getOp())
  {
      J1pos = a1/1024.0*100 +40;
      J2pos = a2/1024.0*100 +40;
      J3pos = a3/1024.0*100 +40;
  }
  else
  {
      J1pos = J1zeropos;
      J2pos = J2zeropos;
      J3pos = J3zeropos;
  }
  J1.write(J1pos);
  J2.write(J2pos);
  J3.write(J3pos);
*/

}