/*************************************************************************
* File Name          : TestInfaredReceiver.ino
* Author             : Xixoyu

* Version            : V1.0.1
* Date               : 6/23/2013
* Description        : Example for Makeblock Electronic modules of Me - 
                       Infrared Receiver. 
* License            : CC-BY-SA 3.0
* Copyright (C) 2013 Maker Works Technology Co., Ltd. All right reserved.
* http://www.makeblock.cc/
**************************************************************************/
#include <Makeblock.h>
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <Servo.h> //include the Servo library;
#include <Array.h>

const byte size = 1000;
int rawArray[size] = {0};
Array<int> arraypos1 = Array<int>(rawArray,size);
Array<int> arraypos2 = Array<int>(rawArray,size);
Array<int> arrayposservo = Array<int>(rawArray,size);
MeDCMotor motor4(M2);
MePort port(PORT_2);
Servo servoDriver1;  // create servo object to control a servo 
Servo servoDriver2;  // create servo object to control another servo
int servo2pin =  port.pin1();//attaches the servo on PORT_1 SLOT1 to the servo object
int servo1pin =  port.pin2();//attaches the servo on PORT_1 SLOT2 to the servo object
int pos1 = 0;
int pos2 = 0;
int posdef = 85;
int posset = 85;
int counter=0;
int i=0;


MeEncoderMotor motor1(0x09, SLOT1);   //  Motor at slot1
MeEncoderMotor motor2(0x09, SLOT2);   //  motor at slot2

MeInfraredReceiver infraredReceiverDecode(PORT_6); 
const byte LED = 13;
boolean state = false;
boolean play = false;
boolean m1forward = false;
boolean m1backward = false;
boolean m2forward = false;
boolean m2backward = false;

void setup()
{
  pinMode (LED, OUTPUT);



    servoDriver1.attach(servo1pin);  // attaches the servo on servopin1
    servoDriver2.attach(servo2pin);
    servoDriver1.write(posdef);
    motor1.begin();
    motor2.begin();
    infraredReceiverDecode.begin();
    Serial.begin(9600);
    Serial.println("InfraredReceiverDecode Start!");
}

void loop()
{
    if(infraredReceiverDecode.available())
    {
        switch(infraredReceiverDecode.read())
        {
            case IR_BUTTON_A:  state = !state;break;
            case IR_BUTTON_B: Serial.println("Press B.");break;
            case IR_BUTTON_C: pos1=0;pos2=0;posset=85;motor1.Reset();motor2.Reset();break;
            case IR_BUTTON_D: Serial.println("Press D.");break;
            case IR_BUTTON_E: pos1=0;pos2=0;posset=85;break;
            case IR_BUTTON_F: Serial.println("Press F.");break;
            case IR_BUTTON_SETTING: i=0; play = !play; break;
            case IR_BUTTON_UP:  motor4.run(-100);delay(3000);motor4.stop();break;
            case IR_BUTTON_DOWN: motor4.run(100);delay(3000);motor4.stop();break;
            case IR_BUTTON_LEFT: Serial.println("Press Left.");break;
            case IR_BUTTON_RIGHT: Serial.println("Press Right.");break;
            case IR_BUTTON_0: break;
            case IR_BUTTON_1: pos1=pos1-10;Serial.print("Motor1Pos=");Serial.println(motor1.GetCurrentPosition());break;
            case IR_BUTTON_2: pos1=pos1+10;Serial.print("Motor1Pos=");Serial.println(motor1.GetCurrentPosition());break;
            case IR_BUTTON_3: break;
            case IR_BUTTON_4: pos2=pos2-10;Serial.print("Motor2Pos=");Serial.println(motor2.GetCurrentPosition());break;
            case IR_BUTTON_5: pos2=pos2+10;Serial.print("Motor2Pos=");Serial.println(motor2.GetCurrentPosition());break;
            case IR_BUTTON_6: Serial.println("Press 6.");break;
            case IR_BUTTON_7: Serial.println("Press 7.");break;
            case IR_BUTTON_8: posset=posset-5;if (posset<0){posset=0;}Serial.print("ServoPos=");Serial.println(servoDriver1.read());break;
            case IR_BUTTON_9: posset=posset+5;if (posset>180){posset=180;}Serial.print("ServoPos=");Serial.println(servoDriver1.read());break;
            default:break;
        }
    }
    
if(posdef<posset)
{
    posdef++;
    servoDriver1.write(posdef);
    delay(50);
}

if(posdef>posset)
{
    posdef--;
    servoDriver1.write(posdef);
    delay(50);
}
motor1.Move(pos1, 255);
motor2.Move(pos2, 200);

if (state==true)
{
counter++;
if(counter == 500)
{
//static boolean state = false;
  arraypos1[i]=motor1.GetCurrentPosition();
  arraypos2[i]=motor2.GetCurrentPosition();
  arrayposservo[i]=servoDriver1.read();
  counter =0;
  i++;
}
}

if (play==true)
{
  
counter++;
if(counter == 500)
{
//static boolean state = false;
  pos1=arraypos1[i];
  pos2=arraypos2[i];
  posset=arrayposservo[i];
  counter =0;
  i++;
}
}


  digitalWrite (LED, state ? HIGH : LOW);
}