#![cfg_attr(not(test), no_std)]
#![no_main]
#![feature(abi_avr_interrupt, cell_update)]

use core::sync::atomic::Ordering;

use arduino_hal::{default_serial, delay_ms, hal::port::{PD4, PD5}, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
use avr_device::interrupt;
use encoder::{rotations, setup_encoder, update_encoder, COUNTS, TICKS_PER_ROT};
use panic_halt as _;
mod servo;
use servo::{configure_timer_one, Servo};
use ufmt::uwriteln;

use crate::servo::calculate_duty;
mod encoder;

// d3: encoder
// d4: encoder
// d5: limit switch
// d6: lift piston
// d9: carriage
// d10: claw

#[arduino_hal::entry]
fn main() -> ! {
    let dp = arduino_hal::Peripherals::take().unwrap();
    let pins = arduino_hal::pins!(dp);

    let mut serial = arduino_hal::Usart::new(                                        
        dp.USART0,                                                               
        pins.d0,                                                                 
        pins.d1.into_output(),                                                   
        arduino_hal::hal::usart::BaudrateArduinoExt::into_baudrate(57600),       
    );       

    // configure encoder
    setup_encoder(pins.d3, pins.d4, dp.EXINT);

    // configure timer for servos
    pins.d9.into_output(); // carriage
    pins.d10.into_output(); // claw
    let (carriage, claw) = configure_timer_one(&dp.TC1);

    let switch = pins.d5.into_pull_up_input();

    //home(&carriage, &switch);

    //uwriteln!(serial, "homed");

    //claw.set_speed(-0.8);
    //delay_ms(500);
    //claw.set_speed(0.0);
    //uwriteln!(serial, "gripped");
    //move_to(&carriage, 0.125);
    //uwriteln!(serial, "moved");
    ////claw.set_speed(0.4);

    loop {
        //uwriteln!(serial, "svith {}", switch.is_high());
        let gain = rotations();
        uwriteln!(serial, "speed {}", (gain * 1000.) as i16);
        //uwriteln!(serial, "speed {}", calculate_duty(gain));

        carriage.set_speed(gain);
        arduino_hal::delay_ms(20);
    }
}

const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
const ACCEPTABLE_ERROR: f32 = 0.02; // rotations
const KP: f32 = 2.2;
const MIN_SPEED: f32 = 0.128; // motor speed that overcomes the friction of the table
fn move_to(carriage: &OCR1A, position: f32) {
    loop {
        let current = rotations();
        let setpoint = approach(current, position, MAX_VELOCITY);
        let error = current - setpoint;

        let out = error * KP;
        carriage.set_speed(-half_deadzone(out, MIN_SPEED));
        if abs(error) < ACCEPTABLE_ERROR && setpoint == position {
            break;
        }
        arduino_hal::delay_ms(10);
    }
}

/// If val is != 0, map it from 0..1 to min..1 (or negative)
fn half_deadzone(val:f32, min:f32) -> f32 {
    if val == 0. { return 0.; }
    let neg = val < 0.;
    let val = abs(val);

    let val = (val * (1. - min)) + min;
    if neg {-val} else {val}
}

fn approach(current: f32, goal: f32, max: f32) -> f32 {
    if current > goal {
        goal.max(current - max)
    } else {
        goal.min(current + max)
    }
}

// not in core for whatever reason (probably nan or something)
fn abs(val:f32) -> f32 {
    // TODO: bit-twiddling
    if val < 0. {
        -val
    } else {
        val
    }
}

fn home(carriage: &OCR1A, switch: &Pin<Input<PullUp>, PD5>) {
    carriage.set_speed(-0.8);
    while switch.is_high() { arduino_hal::delay_us(5) };
    carriage.set_speed(0.7);
    while switch.is_low() { arduino_hal::delay_us(5) };
    carriage.set_speed(0.0);

    interrupt::free(|cs| COUNTS.borrow(cs).set(0));
}