#define F_CPU 3333333
#define USART0_BAUD_RATE(BAUD_RATE) ((float)(3333333 * 64 / (16 * (float)BAUD_RATE)) + 0.5)
#include <avr/io.h>
#include <stdbool.h>
#include <util/delay.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "helpers_uart.h"

#include <xc.h>
#include "twi0_master_example.h"


#define delay_time 50

// Zoom, Focus, Iris
struct PORT_struct *port_1[] = {&PORTA, &PORTB, &PORTC};
struct PORT_struct *port_2[] = {&PORTA, &PORTB, &PORTC};
struct PORT_struct *port_3[] = {&PORTA, &PORTC, &PORTC};
struct PORT_struct *port_4[] = {&PORTA, &PORTC, &PORTC};

uint8_t pin_1[] = {2, 5, 4};
uint8_t pin_2[] = {3, 4, 5};
uint8_t pin_3[] = {4, 1, 6};
uint8_t pin_4[] = {5, 0, 7};

uint8_t delay[] = {15, 15, 50}; // Multiples of "delay_time"
uint16_t max_steps[] = {2994, 5180, 77};
int16_t current_step[] = {0, 0, 0};

uint8_t enable_port[] = {2, 3, 4};

bool motor_max_pos;
bool motor_min_pos;
uint8_t phase;

uint8_t motor_num;
uint8_t port_val;

void init_port_expander()
{
	I2C0_example_write1ByteRegister(32, 0x00, 0x00);
	I2C0_example_write1ByteRegister(32, 0x00, 0x00);
}

void set_expanded_port_on(uint8_t num)
{
	port_val = I2C0_example_read1ByteRegister(32, 0x09);
	port_val |= _BV(num);
	I2C0_example_write1ByteRegister(32, 0x09, port_val);
}

void set_expanded_port_off(uint8_t num)
{
	port_val = I2C0_example_read1ByteRegister(32, 0x09);
	port_val &= ~_BV(num);
	I2C0_example_write1ByteRegister(32, 0x09, port_val);
}


void battery_charging_on()
{
	set_expanded_port_on(0);
}

void battery_charging_off()
{
	set_expanded_port_off(0);
}

void battery_power_supply_on()
{
	set_expanded_port_on(1);
}

void battery_power_supply_off()
{
	set_expanded_port_off(1);
}

void power_3v3_on()
{
	// Turn on 3.3V relay
	PORTA.DIRSET = _BV(6);
	PORTA.OUTSET = _BV(6);
}

void power_3v3_off()
{
	// Turn off 3.3V relay
	PORTA.DIRSET = _BV(6);
	PORTA.OUTCLR = _BV(6);
}


void delay_n_units(uint8_t n)
{
	for (int i = 0; i < n; i++) {
		_delay_us(delay_time);
	}

};

void step_phased(struct PORT_struct * p1, struct PORT_struct * p2, struct PORT_struct * p3, struct PORT_struct * p4, uint8_t m1, uint8_t m2, uint8_t m3, uint8_t m4, uint8_t phase)
{


	if (phase == 0) {
		p1->OUTSET = _BV(m1);
		p2->OUTCLR = _BV(m2);
		p3->OUTSET = _BV(m3);
		p4->OUTCLR = _BV(m4);
	} else if (phase == 1) {
		p1->OUTCLR = _BV(m1);
		p2->OUTSET = _BV(m2);
		p3->OUTSET = _BV(m3);
		p4->OUTCLR = _BV(m4);
	} else if (phase == 2) {
		p1->OUTCLR = _BV(m1);
		p2->OUTSET = _BV(m2);
		p3->OUTCLR = _BV(m3);
		p4->OUTSET = _BV(m4);
	} else if (phase == 3) {
		p1->OUTSET = _BV(m1);
		p2->OUTCLR = _BV(m2);
		p3->OUTCLR = _BV(m3);
		p4->OUTSET = _BV(m4);
	}

}

void step_motor(uint8_t i, int s)
{

	set_expanded_port_on(i);
	for (int n = 0; i < abs(s); n++) {

		phase = abs(current_step[i] % 4);
		motor_max_pos = (current_step[i] >= max_steps[i]) && (s > 0);
		motor_min_pos = (current_step[i] <= 0) && (s < 0);

		if (motor_max_pos || motor_min_pos) {
			return;
		}

		step_phased(port_1[i], port_2[i], port_3[i], port_4[i], pin_1[i], pin_2[i], pin_3[i], pin_4[i], phase);
		delay_n_units(delay[i]);

		if (s > 0) {
			current_step[i]++;
		} else if (s < 0) {
			current_step[i]--;
		}
	}


}

void step_iris_phased(int steps)
{
	set_expanded_port_on(2);
	step_motor(2, steps);
	set_expanded_port_off(2);
}

void step_zoom_phased(int steps)
{
	set_expanded_port_on(1);
	step_motor(1, steps);
	set_expanded_port_off(1);
}

void step_focus_phased(int steps)
{
	set_expanded_port_on(0);
	step_motor(0, steps);
	set_expanded_port_off(0);
}

bool at_zoom_limit()
{
	if (PORTD.IN & PIN0_bm) {
		return false;
	} else {
		return true;
	}
}

bool at_focus_limit()
{

	if (PORTD.IN & PIN1_bm) {
		return false;
	} else {
		return true;
	}

}

void home_focus()
{
	motor_num = 0;
	current_step[motor_num] = max_steps[motor_num] + 200;
	step_motor(motor_num, -100);
	for (int i = 0; i < max_steps[motor_num]; i++) {
		step_motor(motor_num, -1);
		if (at_focus_limit()) {
			break;
		}
	}
	current_step[motor_num] = 0;
}

void home_zoom()
{
	motor_num = 1;
	current_step[motor_num] = max_steps[motor_num] + 200;
	step_motor(motor_num, -100);
	for (int i = 0; i < max_steps[motor_num]; i++) {
		step_motor(motor_num, -1);
		if (at_zoom_limit()) {
			break;
		}
	}
	current_step[motor_num] = 0;
}

void home_iris()
{
	motor_num = 3;
	current_step[motor_num] = max_steps[motor_num] + 200;
	step_motor(motor_num, -100);
	current_step[motor_num] = 0;
}

void ir_filter_on()
{
	PORTB.OUTCLR = _BV(2);
	PORTB.OUTSET = _BV(3);
	_delay_ms(100);
	PORTB.OUTCLR = _BV(3);
}

void ir_filter_off()
{
	PORTB.OUTCLR = _BV(3);
	PORTB.OUTSET = _BV(2);
	_delay_ms(100);
	PORTB.OUTCLR = _BV(2);
}

void init_pins()
{
	// Pins for reading endstops on lens assembly
	PORTD.DIRCLR = PIN0_bm;
	PORTD.DIRCLR = PIN1_bm;

	// IR Filter
	PORTB.DIRSET = _BV(2);
	PORTB.DIRSET = _BV(3);
	
	//i2c muxing
	PORTMUX.TWISPIROUTEA = 0x23;
}

int main(void)
{

	I2C0_Initialize();
	init_port_expander();
	USART0_init();

	while (1) {
		step_zoom_phased(6000);
		step_iris_phased(100);
		step_focus_phased(2000);

		step_zoom_phased(-6000);
		step_iris_phased(-100);
		step_focus_phased(-2000);
	}
	return 0;
}