'use strict';

goog.provide('Blockly.Python.robot');
goog.require('Blockly.Python');

/* Blockly.Python['robot_biped_init'] = function(block) {
  var text_pin_11 = block.getFieldValue('pin_11');
  var text_pin_10 = block.getFieldValue('pin_10');
  var text_pin_9 = block.getFieldValue('pin_9');
  var text_pin_6 = block.getFieldValue('pin_6');

  var text_pin_rr = block.getFieldValue('pin_rr');
  var text_pin_rl = block.getFieldValue('pin_rl');
  var text_pin_yr = block.getFieldValue('pin_yr');
  var text_pin_yl = block.getFieldValue('pin_yl');
  var text_intervaltime = block.getFieldValue('intervaltime');
  var robot_biped_pin = '#define PIN_RR ' + text_pin_rr +
             '\n#define PIN_RL ' + text_pin_rl + 
             '\n#define PIN_YR ' + text_pin_yr +
             '\n#define PIN_YL ' + text_pin_yl +
             '\n#define INTERVALTIME ' + text_intervaltime + '\n';
  Blockly.Python.addDeclaration('robot_biped_pin', robot_biped_pin);
  var robot_biped_decl = '#include <Servo.h>\n' +
						'#include <Oscillator.h>\n' +
						'#include <EEPROM.h>\n' +
						'#define N_SERVOS 4\n' +
						'\n' +
						'//-- First step: Configure the pins where the servos are attached\n' +
						'/*\n' +
						'         --------------- \n' +
						'        |     O   O     |\n' +
						'        |---------------|\n' +
						'YR 3==> |               | <== YL 2\n' +
						'         --------------- \n' +
						'            ||     ||\n' +
						'            ||     ||\n' +
						'RR 5==>   -----   ------  <== RL 4\n' +
						'         |-----   ------|\n' +
						'\n' +
						'#define EEPROM_TRIM false \n' +
						'\n' +
						'// Activate to take callibration data from internal memory\n' +
						'#define TRIM_RR 7\n' +
						'#define TRIM_RL 4\n' +
						'#define TRIM_YR  4\n' +
						'#define TRIM_YL -7\n' +
						'//OTTO.setTrims(-7,-4,-4,7);\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' +
						'//Tempo\n' +
						'int t=495;\n'+
						'double pause=0;\n';
  Blockly.Python.addDeclaration('robot_biped_decl', robot_biped_decl);
  var robot_biped_setup = 'Serial.begin(19200);\n' +
							'  servo[0].attach(PIN_RR);\n' +
							'  servo[1].attach(PIN_RL);\n' +
							'  servo[2].attach(PIN_YR);\n' +
							'  servo[3].attach(PIN_YL);\n' +
							'  \n' +
							'  int trim;\n' +
							'  \n' +
							'  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' +
							'  }\n' +
							'  \n' +
							'  for(int i=0;i<3;i++) { servo[i].SetPosition(90); }\n' +
							'  servo[3].SetPosition(20);\n';
  Blockly.Python.addSetup('robot_biped_setup', robot_biped_setup);
  
  //load all robot's actions
  var robot_biped_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' +
							'}\n';
	Blockly.Python.addFunction('robot_biped_oscillate', robot_biped_oscillate);
  //moveNServos needs more declaration
  var robot_biped_moveNServos_decl = 'unsigned long final_time;\n' +
									'unsigned long interval_time;\n' +
									'int oneTime;\n' +
									'int iteration;\n' +
									'float increment[N_SERVOS]; \n' +
									'int oldPosition[]={90,90,90,90};\n';
  Blockly.Python.addDeclaration('robot_biped_moveNServos_decl',robot_biped_moveNServos_decl);

  var robot_biped_moveNServos = 'void moveNServos(int time, int  newPosition[]){\n' +
								'  for(int i=0;i<N_SERVOS;i++) increment[i] = ((newPosition[i])-oldPosition[i])/(time/INTERVALTIME);\n' +
								'  \n' +
								'  final_time =  millis() + time; \n' +
								'  \n' +
								'  iteration = 1; \n' +
								'  while(millis() < final_time){ //Javi del futuro cambia esto  \n' +
								'      interval_time = millis()+INTERVALTIME;  \n' +
								'      \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' +
								'\n' +
								'  for(int i=0;i<N_SERVOS;i++){  \n' +
								'    oldPosition[i] = newPosition[i];\n' +
								'  }   \n' +
								'}\n';
  Blockly.Python.addFunction('robot_biped_moveNServos', robot_biped_moveNServos);

  var robot_biped_goingUp = 'void goingUp(int tempo){\n' +
							'  \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' +
							'      \n' +
							'      while(millis()<pause+8*t);\n' +
							'}\n';
  Blockly.Python.addFunction('robot_biped_goingUp', robot_biped_goingUp);
  var robot_biped_drunk = 'void drunk (int tempo){\n' +
							'  pause=millis();\n' +
							'  \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' +
							'  \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' +
							'}\n';
  Blockly.Python.addFunction('robot_biped_drunk', robot_biped_drunk);
  var robot_biped_noGavity = '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' +
							'  \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' +
							'}\n';
  Blockly.Python.addFunction('robot_biped_noGavity', robot_biped_noGavity);
  var robot_biped_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); //pie derecho\n' +
							'  servo[1].SetPosition(130); //pie izquiero\n' +
							'  delay(tempo);\n' +
							'  servo[0].SetPosition(110); //pie derecho\n' +
							'  servo[1].SetPosition(100); //pie izquiero\n' +
							'  delay(tempo/4);\n' +
							'  servo[0].SetPosition(110); //pie derecho\n' +
							'  servo[1].SetPosition(150); //pie izquiero\n' +
							'  delay(tempo/4);\n' +
							'  servo[0].SetPosition(110); //pie derecho\n' +
							'  servo[1].SetPosition(80); //pie izquiero\n' +
							'  delay(tempo/4);\n' +
							'  servo[0].SetPosition(110); //pie derecho\n' +
							'  servo[1].SetPosition(150); //pie izquiero\n' +
							'  delay(tempo/4);\n' +
							'  servo[0].SetPosition(110); //pie derecho\n' +
							'  servo[1].SetPosition(100); //pie izquiero\n' +
							'  delay(tempo);\n' +
							'}\n';
  Blockly.Python.addFunction('robot_biped_kickRight', robot_biped_kickRight);
  var robot_biped_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); //pie derecho\n' +
								'  servo[1].SetPosition(70); //pie izquiero\n' +
								'  delay(tempo);\n' +
								'  servo[0].SetPosition(80); //pie derecho\n' +
								'  servo[1].SetPosition(70); //pie izquiero\n' +
								'  delay(tempo/4);\n' +
								'  servo[0].SetPosition(30); //pie derecho\n' +
								'  servo[1].SetPosition(70); //pie izquiero\n' +
								'  delay(tempo/4);\n' +
								'  servo[0].SetPosition(80); //pie derecho\n' +
								'  servo[1].SetPosition(70); //pie izquiero\n' +
								'  delay(tempo/4);\n' +
								'  servo[0].SetPosition(30); //pie derecho\n' +
								'  servo[1].SetPosition(70); //pie izquiero\n' +
								'  delay(tempo/4);\n' +
								'  servo[0].SetPosition(80); //pie derecho\n' +
								'  servo[1].SetPosition(70); //pie izquiero\n' +
								'  delay(tempo);\n' +
								'}\n';
  Blockly.Python.addFunction('robot_biped_kickLeft', robot_biped_kickLeft);
  var robot_biped_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' +
						'    \n' +
						'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
						'}\n';
  Blockly.Python.addFunction('robot_biped_walk', robot_biped_walk);
  var robot_biped_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' +
						'    \n' +
						'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
						'}\n';
  Blockly.Python.addFunction('robot_biped_run', robot_biped_run);
  var robot_biped_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' +
							'    \n' +
							'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
							'}\n';
  Blockly.Python.addFunction('robot_biped_backyard', robot_biped_backyard);
  var robot_biped_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' +
								'    \n' +
								'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
								'}\n';
  Blockly.Python.addFunction('robot_biped_backyardSlow', robot_biped_backyardSlow);
  var robot_biped_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' +
							'    \n' +
							'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
							'}\n';
  Blockly.Python.addFunction('robot_biped_turnLeft', robot_biped_turnLeft);

  var robot_biped_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' +
								'    \n' +
								'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
								'}\n';
  Blockly.Python.addFunction('robot_biped_turnRight', robot_biped_turnRight);
  var robot_biped_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' +
									'    \n' +
									'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
									'}\n';
  Blockly.Python.addFunction('robot_biped_moonWalkRight', robot_biped_moonWalkRight);
  var robot_biped_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' +
									'    \n' +
									'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
									'}\n';
  Blockly.Python.addFunction('robot_biped_moonWalkLeft', robot_biped_moonWalkLeft);
  var robot_biped_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' +
							'    \n' +
							'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
							'}\n';
  Blockly.Python.addFunction('robot_biped_crusaito', robot_biped_crusaito);
  var robot_biped_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' +
							'    \n' +
							'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
							'}\n';
  Blockly.Python.addFunction('robot_biped_swing', robot_biped_swing);
  var robot_biped_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' +
							'    \n' +
							'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' +
							'}\n';
  Blockly.Python.addFunction('robot_biped_upDown', robot_biped_upDown);
  var robot_biped_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' + 
							'    \n' + 
							'    for(int i=0;i<steps;i++)oscillate(A,O, T, phase_diff);\n' + 
							'}\n';
  Blockly.Python.addFunction('robot_biped_flapping', robot_biped_flapping);
  var robot_biped_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' + 
									'}\n';
  Blockly.Python.addFunction('robot_biped_lateral_fuerte', robot_biped_lateral_fuerte);
  var robot_biped_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' +
					'    \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' +
					'       \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' +
					'  \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' +
					' \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' +
					'}\n';
  Blockly.Python.addFunction('robot_biped_segunda_parte', robot_biped_segunda_parte);
  var robot_biped_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' +
					'    \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' +
					'}\n';
  Blockly.Python.addFunction('robot_biped_primera_parte', robot_biped_primera_parte);

  return '';
};

Blockly.Python['robot_biped_movement'] = function(block) {
  var movement = block.getFieldValue('robot_biped_movement');
  return movement + '\n';
};

Blockly.Python['robot_biped_initialize'] = function(block) {
  var code = 'for(int i=0;i<4;i++) servo[i].SetPosition(90);';
  return code;
};*/