/**
 * @license Licensed under the Apache License, Version 2.0 (the "License"):
 *          http://www.apache.org/licenses/LICENSE-2.0
 */

/**
 * @fileoverview Arduino code generator for the Servo library blocks.
 *     The Arduino Servo library docs: http://arduino.cc/en/reference/servo
 *
 * TODO: If angle selector added to blocks edit code here.
 */
'use strict';

goog.provide('Blockly.Python.servo');

goog.require('Blockly.Python');

/**
 * Code generator to setup a servo.
 * Arduino code: #include <Servo.h>
 *               Servo myServoX;
 * @param {!Blockly.Block} block Block to generate the code from.
 * @return {string} Completed code.
 */
Blockly.Python['servo_setup'] = function(block) {
    //var pinKey = block.getFieldValue('SERVO_PIN');
    //var servoName = 'myServo' + pinKey;
    Blockly.Python.addInclude('servo', '#include <Servo.h>');
    //Blockly.Python.addDeclaration('servo_' + pinKey, 'Servo ' + servoName + ';'); 
    var code = '';
    return code;
};

/**
 * Code generator to set an angle (Y) value to a servo pin (X).
 * Arduino code: #include <Servo.h>
 *               Servo myServoX;
 *               setup { myServoX.attach(X); }
 *               loop  { myServoX.write(Y);  }
 * @param {!Blockly.Block} block Block to generate the code from.
 * @return {string} Completed code.
 */
Blockly.Python['servo_write'] = function(block) {
    var pinKey = block.getFieldValue('SERVO_PIN');
    var servoAngle = Blockly.Python.valueToCode(
        block, 'SERVO_ANGLE', Blockly.Python.ORDER_ATOMIC) || '90';
    var servoName = 'myServo' + pinKey;

    Blockly.Python.reservePin(
        block, pinKey, Blockly.Python.PinTypes.SERVO, 'Servo Write');

    Blockly.Python.addDeclaration('servo_' + pinKey, 'Servo ' + servoName + ';');

    var setupCode = servoName + '.attach(' + pinKey + ');';
    Blockly.Python.addSetup('servo_' + pinKey, setupCode, true);

    var code = servoName + '.write(' + servoAngle + ');\n';
    return code;
};

/**
 * Code generator to read an angle value from a servo pin (X).
 * Arduino code: #include <Servo.h>
 *               Servo myServoX;
 *               setup { myServoX.attach(X); }
 *               loop  { myServoX.read();    }
 * @param {!Blockly.Block} block Block to generate the code from.
 * @return {array} Completed code with order of operation.
 */
Blockly.Python['servo_read'] = function(block) {
    var pinKey = block.getFieldValue('SERVO_PIN');
    var servoName = 'myServo' + pinKey;

    Blockly.Python.reservePin(
        block, pinKey, Blockly.Python.PinTypes.SERVO, 'Servo Read');

    Blockly.Python.addDeclaration('servo_' + pinKey, 'Servo ' + servoName + ';');

    var setupCode = servoName + '.attach(' + pinKey + ');';
    Blockly.Python.addSetup('servo_' + pinKey, setupCode, true);

    var code = servoName + '.read()';
    return [code, Blockly.Python.ORDER_ATOMIC];
};

Blockly.Python['robot_setup'] = function(block) {

    var YR_pin = block.getFieldValue('YR_PIN');
    var YL_pin = block.getFieldValue('YL_PIN');
    var RR_pin = block.getFieldValue('RR_PIN');
    var RL_pin = block.getFieldValue('RL_PIN');

    Blockly.Python.reservePin(
        block, YR_pin, Blockly.Python.PinTypes.OUTPUT, 'YR_pin');
    Blockly.Python.reservePin(
        block, YL_pin, Blockly.Python.PinTypes.OUTPUT, 'YL_pin');
    Blockly.Python.reservePin(
        block, RR_pin, Blockly.Python.PinTypes.OUTPUT, 'RR_pin');
    Blockly.Python.reservePin(
        block, RL_pin, Blockly.Python.PinTypes.OUTPUT, 'RL_pin');

    Blockly.Python.addInclude('servo', '#include <Servo.h>');
    Blockly.Python.addInclude('oscillator', '#include <Oscillator.h>');
    Blockly.Python.addInclude('EEPROM', '#include <EEPROM.h>');
    Blockly.Python.addInclude('SERVO_QUANT_AMOUNTS', '#define N_SERVOS    4');
    Blockly.Python.addInclude('EEPROM_TRIM', '#define EEPROM_TRIM false');
    Blockly.Python.addInclude('SERVO_TRIMRR', '#define TRIM_RR    7');
    Blockly.Python.addInclude('SERVO_TRIMRL', '#define TRIM_RL    4');
    Blockly.Python.addInclude('SERVO_TRIMYR', '#define TRIM_YR    4');
    Blockly.Python.addInclude('SERVO_TRIMYL', '#define TRIM_YL    7\n');

    var robot_def = '#define PIN_YR    ' + YR_pin + '\n' +
        '#define PIN_YL    ' + YL_pin + '\n' +
        '#define PIN_RR    ' + RR_pin + '\n' +
        '#define PIN_RL    ' + RL_pin + '\n' +
        '#define INTERVALTIME    10.0\n\n' +
        'Oscillator servo[N_SERVOS];\n\n' +
        'void goingUp(int tempo);\n' +
        'void drunk (int tempo);\n' +
        'void noGravity(int tempo);\n' +
        'void kickLeft(int tempo);\n' +
        'void kickRight(int tempo);\n' +
        'void run(int steps, int T = 500);\n' +
        'void walk(int steps, int T = 1000);\n' +
        'void backyard(int steps, int T = 3000);\n' +
        'void backyardSlow(int steps, int T = 5000);\n' +
        'void turnLeft(int steps, int T = 3000);\n' +
        'void turnRight(int steps, int T = 3000);\n' +
        'void moonWalkLeft(int steps, int T = 1000);\n' +
        'void moonWalkRight(int steps, int T = 1000);\n' +
        'void crusaito(int steps, int T = 1000);\n' +
        'void swing(int steps, int T = 1000);\n' +
        'void upDown(int steps, int T = 1000);\n' +
        'void flapping(int steps, int T = 1000);\n';

    Blockly.Python.addDeclaration('robot_def', robot_def);


    var servo_setup = 'servo[0].attach(PIN_RR);\n' +
        '  servo[1].attach(PIN_RL);\n' +
        '  servo[2].attach(PIN_YR);\n' +
        '  servo[3].attach(PIN_YL);';
    var trim_setup = 'int trim;';
    var eeprom_setup = 'if (EEPROM_TRIM) {\n' +
        '    for (int x = 0; x < 4; x++) {\n' +
        '    trim = EEPROM.read(x);\n' +
        '    if (trim > 128)trim = trim - 256;\n' +
        '      Serial.print("TRIM ");\n' +
        '      Serial.print(x);\n' +
        '      Serial.print(" en ");\n' +
        '      Serial.println(trim);\n' +
        '      servo[x].SetTrim(trim);\n    }\n  }\n' +
        '  else {\n' +
        '    servo[0].SetTrim(TRIM_RR);\n' +
        '    servo[1].SetTrim(TRIM_RL);\n' +
        '    servo[2].SetTrim(TRIM_YR);\n' +
        '    servo[3].SetTrim(TRIM_YL);\n  }';
    Blockly.Python.addSetup('servo_setup', servo_setup);
    Blockly.Python.addSetup('trim_setup', trim_setup);
    Blockly.Python.addSetup('eeprom_setup', eeprom_setup);

    var dance = 'void dance() {' +
        '\n  for ( int i = 0; i < 4; i++ ) {' +
        '\n    servo[i].SetPosition(90);\n  }' +
        '\n  primera_parte();' +
        '\n  segunda_parte();' +
        '\n  moonWalkLeft(4, t * 2);' +
        '\n  moonWalkRight(4, t * 2);' +
        '\n  moonWalkLeft(4, t * 2);' +
        '\n  moonWalkRight(4, t * 2);' +
        '\n  primera_parte();' +
        '\n  crusaito(1, t * 8);' +
        '\n  crusaito(1, t * 7);' +
        '\n  for (int i = 0; i < 16; i++) {' +
        '\n    flapping(1, t / 4);' +
        '\n    delay(3 * t / 4);\n  }' +
        '\n  moonWalkRight(4, t * 2);' +
        '\n  moonWalkLeft(4, t * 2);' +
        '\n  moonWalkRight(4, t * 2);' +
        '\n  moonWalkLeft(4, t * 2);' +
        '\n  drunk(t * 4);' +
        '\n  drunk(t * 4);' +
        '\n  kickLeft(t);' +
        '\n  kickRight(t);' +
        '\n  delay(t * 4);' +
        '\n  drunk(t / 2);' +
        '\n  delay(t * 4);' +
        '\n  walk(2, t * 2);' +
        '\n  backyard(2, t * 2);' +
        '\n  goingUp(t * 2);' +
        '\n  goingUp(t * 1);' +
        '\n  noGravity(t * 2);' +
        '\n  crusaito(1, t * 2);' +
        '\n  crusaito(1, t * 8);' +
        '\n  crusaito(1, t * 2);' +
        '\n  crusaito(1, t * 8);' +
        '\n  crusaito(1, t * 2);' +
        '\n  crusaito(1, t * 3);' +
        '\n  delay(t);' +
        '\n  primera_parte();' +
        '\n  for (int i = 0; i < 32; i++) {' +
        '\n    flapping(1, t / 2);' +
        '\n    delay(t / 2);\n  }' +
        '\n  for ( int i = 0; i < 4; i++ ) {' +
        '\n    servo[i].SetPosition(90);\n  }\n}\n';
    Blockly.Python.addFunction('dance', dance);

    var oscillate = 'void oscillate(int A[N_SERVOS], int O[N_SERVOS], int T, double phase_diff[N_SERVOS]) {' +
        '\n  for (int i = 0; i < 4; i++) {' +
        '\n    servo[i].SetO(O[i]);' +
        '\n    servo[i].SetA(A[i]);' +
        '\n    servo[i].SetT(T);' +
        '\n    servo[i].SetPh(phase_diff[i]);\n  }' +
        '\n  double ref = millis();' +
        '\n  for (double x = ref; x < T + ref; x = millis()) {' +
        '\n    for (int i = 0; i < 4; i++) {' +
        '\n      servo[i].refresh();' +
        '\n    }' +
        '\n  }\n}';
    Blockly.Python.addFunction('oscillate', oscillate);

    var moveNServos = 'unsigned long final_time;' +
        '\nunsigned long interval_time;' +
        '\nint oneTime;' +
        '\nint iteration;' +
        '\nfloat increment[N_SERVOS];' +
        '\nint oldPosition[] = {90, 90, 90, 90};\n\n' +
        'void moveNServos(int time, int  newPosition[]) {' +
        '\n  for (int i = 0; i < N_SERVOS; i++)  increment[i] = ((newPosition[i]) - oldPosition[i]) / (time / INTERVALTIME);' +
        '\n  final_time =  millis() + time;' +
        '\n  iteration = 1;' +
        '\n  while (millis() < final_time) { //Javi del futuro cambia esto' +
        '\n    interval_time = millis() + INTERVALTIME;' +
        '\n    oneTime = 0;' +
        '\n    while (millis() < interval_time) {' +
        '\n      if (oneTime < 1) {' +
        '\n        for (int i = 0; i < N_SERVOS; i++) {' +
        '\n          servo[i].SetPosition(oldPosition[i] + (iteration * increment[i]));' +
        '\n        }' +
        '\n        iteration++;' +
        '\n        oneTime++;' +
        '\n      }' +
        '\n    }' +
        '\n  }' +
        '\n  for (int i = 0; i < N_SERVOS; i++) {' +
        '\n    oldPosition[i] = newPosition[i];' +
        '\n  }\n}';
    Blockly.Python.addFunction('moveNServos', moveNServos);

    var goingUp = 'void goingUp(int tempo) {' +
        '\n  pause = millis();' +
        '\n  for (int i = 0; i < 4; i++) servo[i].SetPosition(90);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(80);' +
        '\n  servo[1].SetPosition(100);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(70);' +
        '\n  servo[1].SetPosition(110);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(60);' +
        '\n  servo[1].SetPosition(120);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(50);' +
        '\n  servo[1].SetPosition(130);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(40);' +
        '\n  servo[1].SetPosition(140);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(30);' +
        '\n  servo[1].SetPosition(150);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(20);' +
        '\n  servo[1].SetPosition(160);' +
        '\n  delay(tempo);' +
        '\n  while (millis() < pause + 8 * t);\n}';
    Blockly.Python.addFunction('goingUp', goingUp);

    var primera_parte = 'void primera_parte() {' +
        '\n  int move1[4] = {60, 120, 90, 90};' +
        '\n  int move2[4] = {90, 90, 90, 90};' +
        '\n  int move3[4] = {40, 140, 90, 90};' +
        '\n  for (int x = 0; x < 3; x++) {' +
        '\n    for (int i = 0; i < 3; i++) {' +
        '\n      lateral_fuerte(1, t / 2);' +
        '\n      lateral_fuerte(0, t / 4);' +
        '\n      lateral_fuerte(1, t / 4);' +
        '\n      delay(t);\n    }' +
        '\n    pause = millis();' +
        '\n    for (int i = 0; i < 4; i++) servo[i].SetPosition(90);' +
        '\n    moveNServos(t * 0.4, move1);' +
        '\n    moveNServos(t * 0.4, move2);' +
        '\n    while (millis() < (pause + t * 2));\n  }' +
        '\n  for (int i = 0; i < 2; i++) {' +
        '\n    lateral_fuerte(1, t / 2);' +
        '\n    lateral_fuerte(0, t / 4);' +
        '\n    lateral_fuerte(1, t / 4);' +
        '\n    delay(t);\n  }' +
        '\n  pause = millis();' +
        '\n  for (int i = 0; i < 4; i++) servo[i].SetPosition(90);' +
        '\n  crusaito(1, t * 1.4);' +
        '\n  moveNServos(t * 1, move3);' +
        '\n  for (int i = 0; i < 4; i++) servo[i].SetPosition(90);' +
        '\n  while (millis() < (pause + t * 4));\n}';
    Blockly.Python.addFunction('primera_parte', primera_parte);

    var segunda_parte = 'void segunda_parte() {' +
        '\n  int move1[4] = {90, 90, 80, 100};' +
        '\n  int move2[4] = {90, 90, 100, 80};' +
        '\n  int move3[4] = {90, 90, 80, 100};' +
        '\n  int move4[4] = {90, 90, 100, 80};' +
        '\n  int move5[4] = {40, 140, 80, 100};' +
        '\n  int move6[4] = {40, 140, 100, 80};' +
        '\n  int move7[4] = {90, 90, 80, 100};' +
        '\n  int move8[4] = {90, 90, 100, 80};' +
        '\n  int move9[4] = {40, 140, 80, 100};' +
        '\n  int move10[4] = {40, 140, 100, 80};' +
        '\n  int move11[4] = {90, 90, 80, 100};' +
        '\n  int move12[4] = {90, 90, 100, 80};' +
        '\n  for (int x = 0; x < 7; x++) {' +
        '\n    for (int i = 0; i < 3; i++) {' +
        '\n      pause = millis();' +
        '\n      moveNServos(t * 0.15, move1);' +
        '\n      moveNServos(t * 0.15, move2);' +
        '\n      moveNServos(t * 0.15, move3);' +
        '\n      moveNServos(t * 0.15, move4);' +
        '\n      while (millis() < (pause + t));\n    }' +
        '\n    pause = millis();' +
        '\n    moveNServos(t * 0.15, move5);' +
        '\n    moveNServos(t * 0.15, move6);' +
        '\n    moveNServos(t * 0.15, move7);' +
        '\n    moveNServos(t * 0.15, move8);' +
        '\n    while (millis() < (pause + t));\n  }' +
        '\n  for (int i = 0; i < 3; i++) {' +
        '\n    pause = millis();' +
        '\n    moveNServos(t * 0.15, move9);' +
        '\n    moveNServos(t * 0.15, move10);' +
        '\n    moveNServos(t * 0.15, move11);' +
        '\n    moveNServos(t * 0.15, move12);' +
        '\n    while (millis() < (pause + t));' +
        '\n  }\n}';
    Blockly.Python.addFunction('segunda_parte', segunda_parte);

    var lateral_fuerte = 'void lateral_fuerte(boolean side, int tempo) {' +
        '\n  for (int i = 0; i < 4; i++) servo[i].SetPosition(90);' +
        '\n  if (side) servo[0].SetPosition(40);' +
        '\n  else servo[1].SetPosition(140);' +
        '\n  delay(tempo / 2);' +
        '\n  servo[0].SetPosition(90);' +
        '\n  servo[1].SetPosition(90);' +
        '\n  delay(tempo / 2);\n}';
    Blockly.Python.addFunction('lateral_fuerte', lateral_fuerte);

    var drunk = 'void drunk (int tempo) {' +
        '\n  pause = millis();' +
        '\n  int move1[] = {60, 70, 90, 90};' +
        '\n  int move2[] = {110, 120, 90, 90};' +
        '\n  int move3[] = {60, 70, 90, 90};' +
        '\n  int move4[] = {110, 120, 90, 90};' +
        '\n  moveNServos(tempo * 0.235, move1);' +
        '\n  moveNServos(tempo * 0.235, move2);' +
        '\n  moveNServos(tempo * 0.235, move3);' +
        '\n  moveNServos(tempo * 0.235, move4);' +
        '\n  while (millis() < (pause + tempo));\n}';
    Blockly.Python.addFunction('drunk', drunk);

    var noGravity = 'void noGravity(int tempo) {' +
        '\n  int move1[4] = {120, 140, 90, 90};' +
        '\n  int move2[4] = {140, 140, 90, 90};' +
        '\n  int move3[4] = {120, 140, 90, 90};' +
        '\n  int move4[4] = {90, 90, 90, 90};' +
        '\n  for (int i = 0; i < 4; i++) servo[i].SetPosition(90);' +
        '\n  for (int i = 0; i < N_SERVOS; i++) oldPosition[i] = 90;' +
        '\n  moveNServos(tempo * 2, move1);' +
        '\n  moveNServos(tempo * 2, move2);' +
        '\n  delay(tempo * 2);' +
        '\n  moveNServos(tempo * 2, move3);' +
        '\n  moveNServos(tempo * 2, move4);\n}';
    Blockly.Python.addFunction('noGravity', noGravity);

    var kickLeft = 'void kickLeft(int tempo) {' +
        '\n  for (int i = 0; i < 4; i++) servo[i].SetPosition(90);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(50);' +
        '\n  servo[1].SetPosition(70);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(80);' +
        '\n  servo[1].SetPosition(70);' +
        '\n  delay(tempo / 4);' +
        '\n  servo[0].SetPosition(30);' +
        '\n  servo[1].SetPosition(70);' +
        '\n  delay(tempo / 4);' +
        '\n  servo[0].SetPosition(80);' +
        '\n  servo[1].SetPosition(70);' +
        '\n  delay(tempo / 4);' +
        '\n  servo[0].SetPosition(30);' +
        '\n  servo[1].SetPosition(70);' +
        '\n  delay(tempo / 4);' +
        '\n  servo[0].SetPosition(80);' +
        '\n  servo[1].SetPosition(70);' +
        '\n  delay(tempo);\n}';
    Blockly.Python.addFunction('kickLeft', kickLeft);

    var kickRight = 'void kickRight(int tempo) {' +
        '\n  for (int i = 0; i < 4; i++) servo[i].SetPosition(90);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(110);' +
        '\n  servo[1].SetPosition(130);' +
        '\n  delay(tempo);' +
        '\n  servo[0].SetPosition(110);' +
        '\n  servo[1].SetPosition(100);' +
        '\n  delay(tempo / 4);' +
        '\n  servo[0].SetPosition(110);' +
        '\n  servo[1].SetPosition(150);' +
        '\n  delay(tempo / 4);' +
        '\n  servo[0].SetPosition(110);' +
        '\n  servo[1].SetPosition(80);' +
        '\n  delay(tempo / 4);' +
        '\n  servo[0].SetPosition(110);' +
        '\n  servo[1].SetPosition(150);' +
        '\n  delay(tempo / 4);' +
        '\n  servo[0].SetPosition(110);' +
        '\n  servo[1].SetPosition(100);' +
        '\n  delay(tempo);\n}';
    Blockly.Python.addFunction('kickRight', kickRight);

    var walk = 'void walk(int steps, int T) {' +
        '\n  int A[4] = {15, 15, 30, 30};' +
        '\n  int O[4] = {0, 0, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('walk', walk);

    var run = 'void run(int steps, int T) {' +
        '\n  int A[4] = {10, 10, 10, 10};' +
        '\n  int O[4] = {0, 0, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('run', run);

    var backyard = 'void backyard(int steps, int T) {' +
        '\n  int A[4] = {15, 15, 30, 30};' +
        '\n  int O[4] = {0, 0, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90), DEG2RAD(-90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('backyard', backyard);

    var backyardSlow = 'void backyardSlow(int steps, int T) {' +
        '\n  int A[4] = {15, 15, 30, 30};' +
        '\n  int O[4] = {0, 0, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(-90), DEG2RAD(-90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('backyardSlow', backyardSlow);

    var turnLeft = 'void turnLeft(int steps, int T) {' +
        '\n  int A[4] = {20, 20, 10, 30};' +
        '\n  int O[4] = {0, 0, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('turnLeft', turnLeft);

    var turnRight = 'void turnRight(int steps, int T) {' +
        '\n  int A[4] = {20, 20, 30, 10};' +
        '\n  int O[4] = {0, 0, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('turnRight', turnRight);

    var moonWalkRight = 'void moonWalkRight(int steps, int T) {' +
        '\n  int A[4] = {25, 25, 0, 0};' +
        '\n  int O[4] = { -15 , 15, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('turnRight', turnRight);

    var moonWalkLeft = 'void moonWalkLeft(int steps, int T) {' +
        '\n  int A[4] = {25, 25, 0, 0};' +
        '\n  int O[4] = { -15, 15, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 - 120), DEG2RAD(90), DEG2RAD(90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('turnRight', turnRight);

    var crusaito = 'void crusaito(int steps, int T) {' +
        '\n  int A[4] = {25, 25, 30, 30};' +
        '\n  int O[4] = { - 15, 15, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180 + 120), DEG2RAD(90), DEG2RAD(90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('crusaito', crusaito);

    var swing = 'void swing(int steps, int T) {' +
        '\n  int A[4] = {25, 25, 0, 0};' +
        '\n  int O[4] = { -15, 15, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('swing', swing);

    var upDown = 'void upDown(int steps, int T) {' +
        '\n  int A[4] = {25, 25, 0, 0};' +
        '\n  int O[4] = { -15, 15, 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(180), DEG2RAD(0), DEG2RAD(270), DEG2RAD(270)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('upDown', upDown);

    var flapping = 'void flapping(int steps, int T) {' +
        '\n  int A[4] = {15, 15, 8, 8};' +
        '\n  int O[4] = { -A[0], A[1], 0, 0};' +
        '\n  double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90), DEG2RAD(-90)};' +
        '\n  for (int i = 0; i < steps; i++)oscillate(A, O, T, phase_diff);\n}';
    Blockly.Python.addFunction('flapping', flapping);
}

Blockly.Python['robot_set_movement'] = function(block) {
    var value_movement = Blockly.Python.valueToCode(block, 'MOVEMENT', Blockly.Python.ORDER_ATOMIC);
    var value_tempo = Blockly.Python.valueToCode(block, 'TEMPO', Blockly.Python.ORDER_ATOMIC);
    var robot_setmovement_def = 'int t = ' + value_tempo + ';\n' +
        'double pause = 0;\n';
    Blockly.Python.addDeclaration('robot_setmovement_def', robot_setmovement_def);
    var steps = "";
    switch (value_movement) {
        case "goingUp":
        case "drunk":
        case "kickLeft":
        case "kickRight":
            steps = '(';
            break;
        default:
            steps = '(1, ';
            break;
    }
    var code = value_movement + steps + ' 2*t);\n';
    return code;
};

Blockly.Python['robot_movement'] = function(block) {
    var dropdown_movement = block.getFieldValue('MOVEMENT');
    var code = dropdown_movement;
    return [code, Blockly.Python.ORDER_ATOMIC];
};

Blockly.Python['robot_tempo'] = function(block) {
    var number_tempo = block.getFieldValue('TEMPO');
    var code = number_tempo;
    return [code, Blockly.Python.ORDER_ATOMIC];
};