//まだ終わっていない仕事たち
const double kp[] = {0.300000, 0, 0};
const double ki[] = {0.015017, 0, 0};
const double kd[] = {0.003754, 0, 0};

//check.cpp
const int kBuffer_mm = 100;
//go.cpp
const double kServoTicker_s = 0.03;
const double kServoVelocity_mm = 100;

IsArrived 内の torelanceとcountの最大値

//GoTo0
const double kGoTo0Duty = 0.2;


//ごみ箱
int IsInBanArea(int ban_area_num, const double (&now_position_mm)[3])
{
    int is_in_ban_area = 1;
    for(int i = 0; i < 3; i++) {
        if(min_ban_area[ban_area_num][i] < now_position_mm[i] && now_position_mm[i] < max_ban_area[ban_area_num][i]) {}
        else {
            is_in_ban_area = 0;
            break;
        }
    }
    return is_in_ban_area;
}

/*
void Above(WorkState &state)
{
    int target_pitch_degree = 0;
    int target_yaw_degree = 0;
    switch(state.areaname) {
        case WORKAREA: {
            double target_mm[3] = {};
            for(int i = 0; i < sizeof(target_mm)/sizeof(target_mm[0]); i++) target_mm[i] =  state.position[i];
            target_mm[2] += kCatchBuf_mm;
            PIDAbove(target_mm, state);
            switch(state.color) {
                case RED:
                    target_mm[1] += kArmLength[3];
                    break;
                case YELLOW:
                    target_mm[1] -= kArmLength[3];
                    break;
            }
            PIDAbove(target_mm, state);
            SetNextRadRelative(3, 0*kDegreeToRad);
            ServoMoveOnArm();
            switch(state.color) {
                case RED:
                    SetServoYawRad(0 * kDegreeToRad);
                    ServoMoveOnArm();
                    target_mm[1] -= kArmLength[3];
                    break;
            }


            break;
            case COMMONAREA:
            case BOX:
                SetNextRadRelative(3, 45*kDegreeToRad);
                ServoMoveOnArm();
                break;
            }
    }
}
*/
/*
void Above(WorkState &state)
{
    int target_pitch_degree = 0;
    int target_yaw_degree = 0;
    double target_mm = GetNowElbowZ() + kCatchBuf_mm;
    switch(state.areaname) {
        case WORKAREA:
            for(int i = 0; i < sizeof(calpid)/sizeof(calpid[0]); i++) calpid[i].PileReset();
            while(GetNowElbowZ() < target_mm) {
                output = calpid[0].GetOutPut(target_mm);;
                motor[0].OutPut(GetMotorOutPutZ());
                DEBUG("output %f\r\n", output);
            }
            motor[0].OutPut(0);
            switch(state.color){
                case RED:

            target_pitch_degree = 0;
            target_yaw_degree = 90;
            break;
        case COMMONAREA:
        case BOX:
            target_pitch_degree = 45;
            target_yaw_degree = 90;
            break;
    }
    SetServoYawRad(target_yaw_degree * kDegreeToRad);
    ServoMoveOnArm();
    SetNextRadRelative(3, target_pitch_degree*kDegreeToRad);
    ServoMoveOnArm();

}
*/
/*
void PIDAbove(const double (&target_mm)[3], WorkState &state)
{

    double now_yaw_rad = GetNowRad(2);
    double now_pitch_rad = GetNowRad(3);
    for(int i = 0; i < sizeof(calpid)/sizeof(calpid[0]); i++) calpid[i].PileReset();
    int is_finish = 0;
    while(is_finish == 0) {
        double now_position_mm[3] = {GetNowTipLocateX(), GetNowTipLocateY(), GetNowTipLocateZ()};
        //収束したらその回でループ終了
        if(IsArrived(target_mm, now_position_mm) == 1) is_finish = 1;
        AllTargetToElbow(target_mm, now_yaw_rad, now_pitch_rad, state);
        //motor出力
        if(motor_move_ticket > 0) {
            motor_move_ticket = 0;
            output = calpid[0].GetOutPut(GetTargetElbow(2));;
            motor[0].OutPut(GetMotorOutPutZ());
            //DEBUG("%f\r\n",GetMotorOutPutZ());
        }
        //servoの出力.kPIDTicker_s毎にmotor出力を変更
        if(servo_move_ticket > 0) {
            servo_move_ticket = 0;
            //出力値計算
            CalServoMove();
            //出力
            ServoMoveOnArm();
        }
    }
    for(int i = 0; i < sizeof(motor)/sizeof(motor[0]); i++) motor[i].OutPut(0);
}
*/

/*
int AllowChangeDirection(double yaw_rad, double pitch_rad,
                         const double (&now_position_mm)[3], const double (&now_elbow_mm)[3])
{
    //アーム根本の現在地の取得
    double next[3] = {};//形状変化後の値
    for(int i = 0; i < sizeof(next)/sizeof(next[0]); i++) {
        next[i] = now_elbow_mm[i] + GetFromElbowToHand(i, yaw_rad, pitch_rad);
    }
    int isok = 1;
    for(int i = 0; i < 3; i++) {
        int out_area_num = CheckBanArea(i, next[i], now_position_mm);
        if(out_area_num <= -1 || (out_area_num == 0 && GetCounterpartIsInCommon() == 0)) {}
        else isok = 0;
    }
    return isok;
}
*/

void Priority()
{
    work[23].priority=1000;//SearchCommonArea で使用。最初の時の比較に使用.最後だということを示す。

    work[11].priority=1;
    work[10].priority=2;
    work[9].priority=3;
    work[8].priority=4;
    work[7].priority=5;
    work[6].priority=6;

    work[16].priority=3;
    work[15].priority=4;
    work[14].priority=5;
    work[13].priority=6;
    work[12].priority=7;

    work[22].priority=4;
    work[21].priority=5;
    work[20].priority=6;
    work[19].priority=7;
    work[18].priority=8;
    work[17].priority=9;



    //ワークエリア
    /*
    103 102 0
    102 101  100
    */
    work[0].priority=102;
    work[1].priority=101;
    work[2].priority=100;
    work[3].priority=103;
    work[4].priority=102;
    work[5].priority=0;

}




/*
double GetIdealYawRad(WorkState &state,MoveTiming timing)
{
    double degree = 0;
    switch(state.areaname) {
        case BOX:
        case COMMONAREA:
            degree = 90;
            break;
        case WORKAREA:
            switch(state.color) {
                case RED:
                    degree = 270;
                    break;
                case YELLOW:
                    degree = 90;
                    break;
            }
            break;
    }
    return degree * kDegreeToRad;
}
double GetIdealPitchRad(WorkState &state,MoveTiming timing)
{
    double degree = 0;
    switch(state.areaname) {
        case BOX:
            switch(timing) {
                case PREMOVE:
                    degree = 45;
                    break;
                case FINISHMOVE:
                    degree = 0;
                    break;
            }
            break;
        case COMMONAREA:
            degree = 0;
            break;
        case WORKAREA:
            degree = -90;
            break;
    }
    return degree * kDegreeToRad;
}
*/
/*
int ExitBanArea(const double (&target)[3], LocateParam now, double (&result)[3])
{
    int is_ban_area = 0;
    double after_mm[2][3] = {};//変更後の値。[0][]が先端、[1][]が肘が入っているときの変更値
    int change[3] = {};//どの座標の値を変更するか
    int ban_area[2] = {-1,-1};//[0]:先端が入っている禁止領域,[1]:肘が入っている禁止領域
    for(int i = 0; i < sizeof(result)/sizeof(result[0]); i++) result[i] = target[i];

    //先端のチェック{
    ban_area[0] = CheckBanArea(0, now.position[0], now.position);
#ifdef FORDEBUG
    DEBUG("tip in ban %d \r\n",ban_area[0]);
#endif
    if(ban_area[0] == 0 && GetCounterpartIsInCommon() == 1) {
        after_mm[0][1] = min_ban_area[ban_area[0]][1]  - kStopBuff_mm;
        change[1] = 1;
    } else if(ban_area[0] == 2) {
        after_mm[0][1] = min_ban_area[ban_area[0]][1]  - kStopBuff_mm;
        change[1] = 1;
        if(now.position[2] < center[ban_area[0]][2]) {
            after_mm[0][2] = min_ban_area[ban_area[0]][2]  - kStopBuff_mm;
            change[2] = 2;
        }
    } else if(ban_area[0] > 0) {
        after_mm[0][2] = max_ban_area[ban_area[0]][2]  + kStopBuff_mm;
        change[2] = 1;
    }
    //肘のチェック
    ban_area[1] = CheckBanArea(0, now.elbow[0], now.elbow);
#ifdef FORDEBUG
    DEBUG("now elbow %f, %f, %f\r\n", now.elbow[0],now.elbow[1],now.elbow[2]);
    DEBUG("elbow in ban %d \r\n",ban_area[1]);
#endif
    if(ban_area[1] > 0) {
        after_mm[1][2] = max_ban_area[ban_area[1]][2]  + kStopBuff_mm + GetFromElbowToHand(2, now.yaw_rad, now.pitch_rad);
        change[2] = 1;
    }
    //最終的な変更値
    if(change[1] == 1) {
        result[1] = after_mm[0][1];
        //DEBUG("ExitBanArea  change y %f\r\n",result[1]);
        is_ban_area = 1;
    }
    if(change[2] == 1) {
        if(after_mm[0][2] > after_mm[1][2]) result[2] = after_mm[0][2];
        else result[2] = after_mm[1][2];
        is_ban_area = 1;
    } else if(change[2] == 2) {
        result[2] = after_mm[0][2];
        is_ban_area = 1;
    }
    return is_ban_area;
}
*/
/*
if( is_tip_ban == 1) {
                result[1] = min_ban_area[out_area_num][1] - kStopBuff_mm;
                result[2] = max_ban_area[out_area_num][2]  + kStopBuff_mm;
            } else {
                result[2] = max_ban_area[out_area_num][2]  + kStopBuff_mm
                            + GetFromElbowToHand(2, now.yaw_rad, now.pitch_rad);
            }
        */

void ServoMoveOnArmInTurns()
{
    static int count = 0;
    if(count == 0) {
        for(int i = 0; i < 1; i++) {
            double rad = GetNextRadRelative(i);
            ServoRad(i, rad);
        }
    } else if (count == 1) {
        for(int i = 1; i < 3; i++) {
            double rad = GetNextRadRelative(i);
            ServoRad(i, rad);
        }
    } else if(count == 2) {
        double rad = GetNextRadRelative(3);
        ServoRad(3, rad);
    }
    ++count;
    if(count == 3) count = 0;
}