#include <Servo.h>
#include <Wire.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 pwrpin PORTC3
#define gndpin PORTC2

byte nunchuck_buf[6];
int loop_cnt = 0;
Servo base;
Servo hand1;
Servo hand2;
Servo hand3;

// 台座の角度
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);
  }
}

void nunchuckInit(){
    DDRC |= _BV(pwrpin) | _BV(gndpin);
    PORTC &=~ _BV(gndpin);
    PORTC |=  _BV(pwrpin);
    delay(100);
    Wire.begin();
    Wire.beginTransmission(0x52);
    Wire.write(0x40);// sends memory address
    Wire.write(0x00);// sends sent a zero
    Wire.endTransmission();// stop transmitting
}

// Send a request for data to the nunchuck
// was "send_zero()"
void nunchuck_send_request()
{
    Wire.beginTransmission(0x52);// transmit to device 0x52
    Wire.write(0x00);// sends one byte
    Wire.endTransmission();// stop transmitting
}

// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
char nunchuk_decode_byte (char x)
{
    x = (x ^ 0x17) + 0x17;
    return x;
}

// Receive data back from the nunchuck, 
// returns 1 on successful read. returns 0 on failure
byte nunchuckGet(){
    int cnt=0;
    Wire.requestFrom (0x52, 6);// request data from nunchuck
    while (Wire.available()) {
        // receive byte as an integer
        nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.read());
        cnt++;
    }
    nunchuck_send_request();  // send request for next data payload
    // If we recieved the 6 bytes, then go print them
    if (cnt >= 5) {
        return 1;   // success
    }
    return 0; //failure
}

// 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(){
  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);
  nunchuckInit();
  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(){
  nunchuckGet();
  byte joyx = nunchuck_buf[0];
  byte joyy = nunchuck_buf[1];
  byte accx = nunchuck_buf[2];
  byte accy = nunchuck_buf[3];
  byte accz = nunchuck_buf[4];
  byte zbut = ((~nunchuck_buf[5] >> 0) & 1);
  byte cbut = ((~nunchuck_buf[5] >> 1) & 1);
  if ((nunchuck_buf[5] >> 2) & 1) 
    accx += 2;
  if ((nunchuck_buf[5] >> 3) & 1)
    accx += 1;
  if ((nunchuck_buf[5] >> 4) & 1)
    accy += 2;
  if ((nunchuck_buf[5] >> 5) & 1)
    accy += 1;
  if ((nunchuck_buf[5] >> 6) & 1)
    accz += 2;
  if ((nunchuck_buf[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);
}