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

/**
 * @fileoverview Code generator for the test 2 blocks.
 */
'use strict';

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


Blockly.Python['motion_setup'] = function(block) {
    var motion_include = '#include "I2Cdev.h"\n' +
        '#include "QMI8658_6Axis_MotionApps20.h"\n' +
        '#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE\n' +
        '#include "Wire.h"\n' +
        '#endif\n';
    Blockly.Python.addInclude('motion_include', motion_include);

    var motion_declaration = '' +
        'QMI8658 mpu;\n' +
        '\n' +
        '#define _INTERRUPT_PIN 2\n' +
        '#define _LED_PIN 13\n' +
        'bool _blinkState = false;\n' +
        '\n' +
        'bool dmpReady = false;  // set true if DMP init was successful\n' +
        'uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU\n' +
        'uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)\n' +
        'uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)\n' +
        'uint16_t fifoCount;     // count of all bytes currently in FIFO\n' +
        'uint8_t fifoBuffer[64]; // FIFO storage buffer\n' +
        '\n' +
        'Quaternion q;           // [w, x, y, z]         quaternion container\n' +
        'VectorInt16 aa;         // [x, y, z]            accel sensor measurements\n' +
        'VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements\n' +
        'VectorFloat gravity;    // [x, y, z]            gravity vector\n' +
        'float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector\n' +
        '\n' +
        'volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high\n' +
        '\n' +
        'float _motionGetYaw, _motionGetPitch, _motionGetRoll;\n' +
        'int _directionState = 0;\n' +
        'float _directionValue = 0.00;\n' +
        'void dmpDataReady() {\n' +
        'mpuInterrupt = true;\n' +
        '}\n' +
        '';
    Blockly.Python.addDeclaration('motion_declaration', motion_declaration);

    var motion_Setup = '' +
        'void _motionSensorSetup() {\n' +
        '#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE\n' +
        'Wire.begin();\n' +
        'Wire.setClock(400000);\n' +
        '#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE\n' +
        'Fastwire::setup(400, true);\n' +
        '#endif\n' +
        'Serial.begin(115200);\n' +
        '// while (!Serial);\n' +
        'Serial.println(F("Initializing I2C devices..."));\n' +
        'mpu.initialize();\n' +
        'pinMode(_INTERRUPT_PIN, INPUT);\n' +
        'Serial.println(F("Testing device connections..."));\n' +
        'Serial.println(mpu.testConnection() ? F("QMI8658 connection successful") : F("QMI8658 connection failed"));\n' +
        'Serial.println("Waiting for calibrating... 2s");\n' +
        'delay(1000);\n' +
        'Serial.println("Waiting for calibrating... 1s");\n' +
        'delay(1000);\n' +
        'Serial.println(F("Initializing DMP..."));\n' +
        'devStatus = mpu.dmpInitialize();\n' +
        'mpu.setXGyroOffset(220);\n' +
        'mpu.setYGyroOffset(76);\n' +
        'mpu.setZGyroOffset(-85);\n' +
        'mpu.setZAccelOffset(1788);\n' +
        'if (devStatus == 0) {\n' +
        'mpu.CalibrateAccel(6);\n' +
        'mpu.CalibrateGyro(6);\n' +
        'mpu.PrintActiveOffsets();\n' +
        'Serial.println(F("Enabling DMP..."));\n' +
        'mpu.setDMPEnabled(true);\n' +
        'Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));\n' +
        'Serial.print(digitalPinToInterrupt(_INTERRUPT_PIN));\n' +
        'Serial.println(F(")..."));\n' +
        'attachInterrupt(digitalPinToInterrupt(_INTERRUPT_PIN), dmpDataReady, RISING);\n' +
        'mpuIntStatus = mpu.getIntStatus();\n' +
        'Serial.println(F("DMP ready! Waiting for first interrupt..."));\n' +
        'dmpReady = true;\n' +
        'packetSize = mpu.dmpGetFIFOPacketSize();\n' +
        '} else {\n' +
        'Serial.print(F("DMP Initialization failed (code "));\n' +
        'Serial.print(devStatus);\n' +
        'Serial.println(F(")"));\n' +
        '}\n' +
        'pinMode(_LED_PIN, OUTPUT);\n' +
        '}\n' +
        '';
    Blockly.Python.addFunction('motion_SetupFunc', motion_Setup);

    var motion_Loop = '' +
        'void _motionSensorMain() {\n' +
        'if (!dmpReady) return;\n' +
        'while (!mpuInterrupt && fifoCount < packetSize) {\n' +
        'if (mpuInterrupt && fifoCount < packetSize) {\n' +
        'fifoCount = mpu.getFIFOCount();\n' +
        '}\n' +
        '}\n' +
        'mpuInterrupt = false;\n' +
        'mpuIntStatus = mpu.getIntStatus();\n' +
        'fifoCount = mpu.getFIFOCount();\n' +
        'if (fifoCount < packetSize) {\n' +
        '}\n' +
        'else if ((mpuIntStatus & _BV(QMI8658_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {\n' +
        ' mpu.resetFIFO();\n' +
        'Serial.println(F("FIFO overflow!"));\n' +
        '} else if (mpuIntStatus & _BV(QMI8658_INTERRUPT_DMP_INT_BIT)) {\n' +
        'while (fifoCount >= packetSize) {\n' +
        'mpu.getFIFOBytes(fifoBuffer, packetSize);\n' +
        'fifoCount -= packetSize;\n' +
        '}\n' +
        'mpu.dmpGetQuaternion(&q, fifoBuffer);\n' +
        'mpu.dmpGetGravity(&gravity, &q);\n' +
        'mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);\n' +
        '_motionGetYaw = ypr[0] * 180 / M_PI;\n' +
        '_motionGetPitch = ypr[1] * 180 / M_PI;\n' +
        '_motionGetRoll = ypr[2] * 180 / M_PI;\n' +
        'mpu.dmpGetQuaternion(&q, fifoBuffer);\n' +
        'mpu.dmpGetAccel(&aa, fifoBuffer);\n' +
        'mpu.dmpGetGravity(&gravity, &q);\n' +
        'mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);\n' +
        '_blinkState = !_blinkState;\n' +
        'digitalWrite(_LED_PIN, _blinkState);\n' +
        '}\n' +
        '}\n' +
        '';
    Blockly.Python.addFunction('motion_LoopFunc', motion_Loop);

    Blockly.Python.addSetup("motion_setup", "_motionSensorSetup();\n");

    var code = "_motionSensorMain();\n"
    return code;
};

Blockly.Python['motion_onshake'] = function(block) {
    var onshake_func = '' +
        'boolean _motionOnShake() {\n' +
        'if (aaReal.x < -5000 || aaReal.x > 5000 || aaReal.y < -5000 || aaReal.y > 5000 || aaReal.z < -5000 || aaReal.z > 5000) {\n' +
        'return true;\n' +
        '} else {\n' +
        'return false;\n' +
        '}\n' +
        '}\n' +
        '';

    Blockly.Python.addFunction("onShake_function", onshake_func);
    var code = "_motionOnShake()";

    return [code, Blockly.Python.ORDER_ATOMIC];
}

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

    var direction = block.getFieldValue("DIRECTION");
    var ondirection_func = "int _motionOnTilting(int direction) {\n" +
        "  _directionState = 0;\n" +
        "  switch (direction) {\n" +
        "    case 1:\n" +
        "      if (ypr[2] * 180 / M_PI < -20) {\n" +
        "        _directionState = 1;\n" +
        "      }\n" +
        "      break;\n" +
        "    case 2:\n" +
        "      if (ypr[2] * 180 / M_PI > 20) {\n" +
        "        _directionState = 1;\n" +
        "      }\n" +
        "      break;\n" +
        "    case 3:\n" +
        "      if (ypr[1] * 180 / M_PI < -20) {\n" +
        "        _directionState = 1;\n" +
        "      }\n" +
        "      break;\n" +
        "    case 4:\n" +
        "      if (ypr[1] * 180 / M_PI > 20) {\n" +
        "        _directionState = 1;\n" +
        "      }\n" +
        "      break;\n" +
        "    case 5:\n" +
        "      if (ypr[0] * 180 / M_PI +180 - _directionValue > 0) {\n" +
        "        _directionState = 1;\n" +
        "      }\n" +
        "      break;\n" +
        "    case 6:\n" +
        "      if (ypr[0] * 180 / M_PI +180 - _directionValue < 0) {\n" +
        "        _directionState = 1;\n" +
        "      }\n" +
        "      break;\n" +
        "    default:\n" +
        "      break;\n" +
        "  }\n" +
        "  _directionValue = ypr[0] * 180 / M_PI +180;\n" +
        "  return _directionState;\n" +
        "}\n";
    Blockly.Python.addFunction("onDirection_function", ondirection_func);

    var direction_tag
    switch (direction) {
        case "left":
            direction_tag = 1;
            break;
        case "right":
            direction_tag = 2;
            break;
        case "front":
            direction_tag = 3;
            break;
        case "back":
            direction_tag = 4;
            break;
        case "clockwise":
            direction_tag = 5;
            break;
        case "counterClockwise":
            direction_tag = 6;
            break;
        default:
            break;
    }
    var code = "_motionOnTilting(" + direction_tag + ")";
    return [code, Blockly.Python.ORDER_ATOMIC];
}

Blockly.Python['motion_getRotation_ypr'] = function(block) {
    var rotation = block.getFieldValue("Rotation");
    var code = "ypr[" + rotation + "] * 180 / M_PI";
    return [code, Blockly.Python.ORDER_ATOMIC];
}

Blockly.Python['motion_getAcceleration'] = function(block) {
    var acc = block.getFieldValue("Acceleration");
    var code = "aaReal." + acc;
    return [code, Blockly.Python.ORDER_ATOMIC];
}