#include "sys/alt_stdio.h"
#include "system.h"

void _wait(loop_count) // _wait(600) is 1ms, usleep(700) is 1ms
int loop_count;
{
	volatile int sum, data;
	sum	= 0;
	for (data = 0; data < loop_count; data++) {
		sum = (data << 8);
	}
  return;
}

void SCCBSendBit(int data) {
	*(volatile unsigned long *)(SCL_BASE + 1*4) = 1; // clock dir is output
	*(volatile unsigned long *)SCL_BASE = 0;
	*(volatile unsigned long *)(SDA_BASE + 1*4) = 1; // data dir is output
	*(volatile unsigned long *)SDA_BASE = 0;
	_wait(5);
	if(data == 0) {
		*(volatile unsigned long *)SDA_BASE = 0;
	} else {
		*(volatile unsigned long *)SDA_BASE = 1;
	}
	_wait(1);
	*(volatile unsigned long *)SCL_BASE = 1;
	_wait(5);
	*(volatile unsigned long *)SCL_BASE = 0;
}

int SCCBReceiveBit() {
	unsigned long res;
	*(volatile unsigned long *)(SCL_BASE + 1*4) = 1; // clock dir is output
	*(volatile unsigned long *)SCL_BASE = 0;
	*(volatile unsigned long *)(SDA_BASE + 1*4) = 0; // data dir is input
	_wait(5);
	*(volatile unsigned long *)SCL_BASE = 1;
	_wait(5);
	res = *(volatile unsigned long *)SDA_BASE;
	_wait(5);
	*(volatile unsigned long *)SCL_BASE = 0;
	return res;
}

int SCCBSendByte(int data) {
	unsigned long mask = 1<<7;
	int i;

	for(i = 0; i < 8; i++) {
		SCCBSendBit(data & mask);
		mask = mask >> 1;
	}
	return SCCBReceiveBit();
}

unsigned short SCCBReceiveByte() {
	unsigned short res = 0;
	int i;
	for(i = 0; i < 8; i++) {
		res = res << 1;
		if(SCCBReceiveBit()) {
			res |= 0x01;
		}
	}
	SCCBSendBit(1);
	return res;
}
void SCCBStart() {
	_wait(5);
	*(volatile unsigned long *)(SDA_BASE + 1*4) = 1; // data dir is output
	*(volatile unsigned long *)SDA_BASE = 1;
	*(volatile unsigned long *)(SCL_BASE + 1*4) = 1; // clock dir is output
	*(volatile unsigned long *)SCL_BASE = 1;
	_wait(5);
	*(volatile unsigned long *)SDA_BASE = 0;
	_wait(5);
	*(volatile unsigned long *)SCL_BASE = 0;
}

void SCCBStop() {
	_wait(5);
	*(volatile unsigned long *)(SDA_BASE + 1*4) = 1; // data dir is output
	*(volatile unsigned long *)SDA_BASE = 0;
	*(volatile unsigned long *)(SCL_BASE + 1*4) = 1; // clock dir is output
	*(volatile unsigned long *)SCL_BASE = 1;
	_wait(5);
	*(volatile unsigned long *)SDA_BASE = 1;
	_wait(5);
//	*(volatile unsigned long *)SCL_BASE = 0;
	*(volatile unsigned long *)(SDA_BASE + 1*4) = 0; // data dir is input
	*(volatile unsigned long *)(SCL_BASE + 1*4) = 0; // clock dir is input
	_wait(10);
}

void SCCBWrite(int addr, int data) {
	int devID = 0x80;

	SCCBStart();
	SCCBSendByte(devID);
//	SCCBSendByte((addr >> 8) & 0xFF); // for OV5642
	SCCBSendByte(addr & 0xFF);
	SCCBSendByte(data);
	SCCBStop();
}

unsigned short SCCBRead(int addr) {
	unsigned short res;
	int devID = 0x80;

	SCCBStart();
	SCCBSendByte(devID);
	SCCBSendByte(addr);

	SCCBStart();
	SCCBSendByte(devID|0x01);
	res = SCCBReceiveByte();
	SCCBStop();
	return res;
}

int main()
{
	int i;
	float osc_clock, update_rate, resolution;
	float duty_cycle_max, duty_cycle_min, max_travel;
	float pwm_period, pwm_min, pwm_max, control_angle_0, control_angle_1;
	int prescale_value, old_mode, new_mode, pwm_duty_0, pwm_duty_1;

	// PCA9685のレジスタ名とアドレス
	int MODE1_REGISTER = 0x00;
	int MODE2_REGISTER = 0x01;
	int ALL_LED_ON_L_REG = 0xFA;
	int ALL_LED_ON_H_REG  = 0xFB;
	int ALL_LED_OFF_L_REG = 0xFC;
	int ALL_LED_OFF_H_REG = 0xFD;
	int OUTDRV = 0x04;
	int ALLCALL = 0x01;
	int PRE_SCALE_REGISTER = 0xFE;
	int RESTART = 0x80;
	int LED0_ON_L_REG = 0x06;
	int LED0_ON_H_REG = 0x07;
	int LED0_OFF_L_REG = 0x08;
	int LED0_OFF_H_REG = 0x09;
	int LED1_ON_L_REG = 0x0a;
	int LED1_ON_H_REG = 0x0b;
	int LED1_OFF_L_REG = 0x0c;
	int LED1_OFF_H_REG = 0x0d;
	int tmpint;
	unsigned long colsel;
	unsigned long anspos, answer;
	unsigned long blockH, blockV;

	// ALL_LED full ON/OFF をクリア
	SCCBWrite(ALL_LED_ON_L_REG, 0);
	SCCBWrite(ALL_LED_ON_H_REG, 0);
	SCCBWrite(ALL_LED_OFF_L_REG, 0);
	SCCBWrite(ALL_LED_OFF_H_REG, 0);

	// MODE1とMODE2を再設定
	SCCBWrite(MODE2_REGISTER, OUTDRV);
	SCCBWrite(MODE1_REGISTER, ALLCALL);
	_wait(10000);

	osc_clock = 25e6;   // [Hz]
	update_rate = 160;   // [Hz]
	resolution = 4096;
	prescale_value = (osc_clock / (resolution * update_rate)) - 1;

	old_mode = SCCBRead(MODE1_REGISTER);
	new_mode = (old_mode & 0x7F) | 0x10;
	SCCBWrite(MODE1_REGISTER, new_mode);   // Sleepモード設定

	SCCBWrite(PRE_SCALE_REGISTER, prescale_value);	// プレスケール値の書き込み
	SCCBWrite(MODE1_REGISTER, old_mode);            // Sleepモード解除
	_wait(10000);                                   // wait for oscillator
	SCCBWrite(MODE1_REGISTER, (old_mode | RESTART));

	duty_cycle_max = 2.25;  // [ms]
	duty_cycle_min = 0.75;  // [ms]
	max_travel = 140;       // [deg]
	pwm_period = 1 / update_rate * 1000;	// [ms]
	pwm_max = duty_cycle_max / pwm_period * resolution;
	pwm_min = duty_cycle_min / pwm_period * resolution;

//	tmpint = sizeof(control_angle_0);
	control_angle_0 = 90;     // [deg]
	control_angle_1 = 110;     // [deg]

	blockH = 0;
	blockV = 0;

	while (1) {
		colsel = *(volatile unsigned long *)COLSEL_BASE;

		anspos = (blockV << 5) + blockH;
		*(volatile unsigned long *)ANSADDR_BASE = anspos;
		answer = *(volatile unsigned long *)ANSDATA_BASE;

		if((colsel == 0 && answer == 0) || (colsel == 1 && answer == 1)
			|| (colsel == 2 && answer == 2) || (colsel == 3 && answer == 3)) {
			tmpint = blockH;
			tmpint = blockV;
			if(blockH < 10) {
				control_angle_1 -= 5;
			} else {
				control_angle_1 += 5;
			}

 			if(blockV < 8) {
				control_angle_0 += 5;
			} else {
				control_angle_0 -= 5;
			}

			pwm_duty_1 = pwm_min + (pwm_max - pwm_min) / max_travel * control_angle_1; // 角度をPWM出力値に変換
			_wait(10000);
			SCCBWrite(LED1_ON_L_REG, 0);   // Delay time = 0
			SCCBWrite(LED1_ON_H_REG, 0);   // Delay time = 0
			SCCBWrite(LED1_OFF_L_REG, pwm_duty_1 & 0xFF);
			SCCBWrite(LED1_OFF_H_REG, (pwm_duty_1 >> 8) & 0xFF); 
			_wait(150000);

			pwm_duty_0 = pwm_min + (pwm_max - pwm_min) / max_travel * control_angle_0; // 角度をPWM出力値に変換
			_wait(10000);
			SCCBWrite(LED0_ON_L_REG, 0);   // Delay time = 0%
			SCCBWrite(LED0_ON_H_REG, 0);   // Delay time = 0%
			SCCBWrite(LED0_OFF_L_REG, pwm_duty_0 & 0xFF);
			SCCBWrite(LED0_OFF_H_REG, (pwm_duty_0 >> 8) & 0xFF); 
			_wait(150000);
    		}

		if(blockH == 19) {
			blockH = 0;
			if(blockV == 15) {
				blockV = 0;
			} else {
				blockV++;
			}
		} else {
			blockH++;
		}
	}

	return 0;
}