//! Stage controller for openflexure-delta like stage controllers.

use std::time::Duration;
use thiserror::Error;
use log::debug;

fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3<f64> {
    nalgebra::matrix![
        theta.cos(), -theta.sin(), 0.0;
        theta.sin(), theta.cos(), 0.0;
        0.0, 0.0, 1.0
    ]
}

fn new_delta_into_cartesian(flex_h: f64, flex_a: f64, flex_b: f64) -> nalgebra::Matrix3<f64> {
    let x_fac = (2.0 / 3.0f64.sqrt()) * (flex_b / flex_h);
    let y_fac = -1.0 * flex_b / flex_h;
    let z_fac = 1.0/3.0 * flex_b / flex_a;
    nalgebra::matrix![
        -x_fac, x_fac, 0.0;
        0.5 * y_fac, 0.5 * y_fac, -y_fac;
        z_fac, z_fac, z_fac
    ]
}

#[derive(Error, Debug)]
pub enum StageIOError {
    #[error("could not connect to stage controller")]
    Open(#[from] serialport::Error),
    #[error("error when sending data to the stage controller")]
    Write(std::io::Error),
    #[error("error when receiving data from the stage controller")]
    Read(std::io::Error),
}

#[derive(Error, Debug)]
pub enum StageError {
    #[error("Interface issue")]
    Communication(#[from] StageIOError),
    #[error("protocol error")]
    Mumble,
}

pub struct StageIO {
    serial: Box<dyn serialport::SerialPort>,
}

pub struct Stage {
    stage_io: StageIO,
    delta_pos: nalgebra::Vector3<i32>,
    cartesian_to_delta: nalgebra::Matrix3<f64>,
    delta_to_cartesian: nalgebra::Matrix3<f64>,
}

pub type StageIOResult<T> = std::result::Result<T, StageIOError>;
impl StageIO {
    pub fn new(port: &str, speed: u32) -> StageIOResult<Self> {
	let serial = serialport::new(port, speed).timeout(Duration::from_secs(60)).open()?;
	// TODO: read and discard initial characters; some devices like to be
	// chatty on start-up.
	Ok(Self { serial })
    }

    fn receive_until<S: AsRef<str>>(&mut self, needle: S) -> StageIOResult<String> {
        let mut res: Vec<u8> = vec![];
        loop {
            let mut buf = [0u8; 1];
            self.serial.read_exact(&mut buf).map_err(|e| StageIOError::Read(e))?;
            res.push(buf[0]);
            if res.ends_with(needle.as_ref().as_bytes()) {
                return Ok(String::from_utf8_lossy(&res).to_string());
            }
        }
    }

    // Send request and wait for response deliminated with a newline
    pub fn send_request<S: AsRef<str>>(&mut self, command: S) -> StageIOResult<String> {
	debug!("->: {:?}", command.as_ref());
        self.serial.write_all(command.as_ref().as_bytes()).map_err(|e| StageIOError::Write(e))?;
        let response = self.receive_until("\n")?;
	debug!("<-: {:?}", response);
        Ok(response.trim().to_string())
    }
}

pub type Result<T> = std::result::Result<T, StageError>;
impl Stage {
    pub fn new(stage_io: StageIO, camera_rotation: f64) -> Result<Self> {
        // TODO: non-blocking moves
        let delta_to_cartesian = new_camera_rotation(camera_rotation).try_inverse().unwrap() * new_delta_into_cartesian(80.0, 50.0, 50.0);
        let cartesian_to_delta = delta_to_cartesian.try_inverse().unwrap();
        let mut res = Self { stage_io, delta_pos: nalgebra::vector![0, 0, 0], cartesian_to_delta, delta_to_cartesian };

        let pos = res.stage_io.send_request("p?")?.split(" ").map(|v| v.parse::<i32>().map_err(|_| StageError::Mumble)).collect::<std::result::Result<Vec<_>, _>>()?;
        res.delta_pos = nalgebra::vector![pos[0], pos[1], pos[2]];
        println!("Initialized stage at delta positions {}/{}/{}", pos[0], pos[1], pos[2]);
        Ok(res)
    }


    pub fn get_position_delta(&self) -> nalgebra::Vector3<i32> {
        self.delta_pos.clone()
    }

    pub fn get_position_cartesian(&self) -> nalgebra::Vector3<f64> {
        let pos = nalgebra::vector![self.delta_pos[0] as f64, self.delta_pos[1] as f64, self.delta_pos[2] as f64];
        self.delta_to_cartesian * pos
    }

    pub fn move_relative_delta(&mut self, d: nalgebra::Vector3<i32>) -> Result<()> {
        self.stage_io.send_request(format!("mr {} {} {}\n", d[0], d[1], d[2]))?;
        self.delta_pos += nalgebra::vector![d[0], d[1], d[2]];
        Ok(())
    }

    pub fn move_absolute_delta(&mut self, a: nalgebra::Vector3<i32>) -> Result<()> {
        let diff = a - self.delta_pos;
        self.move_relative_delta(diff)
    }

    pub fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3<f64>) -> Result<()> {
        let current = self.get_position_delta();
        let target = self.cartesian_to_delta * a;
        let target = nalgebra::vector![target[0] as i32, target[1] as i32, target[2] as i32];

        // Calculate backlash compensation - always approach from the same
        // direction by at least 500 units.
        let mut backlash = target.clone();
        for i in [0, 1, 2] {
            if current[i] > target[i] {
                backlash[i] -= 500;
            }
        }
        if target != backlash {
            // Backlash compensation move before main move, if needed.
            self.move_absolute_delta(backlash)?;
        }
        self.move_absolute_delta(target)
    }

    pub fn move_relative_cartesian(&mut self, d: nalgebra::Vector3<f64>) -> Result<()> {
        let abs = self.get_position_cartesian() + d;
        self.move_absolute_cartesian(abs)
    }

    pub fn version(&mut self) -> Result<String> {
        Ok(self.stage_io.send_request("version\n")?)
    }
}