/*
 * File:   main.c
 * Author: thebears
 *
 * Created on September 13, 2021, 3:12 PM
 */

#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 "helpers_i2c.h"
#include <avr/interrupt.h> 
#include <xc.h>
#include "twi0_master_example.h"
uint16_t adcVal;
uint8_t val;
void ADC0_init(void);
uint16_t ADC0_read_a(void);
uint16_t ADC0_read_b(void);
bool limit_zoom = false;
bool limit_focus = false;


uint16_t step_n_zoom = 0;
uint16_t step_n_focus = 0;
uint16_t step_n_iris = 0;


#define max_zoom_steps  2994
#define max_focus_steps 5180
#define max_iris_steps  77
uint8_t phase;

int8_t last_dir_zoom = 0;
int8_t last_dir_focus = 0;

bool focus_iris_max;
bool focus_iris_min;
bool focus_zoom_max;
bool focus_zoom_min;
bool focus_past_max;
bool focus_past_min;
// Zoom 
#define m1_p1 2
#define m1_p2 3
#define m1_p3 4
#define m1_p4 5
#define deltime1 750

// Focus
#define m2_p1 5
#define m2_p2 4
#define m2_p3 1
#define m2_p4 0
#define deltime2 750

// P-Iris
#define m3_p1 4
#define m3_p2 5
#define m3_p3 6
#define m3_p4 7
#define deltime3 2500

#define delay_time 100


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_iris_phased(int steps) {



    for (int i = 0; i < abs(steps); i++) {
        phase = abs(step_n_iris % 4);
        focus_iris_max = (step_n_iris >= max_iris_steps) && (steps > 0);
        focus_iris_min = (step_n_iris <= 0) && (steps < 0);

        if (focus_iris_max || focus_iris_min) {
            return;
        }

        step_phased(&PORTC, &PORTC, &PORTC, &PORTC, m3_p1, m3_p2, m3_p3, m3_p4, phase);
        _delay_us(deltime3);
        if (steps > 0) {
            step_n_iris++;
        } else if (steps < 0) {
            step_n_iris--;
        }
    }
}

void step_zoom_phased(int steps) {


    for (int i = 0; i < abs(steps); i++) {
        phase = abs(step_n_zoom % 4);
        focus_zoom_max = (step_n_zoom >= max_zoom_steps) && (steps > 0);
        focus_zoom_min = (step_n_zoom <= 0) && (steps < 0);
        if (focus_zoom_max || focus_zoom_min) {
            return;
        }

        step_phased(&PORTA, &PORTA, &PORTA, &PORTA, m1_p1, m1_p2, m1_p3, m1_p4, phase);
        _delay_us(deltime1);
        if (steps > 0) {
            step_n_zoom++;
        } else if (steps < 0) {
            step_n_zoom--;
        }


    }

}

void step_focus_phased(int steps) {
    for (int i = 0; i < abs(steps); i++) {
        focus_past_max = (step_n_focus >= max_focus_steps) && (steps > 0);
        focus_past_min = (step_n_focus <= 0) && (steps < 0);
        if (focus_past_max || focus_past_min) {
            return; 
        }

        phase = abs(step_n_focus % 4);
        step_phased(&PORTB, &PORTB, &PORTC, &PORTC, m2_p1, m2_p2, m2_p3, m2_p4, phase);
        _delay_us(deltime2);
        if (steps > 0) {
            step_n_focus++;
        } else if (steps < 0) {
            step_n_focus--;
        }
    }

}

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

bool update_focus_limit() {

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

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 home_focus() {
    step_n_focus = max_focus_steps + 200;
    step_focus_phased(-100);
    for (int i = 0; i < max_focus_steps; i++) {
        step_focus_phased(-1);
        if (update_focus_limit()) {
            break;
        }
    }
    step_n_focus = 0;
}

void home_zoom() {
    step_n_zoom = max_zoom_steps + 200;
    step_zoom_phased(-100);
    for (int i = 0; i < max_zoom_steps; i++) {
        step_zoom_phased(-1);
        if (update_zoom_limit()) {
            break;
        }
    }
    step_n_zoom = 0;
}

void home_iris() {
    step_n_iris = max_iris_steps + 200;
    step_iris_phased(-100);
    step_n_iris = 0;

}

ISR(PORTD_PORT_vect) {

    if (PORTD.INTFLAGS & PIN1_bm) {
        update_focus_limit();
        PORTD.INTFLAGS &= PIN1_bm;
    } else if (PORTD.INTFLAGS & PIN0_bm) {
        update_zoom_limit();
        PORTD.INTFLAGS &= PIN0_bm;

    };
}

int main(void) {


    PORTA.OUTSET = _BV(6);


    PORTD.DIRCLR = PIN0_bm;
    PORTD.DIRCLR = PIN1_bm;

    PORTD.PIN0CTRL |= PORT_ISC_BOTHEDGES_gc;
    PORTD.PIN1CTRL |= PORT_ISC_BOTHEDGES_gc;

    // Turn on 3.3V relay
    PORTA.DIRSET = _BV(6);


    // Zoom 
    PORTA.DIRSET = _BV(m1_p1);
    PORTA.DIRSET = _BV(m1_p4);
    PORTA.DIRSET = _BV(m1_p2);
    PORTA.DIRSET = _BV(m1_p3);

    // Focus 
    PORTB.DIRSET = _BV(m2_p1);
    PORTB.DIRSET = _BV(m2_p2);
    PORTC.DIRSET = _BV(m2_p3);
    PORTC.DIRSET = _BV(m2_p4);

    // Iris motor
    PORTC.DIRSET = _BV(m3_p1);
    PORTC.DIRSET = _BV(m3_p2);
    PORTC.DIRSET = _BV(m3_p3);
    PORTC.DIRSET = _BV(m3_p4);

    // IR Filter
    PORTB.DIRSET = _BV(2);
    PORTB.DIRSET = _BV(3);







    //    sei();
    PORTMUX.TWISPIROUTEA = 0x23;

    _delay_ms(250);
    I2C0_Initialize();
    I2C0_example_write1ByteRegister(32, 0x00, 0x00);
    I2C0_example_write1ByteRegister(32, 0x09, 0xFF);
    USART0_init();
    USART0_sendString("Started\n");

    _delay_ms(250);
    I2C0_example_write1ByteRegister(32, 0x09, 0x00);
    _delay_ms(250);
    I2C0_example_write1ByteRegister(32, 0x09, 0xFF);

    USART0_sendString("focus->");
    home_focus();
    USART0_sendString("\niris->");
    home_iris();
    USART0_sendString("\nhome->");
    home_zoom();
USART0_sendString("\n");

    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);
        
        
        
    _delay_ms(250);
        
        //        ir_filter_off();
        //        step_iris_phased(76);
        //        step_iris_phased(-76);
        //    _delay_ms(25);
        //    ir_filter_on();
        //    step_iris(19);
        //_delay_ms(250);

        //        _delay_ms(500);

        //        _delay_ms(500);
        //
        //        step_focus(1000);
        //        step_focus(-1000);
        //        limit_focus = false;
        //        step_focus(-1000);
        //        limit_focus = false;
        //        step_zoom(1000);
        //        limit_zoom = false;
        //        step_zoom(-1000);
        //        limit_zoom = false;

        //        step_focus(1000);
        //        if (limit_focus)
        //        {
        //            limit_focus = false;
        //            step_focus(-1000);
        //        }

        //        step(1000);
        //        step2(1000);

        //        _delay_ms(100);
        USART0_sendString("-");

        //        uart_print_uint8(PORTD.IN & PIN0_bm,"p0 ");
        //        uart_print_uint8(PORTD.IN & PIN1_bm,"p1 \n ")
        //                step2(1000);
        //        PORTA.OUTSET = _BV(m1_p1);
        //        PORTA.OUTSET = _BV(m1_p2);
        //        PORTA.OUTSET = _BV(m1_p3);
        //        PORTA.OUTSET = _BV(m1_p4);
        //        
        //        PORTB.OUTSET = _BV(m2_p1);
        //        PORTB.OUTSET = _BV(m2_p2);
        //        PORTC.OUTSET = _BV(m2_p3);
        //        PORTC.OUTSET = _BV(m2_p4);

        //        uart_print_binary(val, " \n");
        //        adcVal = ADC0_read_a();
        //        uart_print_uint8(adcVal, " ");
        //        adcVal = ADC0_read_b();
        //        uart_print_uint8(adcVal, " ");

        //        step();
        //        step2();
        //        USART0_sendString("---\n");
    }

    return 0;
}

void ADC0_init(void) {

    PORTD.PIN0CTRL &= ~PORT_ISC_gm;
    PORTD.PIN0CTRL |= PORT_ISC_INPUT_DISABLE_gc;
    PORTD.PIN0CTRL &= ~PORT_PULLUPEN_bm;

    PORTD.PIN1CTRL &= ~PORT_ISC_gm;
    PORTD.PIN1CTRL |= PORT_ISC_INPUT_DISABLE_gc;
    PORTD.PIN1CTRL &= ~PORT_PULLUPEN_bm;

    ADC0.CTRLC = ADC_PRESC_DIV4_gc
            | ADC_REFSEL_VDDREF_gc;
    ADC0.CTRLA = ADC_ENABLE_bm /* ADC Enable: enabled */
            | ADC_RESSEL_10BIT_gc; /* 10-bit mode */
    /* Select ADC channel */

}

uint16_t ADC0_read_a(void) {
    /* Start ADC conversion */
    /* Start ADC conversion */
    ADC0.MUXPOS = ADC_MUXPOS_AIN0_gc;
    ADC0.COMMAND = ADC_STCONV_bm;
    /* Wait until ADC conversion done */
    while (!(ADC0.INTFLAGS & ADC_RESRDY_bm)) {
        ;
    }
    /* Clear the interrupt flag by writing 1: */
    ADC0.INTFLAGS = ADC_RESRDY_bm;
    return ADC0.RES;
}

uint16_t ADC0_read_b(void) {
    /* Start ADC conversion */
    /* Start ADC conversion */
    ADC0.MUXPOS = ADC_MUXPOS_AIN1_gc;
    ADC0.COMMAND = ADC_STCONV_bm;
    /* Wait until ADC conversion done */
    while (!(ADC0.INTFLAGS & ADC_RESRDY_bm)) {
        ;
    }
    /* Clear the interrupt flag by writing 1: */
    ADC0.INTFLAGS = ADC_RESRDY_bm;
    return ADC0.RES;
}