#![no_std]
#![no_main]

use core::{
    arch::asm,
    f32::consts::PI,
    ops::{Add, Mul, Sub},
    slice::Chunks,
};

use fpga_lib::{
    interface::{Buttons, Leds},
    panic_handler,
    register::X31,
    vga::{self, BLACK, BLUE, Color, FONT_WIDTH, GREEN, HEIGHT, RED, VGA, WHITE, WIDTH},
};

panic_handler! {}

#[derive(Clone, Copy)]
pub struct Vec3(f32, f32, f32);
#[derive(Clone, Copy)]
pub struct Mat3x3([[f32; 3]; 3]);

pub fn map_float(x: f32, xmin: f32, xmax: f32, ymin: f32, ymax: f32) -> f32
{
    ((x - xmin) / (xmax - xmin)) * (ymax - ymin) + ymin
}

impl Mat3x3
{
    fn rot_x(amount: f32) -> Self
    {
        Mat3x3([
            [libm::cosf(amount), -libm::sinf(amount), 0.],
            [libm::sinf(amount), libm::cosf(amount), 0.],
            [0., 0., 1.],
        ])
    }

    fn rot_y(amount: f32) -> Self
    {
        Mat3x3([
            [1., 0., 0.],
            [0., libm::cosf(amount), -libm::sinf(amount)],
            [0., libm::sinf(amount), libm::cosf(amount)],
        ])
    }

    fn rot_z(amount: f32) -> Self
    {
        Mat3x3([
            [libm::cosf(amount), 0., -libm::sinf(amount)],
            [0., 1., 0.],
            [libm::sinf(amount), 0., libm::cosf(amount)],
        ])
    }
}

impl Mul<Mat3x3> for Mat3x3
{
    type Output = Mat3x3;

    fn mul(self, rhs: Mat3x3) -> Self::Output
    {
        Mat3x3([
            [
                self.0[0][0] * rhs.0[0][0]
                    + self.0[0][1] * rhs.0[1][0]
                    + self.0[0][2] * rhs.0[2][0],
                self.0[0][0] * rhs.0[0][1]
                    + self.0[0][1] * rhs.0[1][1]
                    + self.0[0][2] * rhs.0[2][1],
                self.0[0][0] * rhs.0[0][2]
                    + self.0[0][1] * rhs.0[1][2]
                    + self.0[0][2] * rhs.0[2][2],
            ],
            [
                self.0[1][0] * rhs.0[0][0]
                    + self.0[1][1] * rhs.0[1][0]
                    + self.0[1][2] * rhs.0[2][0],
                self.0[1][0] * rhs.0[0][1]
                    + self.0[1][1] * rhs.0[1][1]
                    + self.0[1][2] * rhs.0[2][1],
                self.0[1][0] * rhs.0[0][2]
                    + self.0[1][1] * rhs.0[1][2]
                    + self.0[1][2] * rhs.0[2][2],
            ],
            [
                self.0[2][0] * rhs.0[0][0]
                    + self.0[2][1] * rhs.0[1][0]
                    + self.0[2][2] * rhs.0[2][0],
                self.0[2][0] * rhs.0[0][1]
                    + self.0[2][1] * rhs.0[1][1]
                    + self.0[2][2] * rhs.0[2][1],
                self.0[2][0] * rhs.0[0][2]
                    + self.0[2][1] * rhs.0[1][2]
                    + self.0[2][2] * rhs.0[2][2],
            ],
        ])
    }
}

impl Mul<Vec3> for Mat3x3
{
    type Output = Vec3;

    fn mul(self, rhs: Vec3) -> Self::Output
    {
        Vec3(
            self.0[0][0] * rhs.0 + self.0[0][1] * rhs.1 + self.0[0][2] * rhs.2,
            self.0[1][0] * rhs.0 + self.0[1][1] * rhs.1 + self.0[1][2] * rhs.2,
            self.0[2][0] * rhs.0 + self.0[2][1] * rhs.1 + self.0[2][2] * rhs.2,
        )
    }
}

impl Mul<f32> for Vec3
{
    type Output = Vec3;

    fn mul(self, rhs: f32) -> Self::Output
    {
        Vec3(self.0 * rhs, self.1 * rhs, self.2 * rhs)
    }
}

const CUBE: [Vec3; 8] = [
    Vec3(1., 1., 1.),
    Vec3(1., 1., -1.),
    Vec3(1., -1., -1.),
    Vec3(1., -1., 1.),
    Vec3(-1., 1., 1.),
    Vec3(-1., 1., -1.),
    Vec3(-1., -1., -1.),
    Vec3(-1., -1., 1.),
];

#[rustfmt::skip]
const LINES: [(usize, usize); 12] = [
    // Face 1
    (0, 1),
    (1, 2),
    (2, 3),
    (3, 0),

    // Face 2
    (4, 5),
    (5, 6),
    (6, 7),
    (7, 4),

    // Cross faces
    (0, 4),
    (1, 5),
    (2, 6),
    (3, 7)
];

const PRISM: [Vec3; 5] = [
    Vec3(-1., 1., 1.),
    Vec3(-1., 1., -1.),
    Vec3(-1., -1., -1.),
    Vec3(-1., -1., 1.),
    Vec3(1., 0., 0.),
];

const PRISM_LINES: [(usize, usize); 8] = [
    (0, 1),
    (1, 2),
    (2, 3),
    (3, 0),
    (0, 4),
    (1, 4),
    (2, 4),
    (3, 4),
];

fn draw_line(xa: u16, ya: u16, xb: u16, yb: u16, color: Color)
{
    // calculate dx & dy
    let dx = xb as i16 - xa as i16;
    let dy = yb as i16 - ya as i16;

    // calculate steps required for generating pixels
    let steps = if dx.abs() > dy.abs()
    {
        dx.abs()
    }
    else
    {
        dy.abs()
    };

    // calculate increment in x & y for each steps
    let x_inc: f32 = dx as f32 / steps as f32;
    let y_inc: f32 = dy as f32 / steps as f32;

    // Put pixel for each step
    let mut x = xa as f32;
    let mut y = ya as f32;
    for _ in 0..steps
    {
        //putpixel(round(X), round(Y), RED); // put pixel at (X,Y)
        unsafe {
            VGA::write_pixel_unsafe(x as u16, y as u16, color);
        }
        x += x_inc; // increment in x at each step
        y += y_inc; // increment in y at each step
        // generation step by step
    }
}

pub fn draw_shape(positions: &[Vec3], lines: &[(usize, usize)], color: Color)
{
    for (la, lb) in lines
    {
        let a = positions[*la] * 0.5;
        let b = positions[*lb] * 0.5;

        let xa = map_float(a.0, -1., 1., 0., 320.);
        let ya = map_float(a.1, -1., 1., 0., 240.);

        let xb = map_float(b.0, -1., 1., 0., 320.);
        let yb = map_float(b.1, -1., 1., 0., 240.);

        draw_line(xa as u16, ya as u16, xb as u16, yb as u16, color);
    }
}

const CPU_FREQ: usize = 80_000_000;

#[unsafe(no_mangle)]
pub extern "C" fn main() -> !
{
    // CLEAR
    for i in 0..320
    {
        for j in 0..240
        {
            unsafe {
                VGA::write_pixel_unsafe(i, j, BLACK);
            }
        }
    }

    //draw_cube(&CUBE, WHITE);

    let mut rx = 0.;
    let mut ry = 0.;
    let mut rz = 0.;
    let mut prev_cube = CUBE;
    let mut prev_prism = PRISM;
    loop
    {
        rx += 0.03;
        ry += 0.02;
        rz += 0.05;
        let mut cube = CUBE;
        let mut prism = PRISM;

        let mc = Mat3x3::rot_x(rx) * Mat3x3::rot_y(ry) * Mat3x3::rot_z(rz);
        let mp = Mat3x3::rot_x(-PI / 2.) * Mat3x3::rot_y(rx) * Mat3x3::rot_z(0.1);
        for x in cube.iter_mut()
        {
            *x = mc * *x;
        }

        for x in prism.iter_mut()
        {
            *x = mp * *x;
            *x = *x * 0.5;
        }

        draw_shape(&prev_prism, &PRISM_LINES, BLACK);
        draw_shape(&prism, &PRISM_LINES, BLUE);

        draw_shape(&prev_cube, &LINES, BLACK);
        draw_shape(&cube, &LINES, WHITE);

        for _ in 0..(CPU_FREQ / 60)
        {
            unsafe {
                asm!("nop");
            }
        }

        prev_cube = cube;
        prev_prism = prism;
    }
}
