extern crate frcds;

use std::thread;

use frcds::RoboRio;
use frcds::ds_to_rio::udp::*;
use frcds::ds_to_rio::udp::tags::*;
use frcds::rio_to_ds::udp::UdpInPacket;

use std::time::Duration;

fn main() {

    let control = Control::AUTONOMOUS | Control::ENABLED;
    let request = Request::empty();
    let alliance = Alliance::Red(1);
    let tags: Vec<Box<Tag>> = vec!(Box::new(Countdown(35.00001)));//, Box::new(Joystick::new(vec!(-120, 110, 0),vec!(true, false, false, true, true, false, true, false, true, true, true), vec!(-1, 180))));

    let mut buf = [0; 20];

    let mut udp = UdpOutPacket {
        sequence: 0,
        comm_version: 0x01,
        control,
        request,
        alliance,
        tags,
    };


    let rio = RoboRio::new(5119).unwrap();
    println!("{}", rio.target_ip);
    loop {
        rio.send_udp(udp.next()).unwrap();
        rio.recv_udp(&mut buf).unwrap();
        println!("{:?}", UdpInPacket::decode(&mut buf));
        thread::sleep(Duration::from_millis(20));
    }
}