/*
 * Eryoma.cpp
 * 
 * Popov Vitaliy
 *
 */

#ifndef Eryoma_cpp
#define Eryoma_cpp

#include "Eryoma.h"

byte Sensors[9] = {0,0,0,0,0,0,0,0,0};

Eryoma::Eryoma()
{
	//A
	pinMode(7,OUTPUT);
	pinMode(41,OUTPUT);
	pinMode(40,OUTPUT);
	//B
	pinMode(6,OUTPUT);
	pinMode(23,OUTPUT);
	pinMode(22,OUTPUT);
	//C
	pinMode(39,OUTPUT);
	pinMode(38,OUTPUT);
	pinMode(5,OUTPUT);
	//D
	pinMode(25,OUTPUT);
	pinMode(24,OUTPUT);
	pinMode(4,OUTPUT);
}

void Eryoma::setSensor(short sensor, byte connector){
	switch(sensor){
		case ultrasonic:
			if(connector > 0 && connector < 9){
				Sensors[connector] = ultrasonic;
				pinMode(connectorDigital[connector][0],OUTPUT);
				pinMode(connectorDigital[connector][1],INPUT);
			}else{
				Serial.println("ERROR");
			}
		break;
		case light:
			if(connector > 0 && connector < 9 && Sensors[connector] == 0){
				Sensors[connector] = light;
				pinMode(connectorAnalog[connector], INPUT);
			}else{
				Serial.println("ERROR");
			}
		break;	
		case sound:
			if(connector > 0 && connector < 9){
				Sensors[connector] = sound;
				pinMode(connectorAnalog[connector], INPUT);
			}else{
				Serial.println("ERROR");
			}
		break;	
		case touch:
			if(connector > 0 && connector < 9){
				Sensors[connector] = touch;
			}else{
				Serial.println("ERROR");
			}
		break;	
		case encoder:
			if(connector > 0 && connector < 9){
				Sensors[connector] = encoder;
				pinMode(connectorAnalog[connector], INPUT);
			}else{
				Serial.println("ERROR");
			}
		break;	
		case arraySensors:
			if(connector > 0 && connector < 9){
				Sensors[connector] = arraySensors;
				pinMode(connectorDigital[connector][2],OUTPUT);
				pinMode(connectorDigital[connector][1],OUTPUT);
				pinMode(connectorDigital[connector][0],OUTPUT);
				pinMode(connectorAnalog[connector], INPUT);
			}else{
				Serial.println("ERROR");
			}
		break;
		default:
			Serial.println("ERROR");
		break;
	}
}

int Eryoma::sensorRead(byte connector){
	switch(Sensors[connector]){
		case ultrasonic:
			digitalWrite(13, HIGH);
			delayMicroseconds(10);
			digitalWrite(13, LOW);
			return pulseIn(11, HIGH)/58;
		break;
		case light:
			return analogRead(connectorAnalog[connector]);
		break;
		case sound:
			return analogRead(connectorAnalog[connector]) * 5.0 / 1024.0 * 1000.0;
		break;
		case touch:
			return digitalRead(49);
		break;
		case encoder:
			return analogRead(connectorAnalog[connector]);
		break;
		case arraySensors:
			for(byte i = 0; i < 8; i++){
				digitalWrite(connectorDigital[connector][2], val_4051[i][2]);
				digitalWrite(connectorDigital[connector][1], val_4051[i][0]);
				digitalWrite(connectorDigital[connector][0], val_4051[i][1]);
				val_arraySensors[i] = analogRead(connectorAnalog[connector]);
				delay(1);
			}
		return 0;
		break;
		default:
			Serial.println("ERROR");
		break;
				
	}
}

void Eryoma::setMotorSpeed(byte motor, short speed){
	if(speed > 0){
		switch(motor){
			case A:
				analogWrite(7, speed);
				digitalWrite(41, HIGH);
				digitalWrite(40, LOW);
			break;
			case B:
				analogWrite(6, speed);
				digitalWrite(23, HIGH);
				digitalWrite(22, LOW);
			break;
			case C:
				analogWrite(5, speed);
				digitalWrite(39, HIGH);
				digitalWrite(38, LOW);
			break;
			case D:
				analogWrite(4, speed);
				digitalWrite(25, HIGH);
				digitalWrite(24, LOW);
			break;
			default:
				Serial.println("ERROR");
			break;
		}
	}else{
			switch(motor){
			case A:
				analogWrite(7, speed);
				digitalWrite(41, LOW);
				digitalWrite(40, HIGH);
			break;
			case B:
				analogWrite(6, speed);
				digitalWrite(23, LOW);
				digitalWrite(22, HIGH);
			break;
			case C:
				analogWrite(5, speed);
				digitalWrite(39, LOW);
				digitalWrite(38, HIGH);
			break;
			case D:
				analogWrite(4, speed);
				digitalWrite(25, LOW);
				digitalWrite(24, HIGH);
			break;
			default:
				Serial.println("ERROR");
			break;
		}
	}
}

#endif 