#include "SpiTest.h"

#include <fsfw/serviceinterface/ServiceInterface.h>

#include <fcntl.h>
#include <sys/ioctl.h>



SpiTest::SpiTest(object_id_t objectId): SystemObject(objectId) {
    sif::info << "Setting up Raspberry Pi WiringPi library." << std::endl;
//  wiringPiSetupGpio();

//	 pinMode(SS_MGM_0_LIS3, OUTPUT);
//    pinMode(SS_MGM_1_RM, OUTPUT);
//    pinMode(SS_GYRO_0_ADIS, OUTPUT);
//    pinMode(SS_GYRO_1_L3G, OUTPUT);
//    pinMode(SS_GYRO_2_L3G, OUTPUT);
//    pinMode(SS_MGM_2_LIS3, OUTPUT);
//    pinMode(SS_MGM_3_RM, OUTPUT);
//
//    digitalWrite(SS_MGM_0_LIS3, HIGH);
//    digitalWrite(SS_MGM_1_RM, HIGH);
//    digitalWrite(SS_GYRO_0_ADIS, HIGH);
//    digitalWrite(SS_GYRO_1_L3G, HIGH);
//    digitalWrite(SS_GYRO_2_L3G, HIGH);
//    digitalWrite(SS_MGM_2_LIS3, HIGH);
//    digitalWrite(SS_MGM_3_RM, HIGH);

	int spiFd = open(spiDeviceName.c_str(), O_RDWR);
	if (spiFd < 0){
		sif::error << "Could not open SPI device!" << std::endl;
	}

	spiMode = SPI_MODE_3;
	int ret = ioctl(spiFd, SPI_IOC_WR_MODE, &spiMode);
	if(ret < 0) {
		sif::error << "Could not set write mode!" << std::endl;
	}

	/* Datenrate setzen */
	ret = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &spiSpeed);
	if(ret < 0) {
		sif::error << "Could not SPI speed!" << std::endl;
	}
}

ReturnValue_t SpiTest::performOperation(uint8_t opCode) {
	if(oneShot) {

	}
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SpiTest::initialize() {
	//transferHandle.rx_buf = reinterpret_cast<__u64>(receiveBuffer);
	//transferHandle.tx_buf = reinterpret_cast<__u64>(sendBuffer);
	//transferHandle.speed_hz = 976000;
	//transferHandle.len = 2;
	return HasReturnvaluesIF::RETURN_OK;
}