use num::*;
use std::time::Instant;
use esp_idf_hal::i2c::*;
use esp_idf_hal::ledc::*;
use esp_idf_hal::gpio::*;
use esp_idf_hal::prelude::*;
use esp_idf_sys::ledc_set_freq;
use esp_idf_hal::delay::FreeRtos;
use ssd1306::mode::BufferedGraphicsMode;
use esp_idf_hal::peripherals::Peripherals;
use esp_idf_hal::ledc::config::TimerConfig;
use esp_idf_sys::ledc_mode_t_LEDC_LOW_SPEED_MODE;

use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306};

use embedded_graphics::{
    prelude::*,
    text::{Baseline, Text},
    pixelcolor::BinaryColor,
    mono_font::{ascii::FONT_6X10, MonoTextStyleBuilder},
    primitives::{Circle, PrimitiveStyleBuilder, Rectangle}
};

const SCORE_LIMIT: u32 = 5;

const WIN_NOTES: [u32; 12] = [
    880, 988, 523, 988, 523, 587, 
    523, 587, 659, 587, 659, 659
];

const LOSE_NOTES: [u32; 12] = [
    500, 450, 400, 350, 300, 250,
    500, 450, 400, 350, 300, 250
];

fn main() -> anyhow::Result<()> {
    let mut mcu_dir = 0;
    let mut mcu_score = 0;
    let mut player_score = 0;

    let mut ball_dir: [i32; 2] = [0, 0];
    let mut ball_pos: [i32; 2] = [60, 28];
    let mut ball_speed: [i32; 2] = [2, 2];
    let mut serve_dir: [i32; 2] = [1, 1];
    let mut mcu_pos: [i32; 2] = [12, 24];
    let mut player_pos: [i32; 2] = [110, 24];

    let mut mcu_impact = Instant::now();
    let mut player_impact = Instant::now();

    let peripherals = Peripherals::take().unwrap();
    let up = PinDriver::input(peripherals.pins.gpio2)?;
    let down = PinDriver::input(peripherals.pins.gpio3)?;
    
    let freq = 500.Hz().into();
    let res = Resolution::Bits10;
    let mut channel = LedcDriver::new(
        peripherals.ledc.channel0,
        LedcTimerDriver::new(
            peripherals.ledc.timer0,
            &TimerConfig::new().resolution(res).frequency(freq))?,
            peripherals.pins.gpio0
    )?;
    let tone_duty = channel.get_max_duty() / 2;
    
    let i2c0 = peripherals.i2c0;
    let sda = peripherals.pins.gpio9;
    let scl = peripherals.pins.gpio8;
    let disp = DisplaySize128x64;
    let rot = DisplayRotation::Rotate0;
    let config = I2cConfig::new().baudrate(100.kHz().into());
    let i2c = I2cDriver::new(i2c0, sda, scl, &config)?;
    let interface = I2CDisplayInterface::new(i2c);

    let mut display = Ssd1306::new(interface, disp, rot)
        .into_buffered_graphics_mode();

    let on = PrimitiveStyleBuilder::new()
        .stroke_width(1)
        .stroke_color(BinaryColor::On)
        .build();

    display.init().unwrap();

    Rectangle::new(Point::new(0, 0), Size::new(127, 63))
        .into_styled(on)
        .draw(&mut display)
        .unwrap();

    loop {
        clear_field(&mut display, player_pos, mcu_pos, ball_pos);

        if ball_pos[0] >= 116 || ball_pos[0] <= 10 { 
            score(
                &mut channel, &mut ball_pos, &mut ball_dir, &mut ball_speed,
                &mut serve_dir, &mut player_score, &mut mcu_score
            );

            if mcu_score >= SCORE_LIMIT || player_score >= SCORE_LIMIT {
                game_over(
                    &mut display, &mut channel, &mut player_pos, &mut mcu_pos, 
                    &mut ball_dir, &mut player_score, &mut mcu_score
                );
            }
            else
            {
                show_score(&mut display, player_score, mcu_score);
            }
        }

        let player_col = player_collision(player_pos, ball_pos, ball_speed);
        let player_last = player_impact.elapsed().as_millis() as u32;

        if  player_col && player_last > 500 {
            let u = up.is_low();
            let d = down.is_low();
            player_hit_ball(&mut ball_speed, &mut ball_dir, player_pos, u, d);
            player_impact = Instant::now();
            tone(&mut channel, 300, tone_duty, 50);
        }

        let mcu_col = mcu_collision(mcu_pos, ball_pos, ball_speed);
        let mcu_last = mcu_impact.elapsed().as_millis() as u32;

        if  mcu_col && mcu_last > 500 {
            mcu_hit_ball(mcu_dir, &mut ball_dir, &mut ball_speed);
            mcu_impact = Instant::now();
            tone(&mut channel, 200, tone_duty, 50);
        }

        if ball_bounced(&mut ball_dir, &mut ball_pos, ball_speed) { 
            tone(&mut channel, 100, tone_duty, 50);
        }

        let u = up.is_low();
        let d = down.is_low();
        player_paddle_ctrl(&mut player_pos, &mut ball_dir, serve_dir, u, d);
        mcu_paddle_ctrl(&mut mcu_pos, &mut mcu_dir, ball_pos, ball_dir, ball_speed);
        move_ball(&mut ball_pos, &mut ball_speed, ball_dir);
        draw_field(&mut display, player_pos, mcu_pos, ball_pos);
        display.flush().unwrap();
    }
}

fn move_ball(
    ball_pos: &mut [i32; 2],
    ball_speed: &mut [i32; 2],
    ball_dir: [i32; 2]
) {
    ball_speed[0] = clamp(ball_speed[0], 2, 6);
    ball_speed[1] = clamp(ball_speed[1], 2, 6);
    ball_pos[0] += ball_dir[0] * ball_speed[0];
    ball_pos[1] += ball_dir[1] * ball_speed[1];
}

fn ball_bounced(
    ball_dir: &mut [i32; 2],
    ball_pos: &mut [i32; 2],
    ball_speed: [i32; 2]
) -> bool {
    let hit_top = ball_pos[1] <= 1 + ball_speed[1];
    let hit_bottom = ball_pos[1] >= 58 - ball_speed[1];
    if  hit_top || hit_bottom { 
        if hit_top {
            ball_pos[1] = 2;
            ball_dir[1] = 1;
        }
        else {
            ball_pos[1] = 57;
            ball_dir[1] = -1;
        }
        return true;
    }
    return false;
}

fn player_collision(
    player_pos: [i32; 2],
    ball_pos: [i32; 2],
    ball_speed: [i32; 2]
) -> bool {
    let x_col = ball_pos[0] >= (108 - ball_speed[0]);
    let y_col_upper = (player_pos[1] + 8) - (ball_pos[1] + 2) <= 8;
    let y_col_lower = (ball_pos[1] + 2) - (player_pos[1] + 8) <= 8;

    if player_pos[1] + 8 >= ball_pos[1] + 2 {
        if x_col && y_col_upper {
            return true;
        }
    }
    else if x_col && y_col_lower {
        return true;
    }

    return false;
}

fn mcu_collision(
    mcu_pos: [i32; 2],
    ball_pos: [i32; 2],
    ball_speed: [i32; 2]
) -> bool {
    let x_col = ball_pos[0] <= (14 + ball_speed[0]);
    let y_col_upper = (mcu_pos[1] + 8) - (ball_pos[1] + 2) <= 8;
    let y_col_lower = (ball_pos[1] + 2) - (mcu_pos[1] + 8) <= 8;

    if mcu_pos[1] + 8 >= ball_pos[1] + 2 {
        if x_col && y_col_upper {
            return true;
        }
    }
    else if x_col && y_col_lower {
        return true;
    }

    return false;
}

fn player_hit_ball(
    ball_speed: &mut [i32; 2],
    ball_dir: &mut [i32; 2],
    player_pos: [i32; 2],
    up_btn: bool,
    down_btn: bool
) {
    if ball_dir[1] == -1 {
        if up_btn && player_pos[1] > 2  {
            ball_speed[1] += 1;
            ball_speed[0] -= 1;
        }
        if down_btn && player_pos[1] < 44 {
            ball_speed[1] -= 1;
            ball_speed[0] += 1;
        }
    }
    else if ball_dir[1] == 1 {
        if up_btn && player_pos[1] > 2  {
            ball_speed[1] -= 1;
            ball_speed[0] += 1;
        }
        if down_btn && player_pos[1] < 44 {
            ball_speed[1] += 1;
            ball_speed[0] -= 1;
        }
    }
    else {
        ball_speed[1] -= 1;
        ball_speed[0] -= 1;
    }

    ball_dir[0] = -1;
}

fn mcu_hit_ball(mcu_dir: i32,
    ball_dir: &mut [i32; 2],
    ball_speed: &mut [i32; 2]
) {
    if ball_dir[1] == -1 {
        if mcu_dir == -1 {
            ball_speed[1] += 1;
            ball_speed[0] -= 1;
        }
        if mcu_dir == 1 {
            ball_speed[1] -= 1;
            ball_speed[0] += 1;
        }
    }
    else if ball_dir[1] == 1 {
        if mcu_dir == -1 {
            ball_speed[1] -= 1;
            ball_speed[0] += 1;
        }
        if mcu_dir == 1 {
            ball_speed[1] += 1;
            ball_speed[0] -= 1;
        }
    }
    else {
        ball_speed[1] -= 1;
        ball_speed[0] -= 1;
    }
    
    ball_dir[0] = 1;
}

fn player_paddle_ctrl(
    player_pos: &mut [i32; 2],
    ball_dir: &mut [i32; 2],
    serve_dir: [i32; 2],
    up_btn: bool,
    down_btn: bool
) {
    if ball_dir[0] == 0 && (up_btn || down_btn) {
        ball_dir[0] = serve_dir[0];
        ball_dir[1] = serve_dir[1];
    }

    if up_btn && player_pos[1] > 2 {
        player_pos[1] -= 2;
    }

    if down_btn && player_pos[1] < 44 {
        player_pos[1] += 2;
    }
}

fn mcu_paddle_ctrl(
    mcu_pos: &mut [i32; 2],
    mcu_dir: &mut i32,
    ball_pos: [i32; 2],
    ball_dir: [i32; 2],
    ball_speed: [i32; 2]
) {
    if ball_dir[0] == -1 && ball_pos[0] <= 64 {
        let ball_far = ball_pos[0] >= 20;
        let ball_above = (ball_pos[1] + 2) < (mcu_pos[1] + 8);
        let distance_above = (ball_pos[1] + 2) - (mcu_pos[1] + 8);
        let distance_below = (mcu_pos[1] + 8) - (ball_pos[1] + 2);
        let wide_angle = ball_speed[1] > ball_speed[0];
        let predictable = ball_far && wide_angle;

        if ball_above && ball_dir[1] == -1 {
            *mcu_dir = -1;
        }
        else if !ball_above && ball_dir[1] == 1 {
            *mcu_dir = 1;
        }
        else if !ball_above && ball_dir[1] == -1 && predictable {
            *mcu_dir = -1;
        }
        else if ball_above && ball_dir[1] == 1 && predictable {
            *mcu_dir = 1;
        }
        else if ball_above && distance_above <= 10 && !ball_far {
            *mcu_dir = ball_dir[1] * -1;
        }
        else if !ball_above && distance_below <= 10 && !ball_far {
            *mcu_dir = ball_dir[1] * -1;
        }
        else {
            *mcu_dir = 0;
        }
    }
    else {
        if mcu_pos[1] > 24 {
            *mcu_dir = -1;
        }
        else if mcu_pos[1] < 24 {
            *mcu_dir = 1;
        }
        else {
            *mcu_dir = 0;
        }
    }

    mcu_pos[1] += *mcu_dir * 2;
    mcu_pos[1] = clamp(mcu_pos[1], 2, 44);
}

fn score(
    channel: &mut LedcDriver,
    ball_pos: &mut [i32; 2],
    ball_dir: &mut [i32; 2],
    ball_speed: &mut [i32; 2],
    serve_dir: &mut [i32; 2],
    player_score: &mut u32,
    mcu_score: &mut u32,
) {
    if ball_pos[0] >= 116 {
        *mcu_score += 1;
        ball_dir[0] = 1;
        serve_dir[0] = 1;
        tone(channel, 500, channel.get_max_duty() / 2, 250);
        tone(channel, 250, channel.get_max_duty() / 2, 250);
    }
    
    if ball_pos[0] <= 10 {
        *player_score += 1;
        ball_dir[0] = -1;
        serve_dir[0] = -1;
        tone(channel, 250, channel.get_max_duty() / 2, 250);
        tone(channel, 500, channel.get_max_duty() / 2, 250);
    }

    ball_pos[0] = 60;
    ball_pos[1] = 28;
    ball_speed[1] = 1;
    ball_speed[0] = 2;
    ball_dir[1] = serve_dir[1];
    serve_dir[1] *= -1;
}

fn draw_field(
    display: &mut Ssd1306<
        I2CInterface<esp_idf_hal::i2c::I2cDriver<'_>>,
        DisplaySize128x64,
        BufferedGraphicsMode<DisplaySize128x64>
    >,
    player_pos: [i32; 2],
    mcu_pos: [i32; 2],
    ball_pos: [i32; 2]
) {
    let on = PrimitiveStyleBuilder::new()
        .stroke_width(1)
        .stroke_color(BinaryColor::On)
        .build();

    Rectangle::new(Point::new(player_pos[0], player_pos[1]), Size::new(4, 16))
        .into_styled(on)
        .draw(display)
        .unwrap();

    Rectangle::new(Point::new(mcu_pos[0], mcu_pos[1]), Size::new(4, 16))
        .into_styled(on)
        .draw(display)
        .unwrap();

    Circle::new(Point::new(ball_pos[0], ball_pos[1]), 4)
        .into_styled(on)
        .draw(display)
        .unwrap();
}

fn clear_field(
    display: &mut Ssd1306<
        I2CInterface<esp_idf_hal::i2c::I2cDriver<'_>>,
        DisplaySize128x64,
        BufferedGraphicsMode<DisplaySize128x64>
    >,
    player_pos: [i32; 2],
    mcu_pos: [i32; 2],
    ball_pos: [i32; 2]
) {
    let off = PrimitiveStyleBuilder::new()
        .stroke_width(2)
        .stroke_color(BinaryColor::Off)
        .build();

    Rectangle::new(Point::new(player_pos[0], player_pos[1]), Size::new(4, 16))
        .into_styled(off)
        .draw(display)
        .unwrap();

    Rectangle::new(Point::new(mcu_pos[0], mcu_pos[1]), Size::new(4, 16))
        .into_styled(off)
        .draw(display)
        .unwrap();

    Circle::new(Point::new(ball_pos[0], ball_pos[1]), 4)
        .into_styled(off)
        .draw(display)
        .unwrap();
}

fn show_score(
    display: &mut Ssd1306<
        I2CInterface<esp_idf_hal::i2c::I2cDriver<'_>>,
        DisplaySize128x64,
        BufferedGraphicsMode<DisplaySize128x64>
    >,
    player_score: u32,
    mcu_score: u32
) {
    let on = PrimitiveStyleBuilder::new()
        .stroke_width(1)
        .stroke_color(BinaryColor::On)
        .build();

    let text_style = MonoTextStyleBuilder::new()
    .font(&FONT_6X10)
    .text_color(BinaryColor::On)
    .build();

    let score = format!("{} : {}", mcu_score, player_score);

    Text::with_baseline(&score, Point::new(50, 24), text_style, Baseline::Top)
        .draw(display)
        .unwrap();

    display.flush().unwrap();
    FreeRtos::delay_ms(1000);
    display.clear();

    Rectangle::new(Point::new(0, 0), Size::new(127, 63))
        .into_styled(on)
        .draw(display)
        .unwrap();
}

fn game_over(
    display: &mut Ssd1306<
        I2CInterface<esp_idf_hal::i2c::I2cDriver<'_>>,
        DisplaySize128x64,
        BufferedGraphicsMode<DisplaySize128x64>
    >,
    channel: &mut LedcDriver,
    player_pos: &mut [i32; 2],
    mcu_pos: &mut [i32; 2],
    ball_dir: &mut [i32; 2],
    player_score: &mut u32,
    mcu_score: &mut u32,
) {
    let on = PrimitiveStyleBuilder::new()
        .stroke_width(1)
        .stroke_color(BinaryColor::On)
        .build();

    let text_style = MonoTextStyleBuilder::new()
        .font(&FONT_6X10)
        .text_color(BinaryColor::On)
        .build();

    ball_dir[0] = 0;
    ball_dir[1] = 0;
    mcu_pos[1] = 24;
    player_pos[1] = 24;

    let msg = if *player_score >= SCORE_LIMIT { 
        "You win!!" 
    } 
    else { 
        "You lose!" 
    };

    Text::with_baseline(&msg, Point::new(38, 28), text_style, Baseline::Top)
        .draw(display)
        .unwrap();

    display.flush().unwrap();
    FreeRtos::delay_ms(500);

    if *player_score >= SCORE_LIMIT {
        melody(channel, channel.get_max_duty() / 2, WIN_NOTES, 2);
    }
    else {
        melody(channel, channel.get_max_duty() / 2, LOSE_NOTES, 1);
    }

    FreeRtos::delay_ms(2000);
    display.clear();

    Rectangle::new(Point::new(0, 0), Size::new(127, 63))
        .into_styled(on)
        .draw(display)
        .unwrap();

    *mcu_score = 0;
    *player_score = 0;
}

fn tone(
    channel: &mut LedcDriver,
    frequency: u32,
    duty_cycle: u32,
    delay: u32
) {
    let _ = channel.set_duty(duty_cycle);
    unsafe {
        ledc_set_freq(
            ledc_mode_t_LEDC_LOW_SPEED_MODE,
            channel.timer().into(),
            frequency
        );
    }
    FreeRtos::delay_ms(delay);
    let _ = channel.set_duty(0);
}

fn melody(
    channel: &mut LedcDriver,
    duty_cycle: u32,
    notes: [u32; 12],
    cycles: u32
) {
    for _ in 0..cycles {
        for note in notes {
            tone(channel, note, duty_cycle, 50);
        }
    }
}