//  EVShield
// Author: William L
//
// Purpose:
//          A custom serial communication for Ryersons MECHBOT and EVShield. The Mechbot was made
//          such that only 2 pins on both Arduinos are available for communication. On the 
//          Mechbot, only pins PC4 and PC5 are available and on the EVShield only PB1 and PB0 are 
//          available. 
//
#include <Wire.h>
#include <EVShield.h>
#include <USART.h> 
#include <avr/io.h>
#include <mechbotShield.h>

// CODE APPENDIX
//
// 10010 Pick up object on left side
// 10100 pick up object on right side
// 11000 blink EVShield
// 10110 drop object
// 11100 set top claw


//GLOBAL VARIABLES

int data[5]={0,0,0,0,0};
int data_As_Int;
int BUT3 = PD5;
int lego_In = PB0; 
int lego_Out = PB1;

EVShield    evshield(0x34,0x36);

int completion(void){
//------FOR WHEN A SCENARIO IS COMPLETED--------
  while (PIND & (1<<BUT3)){
     if ((PIND & (1<<BUT3)) ==0 ){ 
         printString("\n Completed Task \n");
         PORTB |= (1<<lego_Out); //turn high
         delay_ms(100);
         PORTB &=~(1<<lego_Out); //turn back to low
         break;
        }
  }
}

int recieve_Data(void){
//-----WHEN RECEIVING SIGNAL FROM MECHBOT-----    
      for (int x=0; x <5; x++){
        if (PINB & (1<<lego_In)==1){
          data[x]=1;
        }
        else {
          data[x]=0;
        }
        Serial.print(data[x]);
        delay_ms(220); 
      }
      Serial.print("\n");
      data_As_Int= (data[0]*10000)+(data[1]*1000)+(data[2]*100)+(data[3]*10)+(data[4]*1);
      Serial.print("data_As_Int = ");
      Serial.print(data_As_Int);
      Serial.print("\nRecieved Transmission \n");
}


//------------------------------------SETUP------------------------------
void setup() {
 initUSART();

   //setting the data directions
   //COMM PINS
   DDRB |= (1<<lego_Out);
   DDRB &= ~(1<<lego_In);
   DDRD &= ~(1<<BUT3);
   
 }

void loop() {
  
  //WAIT FOR SIGNAL AND READ IN THE SIGNAL
  if ((PINB & (1<<lego_In))==1){ 
     recieve_Data();
     Serial.print("\n");
     completion();
  }

}