1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

use common_infrastructure::hals::{GenericHal, TrainHal};
use common_infrastructure::messages::{MasterMessage, TrainMessage};
use common_infrastructure::devices::Train;
use esp_idf_svc::log::EspLogger;
use esp_idf_svc::sys;
use anyhow::Result;
use common_infrastructure::Position;
use log::info;

mod train_hal;
use train_hal::EspTrainHal;

const THIS_TRAIN: Train = Train::T2;
const SPEED_OFFSET:f32 = 0.75;

fn main() -> Result<()> {
    sys::link_patches();
    EspLogger::initialize_default();

    let hal = EspTrainHal::new()?;

    let mut last_position_red = None;
    loop {
        
        let position = hal.read_position()?;
        if let Some(position) = position{
            if Some(position) != last_position_red{
                hal.send_message_to_master(
                    MasterMessage::TrainHasReachedPosition(THIS_TRAIN, position)
                )?;
                last_position_red = Some(position);
            }
        }

        let message = hal.get_message()?;
        if let Some(message) = message{
            match message {
                TrainMessage::SetSpeed(speed) => hal.set_speed(speed)?,
                TrainMessage::SetSpeedAndStopAt(speed,position) => {
                    hal.set_speed(speed)?;
                    stop_at_tag(&hal,  speed, position, last_position_red)?;
                    last_position_red = Some(position);
                    hal.send_message_to_master(MasterMessage::TrainHasReachedPosition(THIS_TRAIN, position))?;
                }
                _ => {}
            }
        }
        //hal.sleep_for_ms(1);
        //info!("alive");
    }
}

fn stop_at_tag(hal: &EspTrainHal<'_>, speed:i8, position_to_stop: Position, last_position: Option<Position>) -> Result<()>{

    loop{
        if let Some(position) = hal.read_position()?{

            if Some(position) == last_position{
                continue;
            }

            if position == position_to_stop{
                break;
            }

            // tell to the master that i have reached an unexpected position
            hal.send_message_to_master(MasterMessage::TrainHasReachedPosition(THIS_TRAIN, position))?;
            
            return Err(anyhow::anyhow!("Unexpected position!"));
        }
    }

    hal.set_speed(0)?;
    hal.sleep_for_ms(500);
    if speed>0{
        hal.set_speed(-25)?;
    }else{
        hal.set_speed(25)?;
    }
    
    loop{
        if let Some(position) = hal.read_position()?{

            if Some(position) == last_position{
                continue;
            }

            if position == position_to_stop{
                break;
            }

            // tell to the master that i have reached an unexpected position
            hal.send_message_to_master(MasterMessage::TrainHasReachedPosition(THIS_TRAIN, position))?;
            
            return Err(anyhow::anyhow!("Unexpected position!"));
        }
    }
    hal.set_speed(0)?;

    return Ok(());
}