#include <Servo.h>
#include <Wire.h>
#include <WiiNun.h>

#define PIN_LED 13

#define PIN_MOTOR_HORIZON_1      5
#define PIN_MOTOR_HORIZON_2      4
#define PIN_MOTOR_VERTICAL_1     6
#define PIN_MOTOR_VERTICAL_2     7

#define PIN_SERVO_BASE         9
#define PIN_SERVO_HAND_1      10
#define PIN_SERVO_HAND_2      11
#define PIN_SERVO_HAND_3      12

#define STOP  0
#define GO    1
#define BACK  2
#define BRAKE 3

#define PIN_PWR A3
#define PIN_GND A2

int loop_cnt = 0;
Servo base;
Servo hand1;
Servo hand2;
Servo hand3;
WiiNun WiiNun;

// 台座の角度
byte theta = 90; // degree

void hMotorSpeed(char sspeed){
  byte speed = abs(sspeed)*2;
  if(sspeed<0){
    analogWrite(PIN_MOTOR_HORIZON_1, 0xFF-speed);
    digitalWrite(PIN_MOTOR_HORIZON_2, HIGH);
  }else{
    analogWrite(PIN_MOTOR_HORIZON_1, speed);
    digitalWrite(PIN_MOTOR_HORIZON_2, LOW);
  }
}

void vMotorSpeed(char sspeed){
  byte speed = abs(sspeed)*2;
  if(sspeed<0){
    analogWrite(PIN_MOTOR_VERTICAL_1, 0xFF-speed);
    digitalWrite(PIN_MOTOR_VERTICAL_2, HIGH);
  }else{
    analogWrite(PIN_MOTOR_VERTICAL_1, speed);
    digitalWrite(PIN_MOTOR_VERTICAL_2, LOW);
  }
}

// 0-2piで返すatan
double atan(double x, double y){
    if(x>=0 && y<0) return atan(y/x)+2*PI;
    else if(x<0) return atan(y/x)+PI;
    else return atan(y/x);
}

void setup(){
  pinMode(PIN_PWR, OUTPUT);
  pinMode(PIN_GND, OUTPUT);
  digitalWrite(PIN_PWR, HIGH);
  digitalWrite(PIN_GND, LOW);
  delay(100);
  WiiNun.begin();
  base.attach(PIN_SERVO_BASE);
  hand1.attach(PIN_SERVO_HAND_1);
  hand2.attach(PIN_SERVO_HAND_2);
  hand3.attach(PIN_SERVO_HAND_3);
//  Serial.begin(19200);
  pinMode(PIN_LED , OUTPUT);
  pinMode(PIN_MOTOR_HORIZON_1 , OUTPUT);
  pinMode(PIN_MOTOR_HORIZON_2 , OUTPUT);
  pinMode(PIN_MOTOR_VERTICAL_1, OUTPUT);
  pinMode(PIN_MOTOR_VERTICAL_2, OUTPUT);
}

void loop(){
  WiiNun.get();
  byte joyx = WiiNun.data[0];
  byte joyy = WiiNun.data[1];
  byte accx = WiiNun.data[2];
  byte accy = WiiNun.data[3];
  byte accz = WiiNun.data[4];
  byte zbut = ((~WiiNun.data[5] >> 0) & 1);
  byte cbut = ((~WiiNun.data[5] >> 1) & 1);
  if ((WiiNun.data[5] >> 2) & 1) accx += 2;
  if ((WiiNun.data[5] >> 3) & 1) accx += 1;
  if ((WiiNun.data[5] >> 4) & 1) accy += 2;
  if ((WiiNun.data[5] >> 5) & 1) accy += 1;
  if ((WiiNun.data[5] >> 6) & 1) accz += 2;
  if ((WiiNun.data[5] >> 7) & 1) accz += 1;
  char x = (char)(joyx-0x7F);
  char y = (char)(joyy-0x7F);
  char ay = (char)(accy-0x7F);
  digitalWrite(PIN_LED, LOW);
  if(zbut){
    digitalWrite(PIN_LED, HIGH);
    if(y>0x1F) theta = atan(x,y)*180/PI;
    else theta -= x/32;
    if(theta < 10) theta = 10;
    else if(theta > 170) theta = 170;
    base.write(theta);
  }else{
    hMotorSpeed(y);
  }
  vMotorSpeed(ay);
  
  byte catchDegree = 90;
  if(cbut){
    catchDegree = 30;
  }
  hand1.write(catchDegree);
  hand2.write(catchDegree);
  hand3.write(catchDegree);
  
//  Serial.print("theta: "); Serial.print(theta);
//  Serial.print("\tjoyx: "); Serial.print((byte)joyx,DEC);
//  Serial.print("\tjoyy: "); Serial.print((byte)joyy,DEC);
//  Serial.print("\taccx: "); Serial.print((byte)accx,DEC);
//  Serial.print("\taccy: "); Serial.print((byte)accy,DEC);
//  Serial.print("\taccz: "); Serial.print((byte)accz,DEC);
//  Serial.print("\tzbut: "); Serial.print((byte)zbut,DEC);
//  Serial.print("\tcbut: "); Serial.println((byte)cbut,DEC);
  
  delay(10);
}